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