Skip to content
Snippets Groups Projects
Commit 062343d4 authored by Raphael Grimm's avatar Raphael Grimm
Browse files

Move run for Cartesian impedance controller to class and use it in NJointController

parent f6ce7c8e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+ 136
− 99
View file @ 062343d4
......@@ -19,17 +19,133 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#include "NJointTaskSpaceImpedanceController.h"
#include <boost/algorithm/clamp.hpp>
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <SimoxUtility/math/compare/is_equal.h>
#include <VirtualRobot/MathTools.h>
#include "NJointTaskSpaceImpedanceController.h"
using namespace armarx;
int CartesianImpedanceController::bufferSize() const
{
return targetJointTorques.size();
}
void CartesianImpedanceController::initialize(const VirtualRobot::RobotNodeSetPtr& rns)
{
ARMARX_CHECK_NOT_NULL(rns);
tcp = rns->getTCP();
ARMARX_CHECK_NOT_NULL(tcp);
ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
const auto size = rns->getSize();
qpos.resize(size);
qvel.resize(size);
tcptwist.resize(size);
nullqerror.resize(size);
nullspaceTorque.resize(size);
targetJointTorques.resize(size);
I = Eigen::MatrixXf::Identity(size, size);
}
const Eigen::VectorXf& CartesianImpedanceController::run(
const NJointTaskSpaceImpedanceControllerControlData& cfg,
std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors,
std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors,
std::vector<const SensorValue1DoFActuatorPosition*> positionSensors
)
{
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), cfg.Knull.size())); ///TODO move this to core
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), cfg.Dnull.size()));
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), cfg.desiredJointPosition.size()));
ARMARX_CHECK_GREATER_EQUAL(cfg.torqueLimit, 0);
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), torqueSensors.size()));
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), velocitySensors.size()));
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), positionSensors.size()));
const Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All); ///TODO output param version
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), jacobi.cols()));
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(6, jacobi.rows()));
const Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);///TODO output param version
for (size_t i = 0; i < velocitySensors.size(); ++i)
{
qpos(i) = positionSensors[i]->position;
qvel(i) = velocitySensors[i]->velocity;
}
const Eigen::Vector6f tcptwist = jacobi * qvel;
const Eigen::Vector3f currentTCPLinearVelocity
{
0.001f * tcptwist(0),
0.001f * tcptwist(1),
0.001f * tcptwist(2)
};
const Eigen::Vector3f currentTCPAngularVelocity
{
tcptwist(3),
tcptwist(4),
tcptwist(5)
};
const Eigen::Vector3f currentTCPPosition = tcp->getPositionInRootFrame();
const Eigen::Vector3f tcpForces = 0.001 * cfg.Kpos.cwiseProduct(cfg.desiredPosition - currentTCPPosition);
const Eigen::Vector3f tcpDesiredForce = tcpForces - cfg.Dpos.cwiseProduct(currentTCPLinearVelocity);
const Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
const Eigen::Matrix3f currentRotMat = currentTCPPose.block<3, 3>(0, 0);
const Eigen::Matrix3f desiredMat(cfg.desiredOrientation);
const Eigen::Matrix3f diffMat = desiredMat * currentRotMat.inverse();
// const Eigen::Quaternionf cquat(currentRotMat);
// const Eigen::Quaternionf diffQuaternion = desiredQuaternion * cquat.conjugate();
// const Eigen::AngleAxisf angleAxis(diffQuaternion);
const Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
const Eigen::Vector3f tcpDesiredTorque =
cfg.Kori.cwiseProduct(rpy) -
cfg.Dori.cwiseProduct(currentTCPAngularVelocity);
Eigen::Vector6f tcpDesiredWrench;
tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
// calculate desired joint torque
nullqerror = cfg.desiredJointPosition - qpos;
for (int i = 0; i < nullqerror.rows(); ++i)
{
if (fabs(nullqerror(i)) < 0.09)
{
nullqerror(i) = 0;
}
}
nullspaceTorque = cfg.Knull.cwiseProduct(nullqerror) - cfg.Dnull.cwiseProduct(qvel);
targetJointTorques =
jacobi.transpose() * tcpDesiredWrench +
(I - jacobi.transpose() * jtpinv) * nullspaceTorque;
for (int i = 0; i < targetJointTorques.size(); ++i)
{
targetJointTorques(i) = boost::algorithm::clamp(targetJointTorques(i), -cfg.torqueLimit, cfg.torqueLimit);
}
//write debug data
{
const Eigen::Quaternionf cquat(currentRotMat);
dbg.errorAngle = cquat.angularDistance(cfg.desiredOrientation);
dbg.posiError = (cfg.desiredPosition - currentTCPPosition).norm();
dbg.tcpDesiredForce = tcpDesiredForce;
dbg.tcpDesiredTorque = tcpDesiredTorque;
}
return targetJointTorques;
}
NJointControllerRegistration<NJointTaskSpaceImpedanceController> registrationControllerDSRTController("NJointTaskSpaceImpedanceController");
NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
......@@ -70,8 +186,8 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const Rob
velocitySensors.push_back(velocitySensor);
positionSensors.push_back(positionSensor);
};
tcp = rns->getTCP();
ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
controller.initialize(rns);
NJointTaskSpaceImpedanceControllerControlData initData;
initData.desiredOrientation = cfg->desiredOrientation;
......@@ -92,111 +208,32 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const Rob
void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/, const IceUtil::Time& /*timeSinceLastIteration*/)
{
const Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
ARMARX_CHECK_EQUAL(positionSensors.size(), velocitySensors.size());
Eigen::VectorXf qpos;
Eigen::VectorXf qvel;
qpos.resize(positionSensors.size());
qvel.resize(velocitySensors.size());
const int jointDim = positionSensors.size();
for (size_t i = 0; i < velocitySensors.size(); ++i)
{
qpos(i) = positionSensors[i]->position;
qvel(i) = velocitySensors[i]->velocity;
}
const Eigen::Vector6f tcptwist = jacobi * qvel;
const Eigen::Vector3f currentTCPLinearVelocity
{
0.001f * tcptwist(0),
0.001f * tcptwist(1),
0.001f * tcptwist(2)
};
const Eigen::Vector3f currentTCPAngularVelocity
{
tcptwist(3),
tcptwist(4),
tcptwist(5)
};
rtUpdateControlStruct();
const Eigen::Vector3f desiredPosition = rtGetControlStruct().desiredPosition;
const Eigen::Quaternionf desiredOrientation = rtGetControlStruct().desiredOrientation;
const Eigen::Vector3f Kpos = rtGetControlStruct().Kpos;
const Eigen::Vector3f Dpos = rtGetControlStruct().Dpos;
const Eigen::Vector3f Kori = rtGetControlStruct().Kori;
const Eigen::Vector3f Dori = rtGetControlStruct().Dori;
const Eigen::VectorXf Knull = rtGetControlStruct().Knull;
const Eigen::VectorXf Dnull = rtGetControlStruct().Dnull;
const Eigen::Vector3f currentTCPPosition = tcp->getPositionInRootFrame();
const Eigen::Vector3f tcpForces = 0.001 * Kpos.cwiseProduct(desiredPosition - currentTCPPosition);
const Eigen::Vector3f tcpDesiredForce = tcpForces - Dpos.cwiseProduct(currentTCPLinearVelocity);
const Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
const Eigen::Matrix3f currentRotMat = currentTCPPose.block<3, 3>(0, 0);
const Eigen::Matrix3f desiredMat(desiredOrientation);
const Eigen::Matrix3f diffMat = desiredMat * currentRotMat.inverse();
const Eigen::Quaternionf cquat(currentRotMat);
// const Eigen::Quaternionf diffQuaternion = desiredQuaternion * cquat.conjugate();
// const Eigen::AngleAxisf angleAxis(diffQuaternion);
const Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
const Eigen::Vector3f tcpDesiredTorque = Kori.cwiseProduct(rpy) - Dori.cwiseProduct(currentTCPAngularVelocity);
Eigen::Vector6f tcpDesiredWrench;
tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
// calculate desired joint torque
const Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
const float lambda = 2;
const Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
Eigen::VectorXf nullqerror = rtGetControlStruct().desiredJointPosition - qpos;
const auto& jointDesiredTorques = controller.run(
rtGetControlStruct(),
torqueSensors,
velocitySensors,
positionSensors
);
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(targets.size(), jointDesiredTorques.size()));
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(nullqerror.rows()), targets.size());
for (int i = 0; i < nullqerror.rows(); ++i)
{
if (fabs(nullqerror(i)) < 0.09)
{
nullqerror(i) = 0;
}
}
const Eigen::VectorXf nullspaceTorque = Knull.cwiseProduct(nullqerror) - Dnull.cwiseProduct(qvel);
const Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(jointDesiredTorques.rows()), targets.size());
const auto torqueLimit = rtGetControlStruct().torqueLimit;
for (size_t i = 0; i < targets.size(); ++i)
{
float desiredTorque = jointDesiredTorques(i);
desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
debugDataInfo.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
targets.at(i)->torque = desiredTorque;
targets.at(i)->torque = jointDesiredTorques(i);
if (!targets.at(i)->isValid())
{
targets.at(i)->torque = 0;
}
}
float errorAngle = cquat.angularDistance(desiredOrientation);
float posiError = (desiredPosition - currentTCPPosition).norm();
debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
debugDataInfo.getWriteBuffer().quatError = errorAngle;
debugDataInfo.getWriteBuffer().posiError = posiError;
debugDataInfo.getWriteBuffer().desiredForce_x = controller.dbg.tcpDesiredForce(0);
debugDataInfo.getWriteBuffer().desiredForce_y = controller.dbg.tcpDesiredForce(1);
debugDataInfo.getWriteBuffer().desiredForce_z = controller.dbg.tcpDesiredForce(2);
debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = controller.dbg.tcpDesiredTorque(0);
debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = controller.dbg.tcpDesiredTorque(1);
debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = controller.dbg.tcpDesiredTorque(2);
debugDataInfo.getWriteBuffer().quatError = controller.dbg.errorAngle;
debugDataInfo.getWriteBuffer().posiError = controller.dbg.posiError;
debugDataInfo.commitWrite();
}
......
source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
+ 48
− 10
View file @ 062343d4
......@@ -45,7 +45,47 @@ namespace armarx
Eigen::Vector3f Dori;
Eigen::VectorXf Knull;
Eigen::VectorXf Dnull;
Eigen::VectorXf desiredJointPosition;
float torqueLimit;
};
class CartesianImpedanceController
{
private:
Eigen::VectorXf qpos;
Eigen::VectorXf qvel;
Eigen::VectorXf tcptwist;
Eigen::VectorXf nullqerror;
Eigen::VectorXf nullspaceTorque;
Eigen::VectorXf targetJointTorques;
Eigen::MatrixXf I;
VirtualRobot::DifferentialIKPtr ik;
VirtualRobot::RobotNodePtr tcp;
const float lambda = 2;
public:
//state for debugging
struct Debug
{
float errorAngle;
float posiError;
Eigen::Vector3f tcpDesiredForce;
Eigen::Vector3f tcpDesiredTorque;
};
Debug dbg;
int bufferSize() const;
void initialize(const VirtualRobot::RobotNodeSetPtr& rns);
const Eigen::VectorXf& run(
const NJointTaskSpaceImpedanceControllerControlData& cfg,
std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors,
std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors,
std::vector<const SensorValue1DoFActuatorPosition*> positionSensors
);
};
/**
* @defgroup Library-NJointTaskSpaceImpedanceController NJointTaskSpaceImpedanceController
* @ingroup Library-RobotUnit-NJointControllers
......@@ -76,18 +116,16 @@ namespace armarx
protected:
void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
void setPosition(const Ice::FloatSeq&, const Ice::Current&) override;
void setOrientation(const Ice::FloatSeq&, const Ice::Current&) override;
void setPosition(const Eigen::Vector3f&, const Ice::Current&) override;
void setOrientation(const Eigen::Quaternionf&, const Ice::Current&) override;
void setPositionOrientation(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const Ice::Current&) override;
void setPose(const Eigen::Matrix4f& mat, const Ice::Current&) override;
void setImpedanceParameters(const std::string&, const Ice::FloatSeq&, const Ice::Current&);
void setNullspaceConfig(const Eigen::VectorXf& joint, const Eigen::VectorXf& knull, const Eigen::VectorXf& dnull, const Ice::Current&) override;
void setConfig(const NJointTaskSpaceImpedanceControlConfigPtr& cfg, const Ice::Current&) override;
private:
VirtualRobot::DifferentialIKPtr ik;
Eigen::VectorXf desiredJointPosition;
VirtualRobot::RobotNodePtr tcp;
std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
......@@ -121,8 +159,8 @@ namespace armarx
};
TripleBuffer<NJointTaskSpaceImpedanceControllerDebugInfo> debugDataInfo;
float torqueLimit;
std::vector<std::string> jointNames;
CartesianImpedanceController controller;
// NJointController interface
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment