diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 365d407e9a44080225803a2d572cd639cc3af2b5..3c303ee6d03d4d4905096a44fba89af59859b2ef 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -301,6 +301,7 @@ module armarx
interface NJointPeriodicTSDMPControllerInterface extends NJointControllerInterface
{
void learnDMPFromFiles(Ice::StringSeq trajfiles);
+
bool isFinished();
void runDMP(Ice::DoubleSeq goals, double tau);
@@ -353,12 +354,22 @@ module armarx
float forceDeadZone;
float velFilter;
+ float maxLinearVel;
+ float maxAngularVel;
+
+ Ice::FloatSeq ws_x;
+ Ice::FloatSeq ws_y;
+ Ice::FloatSeq ws_z;
+
+
};
interface NJointPeriodicTSDMPCompliantControllerInterface extends NJointControllerInterface
{
void learnDMPFromFiles(Ice::StringSeq trajfiles);
+ void learnDMPFromTrajectory(TrajectoryBase trajectory);
+
bool isFinished();
void runDMP(Ice::DoubleSeq goals, double tau);
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
index 0f59a08a9485080c6958835133eca0b911c0d3c5..e221ee7d483629731e404c4d1c9c5ef01383ed5d 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
@@ -22,6 +22,8 @@
#include "TaskSpaceDMPController.h"
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
using namespace armarx;
@@ -29,7 +31,11 @@ using namespace armarx;
void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist)
{
+ canVal = flow(canVal, deltaT, currentPose, twist);
+}
+double TaskSpaceDMPController::flow(double canVal, double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist)
+{
if (canVal < 0.1 && config.DMPStyle == "Periodic")
{
canVal = config.motionTimeDuration;
@@ -37,7 +43,7 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
if (canVal < 1e-8 && config.DMPStyle == "Discrete")
{
targetVel.setZero();
- return;
+ return canVal;
}
Eigen::Vector3f currentPosition;
@@ -197,6 +203,8 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
debugData.posiError = posiError;
debugData.mpcFactor = mpcFactor;
debugData.poseError = poseError;
+
+ return canVal;
}
void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& fileNames, const std::vector<double>& ratios)
@@ -214,7 +222,9 @@ void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& f
for (size_t i = 0; i < fileNames.size(); ++i)
{
DMP::SampledTrajectoryV2 traj;
- traj.readFromCSVFile(fileNames.at(i));
+ std::string absPath;
+ ArmarXDataPath::getAbsolutePath(fileNames.at(i), absPath);
+ traj.readFromCSVFile(absPath);
traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
trajs.push_back(traj);
@@ -257,6 +267,28 @@ void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& f
learnDMPFromFiles(fileNames, ratios);
}
+void TaskSpaceDMPController::learnDMPFromTrajectory(const TrajectoryPtr& traj)
+{
+ ARMARX_CHECK_EQUAL(traj->dim(), 7);
+ DMP::SampledTrajectoryV2 dmpTraj;
+
+ DMP::DVec timestamps(traj->getTimestamps());
+ for (size_t i = 0; i < traj->dim(); ++i)
+ {
+ DMP::DVec dimData(traj->getDimensionData(i, 0));
+ dmpTraj.addDimension(timestamps, dimData);
+ }
+
+ DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+
+ dmpTraj = DMP::SampledTrajectoryV2::normalizeTimestamps(dmpTraj, 0, 1);
+ trajs.push_back(dmpTraj);
+ DMP::DVec ratiosVec;
+ ratiosVec.push_back(1.0);
+ dmpPtr->learnFromTrajectories(trajs);
+ dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratiosVec);
+}
+
void TaskSpaceDMPController::setViaPose(double canVal, const Eigen::Matrix4f& viaPose)
{
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
index 177340da00d4609876b6d6d29ccab9aca192a27b..e314ecbf6589f0c51afebf19b57b7a6a4b8c62a4 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -30,7 +30,7 @@
#include <VirtualRobot/RobotNodeSet.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
+#include <RobotAPI/libraries/core/Trajectory.h>
namespace armarx
{
@@ -117,6 +117,7 @@ namespace armarx
void flow(double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist);
+ double flow(double canVal, double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist);
Eigen::VectorXf getTargetVelocity()
{
@@ -154,6 +155,8 @@ namespace armarx
void learnDMPFromFiles(const std::vector<std::string>& fileNames, const std::vector<double>& ratios);
void learnDMPFromFiles(const std::vector<std::string>& fileNames);
+ void learnDMPFromTrajectory(const TrajectoryPtr& traj);
+
void setViaPose(double canVal, const Eigen::Matrix4f& viaPose);
void setViaPose(double canVal, const std::vector<double>& viaPoseWithQuaternion);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index 1f8d3305c2c8853ae58dcd593b1366168c99c51c..d4ad2769f9d4773e722c6233ce0368f40accb1bf 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -100,17 +100,35 @@ namespace armarx
qvel_filtered.setZero(targets.size());
+ ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
+ ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
+ ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
+ // only for ARMAR-6 (safe-guard)
+ ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
+ ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
+ ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
+
+ ARMARX_CHECK_LESS(cfg->ws_y[0], cfg->ws_y[1]);
+ ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
+ ARMARX_CHECK_LESS(0, cfg->ws_y[1]);
+
+ ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
+ ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
+ ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
+
}
void NJointPeriodicTSDMPCompliantController::onInitNJointController()
{
ARMARX_INFO << "init ...";
+
RTToControllerData initSensorData;
initSensorData.deltaT = 0;
initSensorData.currentTime = 0;
initSensorData.currentPose = tcp->getPoseInRootFrame();
initSensorData.currentTwist.setZero();
+ initSensorData.isPhaseStop = false;
rt2CtrlData.reinitAllBuffers(initSensorData);
RTToUserData initInterfaceData;
@@ -162,36 +180,46 @@ namespace armarx
return;
}
- double deltaT = rt2CtrlData.getReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = rt2CtrlData.getReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = rt2CtrlData.getReadBuffer().currentTwist;
+ Eigen::VectorXf targetVels(6);
+ bool isPhaseStop = rt2CtrlData.getReadBuffer().isPhaseStop;
+ if (isPhaseStop)
+ {
+ targetVels.setZero();
+ }
+ else
+ {
- LockGuardType guard {controllerMutex};
- dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
- Eigen::VectorXf targetVels = dmpCtrl->getTargetVelocity();
-
- 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().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 = dmpCtrl->debugData.canVal;
- debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
- debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
- debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
- debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
- debugOutputData.getWriteBuffer().deltaT = deltaT;
- debugOutputData.commitWrite();
+ double deltaT = rt2CtrlData.getReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = rt2CtrlData.getReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = rt2CtrlData.getReadBuffer().currentTwist;
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->flow(deltaT, currentPose, currentTwist);
+
+ targetVels = dmpCtrl->getTargetVelocity();
+
+ 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().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 = dmpCtrl->debugData.canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
+ debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
+ debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
+ debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
+ debugOutputData.commitWrite();
+ }
getWriterControlStruct().targetTSVel = targetVels;
writeControlStruct();
@@ -223,13 +251,8 @@ namespace armarx
qvel_filtered = (1 - cfg->velFilter) * qvel_filtered + cfg->velFilter * qvel;
Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
- rt2CtrlData.getWriteBuffer().currentPose = currentPose;
- rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
- rt2CtrlData.getWriteBuffer().deltaT = deltaT;
- rt2CtrlData.getWriteBuffer().currentTime += deltaT;
- rt2CtrlData.commitWrite();
-
Eigen::VectorXf targetVel(6);
+ targetVel.setZero();
if (firstRun || !dmpRunning)
{
targetPose = currentPose;
@@ -250,7 +273,7 @@ namespace armarx
{
// communicate with DMP controller
rtUpdateControlStruct();
- Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
+ targetVel = rtGetControlStruct().targetTSVel;
targetVel(2) = 0;
// force detection
@@ -316,20 +339,50 @@ namespace armarx
// integrate for targetPose
- float dTf = (float)deltaT;
- targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
- // Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
- // targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
- debugRT.getWriteBuffer().filteredForce = filteredForceInRoot;
- debugRT.getWriteBuffer().targetVel = targetVel;
- debugRT.commitWrite();
+
+
+
}
+ bool isPhaseStop = false;
+
+
+
+ float diff = (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)).norm();
+ if (diff > cfg->phaseDist0)
+ {
+ isPhaseStop = true;
+ }
+ rt2CtrlData.getWriteBuffer().currentPose = currentPose;
+ rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
+ rt2CtrlData.getWriteBuffer().deltaT = deltaT;
+ rt2CtrlData.getWriteBuffer().currentTime += deltaT;
+ rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
+ rt2CtrlData.commitWrite();
+
+
+ float dTf = (float)deltaT;
+ targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
+ targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
+ targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
+
+ targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
+ targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
+
+ targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
+ targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
+
+ debugRT.getWriteBuffer().targetPose = targetPose;
+ debugRT.commitWrite();
+
+ // Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
+ // targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
+
// inverse dynamic controller
jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
@@ -341,7 +394,21 @@ namespace armarx
currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(- currentTCPLinearVelocity);
+
+ Eigen::Vector3f linearVel ;
+ if (isPhaseStop)
+ {
+ linearVel = ((float)cfg->phaseKpPos) * (desiredPosition - currentTCPPosition);
+ for (size_t i = 0; i < 3; ++i)
+ {
+ linearVel(i) = fmin(cfg->maxLinearVel, linearVel(i));
+ }
+ }
+ else
+ {
+ linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
+ }
+ Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(- currentTCPLinearVelocity);
Eigen::Vector3f currentTCPAngularVelocity;
currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
@@ -390,6 +457,18 @@ namespace armarx
}
+ void NJointPeriodicTSDMPCompliantController::learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&)
+ {
+ ARMARX_INFO << "Learning DMP ... ";
+ ARMARX_CHECK_EXPRESSION(trajectory);
+ TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
+ ARMARX_CHECK_EXPRESSION(dmpTraj);
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->learnDMPFromTrajectory(dmpTraj);
+
+ }
+
void NJointPeriodicTSDMPCompliantController::setSpeed(Ice::Double times, const Ice::Current&)
{
LockGuardType guard {controllerMutex};
@@ -443,18 +522,23 @@ namespace armarx
std::string debugName = "Periodic";
StringVariantBaseMap datafields;
- Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
- datafields["filtered_force_x"] = new Variant(filteredForce(0));
- datafields["filtered_force_y"] = new Variant(filteredForce(1));
- datafields["filtered_force_z"] = new Variant(filteredForce(2));
-
- Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
- datafields["targetVel_x"] = new Variant(targetVel(0));
- datafields["targetVel_y"] = new Variant(targetVel(1));
- datafields["targetVel_z"] = new Variant(targetVel(2));
- datafields["targetVel_rx"] = new Variant(targetVel(3));
- datafields["targetVel_ry"] = new Variant(targetVel(4));
- datafields["targetVel_rz"] = new Variant(targetVel(5));
+ Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
+ datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
+ datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
+ datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
+
+ // Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
+ // datafields["filtered_force_x"] = new Variant(filteredForce(0));
+ // datafields["filtered_force_y"] = new Variant(filteredForce(1));
+ // datafields["filtered_force_z"] = new Variant(filteredForce(2));
+
+ // Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
+ // datafields["targetVel_x"] = new Variant(targetVel(0));
+ // datafields["targetVel_y"] = new Variant(targetVel(1));
+ // datafields["targetVel_z"] = new Variant(targetVel(2));
+ // datafields["targetVel_rx"] = new Variant(targetVel(3));
+ // datafields["targetVel_ry"] = new Variant(targetVel(4));
+ // datafields["targetVel_rz"] = new Variant(targetVel(5));
// auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
// for (auto& pair : values)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
index 0696c8474f83fe6620f8d97f91fba923718b2559..634d20dd2b3ff0fb24bf58e07d83fda8c6567d25 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -14,6 +14,7 @@
#include <ArmarXCore/core/time/CycleUtil.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <RobotAPI/libraries/core/Trajectory.h>
namespace armarx
{
@@ -63,6 +64,7 @@ namespace armarx
// NJointPeriodicTSDMPCompliantControllerInterface interface
void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&);
bool isFinished(const Ice::Current&)
{
return false;
@@ -101,10 +103,10 @@ namespace armarx
TripleBuffer<DebugBufferData> debugOutputData;
+
struct DebugRTData
{
- Eigen::Vector3f filteredForce;
- Eigen::VectorXf targetVel;
+ Eigen::Matrix4f targetPose;
};
TripleBuffer<DebugRTData> debugRT;
@@ -114,6 +116,7 @@ namespace armarx
double deltaT;
Eigen::Matrix4f currentPose;
Eigen::VectorXf currentTwist;
+ bool isPhaseStop;
};
TripleBuffer<RTToControllerData> rt2CtrlData;