NJointCCDMPController.cpp
Go to the documentation of this file.
1 #include "NJointCCDMPController.h"
2 #include <VirtualRobot/MathTools.h>
3 
4 namespace armarx::control::deprecated_njoint_mp_controller::task_space
5 {
6  NJointControllerRegistration<NJointCCDMPController>
7  registrationControllerNJointCCDMPController("NJointCCDMPController");
8 
9  NJointCCDMPController::NJointCCDMPController(const RobotUnitPtr&,
10  const armarx::NJointControllerConfigPtr& config,
11  const VirtualRobot::RobotPtr&)
12  {
13  useSynchronizedRtRobot();
14  cfg = NJointCCDMPControllerConfigPtr::dynamicCast(config);
15  ARMARX_CHECK_NOT_NULL(cfg);
16  ARMARX_CHECK_GREATER_EQUAL(cfg->dmpNum, 0);
17  ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
18  VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
19 
20  ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
21  for (size_t i = 0; i < rns->getSize(); ++i)
22  {
23  std::string jointName = rns->getNode(i)->getName();
24  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::VelocityTorque);
25  const SensorValueBase* sv = useSensorValue(jointName);
26  targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
27 
28  const SensorValue1DoFActuatorTorque* torqueSensor =
29  sv->asA<SensorValue1DoFActuatorTorque>();
30  const SensorValue1DoFGravityTorque* gravityTorqueSensor =
31  sv->asA<SensorValue1DoFGravityTorque>();
32  if (!torqueSensor)
33  {
34  ARMARX_WARNING << "No Torque sensor available for " << jointName;
35  }
36  if (!gravityTorqueSensor)
37  {
38  ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
39  }
40 
41  torqueSensors.push_back(torqueSensor);
42  gravityTorqueSensors.push_back(gravityTorqueSensor);
43  };
44 
45  tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
46  ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName;
47 
48  // set tcp controller
49  tcpController.reset(new CartesianVelocityController(rns, tcp));
50  nodeSetName = cfg->nodeSetName;
51  torquePIDs.resize(tcpController->rns->getSize(), pidController());
52 
53 
54  ik.reset(new VirtualRobot::DifferentialIK(
55  rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
56 
57  // set DMP
58  isDisturbance = false;
59 
60  dmpPtrList.resize(cfg->dmpNum);
61  canVals.resize(cfg->dmpNum);
62  timeDurations = cfg->timeDurations;
63  dmpTypes = cfg->dmpTypes;
64  for (size_t i = 0; i < dmpPtrList.size(); ++i)
65  {
66  dmpPtrList[i].reset(new DMP::UMITSMP(cfg->kernelSize, 2, cfg->tau));
67  canVals[i] = timeDurations[i];
68  }
69  finished = false;
70 
71  phaseL = cfg->phaseL;
72  phaseK = cfg->phaseK;
73  phaseDist0 = cfg->phaseDist0;
74  phaseDist1 = cfg->phaseDist1;
75  phaseKpPos = cfg->phaseKpPos;
76  phaseKpOri = cfg->phaseKpOri;
77 
78  posToOriRatio = cfg->posToOriRatio;
79  tau = cfg->tau;
80 
81  // initialize tcp position and orientation
82  Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
83  tcpPosition(0) = pose(0, 3);
84  tcpPosition(1) = pose(1, 3);
85  tcpPosition(2) = pose(2, 3);
86  VirtualRobot::MathTools::Quaternion tcpQ = VirtualRobot::MathTools::eigen4f2quat(pose);
87  tcpOrientation.w() = tcpQ.w;
88  tcpOrientation.x() = tcpQ.x;
89  tcpOrientation.y() = tcpQ.y;
90  tcpOrientation.z() = tcpQ.z;
91 
92  NJointCCDMPControllerSensorData initSensorData;
93  initSensorData.deltaT = 0;
94  initSensorData.currentTime = 0;
95  initSensorData.currentPose = pose;
96  controllerSensorData.reinitAllBuffers(initSensorData);
97 
98 
99  currentStates.resize(cfg->dmpNum);
100  targetSubStates.resize(cfg->dmpNum);
101  targetState.resize(7);
102 
103  targetVels.resize(6);
104  targetVels.setZero();
105  NJointCCDMPControllerControlData initData;
106  initData.targetTSVel.resize(6);
107  initData.targetTSVel.setZero();
108  initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
109  initData.torqueKp.resize(tcpController->rns->getSize(), 0);
110  initData.torqueKd.resize(tcpController->rns->getSize(), 0);
111  initData.mode = ModeFromIce(cfg->mode);
112  reinitTripleBuffer(initData);
113 
114  learnedDMP.clear();
115 
116  amplitudes = cfg->amplitudes;
117  }
118 
119  std::string
120  NJointCCDMPController::getClassName(const Ice::Current&) const
121  {
122  return "NJointCCDMPController";
123  }
124 
125  void
126  NJointCCDMPController::controllerRun()
127  {
128  if (!controllerSensorData.updateReadBuffer())
129  {
130  return;
131  }
132 
133  double deltaT = controllerSensorData.getReadBuffer().deltaT;
134 
135  Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
136  Eigen::Vector3f currentPosition;
137  currentPosition << currentPose(0, 3), currentPose(1, 3), currentPose(2, 3);
138  double posError = 0;
139  for (size_t i = 0; i < 3; ++i)
140  {
141  posError += pow(currentPosition(i) - targetState[i], 2);
142  }
143  posError = sqrt(posError);
144 
145 
146  VirtualRobot::MathTools::Quaternion cQuat =
147  VirtualRobot::MathTools::eigen4f2quat(currentPose);
148  Eigen::Quaterniond currentQ;
149  Eigen::Quaterniond targetQ;
150  currentQ.w() = cQuat.w;
151  currentQ.x() = cQuat.x;
152  currentQ.y() = cQuat.y;
153  currentQ.z() = cQuat.z;
154  targetQ.w() = targetState[3];
155  targetQ.x() = targetState[4];
156  targetQ.y() = targetState[5];
157  targetQ.z() = targetState[6];
158 
159  double oriError = targetQ.angularDistance(currentQ);
160  if (oriError > M_PI)
161  {
162  oriError = 2 * M_PI - oriError;
163  }
164 
165  double error = posError + posToOriRatio * oriError;
166 
167  double phaseDist;
168  if (isDisturbance)
169  {
170  phaseDist = phaseDist1;
171  }
172  else
173  {
174  phaseDist = phaseDist0;
175  }
176 
177  double phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
178  double mpcFactor = 1 - (phaseStop / phaseL);
179 
180 
181  if (mpcFactor < 0.1)
182  {
183  isDisturbance = true;
184  }
185 
186  if (mpcFactor > 0.9)
187  {
188  isDisturbance = false;
189  }
190 
191  // run DMP one after another
192  Eigen::Matrix4f targetPose = Eigen::Matrix4f::Identity();
193 
194  Eigen::VectorXf dmpVels;
195  dmpVels.resize(6);
196  dmpVels.setZero();
197  for (size_t i = 0; i < static_cast<size_t>(cfg->dmpNum); ++i)
198  {
199  double timeDuration = timeDurations[i];
200  std::string dmpType = dmpTypes[i];
201 
202  //double amplitude = 1.0;
203  if (dmpType == "Periodic")
204  {
205  if (canVals[i] <= 1e-8)
206  {
207  canVals[i] = timeDuration;
208  }
209  //amplitude = amplitudes[i];
210  }
211 
212  if (canVals[i] > 1e-8)
213  {
214  DMP::UMITSMPPtr dmpPtr = dmpPtrList[i];
215  double dmpDeltaT = deltaT / timeDuration;
216  double tau = dmpPtr->getTemporalFactor();
217  canVals[i] -= 1 / tau * deltaT * 1 / (1 + phaseStop);
218 
219 
220  currentStates[i] = dmpPtr->calculateDirectlyVelocity(
221  currentStates[i], canVals[i] / timeDurations[i], dmpDeltaT, targetSubStates[i]);
222 
223 
224  for (size_t j = 0; j < 3; ++j)
225  {
226  dmpVels(j) += currentStates[i][j].vel / timeDurations[i];
227  }
228 
229  Eigen::Quaterniond quatVel0;
230  quatVel0.w() = currentStates[i][3].vel;
231  quatVel0.x() = currentStates[i][4].vel;
232  quatVel0.y() = currentStates[i][5].vel;
233  quatVel0.z() = currentStates[i][6].vel;
234  Eigen::Quaterniond dmpQ;
235  dmpQ.w() = currentStates[i][3].pos;
236  dmpQ.x() = currentStates[i][4].pos;
237  dmpQ.y() = currentStates[i][5].pos;
238  dmpQ.z() = currentStates[i][6].pos;
239  //Eigen::Quaterniond angularVel0 = 2.0 * quatVel0 * dmpQ.inverse();
240  const Eigen::Quaterniond aVtmp0 = quatVel0 * dmpQ.inverse();
241  const Eigen::Quaterniond angularVel0{
242  2 * aVtmp0.w(), 2 * aVtmp0.x(), 2 * aVtmp0.y(), 2 * aVtmp0.z()};
243 
244 
245  Eigen::Vector3f angularVelVec;
246  angularVelVec << angularVel0.x() / timeDurations[i],
247  angularVel0.y() / timeDurations[i], angularVel0.z() / timeDurations[i];
248 
249  angularVelVec = targetPose.block<3, 3>(0, 0) * angularVelVec;
250 
251  ARMARX_INFO << "i: " << i << " angularVelVec: " << angularVelVec;
252  dmpVels(3) += angularVelVec(0);
253  dmpVels(4) += angularVelVec(1);
254  dmpVels(5) += angularVelVec(2);
255 
256  finished = false;
257  }
258 
259 
260  Eigen::Matrix4f targetSubMat =
261  VirtualRobot::MathTools::quat2eigen4f(targetSubStates[i][4],
262  targetSubStates[i][5],
263  targetSubStates[i][6],
264  targetSubStates[i][3]);
265  targetSubMat(0, 3) = targetSubStates[i][0];
266  targetSubMat(1, 3) = targetSubStates[i][1];
267  targetSubMat(2, 3) = targetSubStates[i][2];
268 
269  targetPose = targetPose * targetSubMat;
270  }
271 
272  // ARMARX_INFO << "targetPose: " << targetPose;
273  for (size_t i = 0; i < 3; i++)
274  {
275  double vel0 = dmpVels(i);
276  double vel1 = phaseKpPos * (targetState[i] - currentPose(i, 3));
277  targetVels(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
278  }
279 
280 
281  Eigen::Quaterniond diffQ = targetQ * currentQ.inverse();
282  const Eigen::Quaterniond quatVel1{phaseKpOri * diffQ.w(),
283  phaseKpOri * diffQ.x(),
284  phaseKpOri * diffQ.y(),
285  phaseKpOri * diffQ.z()};
286  //Eigen::Quaterniond angularVel1 = 2.0 * quatVel1 * currentQ.inverse();
287  const Eigen::Quaterniond aVtmp1 = quatVel1 * currentQ.inverse();
288  const Eigen::Quaterniond angularVel1{
289  2 * aVtmp1.w(), 2 * aVtmp1.x(), 2 * aVtmp1.y(), 2 * aVtmp1.z()};
290  targetVels(3) = mpcFactor * dmpVels(3) + (1 - mpcFactor) * angularVel1.x();
291  targetVels(4) = mpcFactor * dmpVels(4) + (1 - mpcFactor) * angularVel1.y();
292  targetVels(5) = mpcFactor * dmpVels(5) + (1 - mpcFactor) * angularVel1.z();
293 
294 
295  VirtualRobot::MathTools::Quaternion tQuat =
296  VirtualRobot::MathTools::eigen4f2quat(targetPose);
297  targetState[0] = targetPose(0, 3);
298  targetState[1] = targetPose(1, 3);
299  targetState[2] = targetPose(2, 3);
300  targetState[3] = tQuat.w;
301  targetState[4] = tQuat.x;
302  targetState[5] = tQuat.y;
303  targetState[6] = tQuat.z;
304 
305 
306  debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
307  debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
308  debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
309  debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
310  debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
311  debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
312  debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
313  debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
314  debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
315  debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
316  debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
317  debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
318  debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
319 
320  debugOutputData.getWriteBuffer().realTCP["real_x"] = currentPosition[0];
321  debugOutputData.getWriteBuffer().realTCP["real_y"] = currentPosition[1];
322  debugOutputData.getWriteBuffer().realTCP["real_z"] = currentPosition[2];
323  debugOutputData.getWriteBuffer().realTCP["real_qw"] = cQuat.w;
324  debugOutputData.getWriteBuffer().realTCP["real_qx"] = cQuat.x;
325  debugOutputData.getWriteBuffer().realTCP["real_qy"] = cQuat.y;
326  debugOutputData.getWriteBuffer().realTCP["real_qz"] = cQuat.z;
327 
328  debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
329  debugOutputData.getWriteBuffer().error = error;
330  debugOutputData.getWriteBuffer().phaseStop = phaseStop;
331  debugOutputData.getWriteBuffer().posError = posError;
332  debugOutputData.getWriteBuffer().oriError = oriError;
333  debugOutputData.getWriteBuffer().deltaT = deltaT;
334 
335  debugOutputData.getWriteBuffer().canVal0 = canVals[0];
336 
337 
338  debugOutputData.commitWrite();
339 
340  LockGuardType guard{controlDataMutex};
341  getWriterControlStruct().targetTSVel = targetVels;
342  writeControlStruct();
343  }
344 
345 
346  void
347  NJointCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
348  const IceUtil::Time& timeSinceLastIteration)
349  {
350  double deltaT = timeSinceLastIteration.toSecondsDouble();
351  controllerSensorData.getWriteBuffer().currentPose = tcp->getPoseInRootFrame();
352  controllerSensorData.getWriteBuffer().deltaT = deltaT;
353  controllerSensorData.getWriteBuffer().currentTime += deltaT;
354  controllerSensorData.commitWrite();
355  // cartesian vel controller
356 
357  Eigen::MatrixXf jacobi =
358  ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
359 
360 
361  Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(
362  jacobi, ik->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
363 
364  Eigen::VectorXf jnv = jtpinv * rtGetControlStruct().targetTSVel;
365 
366  for (size_t i = 0; i < targets.size(); ++i)
367  {
368  targets.at(i)->velocity = jnv(i);
369  }
370  }
371 
372 
373  void
374  NJointCCDMPController::learnDMPFromFiles(int dmpId,
375  const Ice::StringSeq& fileNames,
376  const Ice::Current&)
377  {
378  DMP::Vec<DMP::SampledTrajectoryV2> trajs;
379 
380  DMP::DVec ratios;
381  DMP::DVec goals;
382  DMP::Vec<DMP::DMPState> starts;
383 
384  for (size_t i = 0; i < fileNames.size(); ++i)
385  {
386  DMP::SampledTrajectoryV2 traj;
387  traj.readFromCSVFile(fileNames.at(i));
388  traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
389  trajs.push_back(traj);
390 
391  if (i == 0)
392  {
393  goals.resize(traj.dim());
394  starts.resize(traj.dim());
395  for (size_t j = 0; j < goals.size(); ++j)
396  {
397  goals[j] = traj.rbegin()->getPosition(j);
398  starts[j].pos = traj.begin()->getPosition(j);
399  starts[j].vel = traj.begin()->getDeriv(j, 1) * timeDurations[dmpId];
400  }
401  }
402 
403  if (i == 0)
404  {
405  ratios.push_back(1.0);
406  }
407  else
408  {
409  ratios.push_back(0.0);
410  }
411  }
412 
413  dmpPtrList[dmpId]->learnFromTrajectories(trajs);
414  dmpPtrList[dmpId]->styleParas = dmpPtrList[dmpId]->getStyleParasWithRatio(ratios);
415 
416 
417  DMP::Vec<DMP::DMPState> currentState;
418  if (dmpId == 0)
419  {
420  for (size_t i = 0; i < 3; i++)
421  {
422  DMP::DMPState currentPos;
423  currentPos.pos = tcpPosition(i);
424  currentPos.vel = 0;
425  currentState.push_back(currentPos);
426  }
427 
428  DMP::DMPState currentPos;
429  currentPos.pos = tcpOrientation.w();
430  currentPos.vel = 0;
431  currentState.push_back(currentPos);
432 
433  currentPos.pos = tcpOrientation.x();
434  currentPos.vel = 0;
435  currentState.push_back(currentPos);
436 
437  currentPos.pos = tcpOrientation.y();
438  currentPos.vel = 0;
439  currentState.push_back(currentPos);
440 
441  currentPos.pos = tcpOrientation.z();
442  currentPos.vel = 0;
443  currentState.push_back(currentPos);
444  }
445  else
446  {
447  currentState = starts;
448  }
449  for (size_t i = 0; i < 3; i++)
450  {
451  targetState[i] = tcpPosition(i);
452  }
453 
454  targetState[3] = tcpOrientation.w();
455  targetState[4] = tcpOrientation.x();
456  targetState[5] = tcpOrientation.y();
457  targetState[6] = tcpOrientation.z();
458 
459  currentStates[dmpId] = currentState;
460 
461  dmpPtrList[dmpId]->prepareExecution(goals, currentState, 1, tau);
462  dmpPtrList[dmpId]->setTemporalFactor(tau);
463 
464  learnedDMP.push_back(dmpId);
465  ARMARX_INFO << "Learned DMP ... ";
466  }
467 
468  void
469  NJointCCDMPController::setViaPoints(int dmpId,
470  Ice::Double u,
471  const Ice::DoubleSeq& viapoint,
472  const Ice::Current&)
473  {
474 
475  LockGuardType guard(controllerMutex);
476  dmpPtrList[dmpId]->setViaPoint(u, viapoint);
477  }
478 
479  void
480  NJointCCDMPController::setGoals(int dmpId, const Ice::DoubleSeq& goals, const Ice::Current& ice)
481  {
482  setViaPoints(dmpId, dmpPtrList[dmpId]->getUMin(), goals, ice);
483  }
484 
485  void
486  NJointCCDMPController::setControllerTarget(
487  Ice::Float avoidJointLimitsKp,
488  NJointTaskSpaceDMPControllerMode::CartesianSelection mode,
489  const Ice::Current&)
490  {
491  LockGuardType guard{controlDataMutex};
492  getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
493  getWriterControlStruct().mode = ModeFromIce(mode);
494  writeControlStruct();
495  }
496 
497  VirtualRobot::IKSolver::CartesianSelection
498  NJointCCDMPController::ModeFromIce(
499  const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
500  {
501  if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
502  {
503  return VirtualRobot::IKSolver::CartesianSelection::Position;
504  }
505  if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eOrientation)
506  {
507  return VirtualRobot::IKSolver::CartesianSelection::Orientation;
508  }
509  if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eAll)
510  {
511  return VirtualRobot::IKSolver::CartesianSelection::All;
512  }
513  ARMARX_ERROR_S << "invalid mode " << mode;
514  return (VirtualRobot::IKSolver::CartesianSelection)0;
515  }
516 
517 
518  void
519  NJointCCDMPController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
520  {
521  LockGuardType guard{controlDataMutex};
522  for (size_t i = 0; i < tcpController->rns->getSize(); i++)
523  {
524  getWriterControlStruct().torqueKp.at(i) =
525  torqueKp.at(tcpController->rns->getNode(i)->getName());
526  }
527  writeControlStruct();
528  }
529 
530  void
531  NJointCCDMPController::setNullspaceJointVelocities(
532  const StringFloatDictionary& nullspaceJointVelocities,
533  const Ice::Current&)
534  {
535  LockGuardType guard{controlDataMutex};
536  for (size_t i = 0; i < tcpController->rns->getSize(); i++)
537  {
538  getWriterControlStruct().nullspaceJointVelocities.at(i) =
539  nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
540  }
541  writeControlStruct();
542  }
543 
544  void
545  NJointCCDMPController::runDMP(const Ice::Current&)
546  {
547 
548  const auto dmpNum = static_cast<std::size_t>(cfg->dmpNum);
549  finished = false;
550  if (dmpNum != dmpTypes.size() || dmpNum != dmpPtrList.size() ||
551  dmpNum != learnedDMP.size() || dmpNum != canVals.size() ||
552  dmpNum != currentStates.size() || dmpNum != targetSubStates.size())
553  {
554  ARMARX_ERROR << "Error: cannot run CCDMP controller. The reason is that some "
555  "parameters have different sizes";
556  return;
557  }
558  ARMARX_INFO << "run DMP";
559  controllerTask->start();
560  }
561 
562  void
563  NJointCCDMPController::setTemporalFactor(int dmpId, double tau, const Ice::Current&)
564  {
565  LockGuardType guard(controllerMutex);
566  dmpPtrList[dmpId]->setTemporalFactor(tau);
567  }
568 
569  void
570  NJointCCDMPController::rtPreActivateController()
571  {
572  }
573 
574  void
575  NJointCCDMPController::rtPostDeactivateController()
576  {
577  }
578 
579  void
580  NJointCCDMPController::onPublish(const SensorAndControl&,
581  const DebugDrawerInterfacePrx&,
582  const DebugObserverInterfacePrx& debugObs)
583  {
584  StringVariantBaseMap datafields;
585  auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
586  for (auto& pair : values)
587  {
588  datafields[pair.first] = new Variant(pair.second);
589  }
590 
591  auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
592  for (auto& pair : dmpTargets)
593  {
594  datafields[pair.first] = new Variant(pair.second);
595  }
596 
597  auto realTCP = debugOutputData.getUpToDateReadBuffer().realTCP;
598  for (auto& pair : realTCP)
599  {
600  datafields[pair.first] = new Variant(pair.second);
601  }
602 
603 
604  datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
605  datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
606  datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
607  datafields["posError"] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
608  datafields["oriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
609  datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
610  datafields["canVal0"] = new Variant(debugOutputData.getUpToDateReadBuffer().canVal0);
611 
612  debugObs->setDebugChannel("DMPController", datafields);
613  }
614 
615  void
616  NJointCCDMPController::onInitNJointController()
617  {
618  ARMARX_INFO << "init ...";
619  controllerTask = new PeriodicTask<NJointCCDMPController>(
620  this, &NJointCCDMPController::controllerRun, 0.3);
621  }
622 
623  void
624  NJointCCDMPController::onDisconnectNJointController()
625  {
626  controllerTask->stop();
627  ARMARX_INFO << "stopped ...";
628  }
629 
630 
631 } // namespace armarx::ctrl::njoint_ctrl::dmp
armarx::NJointControllerWithTripleBuffer< NJointCCDMPControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const NJointCCDMPControllerControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::ModeFromIce
VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
Definition: NJointCCDMPController.cpp:498
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
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::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:918
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< NJointCCDMPControllerControlData >::rtGetControlStruct
const NJointCCDMPControllerControlData & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
GfxTL::Orientation
ScalarT Orientation(const VectorXD< 2, ScalarT > &p1, const VectorXD< 2, ScalarT > &p2, const VectorXD< 2, ScalarT > &c)
Definition: Orientation.h:9
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::task_space::NJointCCDMPController::setTemporalFactor
void setTemporalFactor(int dmpId, Ice::Double tau, const Ice::Current &) override
Definition: NJointCCDMPController.cpp:563
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::setControllerTarget
void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current &) override
Definition: NJointCCDMPController.cpp:486
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_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPControllerControlData::torqueKd
std::vector< float > torqueKd
Definition: NJointCCDMPController.h:38
armarx::control::deprecated_njoint_mp_controller::task_space
Definition: DeprecatedNJointPeriodicTSDMPCompliantController.cpp:6
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::NJointTaskSpaceDMPControllerMode::ePosition
@ ePosition
Definition: ControllerInterface.ice:36
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::learnDMPFromFiles
void learnDMPFromFiles(int dmpId, const Ice::StringSeq &fileNames, const Ice::Current &) override
Definition: NJointCCDMPController.cpp:374
armarx::CartesianVelocityController
Definition: CartesianVelocityController.h:36
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPControllerControlData::mode
VirtualRobot::IKSolver::CartesianSelection mode
Definition: NJointCCDMPController.h:39
armarx::NJointControllerWithTripleBuffer< NJointCCDMPControllerControlData >::getWriterControlStruct
NJointCCDMPControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::setGoals
void setGoals(int dmpId, const Ice::DoubleSeq &goals, const Ice::Current &) override
Definition: NJointCCDMPController.cpp:480
armarx::NJointControllerWithTripleBuffer< NJointCCDMPControllerControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::onInitNJointController
void onInitNJointController() override
Definition: NJointCCDMPController.cpp:616
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
armarx::control::deprecated_njoint_mp_controller::task_space::registrationControllerNJointCCDMPController
NJointControllerRegistration< NJointCCDMPController > registrationControllerNJointCCDMPController("NJointCCDMPController")
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
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::control::common::mp::DVec
Ice::DoubleSeq DVec
Definition: MP.h:46
ARMARX_ERROR_S
#define ARMARX_ERROR_S
Definition: Logging.h:209
armarx::NJointTaskSpaceDMPControllerMode::CartesianSelection
CartesianSelection
Definition: ControllerInterface.ice:34
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPControllerControlData::torqueKp
std::vector< float > torqueKp
Definition: NJointCCDMPController.h:37
armarx::NJointControllerWithTripleBuffer< NJointCCDMPControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:102
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::setTorqueKp
void setTorqueKp(const StringFloatDictionary &torqueKp, const Ice::Current &) override
Definition: NJointCCDMPController.cpp:519
armarx::NJointControllerWithTripleBuffer< NJointCCDMPControllerControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: NJointCCDMPController.cpp:580
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPControllerControlData::nullspaceJointVelocities
std::vector< float > nullspaceJointVelocities
Definition: NJointCCDMPController.h:35
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::setNullspaceJointVelocities
void setNullspaceJointVelocities(const StringFloatDictionary &nullspaceJointVelocities, const Ice::Current &) override
Definition: NJointCCDMPController.cpp:531
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition: ExpressionException.h:123
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::NJointCCDMPController
NJointCCDMPController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointCCDMPController.cpp:9
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPControllerControlData
Definition: NJointCCDMPController.h:30
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::controllerRun
void controllerRun()
Definition: NJointCCDMPController.cpp:126
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::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::rtPostDeactivateController
void rtPostDeactivateController() override
This function is called after the controller is deactivated.
Definition: NJointCCDMPController.cpp:575
armarx::NJointTaskSpaceDMPControllerMode::eAll
@ eAll
Definition: ControllerInterface.ice:38
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: NJointCCDMPController.cpp:120
armarx::NJointTaskSpaceDMPControllerMode::eOrientation
@ eOrientation
Definition: ControllerInterface.ice:37
NJointCCDMPController.h
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::onDisconnectNJointController
void onDisconnectNJointController() override
Definition: NJointCCDMPController.cpp:624
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: NJointCCDMPController.cpp:347
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
Eigen::Quaterniond
Quaternion< double, 0 > Quaterniond
Definition: EigenForwardDeclarations.h:62
armarx::PeriodicTask
Definition: ArmarXManager.h:70
armarx::navigation::core::Position
Eigen::Vector3f Position
Definition: basic_types.h:36
armarx::TripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:108
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::NJointCCDMPControllerInterface::runDMP
void runDMP()
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::task_space::NJointCCDMPController::setViaPoints
void setViaPoints(int dmpId, Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &) override
Definition: NJointCCDMPController.cpp:469
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPControllerControlData::targetTSVel
Eigen::VectorXf targetTSVel
Definition: NJointCCDMPController.h:33
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::TripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:131
armarx::control::deprecated_njoint_mp_controller::task_space::NJointCCDMPController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: NJointCCDMPController.cpp:570