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