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