DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp
Go to the documentation of this file.
1 #include "DeprecatedNJointTaskSpaceAdmittanceDMPController.h"
2 
3 #include <VirtualRobot/IK/DifferentialIK.h>
4 #include <VirtualRobot/MathTools.h>
5 #include <VirtualRobot/Nodes/RobotNode.h>
6 #include <VirtualRobot/Nodes/Sensor.h>
7 #include <VirtualRobot/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 
17 #include <dmp/representation/dmp/umitsmp.h>
18 
19 namespace armarx::control::deprecated_njoint_mp_controller::task_space
20 {
21  NJointControllerRegistration<DeprecatedNJointTaskSpaceAdmittanceDMPController>
22  registrationControllerDeprecatedNJointTaskSpaceAdmittanceDMPController(
23  "DeprecatedNJointTaskSpaceAdmittanceDMPController");
24 
25  DeprecatedNJointTaskSpaceAdmittanceDMPController::
26  DeprecatedNJointTaskSpaceAdmittanceDMPController(
27  const RobotUnitPtr& robotUnit,
28  const armarx::NJointControllerConfigPtr& config,
29  const VirtualRobot::RobotPtr&)
30  {
31  ARMARX_INFO << "creating task-space admittance dmp controller";
32 
33  useSynchronizedRtRobot();
34  cfg = DeprecatedNJointTaskSpaceAdmittanceDMPControllerConfigPtr::dynamicCast(config);
35  ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
36 
37  /// setup kinematics
38  VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
39  ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
40  for (size_t i = 0; i < rns->getSize(); ++i)
41  {
42  std::string jointName = rns->getNode(i)->getName();
43  jointNames.push_back(jointName);
44 
45  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
46  const SensorValueBase* sv = useSensorValue(jointName);
47  targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
48 
49  const SensorValue1DoFActuatorVelocity* velocitySensor =
50  sv->asA<SensorValue1DoFActuatorVelocity>();
51  const SensorValue1DoFActuatorPosition* positionSensor =
52  sv->asA<SensorValue1DoFActuatorPosition>();
53 
54  if (!velocitySensor)
55  {
56  ARMARX_WARNING << "No velocitySensor available for " << jointName;
57  }
58  if (!positionSensor)
59  {
60  ARMARX_WARNING << "No positionSensor available for " << jointName;
61  }
62 
63  velocitySensors.push_back(velocitySensor);
64  positionSensors.push_back(positionSensor);
65  };
66 
67  tcp = rns->getTCP();
68  ik.reset(new VirtualRobot::DifferentialIK(
69  rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
70  ik->setDampedSvdLambda(0.0001);
71  numOfJoints = targets.size();
72  qvel_filtered.setZero(targets.size());
73  torqueLimit = cfg->jointTorqueLimit;
74 
75  /// setup force sensor and filters
76  const SensorValueBase* svlf =
77  robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
78  forceSensor = svlf->asA<SensorValueForceTorque>();
79 
80  float tcpMass = 0.0f;
81  Eigen::Vector3f tcpCoMInForceSensorFrame;
82  tcpCoMInForceSensorFrame.setZero();
83 
84  enableTCPGravityCompensation.store(cfg->enableTCPGravityCompensation);
85  ARMARX_INFO << VAROUT(enableTCPGravityCompensation);
86  if (enableTCPGravityCompensation.load())
87  {
88  tcpMass = cfg->tcpMass;
89  tcpCoMInForceSensorFrame = cfg->tcpCoMInForceSensorFrame;
90  }
91 
92  forceOffset.setZero();
93  torqueOffset.setZero();
94  filteredForce.setZero();
95  filteredTorque.setZero();
96  filteredForceInRoot.setZero();
97  filteredTorqueInRoot.setZero();
98 
99  /// set MP
100  /// - task space
101  tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
102  taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
103  taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
104  taskSpaceDMPConfig.DMPAmplitude = 1.0;
105  taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
106  taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
107  taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
108  taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
109  taskSpaceDMPConfig.phaseStopParas.Kpos = 0;
110  taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
111  taskSpaceDMPConfig.phaseStopParas.Kori = 0;
112  taskSpaceDMPConfig.phaseStopParas.Dori = 0;
113  taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
114  taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
115  taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
116 
117  dmpCtrl.reset(
118  new tsvmp::TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
119  kinematic_chain = cfg->nodeSetName;
120 
121  /// - null space
122  useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
123  nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
124  isNullSpaceJointDMPLearned = false;
125  ARMARX_CHECK_EQUAL(cfg->targetNullSpaceJointValues.size(), targets.size());
126 
127  /// Admittance vairables
128  virtualAcc.setZero();
129  virtualVel.setZero();
130  virtualPose = Eigen::Matrix4f::Identity();
131 
132  /// initialize control parameters and buffers
133  /// - control parameter
134  ARMARX_CHECK_EQUAL(cfg->Knull.size(), static_cast<int>(targets.size()));
135  ARMARX_CHECK_EQUAL(cfg->Dnull.size(), static_cast<int>(targets.size()));
136  CtrlParams initParams = {cfg->kpImpedance,
137  cfg->kdImpedance,
138  cfg->kpAdmittance,
139  cfg->kdAdmittance,
140  cfg->kmAdmittance,
141  cfg->Knull,
142  cfg->Dnull,
143  Eigen::Vector3f::Zero(),
144  Eigen::Vector3f::Zero(),
145  tcpMass,
146  tcpCoMInForceSensorFrame};
147  ctrlParams.reinitAllBuffers(initParams);
148 
149  ARMARX_INFO << "Finished controller constructor ";
150  }
151 
152  void
153  DeprecatedNJointTaskSpaceAdmittanceDMPController::onInitNJointController()
154  {
155  ARMARX_INFO << "init ...";
156  /// initialize control parameters and buffers
157  /// - control target
158  DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData initData;
159  initData.targetTSPose = tcp->getPoseInRootFrame();
160  initData.targetTSVel.setZero();
161  initData.targetNullSpaceJointValues = cfg->targetNullSpaceJointValues;
162  reinitTripleBuffer(initData);
163 
164  /// - rt to interface
165  RT2InterfaceData rt2InterfaceData;
166  rt2InterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
167  rt2InterfaceBuffer.reinitAllBuffers(rt2InterfaceData);
168 
169  /// - rt to mp
170  RT2MPData rt2MPData;
171  rt2MPData.currentPose = tcp->getPoseInRootFrame();
172  rt2MPData.currentTime = 0;
173  rt2MPData.deltaT = 0;
174  rt2MPData.currentTwist.setZero();
175  rt2MPBuffer.reinitAllBuffers(rt2MPData);
176 
177  /// start mp thread
178  runTask("DeprecatedNJointTaskSpaceAdmittanceDMPController",
179  [&]
180  {
181  CycleUtil c(1);
182  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
183  while (getState() == eManagedIceObjectStarted)
184  {
185  if (isControllerActive())
186  {
187  controllerRun();
188  }
189  c.waitForCycleDuration();
190  }
191  });
192 
193  ARMARX_IMPORTANT << "started controller ";
194  }
195 
196  std::string
197  DeprecatedNJointTaskSpaceAdmittanceDMPController::getClassName(const Ice::Current&) const
198  {
199  return "DeprecatedNJointTaskSpaceAdmittanceDMPController";
200  }
201 
202  void
203  DeprecatedNJointTaskSpaceAdmittanceDMPController::rtPreActivateController()
204  {
205  }
206 
207  void
208  DeprecatedNJointTaskSpaceAdmittanceDMPController::controllerRun()
209  {
210  if (!dmpCtrl || !rt2MPBuffer.updateReadBuffer())
211  {
212  return;
213  }
214 
215  LockGuardType guard(mpMutex);
216  double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
217  Eigen::Matrix4f currentPose = rt2MPBuffer.getReadBuffer().currentPose;
218  Eigen::VectorXf currentTwist = rt2MPBuffer.getReadBuffer().currentTwist;
219 
220  if (mpRunning.load() && dmpCtrl->canVal > 1e-8)
221  {
222  dmpCtrl->flow(deltaT, currentPose, currentTwist);
223 
224  Eigen::VectorXf targetNullSpaceJointValues = cfg->targetNullSpaceJointValues;
225  if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
226  {
227  DMP::DVec targetJointState;
228  currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(
229  currentJointState,
230  dmpCtrl->canVal / cfg->timeDuration,
231  deltaT / cfg->timeDuration,
232  targetJointState);
233 
234  if (targetJointState.size() == targets.size())
235  {
236  for (size_t i = 0; i < targetJointState.size(); ++i)
237  {
238  targetNullSpaceJointValues(i) = targetJointState[i];
239  }
240  }
241  }
242 
243  LockGuardType guard{controlDataMutex};
244  getWriterControlStruct().targetNullSpaceJointValues = targetNullSpaceJointValues;
245  getWriterControlStruct().targetTSVel = dmpCtrl->getTargetVelocity();
246  getWriterControlStruct().targetTSPose = dmpCtrl->getTargetPoseMat();
247  getWriterControlStruct().canVal = dmpCtrl->canVal;
248  getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
249  writeControlStruct();
250  }
251  else
252  {
253  mpRunning.store(false);
254  LockGuardType guard{controlDataMutex};
255  getWriterControlStruct().targetTSVel.setZero();
256  writeControlStruct();
257  }
258  }
259 
260  void
261  DeprecatedNJointTaskSpaceAdmittanceDMPController::rtRun(
262  const IceUtil::Time& sensorValuesTimestamp,
263  const IceUtil::Time& timeSinceLastIteration)
264  {
265  /// ---------------------------- get current kinematics ---------------------------------------------
266  Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
267  Eigen::MatrixXf jacobi =
268  ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
269 
270  Eigen::VectorXf qpos(positionSensors.size());
271  Eigen::VectorXf qvel(velocitySensors.size());
272  for (size_t i = 0; i < positionSensors.size(); ++i)
273  {
274  qpos(i) = positionSensors[i]->position;
275  qvel(i) = velocitySensors[i]->velocity;
276  }
277  qvel_filtered = (1 - cfg->qvelFilter) * qvel_filtered + cfg->qvelFilter * qvel;
278  Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
279  jacobi.block(0, 0, 3, numOfJoints) =
280  0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
281 
282  double deltaT = timeSinceLastIteration.toSecondsDouble();
283 
284  /// write rt buffer
285  {
286  rt2MPBuffer.getWriteBuffer().currentPose = currentPose;
287  rt2MPBuffer.getWriteBuffer().currentTwist = currentTwist;
288  rt2MPBuffer.getWriteBuffer().deltaT = deltaT;
289  rt2MPBuffer.getWriteBuffer().currentTime += deltaT;
290  rt2MPBuffer.commitWrite();
291 
292  rt2InterfaceBuffer.getWriteBuffer().currentTcpPose = currentPose;
293  rt2InterfaceBuffer.commitWrite();
294  }
295 
296  /// ---------------------------- update control parameters ---------------------------------------------
297  Eigen::Vector6f kpImpedance = ctrlParams.getUpToDateReadBuffer().kpImpedance;
298  Eigen::Vector6f kdImpedance = ctrlParams.getUpToDateReadBuffer().kdImpedance;
299 
300  Eigen::Vector6f kpAdmittance = ctrlParams.getUpToDateReadBuffer().kpAdmittance;
301  Eigen::Vector6f kdAdmittance = ctrlParams.getUpToDateReadBuffer().kdAdmittance;
302  Eigen::Vector6f kmAdmittance = Eigen::Vector6f::Zero();
303 
304  Eigen::VectorXf knull = ctrlParams.getUpToDateReadBuffer().knull;
305  Eigen::VectorXf dnull = ctrlParams.getUpToDateReadBuffer().dnull;
306 
307  float tcpMass = ctrlParams.getUpToDateReadBuffer().tcpMass;
308  Eigen::Vector3f tcpCoMInFTFrame =
309  ctrlParams.getUpToDateReadBuffer().tcpCoMInForceSensorFrame;
310 
311  /// ---------------------------- get current force torque value ---------------------------------------------
312  Eigen::Vector3f forceBaseline = ctrlParams.getUpToDateReadBuffer().forceBaseline;
313  Eigen::Vector3f torqueBaseline = ctrlParams.getUpToDateReadBuffer().torqueBaseline;
314  Eigen::Vector6f forceTorque;
315  Eigen::Vector6f tcpGravityCompensation;
316  forceTorque.setZero();
317  tcpGravityCompensation.setZero();
318 
319  /// ---------------------------- get control targets ---------------------------------------------
320  Eigen::Matrix4f targetPose;
321  Eigen::Vector6f targetVel = Eigen::Vector6f::Zero();
322  Eigen::VectorXf targetNullSpaceJointValues =
323  rtGetControlStruct().targetNullSpaceJointValues;
324 
325  if (rtFirstRun.load())
326  {
327  rtReady.store(false);
328  rtFirstRun.store(false);
329  timeForCalibration = 0;
330  forceOffset.setZero();
331  torqueOffset.setZero();
332 
333  targetPose = currentPose;
334  previousTargetPose = currentPose;
335  virtualPose = currentPose;
336 
337  Eigen::Matrix4f forceFrameInRoot =
338  rtGetRobot()->getSensor(cfg->forceSensorName)->getRobotNode()->getPoseInRootFrame();
339  forceFrameInTCP.block<3, 3>(0, 0) =
340  currentPose.block<3, 3>(0, 0).transpose() * forceFrameInRoot.block<3, 3>(0, 0);
341  tcpCoMInTCPFrame = forceFrameInTCP.block<3, 3>(0, 0) * tcpCoMInFTFrame;
342  ARMARX_IMPORTANT << "impedance control first run with\n"
343  << VAROUT(targetPose) << "\nTCP CoM Vec (in TCP frame): \n"
344  << VAROUT(tcpCoMInTCPFrame);
345  }
346  else
347  {
348  if (enableTCPGravityCompensation.load())
349  {
350  tcpGravityCompensation = getTCPGravityCompensation(currentPose, tcpMass);
351  }
352  if (!rtReady.load())
353  {
354  targetPose = previousTargetPose;
355  forceOffset =
356  (1 - cfg->forceFilter) * forceOffset +
357  cfg->forceFilter * (forceSensor->force - tcpGravityCompensation.head<3>());
358  torqueOffset =
359  (1 - cfg->forceFilter) * torqueOffset +
360  cfg->forceFilter * (forceSensor->torque - tcpGravityCompensation.tail<3>());
361  timeForCalibration = timeForCalibration + deltaT;
362  if (timeForCalibration > cfg->waitTimeForCalibration)
363  {
364  /// rt is ready only if the FT sensor is calibrated, and the rt2interface buffer is updated (see above)
365  ARMARX_IMPORTANT << "FT calibration done, RT is ready! \n"
366  << VAROUT(forceOffset) << "\n"
367  << VAROUT(torqueOffset) << "\n"
368  << VAROUT(tcpGravityCompensation);
369  rtReady.store(true);
370  }
371  }
372  else
373  {
374  /// only when the ft sensor is calibrated can we
375  kmAdmittance = ctrlParams.getUpToDateReadBuffer().kmAdmittance;
376 
377  targetPose = rtGetControlStruct().targetTSPose;
378  targetVel = rtGetControlStruct().targetTSVel;
379  if ((previousTargetPose.block<3, 1>(0, 3) - targetPose.block<3, 1>(0, 3)).norm() >
380  100.0f)
381  {
382  ARMARX_WARNING << "new target \n"
383  << VAROUT(targetPose) << "\nis too far away from\n"
384  << VAROUT(previousTargetPose);
385  targetPose = previousTargetPose;
386  }
387  previousTargetPose = targetPose;
388  forceTorque =
389  getFilteredForceTorque(forceBaseline, torqueBaseline, tcpGravityCompensation);
390  }
391  }
392 
393  /// ---------------------------- admittance control ---------------------------------------------
394  /// calculate pose error between the virtual pose and the target pose
395  Eigen::VectorXf poseError(6);
396  poseError.head<3>() = virtualPose.block<3, 1>(0, 3) - targetPose.block<3, 1>(0, 3);
397  Eigen::Matrix3f objDiffMat =
398  virtualPose.block<3, 3>(0, 0) * targetPose.block<3, 3>(0, 0).transpose();
399  poseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
400 
401  /// admittance control law and Euler Integration -> virtual pose
402  Eigen::Vector6f acc = Eigen::Vector6f::Zero();
403  Eigen::Vector6f vel = Eigen::Vector6f::Zero();
404  acc = kmAdmittance.cwiseProduct(forceTorque) - kpAdmittance.cwiseProduct(poseError) -
405  kdAdmittance.cwiseProduct(virtualVel);
406  vel = virtualVel + 0.5 * deltaT * (acc + virtualAcc);
407  Eigen::VectorXf deltaPose = 0.5 * deltaT * (vel + virtualVel);
408  virtualAcc = acc;
409  virtualVel = vel;
410  virtualPose.block<3, 1>(0, 3) += deltaPose.head<3>();
411  Eigen::Matrix3f deltaPoseMat =
412  VirtualRobot::MathTools::rpy2eigen3f(deltaPose(3), deltaPose(4), deltaPose(5));
413  virtualPose.block<3, 3>(0, 0) = deltaPoseMat * virtualPose.block<3, 3>(0, 0);
414 
415  /// ----------------------------- Impedance control ---------------------------------------------
416  /// calculate pose error between virtual pose and current pose
417  /// !!! This is very important: you have to keep postion and orientation both
418  /// with UI unit (meter, radian) to calculate impedance force.
419  Eigen::Vector6f poseErrorImp;
420  Eigen::Matrix3f diffMat =
421  virtualPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).transpose();
422  poseErrorImp.head<3>() =
423  0.001 * (virtualPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3));
424  currentTwist.head<3>() *= 0.001;
425  poseErrorImp.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
426  Eigen::Vector6f forceImpedance =
427  kpImpedance.cwiseProduct(poseErrorImp) - kdImpedance.cwiseProduct(currentTwist);
428 
429  /// ----------------------------- Nullspace PD Control --------------------------------------------------
430  Eigen::VectorXf nullspaceTorque =
431  knull.cwiseProduct(targetNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
432 
433  /// ----------------------------- Map TS target force to JS --------------------------------------------------
434  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
435  Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
436  Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * forceImpedance +
437  (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
438 
439  /// ----------------------------- Send torque command to joint device --------------------------------------------------
440  ARMARX_CHECK_EXPRESSION(!targets.empty());
441  for (size_t i = 0; i < targets.size(); ++i)
442  {
443  targets.at(i)->torque = jointDesiredTorques(i);
444  if (!targets.at(i)->isValid() || isnan(targets.at(i)->torque))
445  {
446  targets.at(i)->torque = 0.0f;
447  }
448  else
449  {
450  if (fabs(targets.at(i)->torque) > fabs(torqueLimit))
451  {
452  targets.at(i)->torque =
453  fabs(torqueLimit) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
454  }
455  }
456 
457  debugRTBuffer.getWriteBuffer().desired_torques[jointNames[i]] = targets.at(i)->torque;
458  debugRTBuffer.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] =
459  targetNullSpaceJointValues(i);
460  }
461 
462 
463  // debug information
464  {
465  debugRTBuffer.getWriteBuffer().forceDesired_x = forceImpedance(0);
466  debugRTBuffer.getWriteBuffer().forceDesired_y = forceImpedance(1);
467  debugRTBuffer.getWriteBuffer().forceDesired_z = forceImpedance(2);
468  debugRTBuffer.getWriteBuffer().forceDesired_rx = forceImpedance(3);
469  debugRTBuffer.getWriteBuffer().forceDesired_ry = forceImpedance(4);
470  debugRTBuffer.getWriteBuffer().forceDesired_rz = forceImpedance(5);
471 
472  debugRTBuffer.getWriteBuffer().ft_InRoot_x = filteredForce(0);
473  debugRTBuffer.getWriteBuffer().ft_InRoot_y = filteredForce(1);
474  debugRTBuffer.getWriteBuffer().ft_InRoot_z = filteredForce(2);
475  debugRTBuffer.getWriteBuffer().ft_InRoot_rx = filteredTorque(0);
476  debugRTBuffer.getWriteBuffer().ft_InRoot_ry = filteredTorque(1);
477  debugRTBuffer.getWriteBuffer().ft_InRoot_rz = filteredTorque(2);
478 
479  debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_x = forceTorque(0);
480  debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_y = forceTorque(1);
481  debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_z = forceTorque(2);
482  debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_rx = forceTorque(3);
483  debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_ry = forceTorque(4);
484  debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_rz = forceTorque(5);
485 
486  debugRTBuffer.getWriteBuffer().targetPose_x = targetPose(0, 3);
487  debugRTBuffer.getWriteBuffer().targetPose_y = targetPose(1, 3);
488  debugRTBuffer.getWriteBuffer().targetPose_z = targetPose(2, 3);
489  VirtualRobot::MathTools::Quaternion targetQuat =
490  VirtualRobot::MathTools::eigen4f2quat(targetPose);
491  debugRTBuffer.getWriteBuffer().targetPose_qw = targetQuat.w;
492  debugRTBuffer.getWriteBuffer().targetPose_qx = targetQuat.x;
493  debugRTBuffer.getWriteBuffer().targetPose_qy = targetQuat.y;
494  debugRTBuffer.getWriteBuffer().targetPose_qz = targetQuat.z;
495 
496  debugRTBuffer.getWriteBuffer().virtualPose_x = virtualPose(0, 3);
497  debugRTBuffer.getWriteBuffer().virtualPose_y = virtualPose(1, 3);
498  debugRTBuffer.getWriteBuffer().virtualPose_z = virtualPose(2, 3);
499  VirtualRobot::MathTools::Quaternion virtualQuat =
500  VirtualRobot::MathTools::eigen4f2quat(virtualPose);
501  debugRTBuffer.getWriteBuffer().virtualPose_qw = virtualQuat.w;
502  debugRTBuffer.getWriteBuffer().virtualPose_qx = virtualQuat.x;
503  debugRTBuffer.getWriteBuffer().virtualPose_qy = virtualQuat.y;
504  debugRTBuffer.getWriteBuffer().virtualPose_qz = virtualQuat.z;
505 
506  debugRTBuffer.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
507 
508  debugRTBuffer.getWriteBuffer().currentPose_x = currentPose(0, 3);
509  debugRTBuffer.getWriteBuffer().currentPose_y = currentPose(1, 3);
510  debugRTBuffer.getWriteBuffer().currentPose_z = currentPose(2, 3);
511  VirtualRobot::MathTools::Quaternion currentQuat =
512  VirtualRobot::MathTools::eigen4f2quat(currentPose);
513  debugRTBuffer.getWriteBuffer().currentPose_qw = currentQuat.w;
514  debugRTBuffer.getWriteBuffer().currentPose_qx = currentQuat.x;
515  debugRTBuffer.getWriteBuffer().currentPose_qy = currentQuat.y;
516  debugRTBuffer.getWriteBuffer().currentPose_qz = currentQuat.z;
517  debugRTBuffer.getWriteBuffer().deltaT = deltaT;
518 
519  debugRTBuffer.getWriteBuffer().currentKpos_x = kpImpedance(0);
520  debugRTBuffer.getWriteBuffer().currentKpos_y = kpImpedance(1);
521  debugRTBuffer.getWriteBuffer().currentKpos_z = kpImpedance(2);
522  debugRTBuffer.getWriteBuffer().currentKori_x = kpImpedance(3);
523  debugRTBuffer.getWriteBuffer().currentKori_y = kpImpedance(4);
524  debugRTBuffer.getWriteBuffer().currentKori_z = kpImpedance(5);
525  debugRTBuffer.getWriteBuffer().currentKnull_x = knull.x();
526  debugRTBuffer.getWriteBuffer().currentKnull_y = knull.y();
527  debugRTBuffer.getWriteBuffer().currentKnull_z = knull.z();
528 
529  debugRTBuffer.getWriteBuffer().currentDpos_x = kdImpedance(0);
530  debugRTBuffer.getWriteBuffer().currentDpos_y = kdImpedance(1);
531  debugRTBuffer.getWriteBuffer().currentDpos_z = kdImpedance(2);
532  debugRTBuffer.getWriteBuffer().currentDori_x = kdImpedance(3);
533  debugRTBuffer.getWriteBuffer().currentDori_y = kdImpedance(4);
534  debugRTBuffer.getWriteBuffer().currentDori_z = kdImpedance(5);
535  debugRTBuffer.getWriteBuffer().currentDnull_x = dnull.x();
536  debugRTBuffer.getWriteBuffer().currentDnull_y = dnull.y();
537  debugRTBuffer.getWriteBuffer().currentDnull_z = dnull.z();
538 
539  debugRTBuffer.getWriteBuffer().kmAdmittance = kmAdmittance;
540  debugRTBuffer.getWriteBuffer().gravityCompensation = tcpGravityCompensation;
541  debugRTBuffer.getWriteBuffer().forceOffset = forceOffset;
542  debugRTBuffer.getWriteBuffer().torqueOffset = torqueOffset;
543  debugRTBuffer.getWriteBuffer().forceBaseline = forceBaseline;
544  debugRTBuffer.getWriteBuffer().torqueBaseline = torqueBaseline;
545 
546  debugRTBuffer.commitWrite();
547  }
548  }
549 
550  Eigen::Vector6f
551  DeprecatedNJointTaskSpaceAdmittanceDMPController::getTCPGravityCompensation(
552  Eigen::Matrix4f& currentPose,
553  float tcpMass)
554  {
555  // static compensation
556  Eigen::Vector3f gravity;
557  gravity << 0.0, 0.0, -9.8;
558  Eigen::Vector3f localGravity = currentPose.block<3, 3>(0, 0).transpose() * gravity;
559  Eigen::Vector3f localForceVec = tcpMass * localGravity;
560  Eigen::Vector3f localTorqueVec = tcpCoMInTCPFrame.cross(localForceVec);
561  // ARMARX_INFO << VAROUT(localForceVec) << VAROUT(localTorqueVec) << VAROUT(tcpMass) << VAROUT(enableTCPGravityCompensation.load());
562 
563  Eigen::Vector6f tcpGravityCompensation;
564  tcpGravityCompensation << localForceVec, localTorqueVec;
565  return tcpGravityCompensation;
566  }
567 
568  Eigen::Vector6f
569  DeprecatedNJointTaskSpaceAdmittanceDMPController::getFilteredForceTorque(
570  Eigen::Vector3f& forceBaseline,
571  Eigen::Vector3f& torqueBaseline,
572  Eigen::Vector6f& handCompensatedFT)
573  {
574  filteredForce =
575  (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * forceSensor->force;
576  filteredTorque =
577  (1 - cfg->forceFilter) * filteredTorque + cfg->forceFilter * forceSensor->torque;
578 
579  Eigen::Matrix4f forceFrameInRoot =
580  rtGetRobot()->getSensor(cfg->forceSensorName)->getRobotNode()->getPoseInRootFrame();
581  filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) *
582  (filteredForce - forceOffset - handCompensatedFT.head<3>()) -
583  forceBaseline;
584  filteredTorqueInRoot = forceFrameInRoot.block<3, 3>(0, 0) *
585  (filteredTorque - torqueOffset - handCompensatedFT.tail<3>()) -
586  torqueBaseline;
587 
588  for (size_t i = 0; i < 3; ++i)
589  {
590  if (fabs(filteredForceInRoot(i)) > cfg->forceDeadZone)
591  {
592  filteredForceInRoot(i) -=
593  (filteredForceInRoot(i) / fabs(filteredForceInRoot(i))) * cfg->forceDeadZone;
594  }
595  else
596  {
597  filteredForceInRoot(i) = 0;
598  }
599 
600  if (fabs(filteredTorqueInRoot(i)) > cfg->torqueDeadZone)
601  {
602  filteredTorqueInRoot(i) -=
603  (filteredTorqueInRoot(i) / fabs(filteredTorqueInRoot(i))) * cfg->torqueDeadZone;
604  }
605  else
606  {
607  filteredTorqueInRoot(i) = 0;
608  }
609  }
610 
611  Eigen::Vector6f forceTorque;
612  forceTorque << filteredForceInRoot, filteredTorqueInRoot;
613  return forceTorque;
614  }
615 
616  std::string
617  DeprecatedNJointTaskSpaceAdmittanceDMPController::getKinematicChainName(const Ice::Current&)
618  {
619  return kinematic_chain;
620  }
621 
622  // RTController::ControllerInfo DeprecatedNJointTaskSpaceAdmittanceDMPController::getControllerInfo(const Ice::Current& ice)
623  // {
624  // return info;
625  // }
626 
627  void
628  DeprecatedNJointTaskSpaceAdmittanceDMPController::learnDMPFromFiles(
629  const Ice::StringSeq& fileNames,
630  const Ice::Current&)
631  {
632  dmpCtrl->learnDMPFromFiles(fileNames);
633  ARMARX_INFO << "Learned DMP ... ";
634  }
635 
636  bool
637  DeprecatedNJointTaskSpaceAdmittanceDMPController::isFinished(const Ice::Current&)
638  {
639  return !mpRunning.load();
640  }
641 
642  void
643  DeprecatedNJointTaskSpaceAdmittanceDMPController::setViaPoints(Ice::Double u,
644  const Ice::DoubleSeq& viapoint,
645  const Ice::Current&)
646  {
647  LockGuardType guard(mpMutex);
648  ARMARX_INFO << "setting via points ";
649  dmpCtrl->setViaPose(u, viapoint);
650  }
651 
652  void
653  DeprecatedNJointTaskSpaceAdmittanceDMPController::setGoals(const Ice::DoubleSeq& goals,
654  const Ice::Current& ice)
655  {
656  dmpCtrl->setGoalPoseVec(goals);
657  }
658 
659  void
660  DeprecatedNJointTaskSpaceAdmittanceDMPController::setStartAndGoals(const Ice::DoubleSeq& starts,
661  const Ice::DoubleSeq& goals,
662  const Ice::Current& ice)
663  {
664  LockGuardType guard(mpMutex);
665  dmpCtrl->removeAllViaPoints();
666  dmpCtrl->setViaPose(1.0, starts);
667  dmpCtrl->setViaPose(1e-8, goals);
668  }
669 
670  void
671  DeprecatedNJointTaskSpaceAdmittanceDMPController::learnJointDMPFromFiles(
672  const std::string& fileName,
673  const Ice::FloatSeq& currentJVS,
674  const Ice::Current&)
675  {
676  DMP::Vec<DMP::SampledTrajectoryV2> trajs;
677  DMP::DVec ratios;
678  DMP::SampledTrajectoryV2 traj;
679  traj.readFromCSVFile(fileName);
680  traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
681  if (traj.dim() != targets.size())
682  {
683  isNullSpaceJointDMPLearned = false;
684  return;
685  }
686 
687  DMP::DVec goal;
688  goal.resize(traj.dim());
689  currentJointState.resize(traj.dim());
690 
691  for (size_t i = 0; i < goal.size(); ++i)
692  {
693  goal.at(i) = traj.rbegin()->getPosition(i);
694  currentJointState.at(i).pos = currentJVS.at(i);
695  currentJointState.at(i).vel = 0;
696  }
697 
698  trajs.push_back(traj);
699  nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
700 
701  // prepare exeuction of joint dmp
702  nullSpaceJointDMPPtr->prepareExecution(goal, currentJointState, 1.0, 1.0);
703  ARMARX_INFO << "prepared nullspace joint dmp";
704  isNullSpaceJointDMPLearned = true;
705  }
706 
707  void
708  DeprecatedNJointTaskSpaceAdmittanceDMPController::resetDMP(const Ice::Current&)
709  {
710  // if (started)
711  if (mpRunning.load())
712  {
713  ARMARX_INFO << "Cannot reset running DMP";
714  }
715  rtFirstRun = true;
716  }
717 
718  void
719  DeprecatedNJointTaskSpaceAdmittanceDMPController::stopDMP(const Ice::Current&)
720  {
721  mpRunning.store(false);
722  // oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
723  // started = false;
724  // stopped = true;
725  }
726 
727  void
728  DeprecatedNJointTaskSpaceAdmittanceDMPController::pauseDMP(const Ice::Current&)
729  {
730  dmpCtrl->pauseController();
731  }
732 
733  std::string
734  DeprecatedNJointTaskSpaceAdmittanceDMPController::getDMPAsString(const Ice::Current&)
735  {
736 
737  return dmpCtrl->saveDMPToString();
738  }
739 
740  Ice::DoubleSeq
741  DeprecatedNJointTaskSpaceAdmittanceDMPController::createDMPFromString(
742  const std::string& dmpString,
743  const Ice::Current&)
744  {
745  dmpCtrl->loadDMPFromString(dmpString);
746  return dmpCtrl->dmpPtr->defaultGoal;
747  }
748 
749  Ice::Double
750  DeprecatedNJointTaskSpaceAdmittanceDMPController::getCanVal(const Ice::Current&)
751  {
752  return dmpCtrl->canVal;
753  }
754 
755  void
756  DeprecatedNJointTaskSpaceAdmittanceDMPController::resumeDMP(const Ice::Current&)
757  {
758  dmpCtrl->resumeController();
759  // stopped = false;
760  }
761 
762  void
763  DeprecatedNJointTaskSpaceAdmittanceDMPController::setUseNullSpaceJointDMP(bool enable,
764  const Ice::Current&)
765  {
766  useNullSpaceJointDMP = enable;
767  }
768 
769  void
770  DeprecatedNJointTaskSpaceAdmittanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals,
771  Ice::Double timeDuration,
772  const Ice::Current&)
773  {
774  dmpCtrl->canVal = timeDuration;
775  dmpCtrl->config.motionTimeDuration = timeDuration;
776 
777  runDMP(goals);
778  }
779 
780  Ice::Double
781  DeprecatedNJointTaskSpaceAdmittanceDMPController::getVirtualTime(const Ice::Current&)
782  {
783  return dmpCtrl->canVal;
784  }
785 
786  void
787  DeprecatedNJointTaskSpaceAdmittanceDMPController::runDMP(const Ice::DoubleSeq& goals,
788  const Ice::Current&)
789  {
790  rtFirstRun.store(true);
791  while (rtFirstRun.load() || !rtReady.load() || !rt2InterfaceBuffer.updateReadBuffer())
792  {
793  usleep(100);
794  }
795 
796  Eigen::Matrix4f pose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentTcpPose;
797 
798  LockGuardType guard{mpMutex};
799  dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
800 
801  if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
802  {
803  ARMARX_INFO << "Using Null Space Joint DMP";
804  }
805  ARMARX_INFO << VAROUT(dmpCtrl->config.motionTimeDuration);
806  ARMARX_INFO << VAROUT(dmpCtrl->dmpPtr->getViaPoints().size());
807 
808  mpRunning.store(true);
809  dmpCtrl->resumeController();
810  ARMARX_INFO << "start mp ...";
811  }
812 
813  void
814  DeprecatedNJointTaskSpaceAdmittanceDMPController::onPublish(
815  const SensorAndControl&,
816  const DebugDrawerInterfacePrx&,
817  const DebugObserverInterfacePrx& debugObs)
818  {
819  StringVariantBaseMap datafields;
820  auto values = debugRTBuffer.getUpToDateReadBuffer().desired_torques;
821  for (auto& pair : values)
822  {
823  datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
824  }
825 
826  auto values_null = debugRTBuffer.getUpToDateReadBuffer().desired_nullspaceJoint;
827  for (auto& pair : values_null)
828  {
829  datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
830  }
831 
832  datafields["canVal"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().currentCanVal);
833  datafields["mpcfactor"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().mpcfactor);
834  datafields["targetPose_x"] =
835  new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_x);
836  datafields["targetPose_y"] =
837  new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_y);
838  datafields["targetPose_z"] =
839  new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_z);
840  datafields["targetPose_qw"] =
841  new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_qw);
842  datafields["targetPose_qx"] =
843  new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_qx);
844  datafields["targetPose_qy"] =
845  new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_qy);
846  datafields["targetPose_qz"] =
847  new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_qz);
848 
849  datafields["virtualPose_x"] =
850  new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_x);
851  datafields["virtualPose_y"] =
852  new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_y);
853  datafields["virtualPose_z"] =
854  new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_z);
855  datafields["virtualPose_qw"] =
856  new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_qw);
857  datafields["virtualPose_qx"] =
858  new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_qx);
859  datafields["virtualPose_qy"] =
860  new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_qy);
861  datafields["virtualPose_qz"] =
862  new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_qz);
863 
864  datafields["currentPose_x"] =
865  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_x);
866  datafields["currentPose_y"] =
867  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_y);
868  datafields["currentPose_z"] =
869  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_z);
870  datafields["currentPose_qw"] =
871  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_qw);
872  datafields["currentPose_qx"] =
873  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_qx);
874  datafields["currentPose_qy"] =
875  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_qy);
876  datafields["currentPose_qz"] =
877  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_qz);
878 
879  datafields["currentKpos_x"] =
880  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKpos_x);
881  datafields["currentKpos_y"] =
882  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKpos_y);
883  datafields["currentKpos_z"] =
884  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKpos_z);
885  datafields["currentKori_x"] =
886  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKori_x);
887  datafields["currentKori_y"] =
888  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKori_y);
889  datafields["currentKori_z"] =
890  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKori_z);
891  datafields["currentKnull_x"] =
892  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKnull_x);
893  datafields["currentKnull_y"] =
894  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKnull_y);
895  datafields["currentKnull_z"] =
896  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKnull_z);
897 
898  datafields["currentDpos_x"] =
899  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDpos_x);
900  datafields["currentDpos_y"] =
901  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDpos_y);
902  datafields["currentDpos_z"] =
903  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDpos_z);
904  datafields["currentDori_x"] =
905  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDori_x);
906  datafields["currentDori_y"] =
907  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDori_y);
908  datafields["currentDori_z"] =
909  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDori_z);
910  datafields["currentDnull_x"] =
911  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDnull_x);
912  datafields["currentDnull_y"] =
913  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDnull_y);
914  datafields["currentDnull_z"] =
915  new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDnull_z);
916 
917  datafields["forceDesired_x"] =
918  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_x);
919  datafields["forceDesired_y"] =
920  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_y);
921  datafields["forceDesired_z"] =
922  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_z);
923  datafields["forceDesired_rx"] =
924  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_rx);
925  datafields["forceDesired_ry"] =
926  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_ry);
927  datafields["forceDesired_rz"] =
928  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_rz);
929 
930  datafields["ft_InRoot_x"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_x);
931  datafields["ft_InRoot_y"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_y);
932  datafields["ft_InRoot_z"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_z);
933  datafields["ft_InRoot_rx"] =
934  new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_rx);
935  datafields["ft_InRoot_ry"] =
936  new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_ry);
937  datafields["ft_InRoot_rz"] =
938  new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_rz);
939 
940  datafields["ft_FilteredInRoot_x"] =
941  new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_x);
942  datafields["ft_FilteredInRoot_y"] =
943  new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_y);
944  datafields["ft_FilteredInRoot_z"] =
945  new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_z);
946  datafields["ft_FilteredInRoot_rx"] =
947  new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_rx);
948  datafields["ft_FilteredInRoot_ry"] =
949  new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_ry);
950  datafields["ft_FilteredInRoot_rz"] =
951  new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_rz);
952 
953  datafields["deltaT"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().deltaT);
954 
955  datafields["kmAdmittance_x"] =
956  new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(0));
957  datafields["kmAdmittance_y"] =
958  new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(1));
959  datafields["kmAdmittance_z"] =
960  new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(2));
961  datafields["kmAdmittance_rx"] =
962  new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(3));
963  datafields["kmAdmittance_ry"] =
964  new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(4));
965  datafields["kmAdmittance_rz"] =
966  new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(5));
967 
968  datafields["gravityCompensation_x"] =
969  new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(0));
970  datafields["gravityCompensation_y"] =
971  new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(1));
972  datafields["gravityCompensation_z"] =
973  new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(2));
974  datafields["gravityCompensation_rx"] =
975  new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(3));
976  datafields["gravityCompensation_ry"] =
977  new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(4));
978  datafields["gravityCompensation_rz"] =
979  new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(5));
980 
981 
982  datafields["ft_offset_x"] =
983  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceOffset(0));
984  datafields["ft_offset_y"] =
985  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceOffset(1));
986  datafields["ft_offset_z"] =
987  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceOffset(2));
988  datafields["ft_offset_rx"] =
989  new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueOffset(0));
990  datafields["ft_offset_ry"] =
991  new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueOffset(1));
992  datafields["ft_offset_rz"] =
993  new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueOffset(2));
994 
995  datafields["ft_Baseline_x"] =
996  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceBaseline(0));
997  datafields["ft_Baseline_y"] =
998  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceBaseline(1));
999  datafields["ft_Baseline_z"] =
1000  new Variant(debugRTBuffer.getUpToDateReadBuffer().forceBaseline(2));
1001  datafields["ft_Baseline_rx"] =
1002  new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueBaseline(0));
1003  datafields["ft_Baseline_ry"] =
1004  new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueBaseline(1));
1005  datafields["ft_Baseline_rz"] =
1006  new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueBaseline(2));
1007 
1008  std::string channelName = cfg->nodeSetName + "_TaskSpaceAdmittanceControl";
1009  debugObs->setDebugChannel(channelName, datafields);
1010  }
1011 
1012  void
1013  DeprecatedNJointTaskSpaceAdmittanceDMPController::onDisconnectNJointController()
1014  {
1015  // controllerTask->stop();
1016  }
1017 
1018  void
1019  DeprecatedNJointTaskSpaceAdmittanceDMPController::setMPWeights(const DoubleSeqSeq& weights,
1020  const Ice::Current&)
1021  {
1022  dmpCtrl->setWeights(weights);
1023  }
1024 
1025  DoubleSeqSeq
1026  DeprecatedNJointTaskSpaceAdmittanceDMPController::getMPWeights(const Ice::Current&)
1027  {
1028  DMP::DVec2d res = dmpCtrl->getWeights();
1029  DoubleSeqSeq resvec;
1030  for (size_t i = 0; i < res.size(); ++i)
1031  {
1032  std::vector<double> cvec;
1033  for (size_t j = 0; j < res[i].size(); ++j)
1034  {
1035  cvec.push_back(res[i][j]);
1036  }
1037  resvec.push_back(cvec);
1038  }
1039 
1040  return resvec;
1041  }
1042 
1043  void
1044  DeprecatedNJointTaskSpaceAdmittanceDMPController::removeAllViaPoints(const Ice::Current&)
1045  {
1046  ARMARX_INFO << "remove all via points";
1047  LockGuardType guard{mpMutex};
1048  dmpCtrl->removeAllViaPoints();
1049  }
1050 
1051  void
1052  DeprecatedNJointTaskSpaceAdmittanceDMPController::setKpImpedance(
1053  const Eigen::Vector6f& kpImpedance,
1054  const Ice::Current&)
1055  {
1056  ctrlParams.getWriteBuffer().kpImpedance = kpImpedance;
1057  ctrlParams.commitWrite();
1058  }
1059 
1060  void
1061  DeprecatedNJointTaskSpaceAdmittanceDMPController::setKdImpedance(
1062  const Eigen::Vector6f& kdImpedance,
1063  const Ice::Current&)
1064  {
1065  ctrlParams.getWriteBuffer().kdImpedance = kdImpedance;
1066  ctrlParams.commitWrite();
1067  }
1068 
1069  void
1070  DeprecatedNJointTaskSpaceAdmittanceDMPController::setKpAdmittance(
1071  const Eigen::Vector6f& kpAdmittance,
1072  const Ice::Current&)
1073  {
1074  ctrlParams.getWriteBuffer().kpAdmittance = kpAdmittance;
1075  ctrlParams.commitWrite();
1076  }
1077 
1078  void
1079  DeprecatedNJointTaskSpaceAdmittanceDMPController::setKdAdmittance(
1080  const Eigen::Vector6f& kdAdmittance,
1081  const Ice::Current&)
1082  {
1083  ctrlParams.getWriteBuffer().kdAdmittance = kdAdmittance;
1084  ctrlParams.commitWrite();
1085  }
1086 
1087  void
1088  DeprecatedNJointTaskSpaceAdmittanceDMPController::setKmAdmittance(
1089  const Eigen::Vector6f& kmAdmittance,
1090  const Ice::Current&)
1091  {
1092  ctrlParams.getWriteBuffer().kmAdmittance = kmAdmittance;
1093  ctrlParams.commitWrite();
1094  }
1095 
1096  void
1097  DeprecatedNJointTaskSpaceAdmittanceDMPController::setTCPMass(Ice::Float mass,
1098  const Ice::Current&)
1099  {
1100  ctrlParams.getWriteBuffer().tcpMass = mass;
1101  ctrlParams.commitWrite();
1102  }
1103 
1104  void
1105  DeprecatedNJointTaskSpaceAdmittanceDMPController::setTCPCoMInFTFrame(const Eigen::Vector3f& com,
1106  const Ice::Current&)
1107  {
1108  ctrlParams.getWriteBuffer().tcpCoMInForceSensorFrame = com;
1109  ctrlParams.commitWrite();
1110  }
1111 
1112  void
1113  DeprecatedNJointTaskSpaceAdmittanceDMPController::setNullspaceVelocityKd(
1114  const Eigen::VectorXf& kd,
1115  const Ice::Current&)
1116  {
1117  ARMARX_CHECK_EQUAL(kd.size(), targets.size());
1118  ARMARX_INFO << "set nullspace damping\n" << VAROUT(kd);
1119  ctrlParams.getWriteBuffer().dnull = kd;
1120  ctrlParams.commitWrite();
1121  }
1122 
1123  void
1124  DeprecatedNJointTaskSpaceAdmittanceDMPController::setNullspaceVelocityKp(
1125  const Eigen::VectorXf& kp,
1126  const Ice::Current&)
1127  {
1128  ARMARX_CHECK_EQUAL(kp.size(), targets.size());
1129  ARMARX_INFO << "set nullspace stiffness\n" << VAROUT(kp);
1130  ctrlParams.getWriteBuffer().knull = kp;
1131  ctrlParams.commitWrite();
1132  }
1133 
1134  void
1135  DeprecatedNJointTaskSpaceAdmittanceDMPController::setTargetNullSpaceJointValues(
1136  const Eigen::VectorXf& jointValues,
1137  const Ice::Current&)
1138  {
1139  ARMARX_CHECK_EQUAL(jointValues.size(), targets.size());
1140  getWriterControlStruct().targetNullSpaceJointValues = jointValues;
1141  writeControlStruct();
1142  }
1143 
1144  void
1145  DeprecatedNJointTaskSpaceAdmittanceDMPController::setPotentialForceBaseline(
1146  const Eigen::Vector3f& force,
1147  const Eigen::Vector3f& torque,
1148  const Ice::Current&)
1149  {
1150  ctrlParams.getWriteBuffer().forceBaseline = force;
1151  ctrlParams.getWriteBuffer().torqueBaseline = torque;
1152  ctrlParams.commitWrite();
1153  }
1154 
1155 
1156 } // namespace armarx::control::deprecated_njoint_mp_controller::task_space
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::ControlTargetBase::asA
const T * asA() const
Definition: ControlTargetBase.h:76
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:223
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kori
float Kori
Definition: TaskSpaceVMP.h:54
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:919
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::runDMPWithTime
void runDMPWithTime(const Ice::DoubleSeq &goals, Ice::Double timeDuration, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:770
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dpos
float Dpos
Definition: TaskSpaceVMP.h:53
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData >::rtGetControlStruct
const DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPKernelSize
int DMPKernelSize
Definition: TaskSpaceVMP.h:61
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
armarx::SensorValueBase
The SensorValueBase class.
Definition: SensorValueBase.h:40
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition: ControlTargetBase.h:47
RobotUnit.h
armarx::WriteBufferedTripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:383
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface::getCanVal
double getCanVal()
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::onDisconnectNJointController
void onDisconnectNJointController() override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1013
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setKpAdmittance
void setKpAdmittance(const Eigen::Vector6f &kpAdmittance, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1070
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::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setKdImpedance
void setKdImpedance(const Eigen::Vector6f &kdImpedance, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1061
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setMPWeights
void setMPWeights(const DoubleSeqSeq &weights, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1019
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData::targetNullSpaceJointValues
Eigen::VectorXf targetNullSpaceJointValues
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.h:38
armarx::control::deprecated_njoint_mp_controller::task_space
Definition: DeprecatedNJointPeriodicTSDMPCompliantController.cpp:20
armarx::WriteBufferedTripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:377
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:197
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
armarx::ManagedIceObject::getState
int getState() const
Retrieve current state of the ManagedIceObject.
Definition: ManagedIceObject.cpp:769
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
SensorValueForceTorque.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setPotentialForceBaseline
void setPotentialForceBaseline(const Eigen::Vector3f &, const Eigen::Vector3f &, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1145
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::DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface::getKinematicChainName
string getKinematicChainName()
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData >::getWriterControlStruct
DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:653
DeprecatedNJointTaskSpaceAdmittanceDMPController.h
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setKdAdmittance
void setKdAdmittance(const Eigen::Vector6f &kdAdmittance, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1079
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition: TaskSpaceVMP.h:48
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::getTCPGravityCompensation
Eigen::Vector6f getTCPGravityCompensation(Eigen::Matrix4f &currentPose, float tcpMass)
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:551
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::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setTCPCoMInFTFrame
void setTCPCoMInFTFrame(const Eigen::Vector3f &com, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1105
armarx::DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface::getMPWeights
DoubleSeqSeq getMPWeights()
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::DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface::stopDMP
void stopDMP()
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
armarx::DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface::resumeDMP
void resumeDMP()
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:920
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:203
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:643
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dori
float Dori
Definition: TaskSpaceVMP.h:55
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setUseNullSpaceJointDMP
void setUseNullSpaceJointDMP(bool enable, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:763
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::onInitNJointController
void onInitNJointController() override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:153
normalizeTimestamps
TrajectoryPtr normalizeTimestamps(TrajectoryPtr t, float maxTransVel, float maxRotVel)
Definition: PlayPlatformTrajectory.cpp:82
armarx::DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface::isFinished
bool isFinished()
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::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setTCPMass
void setTCPMass(Ice::Float mass, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1097
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setTargetNullSpaceJointValues
void setTargetNullSpaceJointValues(const Eigen::VectorXf &jointValues, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1135
armarx::DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface::removeAllViaPoints
void removeAllViaPoints()
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::learnJointDMPFromFiles
void learnJointDMPFromFiles(const std::string &fileName, const Ice::FloatSeq &currentJVS, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:671
armarx::SensorValueForceTorque::torque
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION Eigen::Vector3f torque
Definition: SensorValueForceTorque.h:35
ArmarXObjectScheduler.h
ControlTarget1DoFActuator.h
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setKmAdmittance
void setKmAdmittance(const Eigen::Vector6f &kmAdmittance, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1088
armarx::DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface::getVirtualTime
double getVirtualTime()
armarx::WriteBufferedTripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:325
armarx::NJointControllerWithTripleBuffer< DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition: TaskSpaceVMP.h:64
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData::targetTSVel
Eigen::Vector6f targetTSVel
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.h:36
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::slop
float slop
Definition: TaskSpaceVMP.h:51
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition: TaskSpaceVMP.h:52
NJointController.h
armarx::WriteBufferedTripleBuffer::reinitAllBuffers
void reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:390
armarx::DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface::pauseDMP
void pauseDMP()
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::controllerRun
void controllerRun()
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:208
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
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::runDMP
void runDMP(const Ice::DoubleSeq &goals, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:787
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::learnDMPFromFiles
void learnDMPFromFiles(const Ice::StringSeq &fileNames, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:628
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.h:33
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
IceUtil::Handle< class RobotUnit >
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPStyle
std::string DMPStyle
Definition: TaskSpaceVMP.h:63
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData::targetTSPose
Eigen::Matrix4f targetTSPose
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.h:37
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition: TaskSpaceVMP.h:62
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::createDMPFromString
Ice::DoubleSeq createDMPFromString(const std::string &, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:741
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::DeprecatedNJointTaskSpaceAdmittanceDMPController
DeprecatedNJointTaskSpaceAdmittanceDMPController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:26
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setNullspaceVelocityKp
void setNullspaceVelocityKp(const Eigen::VectorXf &kp, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1124
Eigen::Matrix< float, 6, 1 >
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition: TaskSpaceVMP.h:56
armarx::WriteBufferedTripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:339
armarx::DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface::getDMPAsString
string getDMPAsString()
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setStartAndGoals
void setStartAndGoals(const Ice::DoubleSeq &starts, const Ice::DoubleSeq &goals, const Ice::Current &ice) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:660
armarx::control::deprecated_njoint_mp_controller::task_space::registrationControllerDeprecatedNJointTaskSpaceAdmittanceDMPController
NJointControllerRegistration< DeprecatedNJointTaskSpaceAdmittanceDMPController > registrationControllerDeprecatedNJointTaskSpaceAdmittanceDMPController("DeprecatedNJointTaskSpaceAdmittanceDMPController")
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setNullspaceVelocityKd
void setNullspaceVelocityKd(const Eigen::VectorXf &kd, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1113
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::SensorValueForceTorque
Definition: SensorValueForceTorque.h:32
armarx::WriteBufferedTripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:346
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::getFilteredForceTorque
Eigen::Vector6f getFilteredForceTorque(Eigen::Vector3f &forceBaseline, Eigen::Vector3f &torqueBaseline, Eigen::Vector6f &handCompensatedFT)
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:569
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:772
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:814
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition: TaskSpaceVMP.h:90
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:776
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::setKpImpedance
void setKpImpedance(const Eigen::Vector6f &kpImpedance, const Ice::Current &) override
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:1052
armarx::control::deprecated_njoint_mp_controller::task_space::DeprecatedNJointTaskSpaceAdmittanceDMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp:261
SensorValue1DoFActuator.h
armarx::DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface::resetDMP
void resetDMP()
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