diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index d2824cc6d313c9c6f727b104eda0db0a6b99f560..eba7eca3c78e4cc7ceddd079edd058bd1a4ec76c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -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();
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
index fd5cea5dff98a6207ef9d28fd41faf4a98d426f4..a3cfd8c392a1bf0f30ba511773387ef08d467a97 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
@@ -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