NJointBimanualCCDMPVelocityController.cpp
Go to the documentation of this file.
1 #include "NJointBimanualCCDMPVelocityController.h"
2 
3 #include <ArmarXCore/core/time/CycleUtil.h>
4 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
5 #include <ArmarXCore/core/system/ArmarXDataPath.h>
6 
7 namespace armarx::control::deprecated_njoint_mp_controller::bimanual
8 {
9  NJointControllerRegistration<NJointBimanualCCDMPVelocityController> registrationControllerNJointBimanualCCDMPVelocityController("NJointBimanualCCDMPVelocityController");
10 
11  NJointBimanualCCDMPVelocityController::NJointBimanualCCDMPVelocityController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
12  {
13  ARMARX_INFO << "Preparing ... ";
14  cfg = NJointBimanualCCDMPVelocityControllerConfigPtr::dynamicCast(config);
15 
16  ARMARX_CHECK_EXPRESSION(robotUnit);
17  useSynchronizedRtRobot();
18 
19  leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
20  leftNullSpaceCoefs.resize(leftRNS->getSize());
21  leftNullSpaceCoefs.setOnes();
22 
23  for (size_t i = 0; i < leftRNS->getSize(); ++i)
24  {
25  std::string jointName = leftRNS->getNode(i)->getName();
26 
27  if (leftRNS->getNode(i)->isLimitless())
28  {
29  leftNullSpaceCoefs(i) = 1;
30  }
31 
32  leftJointNames.push_back(jointName);
33  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
34  const SensorValueBase* sv = useSensorValue(jointName);
35  leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
36  const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
37  const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
38  const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
39 
40  if (!velocitySensor)
41  {
42  ARMARX_WARNING << "No velocitySensor available for " << jointName;
43  }
44  if (!positionSensor)
45  {
46  ARMARX_WARNING << "No positionSensor available for " << jointName;
47  }
48 
49  if (!accelerationSensor)
50  {
51  ARMARX_WARNING << "No accelerationSensor available for " << jointName;
52  }
53 
54 
55  leftVelocitySensors.push_back(velocitySensor);
56  leftPositionSensors.push_back(positionSensor);
57  leftAccelerationSensors.push_back(accelerationSensor);
58 
59  };
60  rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
61 
62  rightNullSpaceCoefs.resize(rightRNS->getSize());
63  rightNullSpaceCoefs.setOnes();
64  for (size_t i = 0; i < rightRNS->getSize(); ++i)
65  {
66  std::string jointName = rightRNS->getNode(i)->getName();
67 
68  if (rightRNS->getNode(i)->isLimitless())
69  {
70  rightNullSpaceCoefs(i) = 1;
71  }
72 
73  rightJointNames.push_back(jointName);
74  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
75  const SensorValueBase* sv = useSensorValue(jointName);
76  rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
77  const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
78  const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
79  const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
80 
81 
82  if (!velocitySensor)
83  {
84  ARMARX_WARNING << "No velocitySensor available for " << jointName;
85  }
86  if (!positionSensor)
87  {
88  ARMARX_WARNING << "No positionSensor available for " << jointName;
89  }
90  if (!accelerationSensor)
91  {
92  ARMARX_WARNING << "No accelerationSensor available for " << jointName;
93  }
94 
95  rightVelocitySensors.push_back(velocitySensor);
96  rightPositionSensors.push_back(positionSensor);
97  rightAccelerationSensors.push_back(accelerationSensor);
98 
99  };
100 
101 
102  const SensorValueBase* svlf = useSensorValue("FT L");
103  leftForceTorque = svlf->asA<SensorValueForceTorque>();
104  const SensorValueBase* svrf = useSensorValue("FT R");
105  rightForceTorque = svrf->asA<SensorValueForceTorque>();
106 
107  leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
108  rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
109 
110 
111  leftCtrl.reset(new CartesianVelocityController(leftRNS, leftRNS->getTCP()));
112  rightCtrl.reset(new CartesianVelocityController(rightRNS, rightRNS->getTCP()));
113 
114  tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
115  taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
116  taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
117  taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
118  taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
119  taskSpaceDMPConfig.DMPAmplitude = 1.0;
120 
121 
122  tsvmp::TaskSpaceDMPControllerPtr leftLeader(new tsvmp::TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig, false));
123  tsvmp::TaskSpaceDMPControllerPtr leftFollower(new tsvmp::TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig, false));
124  tsvmp::TaskSpaceDMPControllerPtr rightLeader(new tsvmp::TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig, false));
125  tsvmp::TaskSpaceDMPControllerPtr rightFollower(new tsvmp::TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig, false));
126  leftJointDMP.reset(new DMP::UMIDMP(10));
127  rightJointDMP.reset(new DMP::UMIDMP(10));
128  isLeftJointLearned = false;
129  isRightJointLearned = false;
130 
131  started = false;
132  isDMPRun = false;
133 
134  leftGroup.push_back(leftLeader);
135  leftGroup.push_back(rightFollower);
136 
137  rightGroup.push_back(rightLeader);
138  rightGroup.push_back(leftFollower);
139 
140  bothLeaderGroup.push_back(leftLeader);
141  bothLeaderGroup.push_back(rightLeader);
142 
143 
144  tcpLeft = leftRNS->getTCP();
145  tcpRight = rightRNS->getTCP();
146 
147 
148  leaderName = cfg->defautLeader;
149 
150 
151  leftDesiredJointValues.resize(leftTargets.size());
152  ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
153 
154  for (size_t i = 0; i < leftTargets.size(); ++i)
155  {
156  leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
157  }
158 
159  rightDesiredJointValues.resize(rightTargets.size());
160  ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
161 
162  for (size_t i = 0; i < rightTargets.size(); ++i)
163  {
164  rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
165  }
166 
167 
168  leftKpos << cfg->leftKpos[0], cfg->leftKpos[1], cfg->leftKpos[2];
169  leftDpos << cfg->leftDpos[0], cfg->leftDpos[1], cfg->leftDpos[2];
170  leftKori << cfg->leftKori[0], cfg->leftKori[1], cfg->leftKori[2];
171  leftDori << cfg->leftDori[0], cfg->leftDori[1], cfg->leftDori[2];
172 
173  rightKpos << cfg->rightKpos[0], cfg->rightKpos[1], cfg->rightKpos[2];
174  rightDpos << cfg->rightDpos[0], cfg->rightDpos[1], cfg->rightDpos[2];
175  rightKori << cfg->rightKori[0], cfg->rightKori[1], cfg->rightKori[2];
176  rightDori << cfg->rightDori[0], cfg->rightDori[1], cfg->rightDori[2];
177 
178  knull = cfg->knull;
179  dnull = cfg->dnull;
180 
181 
182  timeDuration = cfg->timeDuration;
183 
184  maxLinearVel = cfg->maxLinearVel;
185  maxAngularVel = cfg->maxAngularVel;
186 
187  NJointBimanualCCDMPVelocityControllerInterfaceData initInterfaceData;
188  initInterfaceData.currentLeftPose = Eigen::Matrix4f::Identity();
189  initInterfaceData.currentRightPose = Eigen::Matrix4f::Identity();
190  initInterfaceData.currentLeftJointVals.setZero(leftTargets.size());
191  initInterfaceData.currentRightJointVals.setZero(rightTargets.size());
192  interfaceData.reinitAllBuffers(initInterfaceData);
193 
194  NJointBimanualCCDMPVelocityControllerSensorData initSensorData;
195  initSensorData.deltaT = 0;
196  initSensorData.currentTime = 0;
197  initSensorData.currentLeftPose = Eigen::Matrix4f::Identity();
198  initSensorData.currentRightPose = Eigen::Matrix4f::Identity();
199  controllerSensorData.reinitAllBuffers(initSensorData);
200  }
201 
202  std::string NJointBimanualCCDMPVelocityController::getClassName(const Ice::Current&) const
203  {
204  return "NJointBimanualCCDMPVelocityController";
205  }
206 
207  void NJointBimanualCCDMPVelocityController::rtPreActivateController()
208  {
209  NJointBimanualCCDMPVelocityControllerControlData initData;
210  initData.leftTargetVel.resize(6);
211  initData.leftTargetVel.setZero();
212  initData.rightTargetVel.resize(6);
213  initData.rightTargetVel.setZero();
214  initData.leftTargetPose = tcpLeft->getPoseInRootFrame();
215  initData.rightTargetPose = tcpRight->getPoseInRootFrame();
216  initData.virtualTime = cfg->timeDuration;
217  reinitTripleBuffer(initData);
218  }
219 
220 
221  void NJointBimanualCCDMPVelocityController::controllerRun()
222  {
223  if (!started)
224  {
225  return;
226  }
227 
228  if (!controllerSensorData.updateReadBuffer())
229  {
230  return;
231  }
232  double deltaT = controllerSensorData.getReadBuffer().deltaT;
233 
234 
235  Eigen::VectorXf leftTargetVel;
236  leftTargetVel.resize(6);
237  leftTargetVel.setZero();
238  Eigen::VectorXf rightTargetVel;
239  rightTargetVel.resize(6);
240  rightTargetVel.setZero();
241  Eigen::Matrix4f leftTargetPose;
242  Eigen::Matrix4f rightTargetPose;
243 
244  std::vector<tsvmp::TaskSpaceDMPControllerPtr > currentControlGroup;
245  Eigen::Matrix4f currentLeaderPose;
246  Eigen::Matrix4f currentFollowerPose;
247  Eigen::VectorXf currentLeaderTwist;
248  Eigen::VectorXf currentFollowerTwist;
249 
250 
251 
252  // get desired joint values
253  if (leaderName == "Both")
254  {
255  currentControlGroup = bothLeaderGroup;
256 
257  tsvmp::TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
258  tsvmp::TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
259  leaderDMPleft->flow(deltaT, controllerSensorData.getReadBuffer().currentLeftPose, controllerSensorData.getReadBuffer().currentLeftTwist);
260  leaderDMPright->flow(deltaT, controllerSensorData.getReadBuffer().currentRightPose, controllerSensorData.getReadBuffer().currentRightTwist);
261 
262  leftTargetVel = leaderDMPleft->getTargetVelocity();
263  leftTargetPose = leaderDMPleft->getTargetPoseMat();
264  rightTargetVel = leaderDMPright->getTargetVelocity();
265  rightTargetPose = leaderDMPright->getTargetPoseMat();
266 
267  virtualtimer = leaderDMPleft->canVal;
268  }
269  else
270  {
271  if (leaderName == "Left")
272  {
273  currentControlGroup = leftGroup;
274  currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
275  currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
276  currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
277  currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
278  }
279  else if (leaderName == "Right")
280  {
281  currentControlGroup = rightGroup;
282  currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
283  currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
284  currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
285  currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
286  }
287 
288  tsvmp::TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
289  tsvmp::TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
290  virtualtimer = leaderDMP->canVal;
291 
292  if (virtualtimer < 1e-8)
293  {
294  finished = true;
295  started = false;
296  isDMPRun = false;
297  }
298 
299 
300  leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
301 
302  Eigen::Matrix4f currentFollowerLocalPose;
303  currentFollowerLocalPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0).inverse() * currentFollowerPose.block<3, 3>(0, 0);
304  currentFollowerLocalPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0).inverse() * (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
305  followerDMP->flow(deltaT, currentFollowerLocalPose, currentFollowerTwist);
306 
307  Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
308  Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
309  Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
310  std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
311 
312  Eigen::Matrix4f followerTargetPose;
313  followerTargetPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 3>(0, 0);
314  followerTargetPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) + currentLeaderPose.block<3, 1>(0, 3);
315 
316 
317  Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
318  Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
319  followerTargetVel.setZero();
320 
321  std::vector<double> leftDMPTarget;
322  std::vector<double> rightDMPTarget;
323 
324  if (leaderName == "Left")
325  {
326  leftTargetVel = leaderTargetVel;
327  rightTargetVel = followerTargetVel;
328  leftDMPTarget = leaderDMP->getTargetPose();
329  rightDMPTarget = followerLocalTargetPoseVec;
330  leftTargetPose = leaderTargetPose;
331  rightTargetPose = followerTargetPose;
332  }
333  else if (leaderName == "Right")
334  {
335  rightTargetVel = leaderTargetVel;
336  leftTargetVel = followerTargetVel;
337  rightDMPTarget = leaderDMP->getTargetPose();
338  leftDMPTarget = followerLocalTargetPoseVec;
339  rightTargetPose = leaderTargetPose;
340  leftTargetPose = followerTargetPose;
341  }
342  }
343 
344 
345  Eigen::VectorXf leftDesiredJoint = leftDesiredJointValues;
346  Eigen::VectorXf rightDesiredJoint = rightDesiredJointValues;
347  if (isLeftJointLearned)
348  {
349  DMP::DVec targetJointState;
350  currentLeftJointState = leftJointDMP->calculateDirectlyVelocity(currentLeftJointState, virtualtimer / timeDuration, deltaT / timeDuration, targetJointState);
351  for (size_t i = 0; i < targetJointState.size(); ++i)
352  {
353  leftDesiredJoint(i) = targetJointState[i];
354  }
355  }
356 
357  if (isRightJointLearned)
358  {
359  DMP::DVec targetJointState;
360  currentRightJointState = rightJointDMP->calculateDirectlyVelocity(currentRightJointState, virtualtimer / timeDuration, deltaT / timeDuration, targetJointState);
361  for (size_t i = 0; i < targetJointState.size(); ++i)
362  {
363  rightDesiredJoint(i) = targetJointState[i];
364  }
365  }
366 
367  LockGuardType guard {controlDataMutex};
368  getWriterControlStruct().leftTargetVel = leftTargetVel;
369  getWriterControlStruct().rightTargetVel = rightTargetVel;
370  getWriterControlStruct().leftTargetPose = leftTargetPose;
371  getWriterControlStruct().rightTargetPose = rightTargetPose;
372  getWriterControlStruct().leftDesiredJoint = leftDesiredJoint;
373  getWriterControlStruct().rightDesiredJoint = rightDesiredJoint;
374  getWriterControlStruct().virtualTime = virtualtimer;
375  writeControlStruct();
376  isDMPRun = true;
377  }
378 
379  Eigen::VectorXf NJointBimanualCCDMPVelocityController::getControlWrench(const Eigen::VectorXf& tcptwist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose)
380  {
381  return Eigen::Vector6f::Zero();
382  }
383 
384 
385 
386  void NJointBimanualCCDMPVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
387  {
388  double deltaT = timeSinceLastIteration.toSecondsDouble();
389 
390  controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
391  controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
392  controllerSensorData.getWriteBuffer().deltaT = deltaT;
393  controllerSensorData.getWriteBuffer().currentTime += deltaT;
394 
395  // cartesian vel controller
396  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
397 
398  Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
399 
400  Eigen::VectorXf leftqpos;
401  Eigen::VectorXf leftqvel;
402  leftqpos.resize(leftPositionSensors.size());
403  leftqvel.resize(leftVelocitySensors.size());
404  for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
405  {
406  leftqpos(i) = leftPositionSensors[i]->position;
407  leftqvel(i) = leftVelocitySensors[i]->velocity;
408  }
409 
410  Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
411  Eigen::VectorXf rightqpos;
412  Eigen::VectorXf rightqvel;
413  rightqpos.resize(rightPositionSensors.size());
414  rightqvel.resize(rightVelocitySensors.size());
415 
416  for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
417  {
418  rightqpos(i) = rightPositionSensors[i]->position;
419  rightqvel(i) = rightVelocitySensors[i]->velocity;
420  }
421 
422  Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
423  Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
424 
425  controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
426  controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
427  controllerSensorData.commitWrite();
428 
429 
430  interfaceData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
431  interfaceData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
432  interfaceData.getWriteBuffer().currentLeftJointVals = leftqpos;
433  interfaceData.getWriteBuffer().currentRightJointVals = rightqpos;
434  interfaceData.commitWrite();
435 
436  if (!isDMPRun)
437  {
438  for (size_t i = 0; i < leftTargets.size(); ++i)
439  {
440  leftTargets.at(i)->velocity = 0;
441  }
442  for (size_t i = 0; i < rightTargets.size(); ++i)
443  {
444  rightTargets.at(i)->velocity = 0;
445  }
446  }
447  else
448  {
449  Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
450  Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
451  double virtualtime = rtGetControlStruct().virtualTime;
452  Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
453  Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
454  Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
455  Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
456  Eigen::VectorXf leftDesiredJoint = rtGetControlStruct().leftDesiredJoint;
457  Eigen::VectorXf rightDesiredJoint = rtGetControlStruct().rightDesiredJoint;
458 
459  // generate joint control signals
460  Eigen::VectorXf leftCartesianTarget(6);
461  {
462  Eigen::Vector3f targetTCPLinearVelocity;
463  targetTCPLinearVelocity << leftTargetVel(0), leftTargetVel(1), leftTargetVel(2);
464  Eigen::Vector3f currentTCPLinearVelocity;
465  currentTCPLinearVelocity << currentLeftTwist(0), currentLeftTwist(1), currentLeftTwist(2);
466  Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
467  Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
468  Eigen::Vector3f posVel = leftKpos.cwiseProduct(desiredPosition - currentTCPPosition) + leftDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
469 
470 
471  Eigen::Vector3f currentTCPAngularVelocity;
472  currentTCPAngularVelocity << currentLeftTwist(3), currentLeftTwist(4), currentLeftTwist(5);
473  Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
474  Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
475  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
476  Eigen::Vector3f oriVel = leftKori.cwiseProduct(rpy) - leftDori.cwiseProduct(currentTCPAngularVelocity);
477 
478  float normLinearVelocity = posVel.norm();
479  if (normLinearVelocity > maxLinearVel)
480  {
481  posVel = maxLinearVel * posVel / normLinearVelocity;
482  }
483  float normAngularVelocity = oriVel.norm();
484  if (normAngularVelocity > maxAngularVel)
485  {
486  oriVel = maxAngularVel * oriVel / normAngularVelocity;
487  }
488 
489  leftCartesianTarget << posVel, oriVel;
490  }
491 
492  Eigen::VectorXf rightCartesianTarget(6);
493  {
494  Eigen::Vector3f targetTCPLinearVelocity;
495  targetTCPLinearVelocity << rightTargetVel(0), rightTargetVel(1), rightTargetVel(2);
496  Eigen::Vector3f currentTCPLinearVelocity;
497  currentTCPLinearVelocity << currentRightTwist(0), currentRightTwist(1), currentRightTwist(2);
498  Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
499  Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
500  Eigen::Vector3f posVel = rightKpos.cwiseProduct(desiredPosition - currentTCPPosition) + rightDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
501 
502 
503  Eigen::Vector3f currentTCPAngularVelocity;
504  currentTCPAngularVelocity << currentRightTwist(3), currentRightTwist(4), currentRightTwist(5);
505  Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
506  Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
507  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
508  Eigen::Vector3f oriVel = rightKori.cwiseProduct(rpy) - rightDori.cwiseProduct(currentTCPAngularVelocity);
509 
510  float normLinearVelocity = posVel.norm();
511  if (normLinearVelocity > maxLinearVel)
512  {
513  posVel = maxLinearVel * posVel / normLinearVelocity;
514  }
515  float normAngularVelocity = oriVel.norm();
516  if (normAngularVelocity > maxAngularVel)
517  {
518  oriVel = maxAngularVel * oriVel / normAngularVelocity;
519  }
520 
521  rightCartesianTarget << posVel, oriVel;
522 
523  }
524 
525  Eigen::VectorXf leftNullspaceVel = knull * (leftDesiredJoint - leftqpos) - dnull * leftqvel;
526  Eigen::VectorXf rightNullspaceVel = knull * (rightDesiredJoint - rightqpos) - dnull * rightqvel;
527  Eigen::VectorXf desiredLeftVels = leftCtrl->calculate(leftCartesianTarget, leftNullspaceVel, VirtualRobot::IKSolver::CartesianSelection::All);
528  Eigen::VectorXf desiredRightVels = rightCtrl->calculate(rightCartesianTarget, rightNullspaceVel, VirtualRobot::IKSolver::CartesianSelection::All);
529 
530  // torque limit
531  for (size_t i = 0; i < leftTargets.size(); ++i)
532  {
533 
534  float desiredVel = desiredLeftVels[i];
535 
536  if (fabs(desiredVel) > cfg->maxJointVel)
537  {
538  desiredVel = cfg->maxJointVel * desiredVel / fabs(desiredVel);
539  }
540 
541  leftTargets.at(i)->velocity = desiredVel;
542  debugDataInfo.getWriteBuffer().desired_velocities[leftJointNames[i]] = desiredVel;
543 
544  }
545 
546  for (size_t i = 0; i < rightTargets.size(); ++i)
547  {
548  float desiredVel = desiredRightVels[i];
549 
550  if (fabs(desiredVel) > cfg->maxJointVel)
551  {
552  desiredVel = cfg->maxJointVel * desiredVel / fabs(desiredVel);
553  }
554 
555  rightTargets.at(i)->velocity = desiredVel;
556  debugDataInfo.getWriteBuffer().desired_velocities[rightJointNames[i]] = desiredVel;
557  }
558 
559 
560  debugDataInfo.getWriteBuffer().leftControlSignal_x = leftCartesianTarget(0);
561  debugDataInfo.getWriteBuffer().leftControlSignal_y = leftCartesianTarget(1);
562  debugDataInfo.getWriteBuffer().leftControlSignal_z = leftCartesianTarget(2);
563  debugDataInfo.getWriteBuffer().leftControlSignal_ro = leftCartesianTarget(3);
564  debugDataInfo.getWriteBuffer().leftControlSignal_pi = leftCartesianTarget(4);
565  debugDataInfo.getWriteBuffer().leftControlSignal_ya = leftCartesianTarget(5);
566 
567  debugDataInfo.getWriteBuffer().rightControlSignal_x = rightCartesianTarget(0);
568  debugDataInfo.getWriteBuffer().rightControlSignal_y = rightCartesianTarget(1);
569  debugDataInfo.getWriteBuffer().rightControlSignal_z = rightCartesianTarget(2);
570  debugDataInfo.getWriteBuffer().rightControlSignal_ro = rightCartesianTarget(3);
571  debugDataInfo.getWriteBuffer().rightControlSignal_pi = rightCartesianTarget(4);
572  debugDataInfo.getWriteBuffer().rightControlSignal_ya = rightCartesianTarget(5);
573 
574  debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
575  debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
576  debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
577  debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
578  debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
579  debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
580 
581 
582  debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
583  debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
584  debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
585 
586  debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
587  debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
588  debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
589  debugDataInfo.getWriteBuffer().virtualTime = virtualtime;
590 
591  debugDataInfo.commitWrite();
592  }
593 
594  }
595 
596  void NJointBimanualCCDMPVelocityController::learnDMPFromFiles(const std::string& name, const Ice::StringSeq& fileNames, const Ice::Current&)
597  {
598  if (name == "LeftLeader")
599  {
600  leftGroup.at(0)->learnDMPFromFiles(fileNames);
601  }
602  else if (name == "LeftFollower")
603  {
604  rightGroup.at(1)->learnDMPFromFiles(fileNames);
605  }
606  else if (name == "RightLeader")
607  {
608  rightGroup.at(0)->learnDMPFromFiles(fileNames);
609  }
610  else if (name == "RightFollower")
611  {
612  leftGroup.at(1)->learnDMPFromFiles(fileNames);
613  }
614  else if (name == "LeftJoint")
615  {
616  DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
617  DMP::SampledTrajectoryV2 traj;
618  std::string absPath;
619  ArmarXDataPath::getAbsolutePath(fileNames.at(0), absPath);
620  traj.readFromCSVFile(absPath);
621  traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
622  trajs.push_back(traj);
623  leftJointDMP->learnFromTrajectories(trajs);
624  }
625  else if (name == "RightJoint")
626  {
627  DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
628  DMP::SampledTrajectoryV2 traj;
629  std::string absPath;
630  ArmarXDataPath::getAbsolutePath(fileNames.at(0), absPath);
631  traj.readFromCSVFile(absPath);
632  traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
633  trajs.push_back(traj);
634  rightJointDMP->learnFromTrajectories(trajs);
635  }
636  else
637  {
638  ARMARX_ERROR << "The given name is not supported by CCDMP, the supported names contain ";
639  }
640 
641  }
642 
643  void NJointBimanualCCDMPVelocityController::learnDMPFromBothFiles(const Ice::StringSeq& leftFiles, const Ice::StringSeq& rightFiles, const Ice::Current&)
644  {
645  ARMARX_CHECK_EQUAL(leftFiles.size(), rightFiles.size());
646 
647  DMP::Vec<DMP::SampledTrajectoryV2 > leftFollowerTrajs;
648  DMP::Vec<DMP::SampledTrajectoryV2 > rightFollowerTrajs;
649 
650  for (size_t i = 0; i < leftFiles.size(); ++i)
651  {
652  DMP::SampledTrajectoryV2 leftFollowerTraj = DMP::RobotHelpers::getRelativeTrajFromFiles(leftFiles[i], rightFiles[i]);
653  leftFollowerTrajs.push_back(leftFollowerTraj);
654  DMP::SampledTrajectoryV2 rightFollowerTraj = DMP::RobotHelpers::getRelativeTrajFromFiles(rightFiles[i], leftFiles[i]);
655  rightFollowerTrajs.push_back(rightFollowerTraj);
656  }
657 
658  leftGroup.at(0)->learnDMPFromFiles(leftFiles, cfg->initRatio);
659  leftGroup.at(1)->learnDMPFromSampledTrajectory(leftFollowerTrajs, cfg->initRatio);
660  rightGroup.at(0)->learnDMPFromFiles(rightFiles, cfg->initRatio);
661  rightGroup.at(1)->learnDMPFromSampledTrajectory(rightFollowerTrajs, cfg->initRatio);
662  }
663 
664  void NJointBimanualCCDMPVelocityController::setRatios(const Ice::DoubleSeq& ratios, const Ice::Current&)
665  {
666  leftGroup.at(0)->setRatios(ratios);
667  leftGroup.at(1)->setRatios(ratios);
668  rightGroup.at(0)->setRatios(ratios);
669  rightGroup.at(1)->setRatios(ratios);
670  }
671 
672 
673  void NJointBimanualCCDMPVelocityController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
674  {
675  LockGuardType guard(controllerMutex);
676  if (leaderName == "Left")
677  {
678  leftGroup.at(0)->setViaPose(u, viapoint);
679  }
680  else
681  {
682  rightGroup.at(0)->setViaPose(u, viapoint);
683  }
684  }
685 
686  void NJointBimanualCCDMPVelocityController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
687  {
688  LockGuardType guard(controllerMutex);
689  if (leaderName == "Left")
690  {
691  leftGroup.at(0)->setGoalPoseVec(goals);
692  }
693  else
694  {
695  rightGroup.at(0)->setGoalPoseVec(goals);
696  }
697 
698  }
699 
700  void NJointBimanualCCDMPVelocityController::changeLeader(const Ice::Current&)
701  {
702  LockGuardType guard(controllerMutex);
703 
704  if (leaderName == "Left")
705  {
706  leaderName = "Right";
707  rightGroup.at(0)->canVal = virtualtimer;
708  rightGroup.at(1)->canVal = virtualtimer;
709  }
710  else
711  {
712  leaderName = "Left";
713  leftGroup.at(0)->canVal = virtualtimer;
714  leftGroup.at(1)->canVal = virtualtimer;
715  }
716 
717  }
718 
719 
720  void NJointBimanualCCDMPVelocityController::runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&)
721  {
722  ARMARX_INFO << "start running ccdmp";
723  while (!interfaceData.updateReadBuffer())
724  {
725  usleep(100);
726  }
727  Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().currentLeftPose;
728  Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().currentRightPose;
729  Eigen::VectorXf leftJointVals = interfaceData.getReadBuffer().currentLeftJointVals;
730  Eigen::VectorXf rightJointVals = interfaceData.getReadBuffer().currentRightJointVals;
731 
732  for (size_t i = 0; i < leftTargets.size(); ++i)
733  {
734  DMP::DMPState jstate;
735  jstate.pos = leftJointVals(i);
736  jstate.vel = 0;
737  currentLeftJointState.push_back(jstate);
738  }
739 
740  for (size_t i = 0; i < rightTargets.size(); ++i)
741  {
742  DMP::DMPState jstate;
743  jstate.pos = rightJointVals(i);
744  jstate.vel = 0;
745  currentRightJointState.push_back(jstate);
746  }
747 
748  virtualtimer = cfg->timeDuration;
749  leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
750  rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
751 
752 
753  ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
754 
755  leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose), getLocalPose(leftGoals, rightGoals));
756  rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose), getLocalPose(rightGoals, leftGoals));
757 
758  finished = false;
759  started = true;
760  }
761 
762  Eigen::Matrix4f NJointBimanualCCDMPVelocityController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
763  {
764  Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
765 
766  localPose.block<3, 3>(0, 0) = newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
767  localPose.block<3, 1>(0, 3) = newCoordinate.block<3, 3>(0, 0).inverse() * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
768 
769 
770  return localPose;
771  }
772 
773 
774  void NJointBimanualCCDMPVelocityController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
775  {
776 
777  StringVariantBaseMap datafields;
778  auto values = debugDataInfo.getUpToDateReadBuffer().desired_velocities;
779  for (auto& pair : values)
780  {
781  datafields[pair.first] = new Variant(pair.second);
782  }
783 
784  auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
785  for (auto& pair : constrained_force)
786  {
787  datafields[pair.first] = new Variant(pair.second);
788  }
789 
790 
791  datafields["leftTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
792  datafields["leftTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
793  datafields["leftTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
794  datafields["rightTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
795  datafields["rightTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
796  datafields["rightTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
797 
798 
799  datafields["leftCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
800  datafields["leftCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
801  datafields["leftCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
802  datafields["rightCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
803  datafields["rightCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
804  datafields["rightCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
805 
806  datafields["leftControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_x);
807  datafields["leftControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_y);
808  datafields["leftControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_z);
809  datafields["leftControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ro);
810  datafields["leftControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_pi);
811  datafields["leftControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ya);
812 
813 
814  datafields["rightControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_x);
815  datafields["rightControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_y);
816  datafields["rightControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_z);
817  datafields["rightControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ro);
818  datafields["rightControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_pi);
819  datafields["rightControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ya);
820 
821  datafields["virtual_timer"] = new Variant(debugDataInfo.getUpToDateReadBuffer().virtualTime);
822 
823 
824  debugObs->setDebugChannel("CCDMPVelocityController", datafields);
825  }
826 
827  void NJointBimanualCCDMPVelocityController::onInitNJointController()
828  {
829  ARMARX_INFO << "init ...";
830  started = false;
831  runTask("NJointBimanualCCDMPVelocityController", [&]
832  {
833  CycleUtil c(1);
834  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
835  while (getState() == eManagedIceObjectStarted)
836  {
837  if (isControllerActive())
838  {
839  controllerRun();
840  }
841  c.waitForCycleDuration();
842  }
843  });
844  ARMARX_INFO << "Finished init ...";
845 
846  }
847 
848  void NJointBimanualCCDMPVelocityController::onDisconnectNJointController()
849  {
850  ARMARX_INFO << "stopped ...";
851  }
852 
853 
854 
855 }
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPVelocityControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const NJointBimanualCCDMPVelocityControllerControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::ControlTargetBase::asA
const T * asA() const
Definition: ControlTargetBase.h:76
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::learnDMPFromFiles
void learnDMPFromFiles(const std::string &, const Ice::StringSeq &, const Ice::Current &) override
Definition: NJointBimanualCCDMPVelocityController.cpp:596
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:90
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPVelocityControllerControlData >::rtGetControlStruct
const NJointBimanualCCDMPVelocityControllerControlData & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPKernelSize
int DMPKernelSize
Definition: TaskSpaceVMP.h:54
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityControllerControlData::rightTargetVel
Eigen::VectorXf rightTargetVel
Definition: NJointBimanualCCDMPVelocityController.h:41
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
armarx::SensorValueBase
The SensorValueBase class.
Definition: SensorValueBase.h:40
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition: ControlTargetBase.h:47
armarx::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:146
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::onDisconnectNJointController
void onDisconnectNJointController() override
Definition: NJointBimanualCCDMPVelocityController.cpp:848
armarx::NJointControllerBase::useControlTarget
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition: NJointController.cpp:410
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
armarx::ManagedIceObject::getState
int getState() const
Retrieve current state of the ManagedIceObject.
Definition: ManagedIceObject.cpp:725
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &) override
Definition: NJointBimanualCCDMPVelocityController.cpp:673
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::setRatios
void setRatios(const Ice::DoubleSeq &ratios, const Ice::Current &) override
Definition: NJointBimanualCCDMPVelocityController.cpp:664
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityControllerControlData::leftTargetPose
Eigen::Matrix4f leftTargetPose
Definition: NJointBimanualCCDMPVelocityController.h:43
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::NJointBimanualCCDMPVelocityControllerInterface::changeLeader
void changeLeader()
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig
Definition: TaskSpaceVMP.h:52
armarx::CartesianVelocityController
Definition: CartesianVelocityController.h:36
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPVelocityControllerControlData >::getWriterControlStruct
NJointBimanualCCDMPVelocityControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPVelocityControllerControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::controllerRun
void controllerRun()
Definition: NJointBimanualCCDMPVelocityController.cpp:221
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::motionTimeDuration
float motionTimeDuration
Definition: TaskSpaceVMP.h:59
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:845
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:177
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:919
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
normalizeTimestamps
TrajectoryPtr normalizeTimestamps(TrajectoryPtr t, float maxTransVel, float maxRotVel)
Definition: PlayPlatformTrajectory.cpp:76
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition: NJointControllerBase.h:777
armarx::control::common::mp::DVec
Ice::DoubleSeq DVec
Definition: MP.h:46
armarx::NJointControllerBase::runTask
void runTask(const std::string &taskName, Task &&task)
Executes a given task in a separate thread from the Application ThreadPool.
Definition: NJointControllerBase.h:753
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: NJointBimanualCCDMPVelocityController.cpp:202
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPVelocityControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:102
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: NJointBimanualCCDMPVelocityController.cpp:207
armarx::control::deprecated_njoint_mp_controller::bimanual::registrationControllerNJointBimanualCCDMPVelocityController
NJointControllerRegistration< NJointBimanualCCDMPVelocityController > registrationControllerNJointBimanualCCDMPVelocityController("NJointBimanualCCDMPVelocityController")
ArmarXObjectScheduler.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityControllerControlData::leftTargetVel
Eigen::VectorXf leftTargetVel
Definition: NJointBimanualCCDMPVelocityController.h:40
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: NJointBimanualCCDMPVelocityController.cpp:386
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPVelocityControllerControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::onInitNJointController
void onInitNJointController() override
Definition: NJointBimanualCCDMPVelocityController.cpp:827
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition: TaskSpaceVMP.h:57
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::NJointBimanualCCDMPVelocityController
NJointBimanualCCDMPVelocityController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointBimanualCCDMPVelocityController.cpp:11
NJointBimanualCCDMPVelocityController.h
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityControllerControlData::rightTargetPose
Eigen::Matrix4f rightTargetPose
Definition: NJointBimanualCCDMPVelocityController.h:44
CycleUtil.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::runDMP
void runDMP(const Ice::DoubleSeq &leftGoals, const Ice::DoubleSeq &rightGoals, const Ice::Current &) override
Definition: NJointBimanualCCDMPVelocityController.cpp:720
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerPtr
std::shared_ptr< TaskSpaceDMPController > TaskSpaceDMPControllerPtr
Definition: NJointTaskSpaceImpedanceDMPController.h:24
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPStyle
std::string DMPStyle
Definition: TaskSpaceVMP.h:56
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityControllerControlData::virtualTime
double virtualTime
Definition: NJointBimanualCCDMPVelocityController.h:46
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition: TaskSpaceVMP.h:55
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: NJointBimanualCCDMPVelocityController.cpp:774
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:111
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityControllerControlData
Definition: NJointBimanualCCDMPVelocityController.h:37
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &) override
Definition: NJointBimanualCCDMPVelocityController.cpp:686
armarx::TripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:108
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::SensorValueForceTorque
Definition: SensorValueForceTorque.h:31
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
ArmarXDataPath.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::learnDMPFromBothFiles
void learnDMPFromBothFiles(const Ice::StringSeq &, const Ice::StringSeq &, const Ice::Current &) override
Definition: NJointBimanualCCDMPVelocityController.cpp:643
armarx::TripleBuffer::reinitAllBuffers
std::enable_if< std::is_copy_constructible< U >::value >::type reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:153
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition: TaskSpaceVMP.h:83
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:731
armarx::NJointControllerBase::useSensorValue
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
Definition: NJointController.cpp:383
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
armarx::control::deprecated_njoint_mp_controller::bimanual
Definition: NJointBimanualCartesianAdmittanceController.cpp:9
armarx::TripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:131