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