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