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

Make code more const correct

parent 8b2ee72f
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
+ 115
− 109
View file @ f6ce7c8e
......@@ -22,6 +22,9 @@
#include "NJointTaskSpaceImpedanceController.h"
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <VirtualRobot/MathTools.h>
......@@ -29,7 +32,6 @@ using namespace armarx;
NJointControllerRegistration<NJointTaskSpaceImpedanceController> registrationControllerDSRTController("NJointTaskSpaceImpedanceController");
NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
......@@ -71,67 +73,26 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const Rob
tcp = rns->getTCP();
ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
NJointTaskSpaceImpedanceControllerControlData initData;
initData.desiredOrientation = cfg->desiredOrientation;
initData.desiredPosition = cfg->desiredPosition;
torqueLimit = cfg->torqueLimit;
desiredJointPosition.resize(targets.size());
ARMARX_CHECK_EQUAL(cfg->desiredJointPositions.size(), targets.size());
for (size_t i = 0; i < targets.size(); ++i)
{
desiredJointPosition(i) = cfg->desiredJointPositions.at(i);
}
initData.Kpos = cfg->Kpos;
initData.Dpos = cfg->Dpos;
initData.Kori = cfg->Kori;
initData.Dori = cfg->Dori;
ARMARX_CHECK_EQUAL(cfg->desiredPoseVec.size(), 7);
Eigen::Vector3f initDesiredPosition;
Eigen::Quaternionf initDesiredOrientation;
initDesiredPosition << cfg->desiredPoseVec[0], cfg->desiredPoseVec[1], cfg->desiredPoseVec[2];
initDesiredOrientation.w() = cfg->desiredPoseVec[3];
initDesiredOrientation.x() = cfg->desiredPoseVec[4];
initDesiredOrientation.y() = cfg->desiredPoseVec[5];
initDesiredOrientation.z() = cfg->desiredPoseVec[6];
ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
Eigen::Vector3f Kpos;
Kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
Eigen::Vector3f Dpos;
Dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
Eigen::Vector3f Kori;
Kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
Eigen::Vector3f Dori;
Dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
ARMARX_CHECK_EQUAL(cfg->Knull.size(), 8);
ARMARX_CHECK_EQUAL(cfg->Dnull.size(), 8);
Eigen::VectorXf Knull;
Eigen::VectorXf Dnull;
Knull.setZero(8);
Dnull.setZero(8);
for (size_t i = 0; i < 8; ++i)
{
Knull[i] = cfg->Knull.at(i);
Dnull[i] = cfg->Dnull.at(i);
}
initData.Knull = cfg->Knull;
initData.Dnull = cfg->Dnull;
initData.desiredJointPosition = cfg->desiredJointPositions;
NJointTaskSpaceImpedanceControllerControlData initData;
initData.desiredOrientation = initDesiredOrientation;
initData.desiredPosition = initDesiredPosition;
initData.Kpos = Kpos;
initData.Dpos = Dpos;
initData.Kori = Kori;
initData.Dori = Dori;
initData.Knull = Knull;
initData.Dnull = Dnull;
initData.torqueLimit = cfg->torqueLimit;
reinitTripleBuffer(initData);
}
void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/, const IceUtil::Time& /*timeSinceLastIteration*/)
{
Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
const Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
ARMARX_CHECK_EQUAL(positionSensors.size(), velocitySensors.size());
Eigen::VectorXf qpos;
......@@ -139,7 +100,7 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValu
qpos.resize(positionSensors.size());
qvel.resize(velocitySensors.size());
int jointDim = positionSensors.size();
const int jointDim = positionSensors.size();
for (size_t i = 0; i < velocitySensors.size(); ++i)
{
......@@ -147,48 +108,54 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValu
qvel(i) = velocitySensors[i]->velocity;
}
Eigen::VectorXf tcptwist = jacobi * qvel;
ARMARX_CHECK_EQUAL(tcptwist.rows(), 6);
Eigen::Vector3f currentTCPLinearVelocity;
currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
Eigen::Vector3f currentTCPAngularVelocity;
currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
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();
Eigen::Vector3f desiredPosition = rtGetControlStruct().desiredPosition;
Eigen::Quaternionf desiredOrientation = rtGetControlStruct().desiredOrientation;
Eigen::Vector3f Kpos = rtGetControlStruct().Kpos;
Eigen::Vector3f Dpos = rtGetControlStruct().Dpos;
Eigen::Vector3f Kori = rtGetControlStruct().Kori;
Eigen::Vector3f Dori = rtGetControlStruct().Dori;
Eigen::VectorXf Knull = rtGetControlStruct().Knull;
Eigen::VectorXf Dnull = rtGetControlStruct().Dnull;
Eigen::Vector3f currentTCPPosition = tcp->getPositionInRootFrame();
Eigen::Vector3f tcpForces = 0.001 * Kpos.cwiseProduct(desiredPosition - currentTCPPosition);
Eigen::Vector3f tcpDesiredForce = tcpForces - Dpos.cwiseProduct(currentTCPLinearVelocity);
Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
Eigen::Matrix3f currentRotMat = currentTCPPose.block<3, 3>(0, 0);
Eigen::Matrix3f desiredMat(desiredOrientation);
Eigen::Matrix3f diffMat = desiredMat * currentRotMat.inverse();
Eigen::Quaternionf cquat(currentRotMat);
// Eigen::Quaternionf diffQuaternion = desiredQuaternion * cquat.conjugate();
// Eigen::AngleAxisf angleAxis(diffQuaternion);
Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
Eigen::Vector3f tcpDesiredTorque = Kori.cwiseProduct(rpy) - Dori.cwiseProduct(currentTCPAngularVelocity);
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
Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
float lambda = 2;
Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
Eigen::VectorXf nullqerror = desiredJointPosition - qpos;
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;
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(nullqerror.rows()), targets.size());
for (int i = 0; i < nullqerror.rows(); ++i)
......@@ -198,11 +165,12 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValu
nullqerror(i) = 0;
}
}
Eigen::VectorXf nullspaceTorque = Knull.cwiseProduct(nullqerror) - Dnull.cwiseProduct(qvel);
const Eigen::VectorXf nullspaceTorque = Knull.cwiseProduct(nullqerror) - Dnull.cwiseProduct(qvel);
Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
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);
......@@ -258,37 +226,38 @@ void NJointTaskSpaceImpedanceController::onPublish(const SensorAndControl&, cons
}
void NJointTaskSpaceImpedanceController::setPosition(const Ice::FloatSeq& target, const Ice::Current&)
void NJointTaskSpaceImpedanceController::setPosition(const Eigen::Vector3f& target, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(target.size(), 3);
Eigen::Vector3f desiredPosition;
desiredPosition << target[0], target[1], target[2];
LockGuardType guard {controlDataMutex};
getWriterControlStruct().desiredPosition = desiredPosition;
getWriterControlStruct().desiredPosition = target;
writeControlStruct();
}
void NJointTaskSpaceImpedanceController::setOrientation(const Ice::FloatSeq& target, const Ice::Current&)
void NJointTaskSpaceImpedanceController::setOrientation(const Eigen::Quaternionf& target, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(target.size(), 4);
LockGuardType guard {controlDataMutex};
getWriterControlStruct().desiredOrientation = target;
writeControlStruct();
}
Eigen::Quaternionf desiredOrientation;
desiredOrientation.w() = target[0];
desiredOrientation.x() = target[1];
desiredOrientation.y() = target[2];
desiredOrientation.z() = target[3];
void NJointTaskSpaceImpedanceController::setPositionOrientation(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const Ice::Current&)
{
LockGuardType guard {controlDataMutex};
getWriterControlStruct().desiredPosition = pos;
getWriterControlStruct().desiredOrientation = ori;
writeControlStruct();
}
void NJointTaskSpaceImpedanceController::setPose(const Eigen::Matrix4f& mat, const Ice::Current&)
{
LockGuardType guard {controlDataMutex};
getWriterControlStruct().desiredOrientation = desiredOrientation;
getWriterControlStruct().desiredPosition = simox::math::mat4f_to_pos(mat);
getWriterControlStruct().desiredOrientation = simox::math::mat4f_to_quat(mat);
writeControlStruct();
}
void NJointTaskSpaceImpedanceController::setImpedanceParameters(const std::string& name, const Ice::FloatSeq& vals, const Ice::Current&)
{
LockGuardType guard {controlDataMutex};
if (name == "Kpos")
{
......@@ -358,9 +327,46 @@ void NJointTaskSpaceImpedanceController::setImpedanceParameters(const std::strin
}
getWriterControlStruct().Dnull = vec;
}
writeControlStruct();
}
void NJointTaskSpaceImpedanceController::setNullspaceConfig(
const Eigen::VectorXf& joint,
const Eigen::VectorXf& knull,
const Eigen::VectorXf& dnull, const Ice::Current&)
{
LockGuardType guard {controlDataMutex};
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(joint.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(knull.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(dnull.size()), targets.size());
getWriterControlStruct().Knull = knull;
getWriterControlStruct().Dnull = dnull;
getWriterControlStruct().desiredJointPosition = joint;
writeControlStruct();
}
void NJointTaskSpaceImpedanceController::setConfig(const NJointTaskSpaceImpedanceControlConfigPtr& cfg, const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(cfg);
LockGuardType guard {controlDataMutex};
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg->Knull.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg->Dnull.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg->desiredJointPositions.size()), targets.size());
getWriterControlStruct().desiredPosition = cfg->desiredPosition;
getWriterControlStruct().desiredOrientation = cfg->desiredOrientation;
getWriterControlStruct().Kpos = cfg->Kpos;
getWriterControlStruct().Dpos = cfg->Dpos;
getWriterControlStruct().Kori = cfg->Kori;
getWriterControlStruct().Dori = cfg->Dori;
getWriterControlStruct().Knull = cfg->Knull;
getWriterControlStruct().Dnull = cfg->Dnull;
getWriterControlStruct().desiredJointPosition = cfg->desiredJointPositions;
getWriterControlStruct().torqueLimit = cfg->torqueLimit;
writeControlStruct();
}
void armarx::NJointTaskSpaceImpedanceController::rtPreActivateController()
......
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