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