diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
index 857df32bb7bd1387c8892a5aaf12297085723e47..640bd4f88fa337ce1629634e3449554e315b1a61 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
@@ -65,29 +65,29 @@ namespace armarx
make_ControlTarget1DoFActuator(ControlTarget1DoFActuatorTorque, torque, ControlModes::Torque1DoF);
make_ControlTarget1DoFActuator(ControlTarget1DoFActuatorCurrent, current, ControlModes::Current1DoF);
- class VelocityTorqueControlTarget: public ControlTarget1DoFActuatorVelocity
+ class ControlTarget1DoFActuatorTorqueVelocity: public ControlTarget1DoFActuatorVelocity
{
public:
float maxTorque;
const std::string& getControlMode() const override
{
- return ControlModes::Velocity1DoF;
+ return ControlModes::VelocityTorque;
}
void reset() override
{
ControlTarget1DoFActuatorVelocity::reset();
- maxTorque = 10.f;
+ maxTorque = -1.0f; // if < 0, default value for joint is to be used
}
bool isValid() const override
{
- return ControlTarget1DoFActuatorVelocity::isValid() && maxTorque >= 0;
+ return ControlTarget1DoFActuatorVelocity::isValid() && std::isfinite(maxTorque);
}
- static ControlTargetInfo<VelocityTorqueControlTarget> GetClassMemberInfo()
+ static ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity> GetClassMemberInfo()
{
- ControlTargetInfo<VelocityTorqueControlTarget> cti;
+ ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity> cti;
cti.addBaseClass<ControlTarget1DoFActuatorVelocity>();
- cti.addMemberVariable(&VelocityTorqueControlTarget::maxTorque, "maxTorque");
+ cti.addMemberVariable(&ControlTarget1DoFActuatorTorqueVelocity::maxTorque, "maxTorque");
return cti;
}
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
index 61e768884245fc6a1601b3a867d160d2504037db..9245b2086e8e688fafc4ba6cae5b83b8f1fec231 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
@@ -43,7 +43,7 @@ NJointKinematicUnitPassThroughController::NJointKinematicUnitPassThroughControll
ControlTargetBase* ct = useControlTarget(cfg->deviceName, controlMode);
//get sensor
- if (controlMode == ControlModes::Position1DoF)
+ if (ct->isA<ControlTarget1DoFActuatorPosition>())
{
ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorPosition>());
sensor = &(sv->asA<SensorValue1DoFActuatorPosition>()->position);
@@ -51,26 +51,16 @@ NJointKinematicUnitPassThroughController::NJointKinematicUnitPassThroughControll
target = &(ct->asA<ControlTarget1DoFActuatorPosition>()->position);
sensorToControlOnActivateFactor = 1;
}
- else if (controlMode == ControlModes::Velocity1DoF)
+ else if (ct->isA<ControlTarget1DoFActuatorVelocity>())
{
ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorVelocity>());
sensor = &(sv->asA<SensorValue1DoFActuatorVelocity>()->velocity);
ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorVelocity>());
target = &(ct->asA<ControlTarget1DoFActuatorVelocity>()->velocity);
sensorToControlOnActivateFactor = 1;
- resetZeroThreshold = 0.1;
+ resetZeroThreshold = 0.1f; // TODO: way to big value?!
}
- else if (controlMode == ControlModes::VelocityTorque)
- {
- ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorVelocity>());
- sensor = &(sv->asA<SensorValue1DoFActuatorVelocity>()->velocity);
- ARMARX_CHECK_EXPRESSION(ct->asA<VelocityTorqueControlTarget>());
- target = &(ct->asA<VelocityTorqueControlTarget>()->velocity);
- target2 = &(ct->asA<VelocityTorqueControlTarget>()->maxTorque);
- sensorToControlOnActivateFactor = 1;
- resetZeroThreshold = 0.1;
- }
- else if (controlMode == ControlModes::Torque1DoF)
+ else if (ct->isA<ControlTarget1DoFActuatorTorque>())
{
ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorTorque>());
sensor = &(sv->asA<SensorValue1DoFActuatorTorque>()->torque);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index 3119a0fc23a89029515d6829087798f358ed49b5..371a6e462f686653104f35d137125d3704bade63 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -60,10 +60,6 @@ namespace armarx
inline void rtRun(const IceUtil::Time&, const IceUtil::Time&) override
{
*target = control;
- if (target2)
- {
- *target2 = 8;
- }
}
inline void rtPreActivateController() override
{
@@ -93,7 +89,6 @@ namespace armarx
protected:
std::atomic<float> control {0};
float* target {nullptr};
- float* target2 {nullptr};
const float* sensor
{
nullptr
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 2eb2753c7697a43743707e03c1f0adc9b81c238b..5d1edef07e3d71d8b86fb8b222d49dce12662ae9 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -45,7 +45,9 @@ module armarx
float DMPKd = 20;
int kernelSize = 100;
double tau = 1;
- int baseMode = 1;
+ string dmpMode = "MinimumJerk";
+ string dmpStyle = "Discrete";
+ double dmpAmplitude = 1;
double phaseL = 10;
double phaseK = 10;
@@ -53,9 +55,7 @@ module armarx
double phaseDist1 = 10;
double phaseKpPos = 1;
double phaseKpOri = 0.1;
-
double timeDuration = 10;
-
double posToOriRatio = 100;
// velocity controller configuration
@@ -63,6 +63,9 @@ module armarx
string tcpName = "";
NJointTaskSpaceDMPControllerMode::CartesianSelection mode = NJointTaskSpaceDMPControllerMode::eAll;
+ double maxLinearVel;
+ double maxAngularVel;
+
};
@@ -72,10 +75,12 @@ module armarx
bool isFinished();
void runDMP(Ice::DoubleSeq goals, double tau);
- void setTemporalFactor(double tau);
+ void setSpeed(double times);
void setViaPoints(double canVal, Ice::DoubleSeq point);
void setGoals(Ice::DoubleSeq goals);
+ double getCanVal();
+
void setControllerTarget(float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
void setTorqueKp(StringFloatDictionary torqueKp);
void setNullspaceJointVelocities(StringFloatDictionary nullspaceJointVelocities);
@@ -88,7 +93,7 @@ module armarx
float DMPKd = 20;
int kernelSize = 100;
double tau = 1;
- int baseMode = 1;
+ int dmpMode = 1;
int dmpNum = 2;
// phaseStop technique
@@ -128,4 +133,69 @@ module armarx
void setTorqueKp(StringFloatDictionary torqueKp);
void setNullspaceJointVelocities(StringFloatDictionary nullspaceJointVelocities);
};
+
+ class NJointBimanualCCDMPControllerConfig extends NJointControllerConfig
+ {
+
+ // dmp configuration
+ int kernelSize = 100;
+ string dmpMode = "MinimumJerk";
+ string dmpType = "Discrete";
+
+ // phaseStop technique
+ double phaseL = 10;
+ double phaseK = 10;
+ double phaseDist0 = 50;
+ double phaseDist1 = 10;
+ double phaseKpPos = 1;
+ double phaseKpOri = 0.1;
+ double posToOriRatio = 10;
+ double timeDuration = 10;
+
+ string defautLeader = "Left";
+
+ Ice::FloatSeq leftDesiredJointValues;
+ Ice::FloatSeq rightDesiredJointValues;
+
+ float KoriFollower = 1;
+ float KposFollower = 1;
+
+ double maxLinearVel;
+ double maxAngularVel;
+
+ Ice::FloatSeq Kpos;
+ Ice::FloatSeq Dpos;
+ Ice::FloatSeq Kori;
+ Ice::FloatSeq Dori;
+
+ float knull;
+ float dnull;
+
+ float torqueLimit;
+
+ Ice::FloatSeq Kpf;
+ Ice::FloatSeq Kif;
+ Ice::FloatSeq DesiredForce;
+
+ float BoxWidth;
+
+ };
+
+ interface NJointBimanualCCDMPControllerInterface extends NJointControllerInterface
+ {
+ void learnDMPFromFiles(string whichDMP, Ice::StringSeq trajfiles);
+ bool isFinished();
+ void runDMP(Ice::DoubleSeq leftGoals, Ice::DoubleSeq rightGoals);
+
+ void setViaPoints(double canVal, Ice::DoubleSeq point);
+ void setGoals(Ice::DoubleSeq goals);
+
+ void changeLeader();
+ double getVirtualTime();
+
+ string getLeaderName();
+
+
+ };
};
+
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index 629d152d94faa27ceb0b3d4f4fb6796f4bd5ed8c..1a342947e2210b180d2416b14e3bc78deedd72e8 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -52,11 +52,15 @@ module armarx
byte b;
};
+
struct HsvColor
{
- byte h;
- byte s;
- byte v;
+ //! 0-360
+ float h;
+ //! 0 - 1
+ float s;
+ //! 0 - 1
+ float v;
};
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 63093136d096c560bce41e766cc10f9aaa44ade2..b0a1a80cab68dc9f10e178cc00da1e1b65b49c94 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -4,3 +4,5 @@ add_subdirectory(SimpleJsonLogger)
add_subdirectory(RobotAPIVariantWidget)
add_subdirectory(RobotAPINJointControllers)
+
+add_subdirectory(DMPController)
diff --git a/source/RobotAPI/libraries/DMPController/CMakeLists.txt b/source/RobotAPI/libraries/DMPController/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3a36d3bdcf71fb9f3726a38857d54f05bb108cec
--- /dev/null
+++ b/source/RobotAPI/libraries/DMPController/CMakeLists.txt
@@ -0,0 +1,50 @@
+set(LIB_NAME DMPController)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+
+find_package(Eigen3 QUIET)
+find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
+find_package(DMP QUIET)
+
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+armarx_build_if(DMP_FOUND "DMP not available")
+
+if (Eigen3_FOUND AND Simox_FOUND AND DMP_FOUND)
+ include_directories(${Simox_INCLUDE_DIRS})
+ include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
+ include_directories(${DMP_INCLUDE_DIRS})
+endif()
+
+set(LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ RobotAPIInterfaces
+ ArmarXCoreObservers
+ ArmarXCoreStatechart
+ ArmarXCoreEigen3Variants
+ VirtualRobot
+ Saba
+ SimDynamics
+ RobotUnit
+ ${DMP_LIBRARIES}
+ )
+
+set(LIB_FILES
+./TaskSpaceDMPController.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+./TaskSpaceDMPController.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b712d6491dfee79e774e22748a6c16733af466b3
--- /dev/null
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
@@ -0,0 +1,327 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::DMPController
+ * @author zhou ( you dot zhou at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "TaskSpaceDMPController.h"
+
+
+using namespace armarx;
+
+
+
+void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist)
+{
+ if (canVal < 1e-8)
+ {
+ if (config.DMPStyle == "Periodic")
+ {
+ prepareExecution(eigen4f2vec(currentPose), goalPoseVec);
+ }
+ else
+ {
+ targetVel.setZero();
+ return;
+ }
+ }
+
+ Eigen::Vector3f currentPosition;
+ Eigen::Quaterniond currentOrientation;
+ double posiError = 0;
+ double oriError = 0;
+
+ getError(currentPose, currentPosition, currentOrientation, posiError, oriError);
+
+ double poseError = posiError + config.phaseStopParas.mm2radi * oriError;
+
+ double phaseDist;
+ if (isDisturbance)
+ {
+ phaseDist = config.phaseStopParas.backDist;
+ }
+ else
+ {
+ phaseDist = config.phaseStopParas.goDist;
+ }
+ double phaseL = config.phaseStopParas.maxValue;
+ double phaseK = config.phaseStopParas.slop;
+
+ double phaseStop = phaseL / (1 + exp(-phaseK * (poseError - phaseDist)));
+ double mpcFactor = 1 - (phaseStop / phaseL);
+
+ if (mpcFactor < 0.1)
+ {
+ isDisturbance = true;
+ }
+
+ if (mpcFactor > 0.9)
+ {
+ isDisturbance = false;
+ }
+
+ double tau = dmpPtr->getTemporalFactor();
+ double timeDuration = config.motionTimeDuration;
+ canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop) ;
+
+
+ for (size_t i = 0; i < 7; ++i)
+ {
+ currentState[i].vel = currentState[i].vel * config.DMPAmplitude;
+ }
+
+ currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, deltaT / timeDuration, targetPoseVec);
+
+ for (size_t i = 0; i < 7; ++i)
+ {
+ targetPoseVec[i] = currentState[i].pos;
+ }
+
+ float vel0, vel1;
+
+ Eigen::Vector3f linearVel;
+ linearVel << twist(0), twist(1), twist(2);
+ for (size_t i = 0; i < 3; i++)
+ {
+ vel0 = currentState[i].vel / timeDuration;
+ vel1 = config.phaseStopParas.Kpos * (targetPoseVec[i] - currentPosition(i)) - config.phaseStopParas.Dpos * linearVel(i);
+ targetVel(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ }
+
+
+ Eigen::Quaterniond dmpQuaternionVel;
+ dmpQuaternionVel.w() = currentState[3].vel;
+ dmpQuaternionVel.x() = currentState[4].vel;
+ dmpQuaternionVel.y() = currentState[5].vel;
+ dmpQuaternionVel.z() = currentState[6].vel;
+
+ Eigen::Quaterniond dmpQuaternionPosi;
+ dmpQuaternionPosi.w() = currentState[3].pos;
+ dmpQuaternionPosi.x() = currentState[4].pos;
+ dmpQuaternionPosi.y() = currentState[5].pos;
+ dmpQuaternionPosi.z() = currentState[6].pos;
+
+
+ Eigen::Quaterniond angularVel0 = dmpQuaternionVel * dmpQuaternionPosi.inverse();
+ angularVel0.w() *= 2;
+ angularVel0.x() *= 2;
+ angularVel0.y() *= 2;
+ angularVel0.z() *= 2;
+
+
+ double angularChange = angularVel0.angularDistance(oldDMPAngularVelocity);
+ if (angularVel0.w() > 0 && oldDMPAngularVelocity.w() < 0 && angularChange < 1e-2)
+ {
+ angularVel0.w() = - angularVel0.w();
+ angularVel0.x() = - angularVel0.x();
+ angularVel0.y() = - angularVel0.y();
+ angularVel0.z() = - angularVel0.z();
+ }
+ oldDMPAngularVelocity = angularVel0;
+
+ Eigen::Vector3f currentAngularVel;
+ currentAngularVel << twist(3), twist(4), twist(5);
+
+ Eigen::Quaternionf targetQuaternionf;
+ targetQuaternionf.w() = targetPoseVec[3];
+ targetQuaternionf.x() = targetPoseVec[4];
+ targetQuaternionf.y() = targetPoseVec[5];
+ targetQuaternionf.z() = targetPoseVec[6];
+
+ Eigen::Matrix3f desiredMat(targetQuaternionf);
+ Eigen::Matrix3f currentMat = currentPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f diffMat = desiredMat * currentMat.inverse();
+ Eigen::Vector3f diffRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+ Eigen::Vector3f angularVel1 = config.phaseStopParas.Kori * diffRPY - config.phaseStopParas.Dori * currentAngularVel;
+
+ targetVel(3) = mpcFactor * angularVel0.x() / timeDuration + (1 - mpcFactor) * angularVel1(0);
+ targetVel(4) = mpcFactor * angularVel0.y() / timeDuration + (1 - mpcFactor) * angularVel1(1);
+ targetVel(5) = mpcFactor * angularVel0.z() / timeDuration + (1 - mpcFactor) * angularVel1(2);
+
+ debugData.canVal = canVal;
+ debugData.oriError = oriError;
+ debugData.posiError = posiError;
+ debugData.mpcFactor = mpcFactor;
+ debugData.poseError = poseError;
+}
+
+void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& fileNames, const std::vector<double>& ratios)
+{
+ if (ratios.size() != fileNames.size())
+ {
+ ARMARX_ERROR << "ratios should have the same size with files";
+ return;
+ }
+
+
+ DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+
+ double ratioSum = 0;
+ for (size_t i = 0; i < fileNames.size(); ++i)
+ {
+ DMP::SampledTrajectoryV2 traj;
+ traj.readFromCSVFile(fileNames.at(i));
+ traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
+ trajs.push_back(traj);
+
+ ratioSum += ratios.at(i);
+ }
+
+ if (ratioSum == 0)
+ {
+ ARMARX_ERROR << "ratios are invalid. The sum is equal to 0";
+ return;
+ }
+
+ DMP::DVec ratiosVec;
+ ratiosVec.resize(ratios.size());
+ for (size_t i = 0; i < ratios.size(); ++i)
+ {
+ ratiosVec.at(i) = ratios.at(i) / ratioSum;
+ }
+
+ ARMARX_INFO << "ratios: " << ratios.at(0);
+ dmpPtr->learnFromTrajectories(trajs);
+ dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratiosVec);
+}
+
+void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& fileNames)
+{
+ std::vector<double> ratios;
+ for (size_t i = 0; i < fileNames.size(); ++i)
+ {
+ if (i == 0)
+ {
+ ratios.push_back(1.0);
+ }
+ else
+ {
+ ratios.push_back(0.0);
+ }
+ }
+
+ learnDMPFromFiles(fileNames, ratios);
+}
+
+void TaskSpaceDMPController::setViaPose(double canVal, const Eigen::Matrix4f& viaPose)
+{
+
+ setViaPose(canVal, eigen4f2vec(viaPose));
+}
+
+void TaskSpaceDMPController::setViaPose(double canVal, const std::vector<double>& viaPoseWithQuaternion)
+{
+ if (canVal <= dmpPtr->getUMin())
+ {
+ goalPoseVec = viaPoseWithQuaternion;
+ }
+ dmpPtr->setViaPoint(canVal, viaPoseWithQuaternion);
+}
+
+void TaskSpaceDMPController::prepareExecution(const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& goalPose)
+{
+ std::vector<double> currentPoseVec = eigen4f2vec(currentPose);
+ std::vector<double> goalPoseVec = eigen4f2vec(goalPose);
+
+ prepareExecution(currentPoseVec, goalPoseVec);
+}
+
+void TaskSpaceDMPController::prepareExecution(const std::vector<double>& currentPoseVec, const std::vector<double>& goalPoseVec)
+{
+ ARMARX_CHECK_EQUAL(currentPoseVec.size(), 7);
+ ARMARX_CHECK_EQUAL(goalPoseVec.size(), 7);
+
+ for (size_t i = 0; i < currentPoseVec.size(); ++i)
+ {
+ currentState[i].pos = currentPoseVec.at(i);
+ currentState[i].vel = 0;
+ targetPoseVec.at(i) = currentPoseVec.at(i);
+ }
+
+ dmpPtr->prepareExecution(goalPoseVec, currentState, 1, 1);
+ this->goalPoseVec = goalPoseVec;
+ isDisturbance = false;
+ canVal = config.motionTimeDuration;
+ oldDMPAngularVelocity.setIdentity();
+
+}
+
+void TaskSpaceDMPController::setSpeed(double times)
+{
+ if (times == 0)
+ {
+ return;
+ }
+
+ config.motionTimeDuration /= times;
+}
+
+std::vector<double> TaskSpaceDMPController::eigen4f2vec(const Eigen::Matrix4f& pose)
+{
+ std::vector<double> viaPoseVec;
+ viaPoseVec.resize(7);
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ viaPoseVec.at(i) = pose(i, 3);
+ }
+
+ VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(pose);
+
+ viaPoseVec.at(3) = quat.w;
+ viaPoseVec.at(4) = quat.x;
+ viaPoseVec.at(5) = quat.y;
+ viaPoseVec.at(6) = quat.z;
+
+ return viaPoseVec;
+}
+
+void TaskSpaceDMPController::getError(const Eigen::Matrix4f& currentPose, Eigen::Vector3f& currentPosition, Eigen::Quaterniond& currentOrientation, double& posiError, double& oriError)
+{
+ currentPosition.setZero();
+ currentPosition << currentPose(0, 3), currentPose(1, 3), currentPose(2, 3);
+
+ VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
+ currentOrientation.w() = quat.w;
+ currentOrientation.x() = quat.x;
+ currentOrientation.y() = quat.y;
+ currentOrientation.z() = quat.z;
+
+ posiError = 0;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ posiError += pow(currentPosition(i) - targetPoseVec[i], 2);
+ }
+ posiError = sqrt(posiError);
+
+ Eigen::Quaterniond targetQuaternion;
+ targetQuaternion.w() = targetPoseVec[3];
+ targetQuaternion.x() = targetPoseVec[4];
+ targetQuaternion.y() = targetPoseVec[5];
+ targetQuaternion.z() = targetPoseVec[6];
+
+ oriError = targetQuaternion.angularDistance(currentOrientation);
+ if (oriError > M_PI)
+ {
+ oriError = 2 * M_PI - oriError;
+ }
+
+}
+
+
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
new file mode 100644
index 0000000000000000000000000000000000000000..c62fef3fb25c3bbf43fd8093db3308110b68d195
--- /dev/null
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -0,0 +1,196 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::DMPController
+ * @author zhou ( you dot zhou at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_TaskSpaceDMPController_H
+#define _ARMARX_LIB_RobotAPI_TaskSpaceDMPController_H
+
+
+#include <dmp/representation/dmp/umitsmp.h>
+
+
+#include <VirtualRobot/RobotNodeSet.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx
+{
+
+ struct PhaseStopParams
+ {
+ float goDist = 80;
+ float backDist = 50;
+ float maxValue = 100;
+ float slop = 1000;
+ float Kpos = 1;
+ float Dpos = 2;
+ float Kori = 1;
+ float Dori = 0;
+ float mm2radi = 10;
+ };
+
+ struct TaskSpaceDMPControllerConfig
+ {
+ int DMPKernelSize = 50;
+ std::string DMPMode = "Linear";
+ std::string DMPStyle = "Discrete";
+ float DMPAmplitude = 1;
+ float motionTimeDuration = 10;
+ PhaseStopParams phaseStopParas;
+ };
+
+ struct DebugInfo
+ {
+ double canVal;
+ double mpcFactor;
+ double poseError;
+ double posiError;
+ double oriError;
+ };
+
+ /**
+ * @defgroup Library-TaskSpaceDMPController TaskSpaceDMPController
+ * @ingroup RobotAPI
+ * A description of the library TaskSpaceDMPController.
+ *
+ * @class TaskSpaceDMPController
+ * @ingroup Library-TaskSpaceDMPController
+ * @brief Brief description of class TaskSpaceDMPController.
+ *
+ * Detailed description of class TaskSpaceDMPController.
+ */
+ class TaskSpaceDMPController
+ {
+ public:
+ TaskSpaceDMPController(std::string name, const TaskSpaceDMPControllerConfig& config, bool isPhaseStop = true)
+ {
+ this->config = config;
+
+ int ModeInd;
+ if (config.DMPMode == "MinimumJerk")
+ {
+ ModeInd = 2;
+ }
+ else
+ {
+ ModeInd = 1;
+ }
+
+
+ dmpPtr.reset(new DMP::UMITSMP(config.DMPKernelSize, ModeInd));
+ canVal = config.motionTimeDuration;
+
+ targetPoseVec.resize(7);
+ targetVel.resize(6);
+ targetVel.setZero();
+ currentState.resize(7);
+
+ this->isPhaseStop = isPhaseStop;
+ dmpName = name;
+ }
+
+ std::string getName()
+ {
+ return dmpName;
+ }
+
+
+ void flow(double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist);
+
+ Eigen::VectorXf getTargetVelocity()
+ {
+ return targetVel;
+ }
+
+ std::vector<double> getTargetPose()
+ {
+ return targetPoseVec;
+ }
+
+ Eigen::Matrix4f getTargetPoseMat()
+ {
+ Eigen::Matrix4f res = VirtualRobot::MathTools::quat2eigen4f(targetPoseVec.at(4), targetPoseVec.at(5), targetPoseVec.at(6), targetPoseVec.at(3));
+ res(0, 3) = targetPoseVec.at(0);
+ res(1, 3) = targetPoseVec.at(1);
+ res(2, 3) = targetPoseVec.at(2);
+
+ return res;
+ }
+
+ void learnDMPFromFiles(const std::vector<std::string>& fileNames, const std::vector<double>& ratios);
+ void learnDMPFromFiles(const std::vector<std::string>& fileNames);
+
+ void setViaPose(double canVal, const Eigen::Matrix4f& viaPose);
+ void setViaPose(double canVal, const std::vector<double>& viaPoseWithQuaternion);
+
+ void prepareExecution(const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& goalPose);
+ void prepareExecution(const std::vector<double>& currentPoseVec, const std::vector<double>& goalPoseVec);
+
+ void setSpeed(double times);
+
+ void setGoalPose(const Eigen::Matrix4f& goalPose)
+ {
+ setViaPose(dmpPtr->getUMin(), goalPose);
+ }
+
+ void setGoalPoseVec(const std::vector<double> goalPoseVec)
+ {
+ setViaPose(dmpPtr->getUMin(), goalPoseVec);
+ }
+
+ DebugInfo debugData;
+ std::vector<double> eigen4f2vec(const Eigen::Matrix4f& pose);
+
+ DMP::UMITSMPPtr getDMP()
+ {
+ return dmpPtr;
+ }
+
+ double canVal;
+ bool isPhaseStop;
+ std::string dmpName;
+ private:
+
+
+ DMP::DVec goalPoseVec;
+
+ Eigen::VectorXf targetVel;
+ DMP::DVec targetPoseVec;
+
+ DMP::UMITSMPPtr dmpPtr;
+ DMP::Vec<DMP::DMPState > currentState;
+ TaskSpaceDMPControllerConfig config;
+
+
+ bool isDisturbance;
+
+
+ void getError(const Eigen::Matrix4f& pose, Eigen::Vector3f& position, Eigen::Quaterniond& quaternion, double& posiError, double& oriError);
+
+ Eigen::Quaterniond oldDMPAngularVelocity;
+ };
+
+ typedef boost::shared_ptr<TaskSpaceDMPController> TaskSpaceDMPControllerPtr;
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/DMPController/test/CMakeLists.txt b/source/RobotAPI/libraries/DMPController/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..df7b127d74b14e3a3b6bce5c3fd556324c3dfa73
--- /dev/null
+++ b/source/RobotAPI/libraries/DMPController/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore DMPController)
+
+armarx_add_test(DMPControllerTest DMPControllerTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/DMPController/test/DMPControllerTest.cpp b/source/RobotAPI/libraries/DMPController/test/DMPControllerTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce13dde7e8ae02eeeade7e2be1e52fce411acc34
--- /dev/null
+++ b/source/RobotAPI/libraries/DMPController/test/DMPControllerTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::DMPController
+ * @author zhou ( you dot zhou at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::DMPController
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../TaskSpaceDMPController.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index 5b48d4fe03f9d108934a8a5854d23f234d75f4c4..bbef6320f875fb4b58c9980cbb209a4cd1c9884a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -24,6 +24,7 @@ set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreE
Saba
SimDynamics
RobotUnit
+
)
set(LIB_FILES
@@ -41,22 +42,22 @@ if (DMP_FOUND )
list(APPEND LIB_HEADERS
DMPController/NJointJointSpaceDMPController.h
- DMPController/NJointTaskSpaceDMPController.h
DMPController/NJointJSDMPController.h
DMPController/NJointTSDMPController.h
DMPController/NJointCCDMPController.h
+ DMPController/NJointBimanualCCDMPController.h
)
list(APPEND LIB_FILES
DMPController/NJointJointSpaceDMPController.cpp
- DMPController/NJointTaskSpaceDMPController.cpp
DMPController/NJointJSDMPController.cpp
DMPController/NJointTSDMPController.cpp
DMPController/NJointCCDMPController.cpp
+ DMPController/NJointBimanualCCDMPController.cpp
)
- list(APPEND LIBS ${DMP_LIBRARIES})
+ list(APPEND LIBS ${DMP_LIBRARIES} DMPController)
endif ()
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b0dbb2e71f80899d98427a742988be7826f8a165
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
@@ -0,0 +1,914 @@
+#include "NJointBimanualCCDMPController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointBimanualCCDMPController> registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController");
+
+ NJointBimanualCCDMPController::NJointBimanualCCDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ ARMARX_INFO << "Preparing ... ";
+ cfg = NJointBimanualCCDMPControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION(prov);
+ RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+
+ leftRNS = robotUnit->getRtRobot()->getRobotNodeSet("LeftArm");
+ for (size_t i = 0; i < leftRNS->getSize(); ++i)
+ {
+ std::string jointName = leftRNS->getNode(i)->getName();
+
+ leftJointNames.push_back(jointName);
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+
+ if (!accelerationSensor)
+ {
+ ARMARX_WARNING << "No accelerationSensor available for " << jointName;
+ }
+
+
+ leftVelocitySensors.push_back(velocitySensor);
+ leftPositionSensors.push_back(positionSensor);
+ leftAccelerationSensors.push_back(accelerationSensor);
+
+
+ };
+
+
+
+
+ rightRNS = robotUnit->getRtRobot()->getRobotNodeSet("RightArm");
+ for (size_t i = 0; i < rightRNS->getSize(); ++i)
+ {
+ std::string jointName = rightRNS->getNode(i)->getName();
+ rightJointNames.push_back(jointName);
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
+
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+ if (!accelerationSensor)
+ {
+ ARMARX_WARNING << "No accelerationSensor available for " << jointName;
+ }
+
+ rightVelocitySensors.push_back(velocitySensor);
+ rightPositionSensors.push_back(positionSensor);
+ rightAccelerationSensors.push_back(accelerationSensor);
+
+ };
+
+
+ const SensorValueBase* svlf = prov->getSensorValue("FT L");
+ leftForceTorque = svlf->asA<SensorValueForceTorque>();
+ const SensorValueBase* svrf = prov->getSensorValue("FT R");
+ rightForceTorque = svrf->asA<SensorValueForceTorque>();
+
+ leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
+ taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
+ taskSpaceDMPConfig.DMPAmplitude = 1.0;
+ taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
+
+
+ TaskSpaceDMPControllerPtr leftLeader(new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig));
+ TaskSpaceDMPControllerPtr leftFollower(new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig));
+ TaskSpaceDMPControllerPtr rightLeader(new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig));
+ TaskSpaceDMPControllerPtr rightFollower(new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig));
+
+ leftGroup.push_back(leftLeader);
+ leftGroup.push_back(rightFollower);
+
+ rightGroup.push_back(rightLeader);
+ rightGroup.push_back(leftFollower);
+
+
+ tcpLeft = leftRNS->getTCP();
+ tcpRight = rightRNS->getTCP();
+ NJointBimanualCCDMPControllerSensorData initSensorData;
+ initSensorData.deltaT = 0;
+ initSensorData.currentTime = 0;
+ initSensorData.currentLeftPose = leftRNS->getTCP()->getPoseInRootFrame();
+ initSensorData.currentRightPose = rightRNS->getTCP()->getPoseInRootFrame();
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+ leaderName = "Left";
+
+ NJointBimanualCCDMPControllerControlData initData;
+ initData.leftTargetVel.resize(6);
+ initData.leftTargetVel.setZero();
+ initData.rightTargetVel.resize(6);
+ initData.rightTargetVel.setZero();
+ reinitTripleBuffer(initData);
+
+ leftDesiredJointValues.resize(leftTargets.size());
+ ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
+
+ for (size_t i = 0; i < leftTargets.size(); ++i)
+ {
+ leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
+ }
+
+ rightDesiredJointValues.resize(rightTargets.size());
+ ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
+
+ for (size_t i = 0; i < rightTargets.size(); ++i)
+ {
+ rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
+ }
+
+ KoriFollower = cfg->KoriFollower;
+ KposFollower = cfg->KposFollower;
+
+ kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
+ dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
+ kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
+ dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+
+
+ knull = cfg->knull;
+ dnull = cfg->dnull;
+
+ torqueLimit = cfg->torqueLimit;
+
+ kpf.resize(12);
+ kif.resize(12);
+ forceC_des.resize(12);
+
+ ARMARX_CHECK_EQUAL(cfg->Kpf.size(), kpf.rows());
+ ARMARX_CHECK_EQUAL(cfg->Kif.size(), kif.rows());
+ ARMARX_CHECK_EQUAL(cfg->DesiredForce.size(), forceC_des.rows());
+
+
+ for (size_t i = 0; i < 12; i++)
+ {
+ kpf(i) = cfg->Kpf.at(i);
+ kif(i) = cfg->Kif.at(i);
+ forceC_des(i) = cfg->DesiredForce.at(i);
+ }
+
+ boxWidth = cfg->BoxWidth;
+
+ }
+
+ std::string NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointBimanualCCDMPController";
+ }
+
+ void NJointBimanualCCDMPController::controllerRun()
+ {
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
+
+ double deltaT = controllerSensorData.getReadBuffer().deltaT;
+
+
+ Eigen::VectorXf leftTargetVel;
+ leftTargetVel.resize(6);
+ leftTargetVel.setZero();
+ Eigen::VectorXf rightTargetVel;
+ rightTargetVel.resize(6);
+ rightTargetVel.setZero();
+
+ std::vector<TaskSpaceDMPControllerPtr > currentControlGroup;
+ Eigen::Matrix4f currentLeaderPose;
+ Eigen::Matrix4f currentFollowerPose;
+ Eigen::VectorXf currentLeaderTwist;
+ Eigen::VectorXf currentFollowerTwist;
+ if (leaderName == "Left")
+ {
+ currentControlGroup = leftGroup;
+ currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
+ currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
+ currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
+ currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
+ }
+ else
+ {
+ currentControlGroup = rightGroup;
+ currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
+ currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
+ currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
+ currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
+ }
+
+ TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
+ TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
+
+
+ leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
+
+ Eigen::Matrix4f currentFollowerLocalPose;
+ currentFollowerLocalPose.block<3, 3>(0, 0) = currentFollowerPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0).inverse();
+ currentFollowerLocalPose.block<3, 1>(0, 3) = currentFollowerLocalPose.block<3, 3>(0, 0) * (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
+ followerDMP->flow(deltaT, currentFollowerLocalPose, currentFollowerTwist);
+
+ Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
+ Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
+ Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
+ Eigen::Matrix4f followerTargetPose;
+ followerTargetPose.block<3, 3>(0, 0) = followerLocalTargetPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0);
+ followerTargetPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) + currentLeaderPose.block<3, 1>(0, 3);
+
+
+ Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
+ Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
+ // followerTargetVel.block<3, 1>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetVel.block<3, 1>(0, 0) + leaderTargetVel.block<3, 1>(0, 0);
+ followerTargetVel.block<3, 1>(0, 0) = KposFollower * (followerTargetPose.block<3, 1>(0, 3) - currentFollowerPose.block<3, 1>(0, 3));
+
+
+ Eigen::Matrix3f followerDiffMat = followerTargetPose.block<3, 3>(0, 0) * currentFollowerPose.block<3, 3>(0, 0).inverse();
+ Eigen::Vector3f followerDiffRPY = KoriFollower * VirtualRobot::MathTools::eigen3f2rpy(followerDiffMat);
+ followerTargetVel(3) = followerDiffRPY(0);
+ followerTargetVel(4) = followerDiffRPY(1);
+ followerTargetVel(5) = followerDiffRPY(2);
+
+ virtualtimer = leaderDMP->canVal;
+
+
+ std::vector<double> leftDMPTarget;
+ std::vector<double> rightDMPTarget;
+
+
+ std::vector<double> followerTargetPoseVec = followerDMP->eigen4f2vec(followerTargetPose);
+
+
+ Eigen::Matrix4f leftTargetPose;
+ Eigen::Matrix4f rightTargetPose;
+
+ if (leaderName == "Left")
+ {
+ leftTargetVel = leaderTargetVel;
+ rightTargetVel = followerTargetVel;
+
+ leftDMPTarget = leaderDMP->getTargetPose();
+ rightDMPTarget = followerTargetPoseVec;
+
+
+ leftTargetPose = leaderTargetPose;
+ rightTargetPose = followerTargetPose;
+ }
+ else
+ {
+ rightTargetVel = leaderTargetVel;
+ leftTargetVel = followerTargetVel;
+
+ rightDMPTarget = leaderDMP->getTargetPose();
+ leftDMPTarget = followerTargetPoseVec;
+
+ rightTargetPose = leaderTargetPose;
+ leftTargetPose = followerTargetPose;
+
+ }
+
+
+
+
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_xvel"] = leftTargetVel(0);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yvel"] = leftTargetVel(1);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_zvel"] = leftTargetVel(2);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_rollvel"] = leftTargetVel(3);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_pitchvel"] = leftTargetVel(4);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yawvel"] = leftTargetVel(5);
+
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_xvel"] = rightTargetVel(0);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yvel"] = rightTargetVel(1);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_zvel"] = rightTargetVel(2);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_rollvel"] = rightTargetVel(3);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_pitchvel"] = rightTargetVel(4);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yawvel"] = rightTargetVel(5);
+
+
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_x"] = leftDMPTarget[0];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_y"] = leftDMPTarget[1];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_z"] = leftDMPTarget[2];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qw"] = leftDMPTarget[3];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qx"] = leftDMPTarget[4];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qy"] = leftDMPTarget[5];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qz"] = leftDMPTarget[6];
+
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_x"] = rightDMPTarget[0];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_y"] = rightDMPTarget[1];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_z"] = rightDMPTarget[2];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qw"] = rightDMPTarget[3];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qx"] = rightDMPTarget[4];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qy"] = rightDMPTarget[5];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qz"] = rightDMPTarget[6];
+
+ // std::vector<double> currentLeftPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentLeftPose);
+ // std::vector<double> currentRightPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentRightPose);
+
+
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = currentLeftPoseVec[0];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = currentLeftPoseVec[1];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = currentLeftPoseVec[2];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = currentLeftPoseVec[3];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = currentLeftPoseVec[4];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = currentLeftPoseVec[5];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = currentLeftPoseVec[6];
+
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = currentRightPoseVec[0];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = currentRightPoseVec[1];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = currentRightPoseVec[2];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = currentRightPoseVec[3];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = currentRightPoseVec[4];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = currentRightPoseVec[5];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = currentRightPoseVec[6];
+
+
+ // debugOutputData.getWriteBuffer().leaderCanVal = leaderDMP->debugData.canVal;
+ // debugOutputData.getWriteBuffer().leadermpcFactor = leaderDMP->debugData.mpcFactor;
+ // debugOutputData.getWriteBuffer().leadererror = leaderDMP->debugData.poseError;
+ // debugOutputData.getWriteBuffer().leaderposError = leaderDMP->debugData.posiError;
+ // debugOutputData.getWriteBuffer().leaderoriError = leaderDMP->debugData.oriError;
+
+ // debugOutputData.getWriteBuffer().followerCanVal = followerDMP->debugData.canVal;
+ // debugOutputData.getWriteBuffer().followermpcFactor = followerDMP->debugData.mpcFactor;
+ // debugOutputData.getWriteBuffer().followererror = followerDMP->debugData.poseError;
+ // debugOutputData.getWriteBuffer().followerposError = followerDMP->debugData.posiError;
+ // debugOutputData.getWriteBuffer().followeroriError = followerDMP->debugData.oriError;
+
+ // debugOutputData.commitWrite();
+
+
+
+
+
+
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().leftTargetVel = leftTargetVel;
+ getWriterControlStruct().rightTargetVel = rightTargetVel;
+ getWriterControlStruct().leftTargetPose = leftTargetPose;
+ getWriterControlStruct().rightTargetPose = rightTargetPose;
+ writeControlStruct();
+ }
+
+ Eigen::VectorXf NJointBimanualCCDMPController::getControlWrench(const Eigen::VectorXf& tcptwist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose)
+ {
+ // 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);
+
+ // Eigen::Vector3f currentTCPPosition = currentPose.block<3,1>(0,3);
+ // Eigen::Vector3f desiredPosition = targetPose.block<3,1>(0,3);
+ // Eigen::Vector3f tcpForces = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition);
+ // Eigen::Vector3f tcpDesiredForce = tcpForces - dpos.cwiseProduct(currentTCPLinearVelocity);
+
+ // Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
+ // Eigen::Matrix3f diffMat = targetPose.block<3,3>(0,0) * currentRotMat.inverse();
+ // Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
+ // Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ // Eigen::Vector6f tcpDesiredWrench;
+ // tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
+
+ // return tcpDesiredWrench;
+
+ }
+
+
+
+ void NJointBimanualCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+ controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
+ controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
+ controllerSensorData.getWriteBuffer().deltaT = deltaT;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ // cartesian vel controller
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
+
+ Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
+ jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
+
+ Eigen::VectorXf leftqpos;
+ Eigen::VectorXf leftqvel;
+ leftqpos.resize(leftPositionSensors.size());
+ leftqvel.resize(leftVelocitySensors.size());
+ for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
+ {
+ leftqpos(i) = leftPositionSensors[i]->position;
+ leftqvel(i) = leftVelocitySensors[i]->velocity;
+ }
+
+ Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
+ jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
+
+
+ Eigen::VectorXf rightqpos;
+ Eigen::VectorXf rightqvel;
+ rightqpos.resize(rightPositionSensors.size());
+ rightqvel.resize(rightVelocitySensors.size());
+
+ for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
+ {
+ rightqpos(i) = rightPositionSensors[i]->position;
+ rightqvel(i) = rightVelocitySensors[i]->velocity;
+ }
+
+ Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
+ Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
+
+
+ controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
+ controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
+ controllerSensorData.commitWrite();
+
+
+ Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
+ Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
+ Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
+ Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
+ Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
+ Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
+
+ // Todo: force control
+ int nl = leftRNS->getSize();
+ int nr = rightRNS->getSize();
+ int n = nl + nr;
+ // GraspMatrix
+ Eigen::Vector3f localDistanceCoM;
+ localDistanceCoM << 0.5 * boxWidth, 0, 0;
+
+
+ Eigen::Vector3f rl = -leftCurrentPose.block<3, 3>(0, 0) * localDistanceCoM; //tcpLeft->getRobot()->getRootNode()->toLocalCoordinateSystem(tcpLeft->toGlobalCoordinateSystemVec(localDistanceCoM));
+ Eigen::Vector3f rr = rightCurrentPose.block<3, 3>(0, 0) * localDistanceCoM;
+ Eigen::Matrix6f leftGraspMatrix = Eigen::MatrixXf::Identity(nl, nl);
+ Eigen::Matrix6f rightGraspMatrix = Eigen::MatrixXf::Identity(nr, nr);
+ leftGraspMatrix.block<3, 3>(3, 0) << 0, -rl(2), rl(1),
+ rl(2), 0, -rl(0),
+ -rl(1), rl(0), 0;
+ rightGraspMatrix.block<3, 3>(3, 0) << 0, -rr(2), rr(1),
+ rr(2), 0, -rr(0),
+ -rr(1), rr(0), 0;
+ Eigen::MatrixXf graspMatrix(6, nl + nr);
+ graspMatrix << leftGraspMatrix, rightGraspMatrix;
+
+ // constraint Jacobain and projection matrix
+ Eigen::MatrixXf jacobi(12, n);
+ jacobi.setZero(12, n);
+
+ jacobi.block < 6, 8 > (0, 0) = jacobiL;
+ jacobi.block < 6, 8 > (6, 8) = jacobiR;
+
+ Eigen::MatrixXf pinvGraspMatrix = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 1);
+ Eigen::MatrixXf jacobiC = (Eigen::MatrixXf::Identity(12, 12) - pinvGraspMatrix * graspMatrix) * jacobi;
+
+ Eigen::MatrixXf pinvJacobiC = leftIK->computePseudoInverseJacobianMatrix(jacobiC, 1);
+ Eigen::MatrixXf projection = Eigen::MatrixXf::Identity(12, 12) - pinvJacobiC * jacobiC;
+
+
+
+ // calculate inertia matrix
+ Eigen::MatrixXf leftInertialMatrix;
+ Eigen::MatrixXf rightInertialMatrix;
+ leftInertialMatrix.setZero(nl, nl);
+ rightInertialMatrix.setZero(nr, nr);
+
+ for (size_t i = 0; i < rnsLeftBody->getNodeNames().size(); ++i)
+ {
+ VirtualRobot::RobotNodePtr rnbody = rnsLeftBody->getNode(i);
+ VirtualRobot::RobotNodePtr rnjoint = leftRNS->getNode(i + 1);
+
+ Eigen::MatrixXf jacobipos_rn = 0.001 * leftIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
+ Eigen::MatrixXf jacobiori_rn = leftIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
+
+
+ float m = rnbody->getMass();
+ Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
+ leftInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn + jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
+ }
+
+ for (size_t i = 0; i < rnsRightBody->getNodeNames().size(); ++i)
+ {
+ VirtualRobot::RobotNodePtr rnbody = rnsRightBody->getNode(i);
+ VirtualRobot::RobotNodePtr rnjoint = rightRNS->getNode(i + 1);
+
+ Eigen::MatrixXf jacobipos_rn = 0.001 * rightIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
+ Eigen::MatrixXf jacobiori_rn = rightIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
+
+
+ float m = rnbody->getMass();
+ Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
+
+ rightInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn + jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
+ }
+ Eigen::MatrixXf M = Eigen::MatrixXf::Zero(n, n);
+ M.topLeftCorner(nl, nl) = leftInertialMatrix;
+ M.bottomRightCorner(nr, nr) = rightInertialMatrix;
+
+ Eigen::MatrixXf Mc = M + projection * M - M * projection;
+
+
+
+ // unconstrained space controller
+ Eigen::Vector6f leftJointControlWrench;
+ {
+ Eigen::Vector3f targetTCPLinearVelocity;
+ targetTCPLinearVelocity << 0.001 * leftTargetVel(0), 0.001 * leftTargetVel(1), 0.001 * leftTargetVel(2);
+
+ Eigen::Vector3f currentTCPLinearVelocity;
+ currentTCPLinearVelocity << currentLeftTwist(0), currentLeftTwist(1), currentLeftTwist(2);
+ Eigen::Vector3f currentTCPAngularVelocity;
+ currentTCPAngularVelocity << currentLeftTwist(3), currentLeftTwist(4), currentLeftTwist(5);
+ Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
+ Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
+
+ Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+ Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
+ Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+ Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ leftJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
+ }
+
+
+ Eigen::Vector6f rightJointControlWrench;
+ {
+ Eigen::Vector3f currentTCPLinearVelocity;
+ currentTCPLinearVelocity << currentRightTwist(0), currentRightTwist(1), currentRightTwist(2);
+ Eigen::Vector3f currentTCPAngularVelocity;
+ currentTCPAngularVelocity << currentRightTwist(3), currentRightTwist(4), currentRightTwist(5);
+ Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
+ Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
+
+ Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) - dpos.cwiseProduct(currentTCPLinearVelocity);
+ Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
+ Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+ Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ rightJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
+ }
+
+ Eigen::VectorXf leftNullspaceTorque = knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
+ Eigen::VectorXf rightNullspaceTorque = knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
+
+ float lambda = 2;
+ Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
+ Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
+
+ Eigen::MatrixXf jtpinvR = leftIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
+ Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
+
+ Eigen::VectorXf torqueNotC(16);
+ torqueNotC << leftJointDesiredTorques, rightJointDesiredTorques;
+
+ // constrained space control
+
+ Eigen::MatrixXf mu = (Eigen::MatrixXf::Identity(12, 12) - projection) * M * Mc.inverse();
+ Eigen::VectorXf forceC = Eigen::VectorXf::Zero(12);
+ Eigen::Vector3f leftForce = leftCurrentPose.block<3, 3>(0, 0) * leftForceTorque->force;
+ Eigen::Vector3f leftTorque = leftCurrentPose.block<3, 3>(0, 0) * leftForceTorque->torque;
+ Eigen::Vector3f rightForce = rightCurrentPose.block<3, 3>(0, 0) * rightForceTorque->force;
+ Eigen::Vector3f rightTorque = rightCurrentPose.block<3, 3>(0, 0) * rightForceTorque->torque;
+ for (int i = 0; i < 3; i++)
+ {
+ forceC(i) = forceC_des(i) + kpf(i) * (forceC_des(i) - leftForce(i)); // TODO: add PID controller
+ forceC(i + 3) = forceC_des(i + 3) + kpf(i + 3) * (forceC_des(i + 3) - leftTorque(i)); // TODO: add PID controller
+ forceC(i + 6) = forceC_des(i + 6) + kpf(i + 6) * (forceC_des(i + 6) - rightForce(i)); // TODO: add PID controller
+ forceC(i + 9) = forceC_des(i + 9) + kpf(i + 9) * (forceC_des(i + 9) - rightTorque(i)); // TODO: add PID controller
+ }
+ Eigen::VectorXf qacc;
+ qacc.resize(n);
+ for (size_t i = 0; i < leftAccelerationSensors.size(); ++i)
+ {
+ qacc(i) = 0.001 * leftAccelerationSensors[i]->acceleration;
+ }
+
+ for (size_t i = 0; i < rightAccelerationSensors.size(); ++i)
+ {
+ qacc(i + leftAccelerationSensors.size()) = 0.001 * rightAccelerationSensors[i]->acceleration;
+ }
+
+ Eigen::VectorXf torqueC = mu * (torqueNotC + (Eigen::MatrixXf::Identity(n, n) - projection) * qacc) - jacobiC.transpose() * forceC;
+
+ // all torques
+ Eigen::VectorXf torqueInput = projection * torqueNotC + torqueC;
+ leftJointDesiredTorques = torqueInput.head(nl);
+ rightJointDesiredTorques = torqueInput.block<8, 1>(8, 0);
+
+ // torque limit
+ for (size_t i = 0; i < leftTargets.size(); ++i)
+ {
+ float desiredTorque = leftJointDesiredTorques(i);
+
+ if (isnan(desiredTorque))
+ {
+ desiredTorque = 0;
+ }
+
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
+
+ debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
+
+ leftTargets.at(i)->torque = desiredTorque;
+ }
+
+
+
+ for (size_t i = 0; i < rightTargets.size(); ++i)
+ {
+ float desiredTorque = rightJointDesiredTorques(i);
+
+ if (isnan(desiredTorque))
+ {
+ desiredTorque = 0;
+ }
+
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
+
+ debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
+
+ rightTargets.at(i)->torque = desiredTorque;
+ }
+
+ // debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
+ // debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
+ // debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
+ // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_x = tcpDesiredTorque(0);
+ // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_y = tcpDesiredTorque(1);
+ // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
+ // debugDataInfo.getWriteBuffer().quatError = errorAngle;
+ // debugDataInfo.getWriteBuffer().posiError = posiError;
+ debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
+ debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
+ debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
+ debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
+ debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
+ debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
+
+
+ debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
+ debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
+ debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
+
+ debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
+ debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
+ debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
+
+
+ debugDataInfo.commitWrite();
+
+
+
+ // Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
+ // float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
+ // if (normLinearVelocity > cfg->maxLinearVel)
+ // {
+ // leftTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
+ // }
+ // float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
+ // if (normAngularVelocity > cfg->maxAngularVel)
+ // {
+ // leftTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
+ // }
+
+ // Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
+ // Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
+ // Eigen::VectorXf jnvL = jtpinvL * leftTargetVel + (I - jtpinvL * jacobiL) * leftNullSpaceJointVelocity;
+
+ // for (size_t i = 0; i < leftTargets.size(); ++i)
+ // {
+ // leftTargets.at(i)->velocity = jnvL(i);
+ // }
+
+ // Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
+ // normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
+ // if (normLinearVelocity > cfg->maxLinearVel)
+ // {
+ // rightTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
+ // }
+ // normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
+ // if (normAngularVelocity > cfg->maxAngularVel)
+ // {
+ // rightTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
+ // }
+ // Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
+ // Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
+ // Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
+
+ // for (size_t i = 0; i < rightTargets.size(); ++i)
+ // {
+ // rightTargets.at(i)->velocity = jnvR(i);
+ // }
+ }
+
+ void NJointBimanualCCDMPController::learnDMPFromFiles(const std::string& name, const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ if (name == "LeftLeader")
+ {
+ leftGroup.at(0)->learnDMPFromFiles(fileNames);
+ }
+ else if (name == "LeftFollower")
+ {
+ rightGroup.at(1)->learnDMPFromFiles(fileNames);
+ }
+ else if (name == "RightLeader")
+ {
+ rightGroup.at(0)->learnDMPFromFiles(fileNames);
+ }
+ else
+ {
+ leftGroup.at(1)->learnDMPFromFiles(fileNames);
+ }
+ }
+
+
+ void NJointBimanualCCDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ {
+ LockGuardType guard(controllerMutex);
+ if (leaderName == "Left")
+ {
+ leftGroup.at(0)->setViaPose(u, viapoint);
+ }
+ else
+ {
+ rightGroup.at(0)->setViaPose(u, viapoint);
+ }
+ }
+
+ void NJointBimanualCCDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ LockGuardType guard(controllerMutex);
+ if (leaderName == "Left")
+ {
+ leftGroup.at(0)->setGoalPoseVec(goals);
+ }
+ else
+ {
+ rightGroup.at(0)->setGoalPoseVec(goals);
+ }
+
+ }
+
+ void NJointBimanualCCDMPController::changeLeader(const Ice::Current&)
+ {
+ LockGuardType guard(controllerMutex);
+
+ if (leaderName == "Left")
+ {
+ leaderName = "Right";
+ rightGroup.at(0)->canVal = virtualtimer;
+ rightGroup.at(1)->canVal = virtualtimer;
+ }
+ else
+ {
+ leaderName = "Left";
+ leftGroup.at(0)->canVal = virtualtimer;
+ leftGroup.at(1)->canVal = virtualtimer;
+ }
+
+ }
+
+
+
+
+
+ void NJointBimanualCCDMPController::runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&)
+ {
+ virtualtimer = cfg->timeDuration;
+
+ Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
+ Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
+
+ leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
+ rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
+
+
+ ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
+
+ leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose), getLocalPose(leftGoals, rightGoals));
+ rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose), getLocalPose(rightGoals, leftGoals));
+
+ controllerTask->start();
+ }
+
+ Eigen::Matrix4f NJointBimanualCCDMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
+ {
+ Eigen::Matrix4f localPose;
+ localPose.block<3, 3>(0, 0) = globalTargetPose.block<3, 3>(0, 0) * newCoordinate.block<3, 3>(0, 0).inverse();
+ localPose.block<3, 1>(0, 3) = localPose.block<3, 3>(0, 0) * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
+
+ return localPose;
+ }
+
+
+ void NJointBimanualCCDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
+
+ StringVariantBaseMap datafields;
+ auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+ datafields["leftTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
+ datafields["leftTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
+ datafields["leftTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
+ datafields["rightTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
+ datafields["rightTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
+ datafields["rightTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
+
+
+ datafields["leftCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
+ datafields["leftCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
+ datafields["leftCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
+ datafields["rightCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
+ datafields["rightCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
+ datafields["rightCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
+
+ // StringVariantBaseMap datafields;
+ // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ // for (auto& pair : values)
+ // {
+ // datafields[pair.first] = new Variant(pair.second);
+ // }
+
+ // auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
+ // for (auto& pair : dmpTargets)
+ // {
+ // datafields[pair.first] = new Variant(pair.second);
+ // }
+
+ // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
+ // for (auto& pair : currentPose)
+ // {
+ // datafields[pair.first] = new Variant(pair.second);
+ // }
+
+ // datafields["leaderCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderCanVal);
+ // datafields["leadermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadermpcFactor);
+ // datafields["leaderError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadererror);
+ // datafields["leaderposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderposError);
+ // datafields["leaderoriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderoriError);
+
+ // datafields["followerCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerCanVal);
+ // datafields["followermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().followermpcFactor);
+ // datafields["followerError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followererror);
+ // datafields["followerposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerposError);
+ // datafields["followeroriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followeroriError);
+
+ debugObs->setDebugChannel("DMPController", datafields);
+ }
+
+ void NJointBimanualCCDMPController::onInitComponent()
+ {
+ ARMARX_INFO << "init ...";
+ controllerTask = new PeriodicTask<NJointBimanualCCDMPController>(this, &NJointBimanualCCDMPController::controllerRun, 0.3);
+ }
+
+ void NJointBimanualCCDMPController::onDisconnectComponent()
+ {
+ controllerTask->stop();
+ ARMARX_INFO << "stopped ...";
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
new file mode 100644
index 0000000000000000000000000000000000000000..ee7f250502ec24d64950d9ab09884d1cc93d64d9
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
@@ -0,0 +1,218 @@
+
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <dmp/representation/dmp/umitsmp.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+
+
+using namespace DMP;
+namespace armarx
+{
+
+
+ TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPController);
+ TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPControllerControlData);
+
+ typedef std::pair<double, DVec > ViaPoint;
+ typedef std::vector<ViaPoint > ViaPointsSet;
+ class NJointBimanualCCDMPControllerControlData
+ {
+ public:
+ Eigen::VectorXf leftTargetVel;
+ Eigen::VectorXf rightTargetVel;
+
+ Eigen::Matrix4f leftTargetPose;
+ Eigen::Matrix4f rightTargetPose;
+ };
+
+ class NJointBimanualCCDMPController :
+ public NJointControllerWithTripleBuffer<NJointBimanualCCDMPControllerControlData>,
+ public NJointBimanualCCDMPControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointBimanualCCDMPControllerConfigPtr;
+ NJointBimanualCCDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointBimanualCCDMPControllerInterface interface
+ void learnDMPFromFiles(const std::string&, const Ice::StringSeq&, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return false;
+ }
+
+ void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&);
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+
+ void changeLeader(const Ice::Current&);
+
+ double getVirtualTime(const Ice::Current&)
+ {
+ return virtualtimer;
+ }
+
+ std::string getLeaderName(const Ice::Current&)
+ {
+ return leaderName;
+ }
+
+ protected:
+
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitComponent();
+ void onDisconnectComponent();
+ void controllerRun();
+ private:
+
+ Eigen::VectorXf getControlWrench(const Eigen::VectorXf& twist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose);
+
+ Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
+ Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
+ {
+ Eigen::Matrix4f newCoordinate = VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4), newCoordinateVec.at(5), newCoordinateVec.at(6), newCoordinateVec.at(3));
+ newCoordinate(0, 3) = newCoordinateVec.at(0);
+ newCoordinate(1, 3) = newCoordinateVec.at(1);
+ newCoordinate(2, 3) = newCoordinateVec.at(2);
+
+ Eigen::Matrix4f globalTargetPose = VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4), globalTargetPoseVec.at(5), globalTargetPoseVec.at(6), globalTargetPoseVec.at(3));
+ globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
+ globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
+ globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
+
+ return getLocalPose(newCoordinate, globalTargetPose);
+
+ }
+
+ struct DebugBufferData
+ {
+ StringFloatDictionary desired_torques;
+ float leftTargetPose_x;
+ float leftTargetPose_y;
+ float leftTargetPose_z;
+ float rightTargetPose_x;
+ float rightTargetPose_y;
+ float rightTargetPose_z;
+
+ float leftCurrentPose_x;
+ float leftCurrentPose_y;
+ float leftCurrentPose_z;
+ float rightCurrentPose_x;
+ float rightCurrentPose_y;
+ float rightCurrentPose_z;
+
+ // StringFloatDictionary latestTargetVelocities;
+ // StringFloatDictionary dmpTargets;
+ // StringFloatDictionary currentPose;
+
+ // double leadermpcFactor;
+ // double leadererror;
+ // double leaderposError;
+ // double leaderoriError;
+ // double leaderCanVal;
+
+ // double followermpcFactor;
+ // double followererror;
+ // double followerposError;
+ // double followeroriError;
+ // double followerCanVal;
+
+
+ };
+
+ TripleBuffer<DebugBufferData> debugDataInfo;
+
+ struct NJointBimanualCCDMPControllerSensorData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentLeftPose;
+ Eigen::Matrix4f currentRightPose;
+ Eigen::VectorXf currentLeftTwist;
+ Eigen::VectorXf currentRightTwist;
+ };
+ TripleBuffer<NJointBimanualCCDMPControllerSensorData> controllerSensorData;
+
+
+ std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
+ std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
+
+ std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
+ std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
+
+ const SensorValueForceTorque* rightForceTorque;
+ const SensorValueForceTorque* leftForceTorque;
+
+ NJointBimanualCCDMPControllerConfigPtr cfg;
+ VirtualRobot::DifferentialIKPtr leftIK;
+ VirtualRobot::DifferentialIKPtr rightIK;
+
+ std::vector<TaskSpaceDMPControllerPtr > leftGroup;
+ std::vector<TaskSpaceDMPControllerPtr > rightGroup;
+
+ std::string leaderName;
+
+ VirtualRobot::RobotNodePtr tcpLeft;
+ VirtualRobot::RobotNodePtr tcpRight;
+
+ double virtualtimer;
+
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointBimanualCCDMPController>::pointer_type controllerTask;
+
+ Eigen::VectorXf leftDesiredJointValues;
+ Eigen::VectorXf rightDesiredJointValues;
+
+ float KoriFollower;
+ float KposFollower;
+ float DposFollower;
+ float DoriFollower;
+
+ Eigen::VectorXf forceC_des;
+ float boxWidth;
+
+ Eigen::Vector3f kpos;
+ Eigen::Vector3f kori;
+ Eigen::Vector3f dpos;
+ Eigen::Vector3f dori;
+ Eigen::Vector6f kpf;
+ Eigen::Vector6f kif;
+
+ float knull;
+ float dnull;
+
+ std::vector<std::string> leftJointNames;
+ std::vector<std::string> rightJointNames;
+
+ float torqueLimit;
+ VirtualRobot::RobotNodeSetPtr leftRNS;
+ VirtualRobot::RobotNodeSetPtr rightRNS;
+ VirtualRobot::RobotNodeSetPtr rnsLeftBody;
+ VirtualRobot::RobotNodeSetPtr rnsRightBody;
+
+ // NJointBimanualCCDMPControllerInterface interface
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
index 46cda82c25f0d6d0b4db74b101c05a4ab5d44562..ee30c07a9ce7da07abdcb52adc9d4b20d4c8df06 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
@@ -17,7 +17,7 @@ namespace armarx
for (size_t i = 0; i < rns->getSize(); ++i)
{
std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::VelocityTorque);
const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
@@ -45,6 +45,7 @@ namespace armarx
torquePIDs.resize(tcpController->rns->getSize(), pidController());
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
// set DMP
isDisturbance = false;
@@ -55,7 +56,7 @@ namespace armarx
dmpTypes = cfg->dmpTypes;
for (size_t i = 0; i < dmpPtrList.size(); ++i)
{
- dmpPtrList[i].reset(new UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, cfg->tau));
+ dmpPtrList[i].reset(new UMITSMP(cfg->kernelSize, 2, cfg->tau));
canVals[i] = timeDurations[i];
}
finished = false;
@@ -72,12 +73,14 @@ namespace armarx
// initialize tcp position and orientation
Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
- oldPose = pose;
tcpPosition(0) = pose(0, 3);
tcpPosition(1) = pose(1, 3);
tcpPosition(2) = pose(2, 3);
- tcpOrientation = VirtualRobot::MathTools::eigen4f2rpy(pose);
-
+ VirtualRobot::MathTools::Quaternion tcpQ = VirtualRobot::MathTools::eigen4f2quat(pose);
+ tcpOrientation.w() = tcpQ.w;
+ tcpOrientation.x() = tcpQ.x;
+ tcpOrientation.y() = tcpQ.y;
+ tcpOrientation.z() = tcpQ.z;
NJointCCDMPControllerSensorData initSensorData;
initSensorData.deltaT = 0;
@@ -88,15 +91,13 @@ namespace armarx
currentStates.resize(cfg->dmpNum);
targetSubStates.resize(cfg->dmpNum);
- targetState.resize(6);
+ targetState.resize(7);
targetVels.resize(6);
+ targetVels.setZero();
NJointCCDMPControllerControlData initData;
initData.targetTSVel.resize(6);
- for (size_t i = 0; i < 6; ++i)
- {
- initData.targetTSVel[i] = 0;
- }
+ initData.targetTSVel.setZero();
initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
initData.torqueKp.resize(tcpController->rns->getSize(), 0);
initData.torqueKd.resize(tcpController->rns->getSize(), 0);
@@ -125,21 +126,32 @@ namespace armarx
Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
Eigen::Vector3f currentPosition;
currentPosition << currentPose(0, 3), currentPose(1, 3), currentPose(2, 3);
- Eigen::Vector3f currentRPY = VirtualRobot::MathTools::eigen4f2rpy(currentPose);
-
double posError = 0;
- double oriError = 0;
for (size_t i = 0; i < 3; ++i)
{
posError += pow(currentPosition(i) - targetState[i], 2);
}
- for (size_t i = 0; i < 3; ++i)
+ posError = sqrt(posError);
+
+
+ VirtualRobot::MathTools::Quaternion cQuat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
+ Eigen::Quaterniond currentQ;
+ Eigen::Quaterniond targetQ;
+ currentQ.w() = cQuat.w;
+ currentQ.x() = cQuat.x;
+ currentQ.y() = cQuat.y;
+ currentQ.z() = cQuat.z;
+ targetQ.w() = targetState[3];
+ targetQ.x() = targetState[4];
+ targetQ.y() = targetState[5];
+ targetQ.z() = targetState[6];
+
+ double oriError = targetQ.angularDistance(currentQ);
+ if (oriError > M_PI)
{
- oriError += pow(currentRPY(i) - targetState[i + 3], 2);
+ oriError = 2 * M_PI - oriError;
}
- posError = sqrt(posError);
- oriError = sqrt(oriError);
double error = posError + posToOriRatio * oriError;
double phaseDist;
@@ -168,25 +180,28 @@ namespace armarx
// run DMP one after another
Eigen::Matrix4f targetPose = Eigen::Matrix4f::Identity();
- //bool finished = true;
+
+ Eigen::VectorXf dmpVels;
+ dmpVels.resize(6);
+ dmpVels.setZero();
for (size_t i = 0; i < static_cast<size_t>(cfg->dmpNum); ++i)
{
double timeDuration = timeDurations[i];
std::string dmpType = dmpTypes[i];
- //double amplitude = 1.0;
+ double amplitude = 1.0;
if (dmpType == "Periodic")
{
if (canVals[i] <= 1e-8)
{
canVals[i] = timeDuration;
}
- //amplitude = amplitudes[i];
+ amplitude = amplitudes[i];
}
if (canVals[i] > 1e-8)
{
- DMP::UMIDMPPtr dmpPtr = dmpPtrList[i];
+ DMP::UMITSMPPtr dmpPtr = dmpPtrList[i];
double dmpDeltaT = deltaT / timeDuration;
double tau = dmpPtr->getTemporalFactor();
canVals[i] -= 1 / tau * deltaT * 1 / (1 + phaseStop) ;
@@ -196,70 +211,101 @@ namespace armarx
(currentStates[i], canVals[i] / timeDurations[i], dmpDeltaT, targetSubStates[i]);
- //finished = false;
+
+ for (size_t j = 0; j < 3; ++j)
+ {
+ dmpVels(j) += currentStates[i][j].vel / timeDurations[i];
+ }
+
+ Eigen::Quaterniond quatVel0;
+ quatVel0.w() = currentStates[i][3].vel;
+ quatVel0.x() = currentStates[i][4].vel;
+ quatVel0.y() = currentStates[i][5].vel;
+ quatVel0.z() = currentStates[i][6].vel;
+ Eigen::Quaterniond dmpQ;
+ dmpQ.w() = currentStates[i][3].pos;
+ dmpQ.x() = currentStates[i][4].pos;
+ dmpQ.y() = currentStates[i][5].pos;
+ dmpQ.z() = currentStates[i][6].pos;
+ Eigen::Quaterniond angularVel0 = 2 * quatVel0 * dmpQ.inverse();
+
+
+ Eigen::Vector3f angularVelVec;
+ angularVelVec << angularVel0.x() / timeDurations[i],
+ angularVel0.y() / timeDurations[i],
+ angularVel0.z() / timeDurations[i];
+
+ angularVelVec = targetPose.block<3, 3>(0, 0) * angularVelVec;
+
+ ARMARX_INFO << "i: " << i << " angularVelVec: " << angularVelVec;
+ dmpVels(3) += angularVelVec(0);
+ dmpVels(4) += angularVelVec(1);
+ dmpVels(5) += angularVelVec(2);
+
+ finished = false;
}
- Eigen::Matrix4f targetSubMat = VirtualRobot::MathTools::rpy2eigen4f(targetSubStates[i][3], targetSubStates[i][4], targetSubStates[i][5]);
+
+ Eigen::Matrix4f targetSubMat = VirtualRobot::MathTools::quat2eigen4f(targetSubStates[i][4], targetSubStates[i][5], targetSubStates[i][6], targetSubStates[i][3]);
targetSubMat(0, 3) = targetSubStates[i][0];
targetSubMat(1, 3) = targetSubStates[i][1];
targetSubMat(2, 3) = targetSubStates[i][2];
- ARMARX_INFO << "targetSubMat: " << targetSubMat;
-
targetPose = targetPose * targetSubMat;
}
- ARMARX_INFO << "targetPose: " << targetPose;
-
- Eigen::Vector3f targetRPY = VirtualRobot::MathTools::eigen4f2rpy(targetPose);
- targetState[0] = targetPose(0, 3);
- targetState[1] = targetPose(1, 3);
- targetState[2] = targetPose(2, 3);
- targetState[3] = targetRPY(0);
- targetState[4] = targetRPY(1);
- targetState[5] = targetRPY(2);
-
- Eigen::Vector3f oldRPY = VirtualRobot::MathTools::eigen4f2rpy(oldPose);
-
-
+ // ARMARX_INFO << "targetPose: " << targetPose;
for (size_t i = 0; i < 3; i++)
{
- double vel0 = (targetState[i] - oldPose(i, 3)) / deltaT;
+ double vel0 = dmpVels(i);
double vel1 = phaseKpPos * (targetState[i] - currentPose(i, 3));
- targetVels[i] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ targetVels(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
}
- for (size_t i = 0; i < 3; i++)
- {
- double vel0 = (targetState[i + 3] - oldRPY(i)) / deltaT;
- double vel1 = phaseKpOri * (targetState[i + 3] - currentRPY(i));
- targetVels[i + 3] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- }
- oldPose = targetPose;
+ Eigen::Quaterniond diffQ = targetQ * currentQ.inverse();
+ Eigen::Quaterniond quatVel1 = phaseKpOri * diffQ;
+ Eigen::Quaterniond angularVel1 = 2 * quatVel1 * currentQ.inverse();
+ targetVels(3) = mpcFactor * dmpVels(3) + (1 - mpcFactor) * angularVel1.x();
+ targetVels(4) = mpcFactor * dmpVels(4) + (1 - mpcFactor) * angularVel1.y();
+ targetVels(5) = mpcFactor * dmpVels(5) + (1 - mpcFactor) * angularVel1.z();
- debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels[0];
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels[1];
- debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels[2];
- debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels[3];
- debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels[4];
- debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels[5];
+
+
+ VirtualRobot::MathTools::Quaternion tQuat = VirtualRobot::MathTools::eigen4f2quat(targetPose);
+ targetState[0] = targetPose(0, 3);
+ targetState[1] = targetPose(1, 3);
+ targetState[2] = targetPose(2, 3);
+ targetState[3] = tQuat.w;
+ targetState[4] = tQuat.x;
+ targetState[5] = tQuat.y;
+ targetState[6] = tQuat.z;
+
+
+ debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_roll"] = targetState[3];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_pitch"] = targetState[4];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_yaw"] = targetState[5];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
debugOutputData.getWriteBuffer().realTCP["real_x"] = currentPosition[0];
debugOutputData.getWriteBuffer().realTCP["real_y"] = currentPosition[1];
debugOutputData.getWriteBuffer().realTCP["real_z"] = currentPosition[2];
- debugOutputData.getWriteBuffer().realTCP["real_roll"] = currentRPY[0];
- debugOutputData.getWriteBuffer().realTCP["real_pitch"] = currentRPY[1];
- debugOutputData.getWriteBuffer().realTCP["real_yaw"] = currentRPY[2];
+ debugOutputData.getWriteBuffer().realTCP["real_qw"] = cQuat.w;
+ debugOutputData.getWriteBuffer().realTCP["real_qx"] = cQuat.x;
+ debugOutputData.getWriteBuffer().realTCP["real_qy"] = cQuat.y;
+ debugOutputData.getWriteBuffer().realTCP["real_qz"] = cQuat.z;
debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
debugOutputData.getWriteBuffer().error = error;
@@ -285,68 +331,19 @@ namespace armarx
controllerSensorData.getWriteBuffer().currentPose = tcp->getPoseInRootFrame();
controllerSensorData.getWriteBuffer().deltaT = deltaT;
controllerSensorData.getWriteBuffer().currentTime += deltaT;
-
controllerSensorData.commitWrite();
// cartesian vel controller
- Eigen::VectorXf x;
- auto mode = rtGetControlStruct().mode;
- if (mode == VirtualRobot::IKSolver::All)
- {
- x.resize(6);
- for (size_t i = 0; i < 6; i++)
- {
- x(i) = rtGetControlStruct().targetTSVel[i];
- }
- }
- else if (mode == VirtualRobot::IKSolver::Position)
- {
- x.resize(3);
- for (size_t i = 0; i < 3; i++)
- {
- x(i) = rtGetControlStruct().targetTSVel[i];
- }
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
- }
- else if (mode == VirtualRobot::IKSolver::Orientation)
- {
- x.resize(3);
- for (size_t i = 0; i < 3; i++)
- {
- x(i) = rtGetControlStruct().targetTSVel[i + 3];
- }
- }
- else
- {
- // No mode has been set
- return;
- }
- Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
- float jointLimitAvoidanceKp = rtGetControlStruct().avoidJointLimitsKp;
- if (jointLimitAvoidanceKp > 0)
- {
- jnv += jointLimitAvoidanceKp * tcpController->calculateJointLimitAvoidance();
- }
- for (size_t i = 0; i < tcpController->rns->getSize(); i++)
- {
- jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
- if (torqueSensors.at(i) && gravityTorqueSensors.at(i) && rtGetControlStruct().torqueKp.at(i) != 0)
- {
- torquePIDs.at(i).Kp = rtGetControlStruct().torqueKp.at(i);
- torquePIDs.at(i).Kd = rtGetControlStruct().torqueKd.at(i);
- jnv(i) += torquePIDs.at(i).update(deltaT, torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
- }
- else
- {
- torquePIDs.at(i).lastError = 0;
- }
- }
+ Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
+
+ Eigen::VectorXf jnv = jtpinv * rtGetControlStruct().targetTSVel;
- Eigen::VectorXf jointTargetVelocities = tcpController->calculate(x, jnv, mode);
for (size_t i = 0; i < targets.size(); ++i)
{
- targets.at(i)->velocity = jointTargetVelocities(i);
+ targets.at(i)->velocity = jnv(i);
}
}
@@ -391,7 +388,6 @@ namespace armarx
}
dmpPtrList[dmpId]->learnFromTrajectories(trajs);
- dmpPtrList[dmpId]->setOneStepMPC(true);
dmpPtrList[dmpId]->styleParas = dmpPtrList[dmpId]->getStyleParasWithRatio(ratios);
@@ -405,28 +401,38 @@ namespace armarx
currentPos.vel = 0;
currentState.push_back(currentPos);
}
- for (size_t i = 0; i < 3; i++)
- {
- DMP::DMPState currentPos;
- currentPos.pos = tcpOrientation(i);
- currentPos.vel = 0;
- currentState.push_back(currentPos);
- }
+
+ DMP::DMPState currentPos;
+ currentPos.pos = tcpOrientation.w();
+ currentPos.vel = 0;
+ currentState.push_back(currentPos);
+
+ currentPos.pos = tcpOrientation.x();
+ currentPos.vel = 0;
+ currentState.push_back(currentPos);
+
+ currentPos.pos = tcpOrientation.y();
+ currentPos.vel = 0;
+ currentState.push_back(currentPos);
+
+ currentPos.pos = tcpOrientation.z();
+ currentPos.vel = 0;
+ currentState.push_back(currentPos);
+
}
else
{
currentState = starts;
-
-
}
for (size_t i = 0; i < 3; i++)
{
targetState[i] = tcpPosition(i);
}
- for (size_t i = 0; i < 3; i++)
- {
- targetState[i + 3] = tcpOrientation(i);
- }
+
+ targetState[3] = tcpOrientation.w();
+ targetState[4] = tcpOrientation.x();
+ targetState[5] = tcpOrientation.y();
+ targetState[6] = tcpOrientation.z();
currentStates[dmpId] = currentState;
@@ -439,10 +445,9 @@ namespace armarx
void NJointCCDMPController::setViaPoints(int dmpId, Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
{
- ViaPoint viaPoint(u, viapoint);
LockGuardType guard(controllerMutex);
- dmpPtrList[dmpId]->setViaPoint(viaPoint);
+ dmpPtrList[dmpId]->setViaPoint(u, viapoint);
}
void NJointCCDMPController::setGoals(int dmpId, const Ice::DoubleSeq& goals, const Ice::Current& ice)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
index e3917f63cf874b8b1b08f6922dd66f08f4da6821..215a5e62b0965ecc1acd7de35bfaabc4056b6c4c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
@@ -7,9 +7,11 @@
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umidmp.h>
+#include <dmp/representation/dmp/umitsmp.h>
#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+
using namespace DMP;
@@ -25,7 +27,7 @@ namespace armarx
class NJointCCDMPControllerControlData
{
public:
- std::vector<float> targetTSVel;
+ Eigen::VectorXf targetTSVel;
// cartesian velocity control data
std::vector<float> nullspaceJointVelocities;
float avoidJointLimitsKp = 0;
@@ -124,7 +126,7 @@ namespace armarx
std::string nodeSetName;
// dmp parameters
- std::vector<UMIDMPPtr > dmpPtrList;
+ std::vector<UMITSMPPtr > dmpPtrList;
std::vector<double> canVals;
std::vector<double> timeDurations;
std::vector<std::string > dmpTypes;
@@ -149,15 +151,21 @@ namespace armarx
double posToOriRatio;
- std::vector<float> targetVels;
+ Eigen::VectorXf targetVels;
std::vector<int> learnedDMP;
+
+
+
+
+
NJointCCDMPControllerConfigPtr cfg;
VirtualRobot::RobotNodePtr tcp;
Eigen::Vector3f tcpPosition;
- Eigen::Vector3f tcpOrientation;
+ Eigen::Quaterniond tcpOrientation;
Eigen::Matrix4f oldPose;
+ VirtualRobot::DifferentialIKPtr ik;
DMP::DVec targetState;
mutable MutexType controllerMutex;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
index 3bd5872a9f85027aa46b7b042a61f5d4d732fdb5..fd33f782b3f3e90d99d10c639a8237d93b485928 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
@@ -19,7 +19,7 @@ namespace armarx
for (std::string jointName : cfg->jointNames)
{
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::VelocityTorque);
const SensorValueBase* sv = useSensorValue(jointName);
targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
@@ -53,6 +53,7 @@ namespace armarx
NJointJSDMPControllerSensorData initSensorData;
+ initSensorData.currentTime = 0;
initSensorData.deltaT = 0;
initSensorData.currentState.resize(cfg->jointNames.size());
controllerSensorData.reinitAllBuffers(initSensorData);
@@ -157,6 +158,8 @@ namespace armarx
controllerSensorData.getWriteBuffer().currentState[i] = currentPos;
}
controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
+ controllerSensorData.getWriteBuffer().currentTime += controllerSensorData.getWriteBuffer().deltaT;
+
controllerSensorData.commitWrite();
std::vector<double> targetJointVels = rtGetControlStruct().targetJointVels;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
index 3b904713a90d8190b0691b2ce5d0250a5262e6ed..df54abad00d0885d671e3c2296d9d6b40ac5db6a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
@@ -76,6 +76,7 @@ namespace armarx
struct NJointJSDMPControllerSensorData
{
+ double currentTime;
double deltaT;
DMP::Vec<DMP::DMPState> currentState;
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index 2f95ef4b734ac289e4c84b6f11c4b27f426170b9..1a62a87186637a75fb9269e2b159aaf3f6b681c8 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -20,7 +20,9 @@ namespace armarx
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
if (!torqueSensor)
{
ARMARX_WARNING << "No Torque sensor available for " << jointName;
@@ -32,6 +34,8 @@ namespace armarx
torqueSensors.push_back(torqueSensor);
gravityTorqueSensors.push_back(gravityTorqueSensor);
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
};
tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
@@ -42,42 +46,45 @@ namespace armarx
nodeSetName = cfg->nodeSetName;
torquePIDs.resize(tcpController->rns->getSize(), pidController());
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- // set DMP
- isDisturbance = false;
-
- dmpPtr.reset(new UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, cfg->tau));
- timeDuration = cfg->timeDuration;
- canVal = timeDuration;
finished = false;
-
- phaseL = cfg->phaseL;
- phaseK = cfg->phaseK;
- phaseDist0 = cfg->phaseDist0;
- phaseDist1 = cfg->phaseDist1;
- phaseKpPos = cfg->phaseKpPos;
- phaseKpOri = cfg->phaseKpOri;
-
- posToOriRatio = cfg->posToOriRatio;
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
+ taskSpaceDMPConfig.DMPStyle = cfg->dmpStyle;
+ taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
+ taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
+
+ taskSpaceDMPController.reset(new TaskSpaceDMPController("default", taskSpaceDMPConfig));
// initialize tcp position and orientation
-
NJointTSDMPControllerSensorData initSensorData;
initSensorData.deltaT = 0;
initSensorData.currentTime = 0;
- initSensorData.currentState.resize(6);
+ initSensorData.currentPose.setZero();
+ initSensorData.currentTwist.setZero();
controllerSensorData.reinitAllBuffers(initSensorData);
-
targetVels.resize(6);
NJointTSDMPControllerControlData initData;
initData.targetTSVel.resize(6);
for (size_t i = 0; i < 6; ++i)
{
- initData.targetTSVel[i] = 0;
+ initData.targetTSVel(i) = 0;
+ targetVels(i) = 0;
}
initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
initData.torqueKp.resize(tcpController->rns->getSize(), 0);
@@ -85,6 +92,7 @@ namespace armarx
initData.mode = ModeFromIce(cfg->mode);
reinitTripleBuffer(initData);
+
}
std::string NJointTSDMPController::getClassName(const Ice::Current&) const
@@ -94,126 +102,50 @@ namespace armarx
void NJointTSDMPController::controllerRun()
{
- if (!controllerSensorData.updateReadBuffer())
+ if (!controllerSensorData.updateReadBuffer() || !taskSpaceDMPController)
{
return;
}
double deltaT = controllerSensorData.getReadBuffer().deltaT;
- currentState = controllerSensorData.getReadBuffer().currentState;
-
- double mpcFactor = 1;
- double error = 0;
- double phaseStop = 0;
- double posError = 0;
- double oriError = 0;
-
-
- if (canVal > 1e-8)
- {
-
- std::vector<double> currentposi;
- std::vector<double> currentori;
- currentposi.resize(3);
- currentori.resize(3);
- for (size_t i = 0; i < 3; ++i)
- {
- currentposi[i] = currentState[i].pos;
- posError += pow(currentState[i].pos - targetState[i], 2);
- }
-
- for (size_t i = 0; i < 3; ++i)
- {
- currentori[i] = currentState[i + 3].pos;
- oriError += pow(currentState[i + 3].pos - targetState[i + 3], 2);
- }
-
- posError = sqrt(posError);
- oriError = sqrt(oriError);
-
- error = posError + posToOriRatio * oriError;
- double dmpDeltaT = deltaT / timeDuration;
-
- double phaseDist;
- if (isDisturbance)
- {
- phaseDist = phaseDist1;
- }
- else
- {
- phaseDist = phaseDist0;
- }
-
- phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
- mpcFactor = 1 - (phaseStop / phaseL);
-
- if (mpcFactor < 0.1)
- {
- isDisturbance = true;
- }
-
- if (mpcFactor > 0.9)
- {
- isDisturbance = false;
- }
-
-
- double tau = dmpPtr->getTemporalFactor();
- canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop) ;
- currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
-
-
-
- double vel0, vel1;
-
- for (size_t i = 0; i < 3; i++)
- {
- vel0 = currentState[i].vel / timeDuration;
- vel1 = phaseKpPos * (targetState[i] - currentposi[i]);
- targetVels[i] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- }
- for (size_t i = 0; i < 3; i++)
- {
- vel0 = currentState[i + 3].vel / timeDuration;
- vel1 = phaseKpOri * (targetState[i + 3] - currentori[i]);
- targetVels[i + 3] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- }
-
- }
- else
- {
- finished = true;
-
- for (size_t i = 0; i < targetVels.size(); ++i)
- {
- targetVels[i] = 0;
- }
- }
- // debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels[0];
- // debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels[1];
- // debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels[2];
- // debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels[3];
- // debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels[4];
- // debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels[5];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_roll"] = targetState[3];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_pitch"] = targetState[4];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_yaw"] = targetState[5];
-
-
- debugOutputData.getWriteBuffer().currentCanVal = canVal;
- // debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
- // debugOutputData.getWriteBuffer().error = error;
- // debugOutputData.getWriteBuffer().phaseStop = phaseStop;
- // debugOutputData.getWriteBuffer().posError = posError;
- // debugOutputData.getWriteBuffer().oriError = oriError;
- // debugOutputData.getWriteBuffer().deltaT = deltaT;
-
-
+ Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
+
+ taskSpaceDMPController->flow(deltaT, currentPose, currentTwist);
+ targetVels = taskSpaceDMPController->getTargetVelocity();
+
+
+ std::vector<double> targetState = taskSpaceDMPController->getTargetPose();
+
+
+ debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels[0];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels[1];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels[2];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels[3];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels[4];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels[5];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
+ debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
+
+ VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
+ debugOutputData.getWriteBuffer().currentCanVal = taskSpaceDMPController->debugData.canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = taskSpaceDMPController->debugData.mpcFactor;
+ debugOutputData.getWriteBuffer().error = taskSpaceDMPController->debugData.poseError;
+ debugOutputData.getWriteBuffer().posError = taskSpaceDMPController->debugData.posiError;
+ debugOutputData.getWriteBuffer().oriError = taskSpaceDMPController->debugData.oriError;
debugOutputData.commitWrite();
- // LockGuardType guard {controlDataMutex};
getWriterControlStruct().targetTSVel = targetVels;
writeControlStruct();
}
@@ -222,52 +154,40 @@ namespace armarx
void NJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
- Eigen::Vector3f currPosition;
- Eigen::Vector3f currOrientation = VirtualRobot::MathTools::eigen4f2rpy(pose);
- currPosition(0) = pose(0, 3);
- currPosition(1) = pose(1, 3);
- currPosition(2) = pose(2, 3);
-
double deltaT = timeSinceLastIteration.toSecondsDouble();
- Eigen::Vector3f posVel;
- Eigen::Vector3f oriVel;
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
- if (deltaT == 0)
- {
- posVel << 0, 0 , 0;
- oriVel << 0, 0 , 0;
- }
- else
+ Eigen::VectorXf qvel;
+ qvel.resize(velocitySensors.size());
+ for (size_t i = 0; i < velocitySensors.size(); ++i)
{
- posVel = (currPosition - tcpPosition) / deltaT;
- oriVel = (currOrientation - tcpOrientation) / deltaT;
+ qvel(i) = velocitySensors[i]->velocity;
}
- tcpPosition = currPosition;
- tcpOrientation = currOrientation;
+ Eigen::VectorXf tcptwist = jacobi * qvel;
+
+ controllerSensorData.getWriteBuffer().currentPose = pose;
+ controllerSensorData.getWriteBuffer().currentTwist = tcptwist;
+ controllerSensorData.getWriteBuffer().deltaT = deltaT;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ controllerSensorData.commitWrite();
+
+
+ Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
- for (size_t i = 0; i < 3; i++)
+ float normLinearVelocity = targetVel.block<3, 1>(0, 0).norm();
+ if (normLinearVelocity > cfg->maxLinearVel)
{
- DMP::DMPState currentPos;
- currentPos.pos = tcpPosition(i);
- currentPos.vel = posVel(i) * timeDuration;
- controllerSensorData.getWriteBuffer().currentState[i] = currentPos;
+ targetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * targetVel.block<3, 1>(0, 0) / normLinearVelocity;
}
- for (size_t i = 0; i < 3; i++)
+ float normAngularVelocity = targetVel.block<3, 1>(3, 0).norm();
+ if (normAngularVelocity > cfg->maxAngularVel)
{
- DMP::DMPState currentPos;
- currentPos.pos = tcpOrientation(i);
- currentPos.vel = oriVel(i) * timeDuration;
- controllerSensorData.getWriteBuffer().currentState[i + 3] = currentPos;
+ targetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * targetVel.block<3, 1>(3, 0) / normAngularVelocity;
}
- controllerSensorData.getWriteBuffer().deltaT = deltaT;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
-
- controllerSensorData.commitWrite();
-
// cartesian vel controller
Eigen::VectorXf x;
@@ -278,7 +198,7 @@ namespace armarx
x.resize(6);
for (size_t i = 0; i < 6; i++)
{
- x(i) = rtGetControlStruct().targetTSVel[i];
+ x(i) = targetVel(i);
}
}
else if (mode == VirtualRobot::IKSolver::Position)
@@ -286,7 +206,7 @@ namespace armarx
x.resize(3);
for (size_t i = 0; i < 3; i++)
{
- x(i) = rtGetControlStruct().targetTSVel[i];
+ x(i) = targetVel(i);
}
}
@@ -295,7 +215,7 @@ namespace armarx
x.resize(3);
for (size_t i = 0; i < 3; i++)
{
- x(i) = rtGetControlStruct().targetTSVel[i + 3];
+ x(i) = targetVel(i + 3);
}
}
else
@@ -335,44 +255,28 @@ namespace armarx
void NJointTSDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
{
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
- DMP::DVec ratios;
- for (size_t i = 0; i < fileNames.size(); ++i)
- {
- DMP::SampledTrajectoryV2 traj;
- traj.readFromCSVFile(fileNames.at(i));
- traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
- trajs.push_back(traj);
-
- if (i == 0)
- {
- ratios.push_back(1.0);
- }
- else
- {
- ratios.push_back(0.0);
- }
- }
-
- dmpPtr->learnFromTrajectories(trajs);
- dmpPtr->setOneStepMPC(true);
- dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
+ taskSpaceDMPController->learnDMPFromFiles(fileNames);
ARMARX_INFO << "Learned DMP ... ";
}
- void NJointTSDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ void NJointTSDMPController::setSpeed(Ice::Double times, const Ice::Current&)
{
- ViaPoint viaPoint(u, viapoint);
+ LockGuardType guard(controllerMutex);
+ taskSpaceDMPController->setSpeed(times);
+ }
+ void NJointTSDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ {
LockGuardType guard(controllerMutex);
- dmpPtr->setViaPoint(viaPoint);
+ ARMARX_INFO << "setting via points ";
+ taskSpaceDMPController->setViaPose(u, viapoint);
}
+
void NJointTSDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
{
- setViaPoints(dmpPtr->getUMin(), goals, ice);
+ taskSpaceDMPController->setGoalPoseVec(goals);
}
void NJointTSDMPController::setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&)
@@ -425,46 +329,14 @@ namespace armarx
void NJointTSDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
{
Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
- tcpPosition(0) = pose(0, 3);
- tcpPosition(1) = pose(1, 3);
- tcpPosition(2) = pose(2, 3);
- tcpOrientation = VirtualRobot::MathTools::eigen4f2rpy(pose);
-
- ARMARX_INFO << "tcpPosition: " << tcpPosition;
- ARMARX_INFO << "tcpOrientation: " << tcpOrientation;
-
- currentState.clear();
- targetState.clear();
- for (size_t i = 0; i < 3; i++)
- {
- DMP::DMPState currentPos;
- currentPos.pos = tcpPosition(i);
- currentPos.vel = 0;
- currentState.push_back(currentPos);
- targetState.push_back(currentPos.pos);
- }
- for (size_t i = 0; i < 3; i++)
- {
- DMP::DMPState currentPos;
- currentPos.pos = tcpOrientation(i);
- currentPos.vel = 0;
- currentState.push_back(currentPos);
- targetState.push_back(currentPos.pos);
- }
- dmpPtr->prepareExecution(goals, currentState, 1, tau);
+ taskSpaceDMPController->prepareExecution(taskSpaceDMPController->eigen4f2vec(pose), goals);
finished = false;
- dmpPtr->setTemporalFactor(tau);
ARMARX_INFO << "run DMP";
controllerTask->start();
}
- void NJointTSDMPController::setTemporalFactor(double tau, const Ice::Current&)
- {
- LockGuardType guard(controllerMutex);
- dmpPtr->setTemporalFactor(tau);
- }
void NJointTSDMPController::rtPreActivateController()
{
@@ -478,24 +350,31 @@ namespace armarx
void NJointTSDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
{
StringVariantBaseMap datafields;
- // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- // for (auto& pair : values)
- // {
- // datafields[pair.first] = new Variant(pair.second);
+ auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+ auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
+ for (auto& pair : dmpTargets)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
- // auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
- // for (auto& pair : dmpTargets)
- // {
- // datafields[pair.first] = new Variant(pair.second);
- // }
+ auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
+ for (auto& pair : currentPose)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- // datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- // datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
- // datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
- // datafields["posError"] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
- // datafields["oriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
- // datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+ datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
+ datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
+ datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
+ datafields["posError"] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
+ datafields["oriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
+ datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
debugObs->setDebugChannel("DMPController", datafields);
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index a072119a1b2fd8bf15ceb305f1e3397026474d10..a70f5a2962b929399e8a1a0f4e24d63a0a425687 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -7,10 +7,11 @@
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umidmp.h>
+#include <dmp/representation/dmp/umitsmp.h>
#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
using namespace DMP;
namespace armarx
@@ -25,7 +26,7 @@ namespace armarx
class NJointTSDMPControllerControlData
{
public:
- std::vector<float> targetTSVel;
+ Eigen::VectorXf targetTSVel;
// cartesian velocity control data
std::vector<float> nullspaceJointVelocities;
float avoidJointLimitsKp = 0;
@@ -71,13 +72,20 @@ namespace armarx
}
void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
- void setTemporalFactor(Ice::Double tau, const Ice::Current&);
+ void setSpeed(Ice::Double times, const Ice::Current&);
void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+
void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&);
void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&);
void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&);
+
+ double getCanVal(const Ice::Current &)
+ {
+ return taskSpaceDMPController->canVal;
+ }
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
@@ -91,15 +99,16 @@ namespace armarx
private:
struct DebugBufferData
{
- // StringFloatDictionary latestTargetVelocities;
- // StringFloatDictionary dmpTargets;
+ StringFloatDictionary latestTargetVelocities;
+ StringFloatDictionary dmpTargets;
+ StringFloatDictionary currentPose;
double currentCanVal;
- // double mpcFactor;
- // double error;
- // double phaseStop;
- // double posError;
- // double oriError;
- // double deltaT;
+ double mpcFactor;
+ double error;
+ double phaseStop;
+ double posError;
+ double oriError;
+ double deltaT;
};
TripleBuffer<DebugBufferData> debugOutputData;
@@ -108,7 +117,8 @@ namespace armarx
{
double currentTime;
double deltaT;
- DMP::Vec<DMP::DMPState> currentState;
+ Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
};
TripleBuffer<NJointTSDMPControllerSensorData> controllerSensorData;
@@ -116,6 +126,11 @@ namespace armarx
std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
std::vector<ControlTarget1DoFActuatorVelocity*> targets;
+ std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+
+
+ TaskSpaceDMPControllerPtr taskSpaceDMPController;
// velocity ik controller parameters
std::vector<pidController> torquePIDs;
@@ -123,35 +138,18 @@ namespace armarx
std::string nodeSetName;
// dmp parameters
- UMIDMPPtr dmpPtr;
- double timeDuration;
- double canVal;
bool finished;
- double tau;
- ViaPointsSet viaPoints;
- bool isDisturbance;
-
- // phaseStop parameters
- double phaseL;
- double phaseK;
- double phaseDist0;
- double phaseDist1;
- double phaseKpPos;
- double phaseKpOri;
-
- double posToOriRatio;
-
- std::vector<float> targetVels;
+ VirtualRobot::DifferentialIKPtr ik;
+ VirtualRobot::RobotNodePtr tcp;
+ Eigen::VectorXf targetVels;
NJointTaskSpaceDMPControllerConfigPtr cfg;
- VirtualRobot::RobotNodePtr tcp;
- Eigen::Vector3f tcpPosition;
- Eigen::Vector3f tcpOrientation;
- DMP::Vec<DMP::DMPState> currentState;
- DMP::DVec targetState;
mutable MutexType controllerMutex;
PeriodicTask<NJointTSDMPController>::pointer_type controllerTask;
+
+ // NJointTaskSpaceDMPControllerInterface interface
+
};
} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
index a210d04f27abcfafd3dcd73eef9a697ad77e02bb..0b3858033b0d6b34ae858d1379b34cdc7ccebef8 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
@@ -335,6 +335,7 @@ namespace armarx
writeControlStruct();
}
+
void NJointTaskSpaceDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
{
setViaPoints(dmpPtr->getUMin(), goals, ice);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
index edcb07cffe543ca68bd7505c501bbd705954ca85..e3bb4412951d30c4d2e22209cf67cdb5140863f0 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
@@ -76,6 +76,7 @@ namespace armarx
void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
void setTemporalFactor(Ice::Double tau, const Ice::Current&);
void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+
void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&);
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index d3d1a61fb731dbfa31377e0f741282488eaf3690..928e8451aee81823951866979f5d0a30321ca70b 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -31,7 +31,7 @@
using namespace armarx;
-PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless) :
+PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless, bool threadSafe) :
Kp(Kp),
Ki(Ki),
Kd(Kd),
@@ -44,23 +44,36 @@ PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValu
controlValueDerivation(0),
maxControlValue(maxControlValue),
maxDerivation(maxDerivation),
- limitless(limitless)
+ limitless(limitless),
+ threadSafe(threadSafe)
{
reset();
}
void PIDController::reset()
{
- ScopedRecursiveLock lock(mutex);
+ ScopedRecursiveLockPtr lock = getLock();
firstRun = true;
previousError = 0;
integral = 0;
lastUpdateTime = TimeUtil::GetTime();
}
+ScopedRecursiveLockPtr PIDController::getLock() const
+{
+ if (threadSafe)
+ {
+ return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
+ }
+ else
+ {
+ return ScopedRecursiveLockPtr();
+ }
+}
+
void PIDController::update(double measuredValue, double targetValue)
{
- ScopedRecursiveLock lock(mutex);
+ ScopedRecursiveLockPtr lock = getLock();
IceUtil::Time now = TimeUtil::GetTime();
if (firstRun)
@@ -76,25 +89,25 @@ void PIDController::update(double measuredValue, double targetValue)
void PIDController::update(double measuredValue)
{
- ScopedRecursiveLock lock(mutex);
+ ScopedRecursiveLockPtr lock = getLock();
update(measuredValue, target);
}
void PIDController::setTarget(double newTarget)
{
- ScopedRecursiveLock lock(mutex);
+ ScopedRecursiveLockPtr lock = getLock();
target = newTarget;
}
void PIDController::setMaxControlValue(double newMax)
{
- ScopedRecursiveLock lock(mutex);
+ ScopedRecursiveLockPtr lock = getLock();
maxControlValue = newMax;
}
void PIDController::update(double deltaSec, double measuredValue, double targetValue)
{
- ScopedRecursiveLock lock(mutex);
+ ScopedRecursiveLockPtr lock = getLock();
processValue = measuredValue;
target = targetValue;
@@ -144,7 +157,7 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
double PIDController::getControlValue() const
{
- ScopedRecursiveLock lock(mutex);
+ ScopedRecursiveLockPtr lock = getLock();
return controlValue;
}
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index 0d31868f92290b356c32344f264e6f7556117492..e6e9976b94e31241d086ee513f117d8b3aa81fed 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -39,7 +39,7 @@ namespace armarx
float Kd,
double maxControlValue = std::numeric_limits<double>::max(),
double maxDerivation = std::numeric_limits<double>::max(),
- bool limitless = false);
+ bool limitless = false, bool threadSafe = true);
void update(double deltaSec, double measuredValue, double targetValue);
void update(double measuredValue, double targetValue);
void update(double measuredValue);
@@ -62,6 +62,9 @@ namespace armarx
double maxDerivation;
bool firstRun;
bool limitless;
+ bool threadSafe = true;
+ private:
+ ScopedRecursiveLockPtr getLock() const;
mutable RecursiveMutex mutex;
};
typedef boost::shared_ptr<PIDController> PIDControllerPtr;
diff --git a/source/RobotAPI/libraries/core/math/ColorUtils.h b/source/RobotAPI/libraries/core/math/ColorUtils.h
index b28b8c8ebc1e535c5cf91f1393d92416e889c9c1..fb79987b2e9ba827e23cdcef58696b259628d984 100644
--- a/source/RobotAPI/libraries/core/math/ColorUtils.h
+++ b/source/RobotAPI/libraries/core/math/ColorUtils.h
@@ -30,103 +30,126 @@ namespace armarx
namespace colorutils
{
-
-
-
- DrawColor24Bit HsvToRgb(const HsvColor& hsv)
+ DrawColor24Bit HsvToRgb(const HsvColor& in)
{
- DrawColor24Bit rgb;
- unsigned char region, remainder, p, q, t;
-
- if (hsv.s == 0)
+ double hh, p, q, t, ff;
+ long i;
+ double r, g, b;
+ if (in.s <= 0.0) // < is bogus, just shuts up warnings
{
- rgb.r = hsv.v;
- rgb.g = hsv.v;
- rgb.b = hsv.v;
- return rgb;
+ r = in.v;
+ g = in.v;
+ b = in.v;
+ return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
}
- constexpr float by43 = 1.0f / 43.0f;
- region = hsv.h * by43;
- remainder = (hsv.h - (region * 43)) * 6;
-
- p = (hsv.v * (255 - hsv.s)) >> 8;
- q = (hsv.v * (255 - ((hsv.s * remainder) >> 8))) >> 8;
- t = (hsv.v * (255 - ((hsv.s * (255 - remainder)) >> 8))) >> 8;
-
- switch (region)
+ hh = in.h;
+ if (hh >= 360.0)
+ {
+ hh = 0.0;
+ }
+ hh /= 60.0;
+ i = (long)hh;
+ ff = hh - i;
+ p = in.v * (1.0 - in.s);
+ q = in.v * (1.0 - (in.s * ff));
+ t = in.v * (1.0 - (in.s * (1.0 - ff)));
+
+ switch (i)
{
case 0:
- rgb.r = hsv.v;
- rgb.g = t;
- rgb.b = p;
+ r = in.v;
+ g = t;
+ b = p;
break;
case 1:
- rgb.r = q;
- rgb.g = hsv.v;
- rgb.b = p;
+ r = q;
+ g = in.v;
+ b = p;
break;
case 2:
- rgb.r = p;
- rgb.g = hsv.v;
- rgb.b = t;
+ r = p;
+ g = in.v;
+ b = t;
break;
+
case 3:
- rgb.r = p;
- rgb.g = q;
- rgb.b = hsv.v;
+ r = p;
+ g = q;
+ b = in.v;
break;
case 4:
- rgb.r = t;
- rgb.g = p;
- rgb.b = hsv.v;
+ r = t;
+ g = p;
+ b = in.v;
break;
+ case 5:
default:
- rgb.r = hsv.v;
- rgb.g = p;
- rgb.b = q;
+ r = in.v;
+ g = p;
+ b = q;
break;
}
- return rgb;
+ return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
}
- HsvColor RgbToHsv(const DrawColor24Bit& rgb)
+ HsvColor RgbToHsv(const DrawColor24Bit& in)
{
- HsvColor hsv;
- unsigned char rgbMin, rgbMax;
+ double r = in.r * 0.00392156862;
+ double g = in.g * 0.00392156862;
+ double b = in.b * 0.00392156862;
+ HsvColor out;
+ double min, max, delta;
- rgbMin = rgb.r < rgb.g ? (rgb.r < rgb.b ? rgb.r : rgb.b) : (rgb.g < rgb.b ? rgb.g : rgb.b);
- rgbMax = rgb.r > rgb.g ? (rgb.r > rgb.b ? rgb.r : rgb.b) : (rgb.g > rgb.b ? rgb.g : rgb.b);
+ min = r < g ? r : g;
+ min = min < b ? min : b;
- hsv.v = rgbMax;
- if (hsv.v == 0)
+ max = r > g ? r : g;
+ max = max > b ? max : b;
+
+ out.v = max; // v
+ delta = max - min;
+ if (delta < 0.00001)
{
- hsv.h = 0;
- hsv.s = 0;
- return hsv;
+ out.s = 0;
+ out.h = 0; // undefined, maybe nan?
+ return out;
}
-
- hsv.s = 255 * long(rgbMax - rgbMin) / hsv.v;
- if (hsv.s == 0)
+ if (max > 0.0) // NOTE: if Max is == 0, this divide would cause a crash
{
- hsv.h = 0;
- return hsv;
+ out.s = (delta / max); // s
}
-
- if (rgbMax == rgb.r)
+ else
+ {
+ // if max is 0, then r = g = b = 0
+ // s = 0, h is undefined
+ out.s = 0.0;
+ out.h = 0; // its now undefined
+ return out;
+ }
+ if (r >= max) // > is bogus, just keeps compilor happy
{
- hsv.h = 0 + 43 * (rgb.g - rgb.b) / (rgbMax - rgbMin);
+ out.h = (g - b) / delta; // between yellow & magenta
}
- else if (rgbMax == rgb.g)
+ else if (g >= max)
{
- hsv.h = 85 + 43 * (rgb.b - rgb.r) / (rgbMax - rgbMin);
+ out.h = 2.0 + (b - r) / delta; // between cyan & yellow
}
else
{
- hsv.h = 171 + 43 * (rgb.r - rgb.g) / (rgbMax - rgbMin);
+ out.h = 4.0 + (r - g) / delta; // between magenta & cyan
}
- return hsv;
+ out.h *= 60.0; // degrees
+
+ if (out.h < 0.0)
+ {
+ out.h += 360.0;
+ }
+
+ return out;
+
+
}
/**
@@ -137,8 +160,8 @@ namespace armarx
HsvColor HeatMapColor(float percentage)
{
percentage = math::MathUtils::LimitMinMax(0.0f, 1.0f, percentage);
- constexpr float factor = 240.0 * (255.0 / 360.0);
- return HsvColor {(unsigned char)((1.0 - percentage) * factor), 255, 255};
+
+ return HsvColor {((1.0f - percentage) * 240.f), 1.0f, 1.0f};
}
DrawColor24Bit HeatMapRGBColor(float percentage)
diff --git a/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp b/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
index dd7592f4b8c5ec565a6f20abe25a1cb7c4e86947..73c9326968fc973e8e632c9de1cae7e1136a7035 100644
--- a/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
+++ b/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
@@ -25,6 +25,7 @@
#include <RobotAPI/Test.h>
#include <ArmarXCore/core/test/IceTestHelper.h>
#include "../math/MathUtils.h"
+#include "../math/ColorUtils.h"
using namespace armarx;
void check_close(float a, float b, float tolerance)
@@ -60,3 +61,16 @@ BOOST_AUTO_TEST_CASE(fmodTest)
}
}
+
+BOOST_AUTO_TEST_CASE(ColorUtilTest)
+{
+ auto testColor = [](DrawColor24Bit rgb)
+ {
+ auto hsv = colorutils::RgbToHsv(rgb);
+ ARMARX_INFO << VAROUT(rgb.r) << VAROUT(rgb.g) << VAROUT(rgb.b) << " --> " << VAROUT(hsv.h) << VAROUT(hsv.s) << VAROUT(hsv.v);
+ };
+ testColor({255, 0, 0});
+ testColor({0, 255, 0});
+ testColor({0, 0, 255});
+ testColor({0, 20, 0});
+}