NJointTaskSpaceImpedanceDMPController.cpp
Go to the documentation of this file.
1 // Simox
2 #include <VirtualRobot/IK/DifferentialIK.h>
3 #include <VirtualRobot/Robot.h>
4 
5 // ArmarXCore
6 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
7 #include <ArmarXCore/core/time/CycleUtil.h>
8 
9 // RobotAPI
10 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
11 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
12 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
13 
14 // control
15 #include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
16 #include <armarx/control/deprecated_njoint_mp_controller/task_space/TaskSpaceImpedanceDMPControllerInterface.h>
17 
18 #include "NJointTaskSpaceImpedanceDMPController.h"
19 
20 namespace armarx::control::deprecated_njoint_mp_controller::task_space
21 {
22  NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController>
23  registrationControllerNJointTaskSpaceImpedanceDMPController(
24  "NJointTaskSpaceImpedanceDMPController");
25 
26  NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController(
27  const RobotUnitPtr& robotUnit,
28  const armarx::NJointControllerConfigPtr& config,
29  const VirtualRobot::RobotPtr&)
30  {
31  ARMARX_TRACE;
32  ARMARX_INFO << "creating impedance dmp controller";
33  cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
34  ARMARX_CHECK_NOT_NULL(cfg);
35  useSynchronizedRtRobot();
36  rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
37  ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
38  ARMARX_INFO << "1";
39  for (size_t i = 0; i < rns->getSize(); ++i)
40  {
41  std::string jointName = rns->getNode(i)->getName();
42  jointNames.push_back(jointName);
43  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
44  const SensorValueBase* sv = useSensorValue(jointName);
45  targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
46  const SensorValue1DoFActuatorVelocity* velocitySensor =
47  sv->asA<SensorValue1DoFActuatorVelocity>();
48  const SensorValue1DoFActuatorPosition* positionSensor =
49  sv->asA<SensorValue1DoFActuatorPosition>();
50 
51  if (!velocitySensor)
52  {
53  ARMARX_WARNING << "No velocitySensor available for " << jointName;
54  }
55  if (!positionSensor)
56  {
57  ARMARX_WARNING << "No positionSensor available for " << jointName;
58  }
59 
60  velocitySensors.push_back(velocitySensor);
61  positionSensors.push_back(positionSensor);
62  };
63  const SensorValueBase* svlf =
64  robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
65  forceSensor = svlf->asA<SensorValueForceTorque>();
66 
67  ARMARX_TRACE;
68  forceOffset.setZero();
69  filteredForce.setZero();
70  filteredForceInRoot.setZero();
71  ARMARX_INFO << cfg->forceThreshold;
72  forceThreshold.reinitAllBuffers(cfg->forceThreshold);
73  tcp = rns->getTCP();
74  ik.reset(new VirtualRobot::DifferentialIK(
75  rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
76  ik->setDampedSvdLambda(0.0001);
77 
78  ARMARX_TRACE;
79  numOfJoints = targets.size();
80  // set DMP
81  tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
82  taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
83  taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
84  taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
85  taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
86  taskSpaceDMPConfig.DMPAmplitude = 1.0;
87  taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
88  taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
89  taskSpaceDMPConfig.phaseStopParas.Kpos = 0;
90  taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
91  taskSpaceDMPConfig.phaseStopParas.Kori = 0;
92  taskSpaceDMPConfig.phaseStopParas.Dori = 0;
93  taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
94  taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
95  taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
96 
97  dmpCtrl.reset(
98  new tsvmp::TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
99  finished = false;
100 
101  useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
102  nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
103 
104  isNullSpaceJointDMPLearned = false;
105 
106  Eigen::VectorXf nullspaceValues(targets.size());
107 
108  ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
109 
110  for (size_t i = 0; i < targets.size(); ++i)
111  {
112  nullspaceValues(i) = cfg->defaultNullSpaceJointValues.at(i);
113  }
114  defaultNullSpaceJointValues.reinitAllBuffers(nullspaceValues);
115 
116  ARMARX_TRACE;
117  Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
118  Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
119  Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
120  Eigen::Vector3f dori(cfg->Dori[0], cfg->Dori[1], cfg->Dori[2]);
121  Eigen::VectorXf knull(targets.size());
122  Eigen::VectorXf dnull(targets.size());
123 
124  ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
125  ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
126 
127  for (size_t i = 0; i < targets.size(); ++i)
128  {
129  knull(i) = cfg->Knull.at(i);
130  dnull(i) = cfg->Dnull.at(i);
131  }
132 
133  CtrlParams initParams = {kpos, dpos, kori, dori, knull, dnull};
134  ctrlParams.reinitAllBuffers(initParams);
135 
136  torqueLimit = cfg->torqueLimit;
137  timeDuration = cfg->timeDuration;
138 
139  NJointTaskSpaceImpedanceDMPControllerInterfaceData initInterfaceData;
140  initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
141  interfaceData.reinitAllBuffers(initInterfaceData);
142 
143  NJointTaskSpaceImpedanceDMPControllerSensorData initControllerSensorData;
144  initControllerSensorData.currentPose = Eigen::Matrix4f::Identity();
145  initControllerSensorData.currentTime = 0;
146  initControllerSensorData.deltaT = 0;
147  initControllerSensorData.currentTwist.setZero();
148  controllerSensorData.reinitAllBuffers(initControllerSensorData);
149 
150  firstRun = true;
151  useForceStop = false;
152 
153  ARMARX_INFO << "Finished controller constructor ";
154  }
155 
156  std::string
157  NJointTaskSpaceImpedanceDMPController::getClassName(const Ice::Current&) const
158  {
159  return "NJointTaskSpaceImpedanceDMPController";
160  }
161 
162  void
163  NJointTaskSpaceImpedanceDMPController::rtPreActivateController()
164  {
165  ARMARX_TRACE;
166  NJointTaskSpaceImpedanceDMPControllerControlData initData;
167  initData.targetPose = tcp->getPoseInRootFrame();
168  initData.targetVel.resize(6);
169  initData.targetVel.setZero();
170  initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
171  reinitTripleBuffer(initData);
172  }
173 
174  void
175  NJointTaskSpaceImpedanceDMPController::controllerRun()
176  {
177  if (!dmpCtrl)
178  {
179  return;
180  }
181 
182  if (!controllerSensorData.updateReadBuffer())
183  {
184  return;
185  }
186 
187 
188  double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
189  Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
190  Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
191 
192  if (!started)
193  {
194  LockGuardType guard{controlDataMutex};
195  getWriterControlStruct().desiredNullSpaceJointValues =
196  defaultNullSpaceJointValues.getUpToDateReadBuffer();
197  getWriterControlStruct().targetVel.setZero(6);
198  getWriterControlStruct().targetPose = currentPose;
199  getWriterControlStruct().canVal = 1.0;
200  getWriterControlStruct().mpcFactor = 0.0;
201  writeControlStruct();
202  }
203  else
204  {
205  if (stopped)
206  {
207 
208  LockGuardType guard{controlDataMutex};
209  getWriterControlStruct().desiredNullSpaceJointValues =
210  defaultNullSpaceJointValues.getUpToDateReadBuffer();
211  getWriterControlStruct().targetVel.setZero(6);
212  getWriterControlStruct().targetPose = oldPose;
213  getWriterControlStruct().canVal = dmpCtrl->canVal;
214  getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
215  writeControlStruct();
216  }
217  else
218  {
219  if (dmpCtrl->canVal < 1e-8)
220  {
221  finished = true;
222  LockGuardType guard{controlDataMutex};
223  getWriterControlStruct().targetVel.setZero();
224  writeControlStruct();
225  return;
226  }
227 
228  dmpCtrl->flow(deltaT, currentPose, currentTwist);
229 
230  Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
231  if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
232  {
233  DMP::DVec targetJointState;
234  currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(
235  currentJointState,
236  dmpCtrl->canVal / timeDuration,
237  deltaT / timeDuration,
238  targetJointState);
239 
240  if (targetJointState.size() == jointNames.size())
241  {
242  for (size_t i = 0; i < targetJointState.size(); ++i)
243  {
244  desiredNullSpaceJointValues(i) = targetJointState[i];
245  }
246  }
247  else
248  {
249  desiredNullSpaceJointValues =
250  defaultNullSpaceJointValues.getUpToDateReadBuffer();
251  }
252  }
253  else
254  {
255  desiredNullSpaceJointValues =
256  defaultNullSpaceJointValues.getUpToDateReadBuffer();
257  }
258 
259  LockGuardType guard{controlDataMutex};
260  getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
261  getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
262  getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
263  getWriterControlStruct().canVal = dmpCtrl->canVal;
264  getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
265 
266  writeControlStruct();
267  }
268  }
269  }
270 
271  void
272  NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
273  const IceUtil::Time& timeSinceLastIteration)
274  {
275 
276  Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
277 
278  double deltaT = timeSinceLastIteration.toSecondsDouble();
279  Eigen::Matrix4f targetPose;
280  Eigen::VectorXf targetVel;
281  Eigen::VectorXf desiredNullSpaceJointValues;
282  if (firstRun)
283  {
284  firstRun = false;
285  targetPose = currentPose;
286  stopPose = currentPose;
287  targetVel.setZero(6);
288  desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
289  }
290  else
291  {
292  if (!started)
293  {
294  targetPose = stopPose;
295  targetVel.setZero(6);
296  desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
297  forceOffset =
298  (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
299  timeForCalibration = timeForCalibration + timeSinceLastIteration.toSecondsDouble();
300  }
301  else
302  {
303  targetPose = rtGetControlStruct().targetPose;
304  targetVel = rtGetControlStruct().targetVel;
305  desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
306 
307  if (useForceStop)
308  {
309  /* handle force stop */
310  filteredForce = (1 - cfg->forceFilter) * filteredForce +
311  cfg->forceFilter * (forceSensor->force - forceOffset);
312 
313  for (size_t i = 0; i < 3; ++i)
314  {
315  if (fabs(filteredForce(i)) > cfg->forceDeadZone)
316  {
317  filteredForce(i) -=
318  (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
319  }
320  else
321  {
322  filteredForce(i) = 0;
323  }
324  }
325  Eigen::Matrix4f forceFrameInRoot =
326  rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
327  filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
328 
329  for (size_t i = 0; i < 3; ++i)
330  {
331  if (fabs(filteredForceInRoot[i]) >
332  forceThreshold.getUpToDateReadBuffer()[i])
333  {
334  stopPose = currentPose;
335  targetVel.setZero(6);
336  desiredNullSpaceJointValues =
337  defaultNullSpaceJointValues.getUpToDateReadBuffer();
338  started = false;
339  break;
340  }
341  }
342  }
343  }
344  }
345 
346 
347  Eigen::MatrixXf jacobi =
348  ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
349 
350  Eigen::VectorXf qpos(positionSensors.size());
351  Eigen::VectorXf qvel(velocitySensors.size());
352  for (size_t i = 0; i < positionSensors.size(); ++i)
353  {
354  qpos(i) = positionSensors[i]->position;
355  qvel(i) = velocitySensors[i]->velocity;
356  }
357 
358  Eigen::VectorXf currentTwist = jacobi * qvel;
359 
360  controllerSensorData.getWriteBuffer().currentPose = currentPose;
361  controllerSensorData.getWriteBuffer().currentTwist = currentTwist;
362  controllerSensorData.getWriteBuffer().deltaT = deltaT;
363  controllerSensorData.getWriteBuffer().currentTime += deltaT;
364  controllerSensorData.commitWrite();
365 
366  interfaceData.getWriteBuffer().currentTcpPose = currentPose;
367  interfaceData.commitWrite();
368 
369  jacobi.block(0, 0, 3, numOfJoints) =
370  0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
371 
372  Eigen::Vector3f kpos = ctrlParams.getUpToDateReadBuffer().kpos;
373  Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
374  Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
375  Eigen::Vector3f dori = ctrlParams.getUpToDateReadBuffer().dori;
376  Eigen::VectorXf knull = ctrlParams.getUpToDateReadBuffer().knull;
377  Eigen::VectorXf dnull = ctrlParams.getUpToDateReadBuffer().dnull;
378 
379  Eigen::Vector6f jointControlWrench;
380  {
381  Eigen::Vector3f targetTCPLinearVelocity;
382  targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1),
383  0.001 * targetVel(2);
384  Eigen::Vector3f currentTCPLinearVelocity;
385  currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1),
386  0.001 * currentTwist(2);
387  Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
388  Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
389  Eigen::Vector3f tcpDesiredForce =
390  0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) +
391  dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
392 
393  Eigen::Vector3f currentTCPAngularVelocity;
394  currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
395  Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
396  Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
397  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
398  Eigen::Vector3f tcpDesiredTorque =
399  kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
400  jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
401  }
402 
403  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
404 
405  Eigen::VectorXf nullspaceTorque =
406  knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
407  Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
408  Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench +
409  (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
410 
411 
412  // torque limit
413  ARMARX_CHECK_EXPRESSION(!targets.empty());
414  ARMARX_CHECK_LESS(targets.size(), 1000);
415  for (size_t i = 0; i < targets.size(); ++i)
416  {
417  float desiredTorque = jointDesiredTorques(i);
418 
419  if (isnan(desiredTorque))
420  {
421  desiredTorque = 0;
422  }
423 
424  desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
425  desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
426 
427  debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] =
428  jointDesiredTorques(i);
429  debugOutputData.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] =
430  desiredNullSpaceJointValues(i);
431 
432  targets.at(i)->torque = desiredTorque;
433  if (!targets.at(i)->isValid())
434  {
435  ARMARX_INFO << deactivateSpam(1)
436  << "Torque controller target is invalid - setting to zero! set value: "
437  << targets.at(i)->torque;
438  targets.at(i)->torque = 0.0f;
439  }
440  }
441 
442 
443  debugOutputData.getWriteBuffer().forceDesired_x = jointControlWrench(0);
444  debugOutputData.getWriteBuffer().forceDesired_y = jointControlWrench(1);
445  debugOutputData.getWriteBuffer().forceDesired_z = jointControlWrench(2);
446  debugOutputData.getWriteBuffer().forceDesired_rx = jointControlWrench(3);
447  debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
448  debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
449 
450  // debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
451  // debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
452 
453  debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
454  debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
455  debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
456  VirtualRobot::MathTools::Quaternion targetQuat =
457  VirtualRobot::MathTools::eigen4f2quat(targetPose);
458  debugOutputData.getWriteBuffer().targetPose_qw = targetQuat.w;
459  debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
460  debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
461  debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
462  debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
463 
464  debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
465  debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
466  debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
467  VirtualRobot::MathTools::Quaternion currentQuat =
468  VirtualRobot::MathTools::eigen4f2quat(currentPose);
469  debugOutputData.getWriteBuffer().currentPose_qw = currentQuat.w;
470  debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
471  debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
472  debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
473  debugOutputData.getWriteBuffer().deltaT = deltaT;
474 
475  debugOutputData.getWriteBuffer().currentKpos_x = kpos.x();
476  debugOutputData.getWriteBuffer().currentKpos_y = kpos.y();
477  debugOutputData.getWriteBuffer().currentKpos_z = kpos.z();
478  debugOutputData.getWriteBuffer().currentKori_x = kori.x();
479  debugOutputData.getWriteBuffer().currentKori_y = kori.y();
480  debugOutputData.getWriteBuffer().currentKori_z = kori.z();
481  debugOutputData.getWriteBuffer().currentKnull_x = knull.x();
482  debugOutputData.getWriteBuffer().currentKnull_y = knull.y();
483  debugOutputData.getWriteBuffer().currentKnull_z = knull.z();
484 
485  debugOutputData.getWriteBuffer().currentDpos_x = dpos.x();
486  debugOutputData.getWriteBuffer().currentDpos_y = dpos.y();
487  debugOutputData.getWriteBuffer().currentDpos_z = dpos.z();
488  debugOutputData.getWriteBuffer().currentDori_x = dori.x();
489  debugOutputData.getWriteBuffer().currentDori_y = dori.y();
490  debugOutputData.getWriteBuffer().currentDori_z = dori.z();
491  debugOutputData.getWriteBuffer().currentDnull_x = dnull.x();
492  debugOutputData.getWriteBuffer().currentDnull_y = dnull.y();
493  debugOutputData.getWriteBuffer().currentDnull_z = dnull.z();
494 
495  debugOutputData.commitWrite();
496  }
497 
498  void
499  NJointTaskSpaceImpedanceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
500  const Ice::Current&)
501  {
502  dmpCtrl->learnDMPFromFiles(fileNames);
503  ARMARX_INFO << "Learned DMP ... ";
504  }
505 
506  void
507  NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u,
508  const Ice::DoubleSeq& viapoint,
509  const Ice::Current&)
510  {
511  LockGuardType guard(controllerMutex);
512  ARMARX_INFO << "setting via points ";
513  dmpCtrl->setViaPose(u, viapoint);
514  }
515 
516  void
517  NJointTaskSpaceImpedanceDMPController::setGoals(const Ice::DoubleSeq& goals,
518  const Ice::Current& ice)
519  {
520  dmpCtrl->setGoalPoseVec(goals);
521  }
522 
523  void
524  NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName,
525  const Ice::FloatSeq& currentJVS,
526  const Ice::Current&)
527  {
528  DMP::Vec<DMP::SampledTrajectoryV2> trajs;
529  DMP::DVec ratios;
530  DMP::SampledTrajectoryV2 traj;
531  traj.readFromCSVFile(fileName);
532  traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
533  if (traj.dim() != jointNames.size())
534  {
535  isNullSpaceJointDMPLearned = false;
536  return;
537  }
538 
539  DMP::DVec goal;
540  goal.resize(traj.dim());
541  currentJointState.resize(traj.dim());
542 
543  for (size_t i = 0; i < goal.size(); ++i)
544  {
545  goal.at(i) = traj.rbegin()->getPosition(i);
546  currentJointState.at(i).pos = currentJVS.at(i);
547  currentJointState.at(i).vel = 0;
548  }
549 
550  trajs.push_back(traj);
551  nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
552 
553  // prepare exeuction of joint dmp
554  nullSpaceJointDMPPtr->prepareExecution(goal, currentJointState, 1.0, 1.0);
555  ARMARX_INFO << "prepared nullspace joint dmp";
556  isNullSpaceJointDMPLearned = true;
557  }
558 
559  void
560  NJointTaskSpaceImpedanceDMPController::resetDMP(const Ice::Current&)
561  {
562  if (started)
563  {
564  ARMARX_INFO << "Cannot reset running DMP";
565  }
566  firstRun = true;
567  }
568 
569  void
570  NJointTaskSpaceImpedanceDMPController::stopDMP(const Ice::Current&)
571  {
572  oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
573  stopped = true;
574  prematurely_stopped = true;
575  }
576 
577  void
578  NJointTaskSpaceImpedanceDMPController::resumeDMP(const Ice::Current&)
579  {
580  stopped = false;
581  }
582 
583  void
584  NJointTaskSpaceImpedanceDMPController::setUseNullSpaceJointDMP(bool enable, const Ice::Current&)
585  {
586  useNullSpaceJointDMP = enable;
587  }
588 
589  void
590  NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals,
591  Ice::Double timeDuration,
592  const Ice::Current&)
593  {
594  dmpCtrl->canVal = timeDuration;
595  dmpCtrl->config.motionTimeDuration = timeDuration;
596 
597  runDMP(goals);
598  }
599 
600  void
601  NJointTaskSpaceImpedanceDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
602  {
603  firstRun = true;
604  prematurely_stopped = false;
605  timeForCalibration = 0;
606  started = false;
607 
608  while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
609  {
610  if (prematurely_stopped.load())
611  {
612  ARMARX_WARNING << "StopDMP has been prematurely called; aborting runDMP";
613  return;
614  }
615  usleep(100);
616  }
617 
618  while (!interfaceData.updateReadBuffer())
619  {
620  usleep(100);
621  }
622 
623  Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
624  dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
625 
626  finished = false;
627 
628  if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
629  {
630  ARMARX_INFO << "Using Null Space Joint DMP";
631  }
632 
633  if (prematurely_stopped.load())
634  {
635  ARMARX_WARNING << "StopDMP has been prematurely called; aborting runDMP";
636  return;
637  }
638 
639  started = true;
640  stopped = false;
641  // controllerTask->start();
642  }
643 
644  void
645  NJointTaskSpaceImpedanceDMPController::onPublish(const SensorAndControl&,
646  const DebugDrawerInterfacePrx&,
647  const DebugObserverInterfacePrx& debugObs)
648  {
649  StringVariantBaseMap datafields;
650  auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
651  for (auto& pair : values)
652  {
653  datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
654  }
655 
656  auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
657  for (auto& pair : values_null)
658  {
659  datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
660  }
661 
662  datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
663  datafields["mpcfactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcfactor);
664  datafields["targetPose_x"] =
665  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
666  datafields["targetPose_y"] =
667  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
668  datafields["targetPose_z"] =
669  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
670  datafields["targetPose_qw"] =
671  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qw);
672  datafields["targetPose_qx"] =
673  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qx);
674  datafields["targetPose_qy"] =
675  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qy);
676  datafields["targetPose_qz"] =
677  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qz);
678 
679  datafields["currentPose_x"] =
680  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_x);
681  datafields["currentPose_y"] =
682  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_y);
683  datafields["currentPose_z"] =
684  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_z);
685  datafields["currentPose_qw"] =
686  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qw);
687  datafields["currentPose_qx"] =
688  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qx);
689  datafields["currentPose_qy"] =
690  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
691  datafields["currentPose_qz"] =
692  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
693 
694  datafields["currentKpos_x"] =
695  new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_x);
696  datafields["currentKpos_y"] =
697  new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_y);
698  datafields["currentKpos_z"] =
699  new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_z);
700  datafields["currentKori_x"] =
701  new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_x);
702  datafields["currentKori_y"] =
703  new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_y);
704  datafields["currentKori_z"] =
705  new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_z);
706  datafields["currentKnull_x"] =
707  new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_x);
708  datafields["currentKnull_y"] =
709  new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_y);
710  datafields["currentKnull_z"] =
711  new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_z);
712 
713  datafields["currentDpos_x"] =
714  new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_x);
715  datafields["currentDpos_y"] =
716  new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_y);
717  datafields["currentDpos_z"] =
718  new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_z);
719  datafields["currentDori_x"] =
720  new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_x);
721  datafields["currentDori_y"] =
722  new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_y);
723  datafields["currentDori_z"] =
724  new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_z);
725  datafields["currentDnull_x"] =
726  new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_x);
727  datafields["currentDnull_y"] =
728  new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_y);
729  datafields["currentDnull_z"] =
730  new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_z);
731 
732  datafields["forceDesired_x"] =
733  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
734  datafields["forceDesired_y"] =
735  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
736  datafields["forceDesired_z"] =
737  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
738  datafields["forceDesired_rx"] =
739  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rx);
740  datafields["forceDesired_ry"] =
741  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
742  datafields["forceDesired_rz"] =
743  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
744 
745  datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
746 
747  std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
748  debugObs->setDebugChannel(channelName, datafields);
749  }
750 
751  void
752  NJointTaskSpaceImpedanceDMPController::onInitNJointController()
753  {
754  ARMARX_INFO << "init ...";
755  // controllerTask = new PeriodicTask<NJointTaskSpaceImpedanceDMPController>(this, &NJointTaskSpaceImpedanceDMPController::controllerRun, 1);
756  runTask("NJointTaskSpaceImpedanceDMPController",
757  [&]
758  {
759  CycleUtil c(1);
760  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
761  while (getState() == eManagedIceObjectStarted)
762  {
763  if (isControllerActive())
764  {
765  controllerRun();
766  }
767  c.waitForCycleDuration();
768  }
769  });
770  }
771 
772  void
773  NJointTaskSpaceImpedanceDMPController::onDisconnectNJointController()
774  {
775  // controllerTask->stop();
776  }
777 
778  void
779  NJointTaskSpaceImpedanceDMPController::setMPWeights(const DoubleSeqSeq& weights,
780  const Ice::Current&)
781  {
782  dmpCtrl->setWeights(weights);
783  }
784 
785  DoubleSeqSeq
786  NJointTaskSpaceImpedanceDMPController::getMPWeights(const Ice::Current&)
787  {
788  DMP::DVec2d res = dmpCtrl->getWeights();
789  DoubleSeqSeq resvec;
790  for (size_t i = 0; i < res.size(); ++i)
791  {
792  std::vector<double> cvec;
793  for (size_t j = 0; j < res[i].size(); ++j)
794  {
795  cvec.push_back(res[i][j]);
796  }
797  resvec.push_back(cvec);
798  }
799 
800  return resvec;
801  }
802 
803  void
804  NJointTaskSpaceImpedanceDMPController::removeAllViaPoints(const Ice::Current&)
805  {
806  LockGuardType guard{controllerMutex};
807  ARMARX_INFO << "setting via points ";
808  dmpCtrl->removeAllViaPoints();
809  }
810 
811  void
812  NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Eigen::Vector3f& kd,
813  const Ice::Current&)
814  {
815  ARMARX_CHECK_EQUAL(kd.size(), 3ul);
816  ARMARX_INFO << "set linear kd " << VAROUT(kd);
817  LockGuardType guard(controllerMutex);
818  ctrlParams.getWriteBuffer().dpos = kd;
819  ctrlParams.commitWrite();
820  }
821 
822  void
823  NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Eigen::Vector3f& kp,
824  const Ice::Current&)
825  {
826  ARMARX_CHECK_EQUAL(kp.size(), 3);
827  ARMARX_INFO << "set linear kp " << VAROUT(kp);
828  LockGuardType guard(controllerMutex);
829  ctrlParams.getWriteBuffer().kpos = kp;
830  ctrlParams.commitWrite();
831  }
832 
833  void
834  NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Eigen::Vector3f& kd,
835  const Ice::Current&)
836  {
837  ARMARX_CHECK_EQUAL(kd.size(), 3);
838  ARMARX_INFO << "set angular kd " << VAROUT(kd);
839  LockGuardType guard(controllerMutex);
840  ctrlParams.getWriteBuffer().dori = kd;
841  ctrlParams.commitWrite();
842  }
843 
844  void
845  NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Eigen::Vector3f& kp,
846  const Ice::Current&)
847  {
848  ARMARX_CHECK_EQUAL(kp.size(), 3);
849  ARMARX_INFO << "set angular kp " << VAROUT(kp);
850  LockGuardType guard(controllerMutex);
851  ctrlParams.getWriteBuffer().kori = kp;
852  ctrlParams.commitWrite();
853  }
854 
855  void
856  NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(const Eigen::VectorXf& kd,
857  const Ice::Current&)
858  {
859  ARMARX_CHECK_EQUAL(kd.size(), static_cast<long>(targets.size()));
860  ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
861  LockGuardType guard(controllerMutex);
862  ctrlParams.getWriteBuffer().dnull = kd;
863  ctrlParams.commitWrite();
864  }
865 
866  void
867  NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(const Eigen::VectorXf& kp,
868  const Ice::Current&)
869  {
870  ARMARX_CHECK_EQUAL(kp.size(), static_cast<long>(targets.size()));
871  ARMARX_INFO << "set linear kp " << VAROUT(kp);
872  LockGuardType guard(controllerMutex);
873  ctrlParams.getWriteBuffer().knull = kp;
874  ctrlParams.commitWrite();
875  }
876 
877  void
878  NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(
879  const Eigen::VectorXf& jointValues,
880  const Ice::Current&)
881  {
882  ARMARX_CHECK_EQUAL(jointValues.size(), static_cast<long>(targets.size()));
883  defaultNullSpaceJointValues.getWriteBuffer() = jointValues;
884  defaultNullSpaceJointValues.commitWrite();
885  }
886 
887  Ice::Double
888  NJointTaskSpaceImpedanceDMPController::getVirtualTime(const Ice::Current&)
889  {
890  return dmpCtrl->canVal;
891  }
892 
893 
894 } // namespace armarx::control::deprecated_njoint_mp_controller::task_space
armarx::NJointControllerWithTripleBuffer< NJointTaskSpaceImpedanceDMPControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const NJointTaskSpaceImpedanceDMPControllerControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::NJointTaskSpaceImpedanceDMPControllerInterface::getVirtualTime
double getVirtualTime()
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::tsvmp::PhaseStopParams::Kori
float Kori
Definition: TaskSpaceVMP.h:47
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dpos
float Dpos
Definition: TaskSpaceVMP.h:46
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::NJointControllerWithTripleBuffer< NJointTaskSpaceImpedanceDMPControllerControlData >::rtGetControlStruct
const NJointTaskSpaceImpedanceDMPControllerControlData & 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::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
armarx::SensorValueBase
The SensorValueBase class.
Definition: SensorValueBase.h:40
armarx::NJointTaskSpaceImpedanceDMPControllerInterface::resumeDMP
void resumeDMP()
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition: ControlTargetBase.h:47
armarx::WriteBufferedTripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:326
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:507
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
Definition: DeprecatedNJointPeriodicTSDMPCompliantController.cpp:6
armarx::WriteBufferedTripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:321
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
SensorValueForceTorque.h
ARMARX_CHECK_LESS
#define ARMARX_CHECK_LESS(lhs, rhs)
This macro evaluates whether lhs is less (<) than rhs and if it turns out to be false it will throw a...
Definition: ExpressionException.h:102
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig
Definition: TaskSpaceVMP.h:52
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::phaseStopParas
PhaseStopParams phaseStopParas
Definition: TaskSpaceVMP.h:60
armarx::NJointControllerWithTripleBuffer< NJointTaskSpaceImpedanceDMPControllerControlData >::getWriterControlStruct
NJointTaskSpaceImpedanceDMPControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition: TaskSpaceVMP.h:41
armarx::NJointControllerWithTripleBuffer< NJointTaskSpaceImpedanceDMPControllerControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:517
armarx::SensorValueForceTorque::force
Eigen::Vector3f force
Definition: SensorValueForceTorque.h:35
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::motionTimeDuration
float motionTimeDuration
Definition: TaskSpaceVMP.h:59
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::learnDMPFromFiles
void learnDMPFromFiles(const Ice::StringSeq &fileNames, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:499
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:845
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::backDist
float backDist
Definition: TaskSpaceVMP.h:42
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::maxValue
float maxValue
Definition: TaskSpaceVMP.h:43
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: NJointTaskSpaceImpedanceDMPController.cpp:272
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:177
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:919
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::setMPWeights
void setMPWeights(const DoubleSeqSeq &weights, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:779
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dori
float Dori
Definition: TaskSpaceVMP.h:48
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
NJointTaskSpaceImpedanceDMPController.h
normalizeTimestamps
TrajectoryPtr normalizeTimestamps(TrajectoryPtr t, float maxTransVel, float maxRotVel)
Definition: PlayPlatformTrajectory.cpp:76
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp
void setAngularVelocityKp(const Eigen::Vector3f &kp, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:845
armarx::NJointTaskSpaceImpedanceDMPControllerInterface::getMPWeights
DoubleSeqSeq getMPWeights()
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::NJointTaskSpaceImpedanceDMPControllerInterface::resetDMP
void resetDMP()
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPControllerControlData::desiredNullSpaceJointValues
Eigen::VectorXf desiredNullSpaceJointValues
Definition: NJointTaskSpaceImpedanceDMPController.h:39
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::runDMPWithTime
void runDMPWithTime(const Ice::DoubleSeq &goals, Ice::Double timeDuration, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:590
armarx::NJointControllerWithTripleBuffer< NJointTaskSpaceImpedanceDMPControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPControllerControlData
Definition: NJointTaskSpaceImpedanceDMPController.h:34
ArmarXObjectScheduler.h
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::onInitNJointController
void onInitNJointController() override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:752
ControlTarget1DoFActuator.h
armarx::WriteBufferedTripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:280
armarx::NJointControllerWithTripleBuffer< NJointTaskSpaceImpedanceDMPControllerControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPControllerControlData::targetPose
Eigen::Matrix4f targetPose
Definition: NJointTaskSpaceImpedanceDMPController.h:38
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd
void setLinearVelocityKd(const Eigen::Vector3f &kd, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:812
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition: TaskSpaceVMP.h:57
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::slop
float slop
Definition: TaskSpaceVMP.h:44
TaskSpaceVMP.h
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition: TaskSpaceVMP.h:45
CycleUtil.h
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd
void setAngularVelocityKd(const Eigen::Vector3f &kd, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:834
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles
void learnJointDMPFromFiles(const std::string &fileName, const Ice::FloatSeq &currentJVS, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:524
NJointController.h
armarx::WriteBufferedTripleBuffer::reinitAllBuffers
void reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:332
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::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::setUseNullSpaceJointDMP
void setUseNullSpaceJointDMP(bool enable, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:584
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPControllerControlData::targetVel
Eigen::VectorXf targetVel
Definition: NJointTaskSpaceImpedanceDMPController.h:37
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:157
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::controllerRun
void controllerRun()
Definition: NJointTaskSpaceImpedanceDMPController.cpp:175
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
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::task_space::NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp
void setLinearVelocityKp(const Eigen::Vector3f &kp, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:823
armarx::NJointTaskSpaceImpedanceDMPControllerInterface::removeAllViaPoints
void removeAllViaPoints()
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition: TaskSpaceVMP.h:55
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:92
Eigen::Matrix< float, 6, 1 >
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition: TaskSpaceVMP.h:49
armarx::WriteBufferedTripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:291
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::onDisconnectNJointController
void onDisconnectNJointController() override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:773
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::WriteBufferedTripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:296
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::runDMP
void runDMP(const Ice::DoubleSeq &goals, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:601
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd
void setNullspaceVelocityKd(const Eigen::VectorXf &kd, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:856
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp
void setNullspaceVelocityKp(const Eigen::VectorXf &kp, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:867
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition: NJointTaskSpaceImpedanceDMPController.cpp:163
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:645
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition: TaskSpaceVMP.h:83
armarx::NJointTaskSpaceImpedanceDMPControllerInterface::stopDMP
void stopDMP()
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:731
armarx::control::deprecated_njoint_mp_controller::task_space::registrationControllerNJointTaskSpaceImpedanceDMPController
NJointControllerRegistration< NJointTaskSpaceImpedanceDMPController > registrationControllerNJointTaskSpaceImpedanceDMPController("NJointTaskSpaceImpedanceDMPController")
armarx::control::deprecated_njoint_mp_controller::task_space::NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues
void setDefaultNullSpaceJointValues(const Eigen::VectorXf &jointValues, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceDMPController.cpp:878
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::task_space::NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController
NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointTaskSpaceImpedanceDMPController.cpp:26