diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 2eb2753c7697a43743707e03c1f0adc9b81c238b..3f289eb02017607e2b57917260367b43f143c613 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,55 @@ 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;
+ double knull = 5;
+
+ float KoriFollower = 1;
+ float KposFollower = 1;
+
+ double maxLinearVel;
+ double maxAngularVel;
+
+
+ };
+
+ 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/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 1d2c433b93afb51f8d5b3aba5788440fb6c7f0a9..4f13d687f985f1160efe1b383b1857c17533d134 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -23,6 +23,7 @@ set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreE
Saba
SimDynamics
RobotUnit
+
)
set(LIB_FILES
@@ -40,22 +41,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..2d00b4b9538bcb94c898ba813d8257d5ee6b6801
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
@@ -0,0 +1,556 @@
+#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);
+
+ VirtualRobot::RobotNodeSetPtr leftRNS = robotUnit->getRtRobot()->getRobotNodeSet("LeftArm");
+ for (size_t i = 0; i < leftRNS->getSize(); ++i)
+ {
+ std::string jointName = leftRNS->getNode(i)->getName();
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+
+ leftVelocitySensors.push_back(velocitySensor);
+ leftPositionSensors.push_back(positionSensor);
+
+
+ };
+
+ VirtualRobot::RobotNodeSetPtr rightRNS = robotUnit->getRtRobot()->getRobotNodeSet("RightArm");
+ for (size_t i = 0; i < rightRNS->getSize(); ++i)
+ {
+ std::string jointName = rightRNS->getNode(i)->getName();
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+
+ rightVelocitySensors.push_back(velocitySensor);
+ rightPositionSensors.push_back(positionSensor);
+ };
+
+
+ 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;
+ }
+
+ 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);
+
+
+
+ if (leaderName == "Left")
+ {
+ leftTargetVel = leaderTargetVel;
+ rightTargetVel = followerTargetVel;
+
+ leftDMPTarget = leaderDMP->getTargetPose();
+ rightDMPTarget = followerTargetPoseVec;
+
+ }
+ else
+ {
+ rightTargetVel = leaderTargetVel;
+ leftTargetVel = followerTargetVel;
+
+ rightDMPTarget = leaderDMP->getTargetPose();
+ leftDMPTarget = followerTargetPoseVec;
+
+ }
+
+
+
+
+ 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;
+
+ writeControlStruct();
+ }
+
+
+
+ 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);
+ 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);
+ 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;
+ }
+
+ controllerSensorData.getWriteBuffer().currentLeftTwist = jacobiL * leftqvel;
+ controllerSensorData.getWriteBuffer().currentRightTwist = jacobiR * rightqvel;
+ controllerSensorData.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 = 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..39a1a6e849f546f7561ce42738a6135d3cc69f1e
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
@@ -0,0 +1,168 @@
+
+#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>
+
+
+
+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;
+ };
+
+ 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::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 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> debugOutputData;
+
+ struct NJointBimanualCCDMPControllerSensorData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentLeftPose;
+ Eigen::Matrix4f currentRightPose;
+ Eigen::VectorXf currentLeftTwist;
+ Eigen::VectorXf currentRightTwist;
+ };
+ TripleBuffer<NJointBimanualCCDMPControllerSensorData> controllerSensorData;
+
+
+ std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
+ std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
+
+ std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
+ std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
+
+ 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;
+
+
+ // NJointBimanualCCDMPControllerInterface interface
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
index c00253ed7a2524d5e6d8a591754f458ef2649168..7ec63cc2a13d7b3e23160096c9e7f700fc9c5c2e 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 = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::VelocityTorque);
const SensorValueBase* sv = prov->getSensorValue(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,8 +180,11 @@ namespace armarx
// run DMP one after another
Eigen::Matrix4f targetPose = Eigen::Matrix4f::Identity();
- bool finished = true;
- for (size_t i = 0; i < cfg->dmpNum; ++i)
+
+ Eigen::VectorXf dmpVels;
+ dmpVels.resize(6);
+ dmpVels.setZero();
+ for (int i = 0; i < cfg->dmpNum; ++i)
{
double timeDuration = timeDurations[i];
std::string dmpType = dmpTypes[i];
@@ -186,7 +201,7 @@ namespace armarx
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]);
+
+ 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 7dc42afd1e8c99b9a5d269727cec270753abfc5c..e318e8f68dcd518711614dd759cdf6cf575d42c0 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 4a7652fe06db8b61ea3cb2a1d86d2857313f1407..b91195290ce04ac6f12ac1ce0abcfcd032009ad5 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 = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::VelocityTorque);
const SensorValueBase* sv = prov->getSensorValue(jointName);
targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index f14bf42830f9d87f2b58b7591962e2047ed47bc2..5de57d1714c208a1498a3fd7f8167c583a0b2246 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -51,40 +51,42 @@ namespace armarx
ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- // set DMP
- isDisturbance = false;
-
- dmpPtr.reset(new DMP::UMITSMP(cfg->kernelSize, 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(7);
+ 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);
@@ -92,6 +94,7 @@ namespace armarx
initData.mode = ModeFromIce(cfg->mode);
reinitTripleBuffer(initData);
+
}
std::string NJointTSDMPController::getClassName(const Ice::Current&) const
@@ -101,117 +104,21 @@ 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 angularError = 0;
-
-
- if (canVal > 1e-8)
- {
- std::vector<double> currentposi;
- currentposi.resize(3);
- for (size_t i = 0; i < 3; ++i)
- {
- currentposi[i] = currentState[i].pos;
- posError += pow(currentState[i].pos - targetState[i], 2);
- }
- posError = sqrt(posError);
-
- // calculate quaternion error
- Eigen::Quaterniond currentQ;
- Eigen::Quaterniond targetQ;
- currentQ.w() = currentState[3].pos;
- currentQ.x() = currentState[4].pos;
- currentQ.y() = currentState[5].pos;
- currentQ.z() = currentState[6].pos;
- targetQ.w() = targetState[3];
- targetQ.x() = targetState[4];
- targetQ.y() = targetState[5];
- targetQ.z() = targetState[6];
- angularError = targetQ.angularDistance(currentQ);
- if (angularError > M_PI)
- {
- angularError = 2 * M_PI - angularError;
- }
-
- error = posError + posToOriRatio * angularError;
- double dmpDeltaT = deltaT / timeDuration;
+ Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
- double phaseDist;
- if (isDisturbance)
- {
- phaseDist = phaseDist1;
- }
- else
- {
- phaseDist = phaseDist0;
- }
+ taskSpaceDMPController->flow(deltaT, currentPose, currentTwist);
+ targetVels = taskSpaceDMPController->getTargetVelocity();
- phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
- mpcFactor = 1 - (phaseStop / phaseL);
- if (mpcFactor < 0.1)
- {
- isDisturbance = true;
- }
+ std::vector<double> targetState = taskSpaceDMPController->getTargetPose();
- 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);
-
-
-
- float 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;
- }
-
- Eigen::Quaterniond quatVel0;
- quatVel0.w() = currentState[3].vel;
- quatVel0.x() = currentState[4].vel;
- quatVel0.y() = currentState[5].vel;
- quatVel0.z() = currentState[6].vel;
- Eigen::Quaterniond angularVel0 = 2 * quatVel0 * currentQ.inverse();
-
- Eigen::Quaterniond diffQ = targetQ * currentQ.inverse();
- Eigen::Quaterniond quatVel1 = phaseKpOri * diffQ;
- Eigen::Quaterniond angularVel1 = 2 * quatVel1 * currentQ.inverse();
-
- ARMARX_INFO << "angular vel: " << angularVel0.x() << " " << angularVel0.y() << " " << angularVel0.z();
-
- targetVels[3] = mpcFactor * angularVel0.x() / timeDuration + (1 - mpcFactor) * angularVel1.x();
- targetVels[4] = mpcFactor * angularVel0.y() / timeDuration + (1 - mpcFactor) * angularVel1.y();
- targetVels[5] = mpcFactor * angularVel0.z() / timeDuration + (1 - mpcFactor) * angularVel1.z();
-
- }
- 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];
@@ -225,28 +132,22 @@ namespace armarx
debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
-
-
- // ARMARX_INFO << "targetVels: " << targetVels[0] << " " <<
- // targetVels[1] << " " <<
- // targetVels[2] << " " <<
- // targetVels[3] << " " <<
- // targetVels[4] << " " <<
- // targetVels[5];
-
-
- debugOutputData.getWriteBuffer().currentCanVal = canVal;
- debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
- debugOutputData.getWriteBuffer().error = error;
- debugOutputData.getWriteBuffer().phaseStop = phaseStop;
- debugOutputData.getWriteBuffer().posError = posError;
- debugOutputData.getWriteBuffer().oriError = angularError;
- debugOutputData.getWriteBuffer().deltaT = deltaT;
-
-
+ 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();
}
@@ -255,59 +156,39 @@ namespace armarx
void NJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
- Eigen::Vector3f currPosition;
- currPosition << pose(0, 3), pose(1, 3), pose(2, 3);
- VirtualRobot::MathTools::Quaternion currOrientation = VirtualRobot::MathTools::eigen4f2quat(pose);
- tcpPosition = currPosition;
- tcpOrientation.w() = currOrientation.w;
- tcpOrientation.x() = currOrientation.x;
- tcpOrientation.y() = currOrientation.y;
- tcpOrientation.z() = currOrientation.z;
-
-
double deltaT = timeSinceLastIteration.toSecondsDouble();
Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::VectorXf qpos;
Eigen::VectorXf qvel;
- qpos.resize(positionSensors.size());
qvel.resize(velocitySensors.size());
for (size_t i = 0; i < velocitySensors.size(); ++i)
{
- qpos(i) = positionSensors[i]->position;
qvel(i) = velocitySensors[i]->velocity;
}
Eigen::VectorXf tcptwist = jacobi * qvel;
+ controllerSensorData.getWriteBuffer().currentPose = pose;
+ controllerSensorData.getWriteBuffer().currentTwist = tcptwist;
+ controllerSensorData.getWriteBuffer().deltaT = deltaT;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ controllerSensorData.commitWrite();
+
- for (size_t i = 0; i < 3; i++)
+ Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
+
+ float normLinearVelocity = targetVel.block<3, 1>(0, 0).norm();
+ if (normLinearVelocity > cfg->maxLinearVel)
{
- DMP::DMPState currentPos;
- currentPos.pos = tcpPosition(i);
- currentPos.vel = tcptwist(i) * timeDuration;
- controllerSensorData.getWriteBuffer().currentState[i] = currentPos;
+ targetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * targetVel.block<3, 1>(0, 0) / normLinearVelocity;
}
- Eigen::Quaterniond angularVel;
- angularVel.w() = 0;
- angularVel.x() = tcptwist(3);
- angularVel.y() = tcptwist(4);
- angularVel.z() = tcptwist(5);
-
- Eigen::Quaterniond quatVel = 0.5 * angularVel * tcpOrientation;
- controllerSensorData.getWriteBuffer().currentState[3].pos = tcpOrientation.w();
- controllerSensorData.getWriteBuffer().currentState[3].vel = quatVel.w();
- controllerSensorData.getWriteBuffer().currentState[4].pos = tcpOrientation.x();
- controllerSensorData.getWriteBuffer().currentState[4].vel = quatVel.x();
- controllerSensorData.getWriteBuffer().currentState[5].pos = tcpOrientation.y();
- controllerSensorData.getWriteBuffer().currentState[5].vel = quatVel.y();
- controllerSensorData.getWriteBuffer().currentState[6].pos = tcpOrientation.z();
- controllerSensorData.getWriteBuffer().currentState[6].vel = quatVel.z();
- controllerSensorData.getWriteBuffer().deltaT = deltaT;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
- controllerSensorData.commitWrite();
+ float normAngularVelocity = targetVel.block<3, 1>(3, 0).norm();
+ if (normAngularVelocity > cfg->maxAngularVel)
+ {
+ targetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * targetVel.block<3, 1>(3, 0) / normAngularVelocity;
+ }
// cartesian vel controller
@@ -319,7 +200,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)
@@ -327,7 +208,7 @@ namespace armarx
x.resize(3);
for (size_t i = 0; i < 3; i++)
{
- x(i) = rtGetControlStruct().targetTSVel[i];
+ x(i) = targetVel(i);
}
}
@@ -336,7 +217,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
@@ -376,43 +257,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->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&)
@@ -465,66 +331,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);
- VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(pose);
-
- tcpOrientation.w() = currentQ.w;
- tcpOrientation.x() = currentQ.x;
- tcpOrientation.y() = currentQ.y;
- tcpOrientation.z() = currentQ.z;
-
- ARMARX_INFO << "tcpPosition: " << tcpPosition;
- ARMARX_INFO << "tcpOrientation: " << tcpOrientation.w() << " " << tcpOrientation.x() << " " << tcpOrientation.y() << " " << tcpOrientation.z();
-
- 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);
- }
-
- DMP::DMPState currentPos;
- currentPos.pos = tcpOrientation.w();
- currentPos.vel = 0;
- currentState.push_back(currentPos);
- targetState.push_back(currentPos.pos);
-
- currentPos.pos = tcpOrientation.x();
- currentPos.vel = 0;
- currentState.push_back(currentPos);
- targetState.push_back(currentPos.pos);
-
- currentPos.pos = tcpOrientation.y();
- currentPos.vel = 0;
- currentState.push_back(currentPos);
- targetState.push_back(currentPos.pos);
-
- currentPos.pos = tcpOrientation.z();
- 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()
{
@@ -550,6 +364,12 @@ namespace armarx
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);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index 4479ca64e4c3afa837d703c6fa61c2221f9b6fc6..dbbdb6698058364d79ec25bbc4739ca296dabd6e 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -11,6 +11,7 @@
#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;
@@ -93,6 +101,7 @@ namespace armarx
{
StringFloatDictionary latestTargetVelocities;
StringFloatDictionary dmpTargets;
+ StringFloatDictionary currentPose;
double currentCanVal;
double mpcFactor;
double error;
@@ -108,7 +117,8 @@ namespace armarx
{
double currentTime;
double deltaT;
- DMP::Vec<DMP::DMPState> currentState;
+ Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
};
TripleBuffer<NJointTSDMPControllerSensorData> controllerSensorData;
@@ -119,44 +129,27 @@ namespace armarx
std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+
+ TaskSpaceDMPControllerPtr taskSpaceDMPController;
+
// velocity ik controller parameters
std::vector<pidController> torquePIDs;
CartesianVelocityControllerPtr tcpController;
std::string nodeSetName;
// dmp parameters
- DMP::UMITSMPPtr dmpPtr;
- double timeDuration;
- double canVal;
bool finished;
- double tau;
- ViaPointsSet viaPoints;
- bool isDisturbance;
-
VirtualRobot::DifferentialIKPtr ik;
VirtualRobot::RobotNodePtr tcp;
-
- // phaseStop parameters
- double phaseL;
- double phaseK;
- double phaseDist0;
- double phaseDist1;
- double phaseKpPos;
- double phaseKpOri;
-
- double posToOriRatio;
-
- bool startRun;
- std::vector<float> targetVels;
+ Eigen::VectorXf targetVels;
NJointTaskSpaceDMPControllerConfigPtr cfg;
- Eigen::Vector3f tcpPosition;
- Eigen::Quaterniond 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 f7a2de6fee95c7a188cd816158d88836c974e9c2..aba323812c91a3ae20b88e267db5c2ab0bd371e0 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
@@ -337,6 +337,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 b5f3fa66e39c20002747e7e02372e7f45c5aed86..796044be71f4be83d31dbc18f121a4b6babc77ae 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&);