DeprecatedNJointTaskSpaceImpedanceDMPController.cpp
Go to the documentation of this file.
1 #include "DeprecatedNJointTaskSpaceImpedanceDMPController.h"
2 
3 #include <VirtualRobot/IK/DifferentialIK.h>
4 #include <VirtualRobot/MathTools.h>
5 #include <VirtualRobot/Nodes/RobotNode.h>
6 #include <VirtualRobot/Nodes/Sensor.h>
7 #include <VirtualRobot/Robot.h>
8 #include <VirtualRobot/RobotNodeSet.h>
9 
10 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
11 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
12 #include <ArmarXCore/core/logging/Logging.h>
13 
14 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
15 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
16 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
17 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
18 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
19 
20 #include <dmp/representation/dmp/umitsmp.h>
21 
22 namespace armarx::control::deprecated_njoint_mp_controller::task_space
23 {
24  NJointControllerRegistration<DeprecatedNJointTaskSpaceImpedanceDMPController>
25  registrationControllerDeprecatedNJointTaskSpaceImpedanceDMPController(
26  "DeprecatedNJointTaskSpaceImpedanceDMPController");
27 
28  DeprecatedNJointTaskSpaceImpedanceDMPController::
29  DeprecatedNJointTaskSpaceImpedanceDMPController(
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 = DeprecatedNJointTaskSpaceImpedanceDMPControllerConfigPtr::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  DeprecatedNJointTaskSpaceImpedanceDMPControllerInterfaceData initInterfaceData;
143  initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
144  interfaceData.reinitAllBuffers(initInterfaceData);
145 
146  DeprecatedNJointTaskSpaceImpedanceDMPControllerSensorData 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  DeprecatedNJointTaskSpaceImpedanceDMPController::getClassName(const Ice::Current&) const
161  {
162  return "DeprecatedNJointTaskSpaceImpedanceDMPController";
163  }
164 
165  void
166  DeprecatedNJointTaskSpaceImpedanceDMPController::rtPreActivateController()
167  {
168  ARMARX_TRACE;
169  DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData 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  DeprecatedNJointTaskSpaceImpedanceDMPController::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  started = false;
226  LockGuardType guard{controlDataMutex};
227  getWriterControlStruct().targetVel.setZero();
228  writeControlStruct();
229  return;
230  }
231 
232  dmpCtrl->flow(deltaT, currentPose, currentTwist);
233 
234  Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
235  if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
236  {
237  DMP::DVec targetJointState;
238  currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(
239  currentJointState,
240  dmpCtrl->canVal / timeDuration,
241  deltaT / timeDuration,
242  targetJointState);
243 
244  if (targetJointState.size() == jointNames.size())
245  {
246  for (size_t i = 0; i < targetJointState.size(); ++i)
247  {
248  desiredNullSpaceJointValues(i) = targetJointState[i];
249  }
250  }
251  else
252  {
253  desiredNullSpaceJointValues =
254  defaultNullSpaceJointValues.getUpToDateReadBuffer();
255  }
256  }
257  else
258  {
259  desiredNullSpaceJointValues =
260  defaultNullSpaceJointValues.getUpToDateReadBuffer();
261  }
262 
263  LockGuardType guard{controlDataMutex};
264  getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
265  getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
266  getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
267  getWriterControlStruct().canVal = dmpCtrl->canVal;
268  getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
269 
270  writeControlStruct();
271  }
272  }
273  }
274 
275  void
276  DeprecatedNJointTaskSpaceImpedanceDMPController::rtRun(
277  const IceUtil::Time& sensorValuesTimestamp,
278  const IceUtil::Time& timeSinceLastIteration)
279  {
280 
281  Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
282 
283  double deltaT = timeSinceLastIteration.toSecondsDouble();
284  Eigen::Matrix4f targetPose;
285  Eigen::VectorXf targetVel;
286  Eigen::VectorXf desiredNullSpaceJointValues;
287  if (firstRun)
288  {
289  ARMARX_IMPORTANT << "impedance control first run";
290  firstRun = false;
291  targetPose = currentPose;
292  stopPose = currentPose;
293  targetVel.setZero(6);
294  desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
295  }
296  else
297  {
298  if (!started)
299  {
300  targetPose = stopPose;
301  targetVel.setZero(6);
302  desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
303  forceOffset =
304  (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
305  timeForCalibration = timeForCalibration + timeSinceLastIteration.toSecondsDouble();
306  }
307  else
308  {
309  targetPose = rtGetControlStruct().targetPose;
310  targetVel = rtGetControlStruct().targetVel;
311  desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
312  stopPose = targetPose;
313 
314  if (useForceStop)
315  {
316  /* handle force stop */
317  filteredForce = (1 - cfg->forceFilter) * filteredForce +
318  cfg->forceFilter * (forceSensor->force - forceOffset);
319 
320  for (size_t i = 0; i < 3; ++i)
321  {
322  if (fabs(filteredForce(i)) > cfg->forceDeadZone)
323  {
324  filteredForce(i) -=
325  (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
326  }
327  else
328  {
329  filteredForce(i) = 0;
330  }
331  }
332  Eigen::Matrix4f forceFrameInRoot = rtGetRobot()
333  ->getSensor(cfg->forceSensorName)
334  ->getRobotNode()
335  ->getPoseInRootFrame();
336  filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
337 
338  for (size_t i = 0; i < 3; ++i)
339  {
340  if (fabs(filteredForceInRoot[i]) >
341  forceThreshold.getUpToDateReadBuffer()[i])
342  {
343  stopPose = currentPose;
344  targetVel.setZero(6);
345  desiredNullSpaceJointValues =
346  defaultNullSpaceJointValues.getUpToDateReadBuffer();
347  started = false;
348  break;
349  }
350  }
351  }
352  }
353  }
354 
355 
356  Eigen::MatrixXf jacobi =
357  ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
358 
359  Eigen::VectorXf qpos(positionSensors.size());
360  Eigen::VectorXf qvel(velocitySensors.size());
361  for (size_t i = 0; i < positionSensors.size(); ++i)
362  {
363  qpos(i) = positionSensors[i]->position;
364  qvel(i) = velocitySensors[i]->velocity;
365  }
366 
367  Eigen::VectorXf currentTwist = jacobi * qvel;
368 
369  controllerSensorData.getWriteBuffer().currentPose = currentPose;
370  controllerSensorData.getWriteBuffer().currentTwist = currentTwist;
371  controllerSensorData.getWriteBuffer().deltaT = deltaT;
372  controllerSensorData.getWriteBuffer().currentTime += deltaT;
373  controllerSensorData.commitWrite();
374 
375  interfaceData.getWriteBuffer().currentTcpPose = currentPose;
376  interfaceData.commitWrite();
377 
378  jacobi.block(0, 0, 3, numOfJoints) =
379  0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
380 
381  Eigen::Vector3f kpos = ctrlParams.getUpToDateReadBuffer().kpos;
382  Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
383  Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
384  Eigen::Vector3f dori = ctrlParams.getUpToDateReadBuffer().dori;
385  Eigen::VectorXf knull = ctrlParams.getUpToDateReadBuffer().knull;
386  Eigen::VectorXf dnull = ctrlParams.getUpToDateReadBuffer().dnull;
387 
388  Eigen::Vector6f jointControlWrench;
389  {
390  Eigen::Vector3f targetTCPLinearVelocity;
391  targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1),
392  0.001 * targetVel(2);
393  Eigen::Vector3f currentTCPLinearVelocity;
394  currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1),
395  0.001 * currentTwist(2);
396  Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
397  Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
398  Eigen::Vector3f tcpDesiredForce =
399  0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) +
400  dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
401 
402  Eigen::Vector3f currentTCPAngularVelocity;
403  currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
404  Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
405  Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
406  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
407  Eigen::Vector3f tcpDesiredTorque =
408  kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
409  jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
410  }
411 
412  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
413 
414  Eigen::VectorXf nullspaceTorque =
415  knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
416  Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
417  Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench +
418  (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
419 
420 
421  // torque limit
422  ARMARX_CHECK_EXPRESSION(!targets.empty());
423  ARMARX_CHECK_LESS(targets.size(), 1000);
424  for (size_t i = 0; i < targets.size(); ++i)
425  {
426  float desiredTorque = jointDesiredTorques(i);
427 
428  if (isnan(desiredTorque))
429  {
430  desiredTorque = 0;
431  }
432 
433  desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
434  desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
435 
436  debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] =
437  jointDesiredTorques(i);
438  debugOutputData.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] =
439  desiredNullSpaceJointValues(i);
440 
441  targets.at(i)->torque = desiredTorque;
442  if (!targets.at(i)->isValid())
443  {
444  ARMARX_INFO << deactivateSpam(1)
445  << "Torque controller target is invalid - setting to zero! set value: "
446  << targets.at(i)->torque;
447  targets.at(i)->torque = 0.0f;
448  }
449  }
450 
451 
452  debugOutputData.getWriteBuffer().forceDesired_x = jointControlWrench(0);
453  debugOutputData.getWriteBuffer().forceDesired_y = jointControlWrench(1);
454  debugOutputData.getWriteBuffer().forceDesired_z = jointControlWrench(2);
455  debugOutputData.getWriteBuffer().forceDesired_rx = jointControlWrench(3);
456  debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
457  debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
458 
459  // debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
460  // debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
461 
462  debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
463  debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
464  debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
465  VirtualRobot::MathTools::Quaternion targetQuat =
466  VirtualRobot::MathTools::eigen4f2quat(targetPose);
467  debugOutputData.getWriteBuffer().targetPose_qw = targetQuat.w;
468  debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
469  debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
470  debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
471  debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
472 
473  debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
474  debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
475  debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
476  VirtualRobot::MathTools::Quaternion currentQuat =
477  VirtualRobot::MathTools::eigen4f2quat(currentPose);
478  debugOutputData.getWriteBuffer().currentPose_qw = currentQuat.w;
479  debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
480  debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
481  debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
482  debugOutputData.getWriteBuffer().deltaT = deltaT;
483 
484  debugOutputData.getWriteBuffer().currentKpos_x = kpos.x();
485  debugOutputData.getWriteBuffer().currentKpos_y = kpos.y();
486  debugOutputData.getWriteBuffer().currentKpos_z = kpos.z();
487  debugOutputData.getWriteBuffer().currentKori_x = kori.x();
488  debugOutputData.getWriteBuffer().currentKori_y = kori.y();
489  debugOutputData.getWriteBuffer().currentKori_z = kori.z();
490  debugOutputData.getWriteBuffer().currentKnull_x = knull.x();
491  debugOutputData.getWriteBuffer().currentKnull_y = knull.y();
492  debugOutputData.getWriteBuffer().currentKnull_z = knull.z();
493 
494  debugOutputData.getWriteBuffer().currentDpos_x = dpos.x();
495  debugOutputData.getWriteBuffer().currentDpos_y = dpos.y();
496  debugOutputData.getWriteBuffer().currentDpos_z = dpos.z();
497  debugOutputData.getWriteBuffer().currentDori_x = dori.x();
498  debugOutputData.getWriteBuffer().currentDori_y = dori.y();
499  debugOutputData.getWriteBuffer().currentDori_z = dori.z();
500  debugOutputData.getWriteBuffer().currentDnull_x = dnull.x();
501  debugOutputData.getWriteBuffer().currentDnull_y = dnull.y();
502  debugOutputData.getWriteBuffer().currentDnull_z = dnull.z();
503 
504  debugOutputData.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
505 
506  debugOutputData.commitWrite();
507  }
508 
509  void
510  DeprecatedNJointTaskSpaceImpedanceDMPController::learnDMPFromFiles(
511  const Ice::StringSeq& fileNames,
512  const Ice::Current&)
513  {
514  dmpCtrl->learnDMPFromFiles(fileNames);
515  ARMARX_INFO << "Learned DMP ... ";
516  }
517 
518  void
519  DeprecatedNJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u,
520  const Ice::DoubleSeq& viapoint,
521  const Ice::Current&)
522  {
523  LockGuardType guard(controllerMutex);
524  ARMARX_INFO << "setting via points ";
525  dmpCtrl->setViaPose(u, viapoint);
526  }
527 
528  void
529  DeprecatedNJointTaskSpaceImpedanceDMPController::setGoals(const Ice::DoubleSeq& goals,
530  const Ice::Current& ice)
531  {
532  dmpCtrl->setGoalPoseVec(goals);
533  }
534 
535  void
536  DeprecatedNJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(
537  const std::string& fileName,
538  const Ice::FloatSeq& currentJVS,
539  const Ice::Current&)
540  {
541  DMP::Vec<DMP::SampledTrajectoryV2> trajs;
542  DMP::DVec ratios;
543  DMP::SampledTrajectoryV2 traj;
544  traj.readFromCSVFile(fileName);
545  traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
546  if (traj.dim() != jointNames.size())
547  {
548  isNullSpaceJointDMPLearned = false;
549  return;
550  }
551 
552  DMP::DVec goal;
553  goal.resize(traj.dim());
554  currentJointState.resize(traj.dim());
555 
556  for (size_t i = 0; i < goal.size(); ++i)
557  {
558  goal.at(i) = traj.rbegin()->getPosition(i);
559  currentJointState.at(i).pos = currentJVS.at(i);
560  currentJointState.at(i).vel = 0;
561  }
562 
563  trajs.push_back(traj);
564  nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
565 
566  // prepare exeuction of joint dmp
567  nullSpaceJointDMPPtr->prepareExecution(goal, currentJointState, 1.0, 1.0);
568  ARMARX_INFO << "prepared nullspace joint dmp";
569  isNullSpaceJointDMPLearned = true;
570  }
571 
572  void
573  DeprecatedNJointTaskSpaceImpedanceDMPController::resetDMP(const Ice::Current&)
574  {
575  if (started)
576  {
577  ARMARX_INFO << "Cannot reset running DMP";
578  }
579  firstRun = true;
580  }
581 
582  void
583  DeprecatedNJointTaskSpaceImpedanceDMPController::stopDMP(const Ice::Current&)
584  {
585  // oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
586  started = false;
587  stopped = true;
588  }
589 
590  std::string
591  DeprecatedNJointTaskSpaceImpedanceDMPController::getDMPAsString(const Ice::Current&)
592  {
593 
594  return dmpCtrl->saveDMPToString();
595  }
596 
597  Ice::DoubleSeq
598  DeprecatedNJointTaskSpaceImpedanceDMPController::createDMPFromString(
599  const std::string& dmpString,
600  const Ice::Current&)
601  {
602  dmpCtrl->loadDMPFromString(dmpString);
603  return dmpCtrl->dmpPtr->defaultGoal;
604  }
605 
606  Ice::Double
607  DeprecatedNJointTaskSpaceImpedanceDMPController::getCanVal(const Ice::Current&)
608  {
609  return dmpCtrl->canVal;
610  }
611 
612  void
613  DeprecatedNJointTaskSpaceImpedanceDMPController::resumeDMP(const Ice::Current&)
614  {
615  stopped = false;
616  }
617 
618  void
619  DeprecatedNJointTaskSpaceImpedanceDMPController::setUseNullSpaceJointDMP(bool enable,
620  const Ice::Current&)
621  {
622  useNullSpaceJointDMP = enable;
623  }
624 
625  void
626  DeprecatedNJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals,
627  Ice::Double timeDuration,
628  const Ice::Current&)
629  {
630  dmpCtrl->canVal = timeDuration;
631  dmpCtrl->config.motionTimeDuration = timeDuration;
632 
633  runDMP(goals);
634  }
635 
636  void
637  DeprecatedNJointTaskSpaceImpedanceDMPController::runDMP(const Ice::DoubleSeq& goals,
638  const Ice::Current&)
639  {
640  firstRun = true;
641  timeForCalibration = 0;
642  started = false;
643 
644  while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
645  {
646  usleep(100);
647  }
648  while (!interfaceData.updateReadBuffer())
649  {
650  usleep(100);
651  }
652 
653  while (!interfaceData.updateReadBuffer())
654  {
655  usleep(100);
656  }
657 
658  Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
659  ARMARX_IMPORTANT << "start to run dmp from: " << VAROUT(pose);
660  LockGuardType guard{controllerMutex};
661  dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
662  finished = false;
663 
664  if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
665  {
666  ARMARX_INFO << "Using Null Space Joint DMP";
667  }
668  ARMARX_INFO << VAROUT(dmpCtrl->config.motionTimeDuration);
669 
670  started = true;
671  stopped = false;
672  }
673 
674  void
675  DeprecatedNJointTaskSpaceImpedanceDMPController::onPublish(
676  const SensorAndControl&,
677  const DebugDrawerInterfacePrx&,
678  const DebugObserverInterfacePrx& debugObs)
679  {
680  StringVariantBaseMap datafields;
681  auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
682  for (auto& pair : values)
683  {
684  datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
685  }
686 
687  auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
688  for (auto& pair : values_null)
689  {
690  datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
691  }
692 
693  datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
694  datafields["mpcfactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcfactor);
695  datafields["targetPose_x"] =
696  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
697  datafields["targetPose_y"] =
698  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
699  datafields["targetPose_z"] =
700  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
701  datafields["targetPose_qw"] =
702  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qw);
703  datafields["targetPose_qx"] =
704  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qx);
705  datafields["targetPose_qy"] =
706  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qy);
707  datafields["targetPose_qz"] =
708  new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qz);
709 
710  datafields["currentPose_x"] =
711  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_x);
712  datafields["currentPose_y"] =
713  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_y);
714  datafields["currentPose_z"] =
715  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_z);
716  datafields["currentPose_qw"] =
717  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qw);
718  datafields["currentPose_qx"] =
719  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qx);
720  datafields["currentPose_qy"] =
721  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
722  datafields["currentPose_qz"] =
723  new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
724 
725  datafields["currentKpos_x"] =
726  new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_x);
727  datafields["currentKpos_y"] =
728  new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_y);
729  datafields["currentKpos_z"] =
730  new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_z);
731  datafields["currentKori_x"] =
732  new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_x);
733  datafields["currentKori_y"] =
734  new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_y);
735  datafields["currentKori_z"] =
736  new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_z);
737  datafields["currentKnull_x"] =
738  new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_x);
739  datafields["currentKnull_y"] =
740  new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_y);
741  datafields["currentKnull_z"] =
742  new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_z);
743 
744  datafields["currentDpos_x"] =
745  new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_x);
746  datafields["currentDpos_y"] =
747  new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_y);
748  datafields["currentDpos_z"] =
749  new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_z);
750  datafields["currentDori_x"] =
751  new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_x);
752  datafields["currentDori_y"] =
753  new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_y);
754  datafields["currentDori_z"] =
755  new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_z);
756  datafields["currentDnull_x"] =
757  new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_x);
758  datafields["currentDnull_y"] =
759  new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_y);
760  datafields["currentDnull_z"] =
761  new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_z);
762 
763  datafields["forceDesired_x"] =
764  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
765  datafields["forceDesired_y"] =
766  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
767  datafields["forceDesired_z"] =
768  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
769  datafields["forceDesired_rx"] =
770  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rx);
771  datafields["forceDesired_ry"] =
772  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
773  datafields["forceDesired_rz"] =
774  new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
775 
776  datafields["filteredForceInRoot_x"] =
777  new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[0]);
778  datafields["filteredForceInRoot_y"] =
779  new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[1]);
780  datafields["filteredForceInRoot_z"] =
781  new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[2]);
782 
783  datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
784 
785  std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
786  debugObs->setDebugChannel(channelName, datafields);
787  }
788 
789  void
790  DeprecatedNJointTaskSpaceImpedanceDMPController::onInitNJointController()
791  {
792  ARMARX_INFO << "init ...";
793  // controllerTask = new PeriodicTask<DeprecatedNJointTaskSpaceImpedanceDMPController>(this, &DeprecatedNJointTaskSpaceImpedanceDMPController::controllerRun, 1);
794  runTask("DeprecatedNJointTaskSpaceImpedanceDMPController",
795  [&]
796  {
797  CycleUtil c(1);
798  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
799  while (getState() == eManagedIceObjectStarted)
800  {
801  if (isControllerActive())
802  {
803  controllerRun();
804  }
805  c.waitForCycleDuration();
806  }
807  });
808  }
809 
810  void
811  DeprecatedNJointTaskSpaceImpedanceDMPController::onDisconnectNJointController()
812  {
813  // controllerTask->stop();
814  }
815 
816  void
817  DeprecatedNJointTaskSpaceImpedanceDMPController::setMPWeights(const DoubleSeqSeq& weights,
818  const Ice::Current&)
819  {
820  dmpCtrl->setWeights(weights);
821  }
822 
823  DoubleSeqSeq
824  DeprecatedNJointTaskSpaceImpedanceDMPController::getMPWeights(const Ice::Current&)
825  {
826  DMP::DVec2d res = dmpCtrl->getWeights();
827  DoubleSeqSeq resvec;
828  for (size_t i = 0; i < res.size(); ++i)
829  {
830  std::vector<double> cvec;
831  for (size_t j = 0; j < res[i].size(); ++j)
832  {
833  cvec.push_back(res[i][j]);
834  }
835  resvec.push_back(cvec);
836  }
837 
838  return resvec;
839  }
840 
841  void
842  DeprecatedNJointTaskSpaceImpedanceDMPController::removeAllViaPoints(const Ice::Current&)
843  {
844  LockGuardType guard{controllerMutex};
845  ARMARX_INFO << "setting via points ";
846  dmpCtrl->removeAllViaPoints();
847  }
848 
849  void
850  DeprecatedNJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Eigen::Vector3f& kd,
851  const Ice::Current&)
852  {
853  ARMARX_CHECK_EQUAL(kd.size(), 3);
854  ARMARX_INFO << "set linear kd " << VAROUT(kd);
855  LockGuardType guard(controllerMutex);
856  ctrlParams.getWriteBuffer().dpos = kd;
857  ctrlParams.commitWrite();
858  }
859 
860  void
861  DeprecatedNJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Eigen::Vector3f& kp,
862  const Ice::Current&)
863  {
864  ARMARX_CHECK_EQUAL(kp.size(), 3);
865  ARMARX_INFO << "set linear kp " << VAROUT(kp);
866  LockGuardType guard(controllerMutex);
867  ctrlParams.getWriteBuffer().kpos = kp;
868  ctrlParams.commitWrite();
869  }
870 
871  void
872  DeprecatedNJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Eigen::Vector3f& kd,
873  const Ice::Current&)
874  {
875  ARMARX_CHECK_EQUAL(kd.size(), 3);
876  ARMARX_INFO << "set angular kd " << VAROUT(kd);
877  LockGuardType guard(controllerMutex);
878  ctrlParams.getWriteBuffer().dori = kd;
879  ctrlParams.commitWrite();
880  }
881 
882  void
883  DeprecatedNJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Eigen::Vector3f& kp,
884  const Ice::Current&)
885  {
886  ARMARX_CHECK_EQUAL(kp.size(), 3);
887  ARMARX_INFO << "set angular kp " << VAROUT(kp);
888  LockGuardType guard(controllerMutex);
889  ctrlParams.getWriteBuffer().kori = kp;
890  ctrlParams.commitWrite();
891  }
892 
893  void
894  DeprecatedNJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(
895  const Eigen::VectorXf& kd,
896  const Ice::Current&)
897  {
898  ARMARX_CHECK_EQUAL((std::size_t)kd.size(), targets.size());
899  ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
900  LockGuardType guard(controllerMutex);
901  ctrlParams.getWriteBuffer().dnull = kd;
902  ctrlParams.commitWrite();
903  }
904 
905  void
906  DeprecatedNJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(
907  const Eigen::VectorXf& kp,
908  const Ice::Current&)
909  {
910  ARMARX_CHECK_EQUAL((std::size_t)kp.size(), targets.size());
911  ARMARX_INFO << "set linear kp " << VAROUT(kp);
912  LockGuardType guard(controllerMutex);
913  ctrlParams.getWriteBuffer().knull = kp;
914  ctrlParams.commitWrite();
915  }
916 
917  void
918  DeprecatedNJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(
919  const Eigen::VectorXf& jointValues,
920  const Ice::Current&)
921  {
922  ARMARX_CHECK_EQUAL((std::size_t)jointValues.size(), targets.size());
923  defaultNullSpaceJointValues.getWriteBuffer() = jointValues;
924  defaultNullSpaceJointValues.commitWrite();
925  }
926 
927 
928 } // namespace armarx::control::deprecated_njoint_mp_controller::task_space
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::DeprecatedNJointTaskSpaceImpedanceDMPControllerInterface::stopDMP
void stopDMP()
armarx::DeprecatedNJointTaskSpaceImpedanceDMPControllerInterface::getCanVal
double getCanVal()
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData::desiredNullSpaceJointValues
Eigen::VectorXf desiredNullSpaceJointValues
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.h:37
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::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:529
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kori
float Kori
Definition: TaskSpaceVMP.h:54
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::DeprecatedNJointTaskSpaceImpedanceDMPController
DeprecatedNJointTaskSpaceImpedanceDMPController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:29
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dpos
float Dpos
Definition: TaskSpaceVMP.h:53
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::setMPWeights
void setMPWeights(const DoubleSeqSeq &weights, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:817
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData >::rtGetControlStruct
const DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData & 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_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::createDMPFromString
Ice::DoubleSeq createDMPFromString(const std::string &, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:598
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
armarx::SensorValueBase
The SensorValueBase class.
Definition: SensorValueBase.h:40
armarx::DeprecatedNJointTaskSpaceImpedanceDMPControllerInterface::resumeDMP
void resumeDMP()
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition: ControlTargetBase.h:47
RobotUnit.h
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::runDMP
void runDMP(const Ice::DoubleSeq &goals, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:637
armarx::WriteBufferedTripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:383
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::runDMPWithTime
void runDMPWithTime(const Ice::DoubleSeq &goals, Ice::Double timeDuration, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:626
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:166
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::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:519
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::setUseNullSpaceJointDMP
void setUseNullSpaceJointDMP(bool enable, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:619
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
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData::targetVel
Eigen::VectorXf targetVel
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.h:35
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp
void setNullspaceVelocityKp(const Eigen::VectorXf &kp, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:906
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::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::onDisconnectNJointController
void onDisconnectNJointController() override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:811
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData >::getWriterControlStruct
DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::DeprecatedNJointTaskSpaceImpedanceDMPControllerInterface::resetDMP
void resetDMP()
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition: TaskSpaceVMP.h:48
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
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::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::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:920
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
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::setAngularVelocityKd
void setAngularVelocityKd(const Eigen::Vector3f &kd, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:872
normalizeTimestamps
TrajectoryPtr normalizeTimestamps(TrajectoryPtr t, float maxTransVel, float maxRotVel)
Definition: PlayPlatformTrajectory.cpp:82
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::setLinearVelocityKp
void setLinearVelocityKp(const Eigen::Vector3f &kp, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:861
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::DeprecatedNJointTaskSpaceImpedanceDMPControllerInterface::getMPWeights
DoubleSeqSeq getMPWeights()
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
ArmarXObjectScheduler.h
ControlTarget1DoFActuator.h
armarx::DeprecatedNJointTaskSpaceImpedanceDMPControllerInterface::removeAllViaPoints
void removeAllViaPoints()
armarx::WriteBufferedTripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:325
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:276
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition: TaskSpaceVMP.h:64
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles
void learnJointDMPFromFiles(const std::string &fileName, const Ice::FloatSeq &currentJVS, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:536
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
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition: TaskSpaceVMP.h:52
ExpressionException.h
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::DeprecatedNJointTaskSpaceImpedanceDMPController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:675
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
IceUtil::Handle< class RobotUnit >
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::setLinearVelocityKd
void setLinearVelocityKd(const Eigen::Vector3f &kd, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:850
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::onInitNJointController
void onInitNJointController() override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:790
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::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition: TaskSpaceVMP.h:62
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues
void setDefaultNullSpaceJointValues(const Eigen::VectorXf &jointValues, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:918
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
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::learnDMPFromFiles
void learnDMPFromFiles(const Ice::StringSeq &fileNames, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:510
Eigen::Matrix< float, 6, 1 >
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition: TaskSpaceVMP.h:56
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::controllerRun
void controllerRun()
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:178
armarx::WriteBufferedTripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:339
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData::targetPose
Eigen::Matrix4f targetPose
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.h:36
Logging.h
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::setAngularVelocityKp
void setAngularVelocityKp(const Eigen::Vector3f &kp, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:883
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.h:32
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_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
DeprecatedNJointTaskSpaceImpedanceDMPController.h
armarx::control::deprecated_njoint_mp_controller::task_space::registrationControllerDeprecatedNJointTaskSpaceImpedanceDMPController
NJointControllerRegistration< DeprecatedNJointTaskSpaceImpedanceDMPController > registrationControllerDeprecatedNJointTaskSpaceImpedanceDMPController("DeprecatedNJointTaskSpaceImpedanceDMPController")
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd
void setNullspaceVelocityKd(const Eigen::VectorXf &kd, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:894
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition: TaskSpaceVMP.h:90
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:776
armarx::DeprecatedNJointTaskSpaceImpedanceDMPControllerInterface::getDMPAsString
string getDMPAsString()
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceImpedanceDMPController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: DeprecatedNJointTaskSpaceImpedanceDMPController.cpp:160
SensorValue1DoFActuator.h
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