NJointBimanualObjLevelMultiMPController.cpp
Go to the documentation of this file.
1 #include "NJointBimanualObjLevelMultiMPController.h"
2 
3 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
4 #include <ArmarXCore/core/time/CycleUtil.h>
5 
6 
7 // Simox
8 #include <VirtualRobot/IK/DifferentialIK.h>
9 #include <VirtualRobot/MathTools.h>
10 #include <VirtualRobot/Nodes/RobotNode.h>
11 #include <VirtualRobot/Robot.h>
12 #include <VirtualRobot/RobotNodeSet.h>
13 
14 // armarx
15 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
16 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
17 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
18 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
19 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
20 #include <RobotAPI/libraries/core/PIDController.h>
21 #include <RobotAPI/libraries/core/math/MathUtils.h>
22 
23 namespace armarx::control::deprecated_njoint_mp_controller::bimanual
24 {
25  NJointControllerRegistration<NJointBimanualObjLevelMultiMPController>
26  registrationControllerNJointBimanualObjLevelMultiMPController(
27  "NJointBimanualObjLevelMultiMPController");
28 
29  NJointBimanualObjLevelMultiMPController::NJointBimanualObjLevelMultiMPController(
30  const RobotUnitPtr& robUnit,
31  const armarx::NJointControllerConfigPtr& config,
32  const VirtualRobot::RobotPtr&)
33  {
34  ARMARX_INFO << "Initializing Bimanual Object Level Controller";
35 
36  // initialize robot kinematic chain and sensors
37  useSynchronizedRtRobot();
38  cfg = NJointBimanualObjLevelMultiMPControllerConfigPtr::dynamicCast(config);
39 
40  leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
41 
42  for (size_t i = 0; i < leftRNS->getSize(); ++i)
43  {
44  std::string jointName = leftRNS->getNode(i)->getName();
45  leftJointNames.push_back(jointName);
46  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
47  const SensorValueBase* sv = useSensorValue(jointName);
48  leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
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 
64  leftVelocitySensors.push_back(velocitySensor);
65  leftPositionSensors.push_back(positionSensor);
66  };
67 
68  rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
69 
70  for (size_t i = 0; i < rightRNS->getSize(); ++i)
71  {
72  std::string jointName = rightRNS->getNode(i)->getName();
73  rightJointNames.push_back(jointName);
74  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
75  const SensorValueBase* sv = useSensorValue(jointName);
76  rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
77 
78  const SensorValue1DoFActuatorVelocity* velocitySensor =
79  sv->asA<SensorValue1DoFActuatorVelocity>();
80  const SensorValue1DoFActuatorPosition* positionSensor =
81  sv->asA<SensorValue1DoFActuatorPosition>();
82 
83  if (!velocitySensor)
84  {
85  ARMARX_WARNING << "No velocitySensor available for " << jointName;
86  }
87  if (!positionSensor)
88  {
89  ARMARX_WARNING << "No positionSensor available for " << jointName;
90  }
91 
92  rightVelocitySensors.push_back(velocitySensor);
93  rightPositionSensors.push_back(positionSensor);
94  };
95 
96 
97  const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
98  leftForceTorque = svlf->asA<SensorValueForceTorque>();
99  const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
100  rightForceTorque = svrf->asA<SensorValueForceTorque>();
101 
102  leftIK.reset(new VirtualRobot::DifferentialIK(
103  leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
104  rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS,
105  rightRNS->getRobot()->getRootNode(),
106  VirtualRobot::JacobiProvider::eSVDDamped));
107 
108  tcpLeft = leftRNS->getTCP();
109  tcpRight = rightRNS->getTCP();
110 
111  /* ----------------- initialize DMP ----------------- */
112  tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
113  taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
114  taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
115  taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
116  taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
117  taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
118  taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
119  taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
120  taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
121  taskSpaceDMPConfig.phaseStopParas.Dori = 0;
122  taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
123  taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
124  taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
125  taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
126 
127  taskSpaceDMPConfig.DMPStyle = cfg->dmpObjType;
128  objectDMP.reset(new tsvmp::TaskSpaceDMPController("objDMP", taskSpaceDMPConfig, false));
129  taskSpaceDMPConfig.DMPStyle = cfg->dmpLeftType;
130  leftTCPInObjDMP.reset(
131  new tsvmp::TaskSpaceDMPController("leftDMP", taskSpaceDMPConfig, false));
132  taskSpaceDMPConfig.DMPStyle = cfg->dmpRightType;
133  rightTCPInObjDMP.reset(
134  new tsvmp::TaskSpaceDMPController("rightDMP", taskSpaceDMPConfig, false));
135  ARMARX_IMPORTANT << "dmps initialized";
136 
137  /* ----------------- initialize control parameters ----------------- */
138  auto setParamVec = [&](Eigen::VectorXf& param, ::Ice::FloatSeq& cfgParam, const size_t n)
139  {
140  param.setZero(n);
141  ARMARX_CHECK_EQUAL(cfgParam.size(), n);
142 
143  for (size_t i = 0; i < n; ++i)
144  {
145  param(i) = cfgParam.at(i);
146  }
147  };
148 
149  setParamVec(KpImpedance, cfg->KpImpedance, 12);
150  setParamVec(KdImpedance, cfg->KdImpedance, 12);
151  setParamVec(KpAdmittance, cfg->KpAdmittance, 6);
152  setParamVec(KdAdmittance, cfg->KdAdmittance, 6);
153  setParamVec(KmAdmittance, cfg->KmAdmittance, 6);
154  setParamVec(KmPID, cfg->KmPID, 6);
155 
156  ARMARX_INFO << "got controller params";
157 
158  /* ------------- initialize default joint values ------------------- */
159  leftDesiredJointValues.resize(leftTargets.size());
160  ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
161 
162  for (size_t i = 0; i < leftTargets.size(); ++i)
163  {
164  leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
165  }
166 
167  rightDesiredJointValues.resize(rightTargets.size());
168  ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
169 
170  for (size_t i = 0; i < rightTargets.size(); ++i)
171  {
172  rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
173  }
174 
175  /* ------------- obtain object initial poses ------------------- */
176  // object pose (position in meter)
177  objInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->objInitialPose[4],
178  cfg->objInitialPose[5],
179  cfg->objInitialPose[6],
180  cfg->objInitialPose[3]);
181  for (int i = 0; i < 3; ++i)
182  {
183  objInitialPose(i, 3) = cfg->objInitialPose[i];
184  }
185 
186  virtualAcc.setZero(6);
187  virtualVel.setZero(6);
188  virtualPose = objInitialPose;
189 
190  objTransMatrixInAnchor = Eigen::Matrix4f::Identity();
191 
192  // obtain left & right initial pose in the object frame (in meter)
193  leftInitialPose = Eigen::Matrix4f::Identity();
194  rightInitialPose = Eigen::Matrix4f::Identity();
195  Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
196  Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
197  leftInitialPose.block<3, 3>(0, 0) =
198  objInitialPose.block<3, 3>(0, 0).transpose() * leftPose.block<3, 3>(0, 0);
199  leftInitialPose.block<3, 1>(0, 3) =
200  objInitialPose.block<3, 3>(0, 0).transpose() *
201  (leftPose.block<3, 1>(0, 3) * 0.001 - objInitialPose.block<3, 1>(0, 3));
202  rightInitialPose.block<3, 3>(0, 0) =
203  objInitialPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
204  rightInitialPose.block<3, 1>(0, 3) =
205  objInitialPose.block<3, 3>(0, 0).transpose() *
206  (rightPose.block<3, 1>(0, 3) * 0.001 - objInitialPose.block<3, 1>(0, 3));
207 
208  integratedPoseObj = virtualPose;
209  integratedPoseLeft = leftInitialPose; // need to be represented in the object local frame.
210  integratedPoseRight = rightInitialPose;
211  /* ------------- initialize buffers ------------------- */
212  // interface to RT
213  Inferface2rtData interface2rtData;
214  interface2rtData.KpImpedance = KpImpedance;
215  interface2rtData.KdImpedance = KdImpedance;
216  interface2rtData.KmAdmittance = KmAdmittance;
217  interface2rtData.KpAdmittance = KpAdmittance;
218  interface2rtData.KdAdmittance = KdAdmittance;
219  interface2rtBuffer.reinitAllBuffers(interface2rtData);
220 
221  // RT to DMP
222  RT2ControlData rt2ControlData;
223  rt2ControlData.deltaT = 0;
224  rt2ControlData.currentTime = 0;
225  rt2ControlData.currentPoseObj = objInitialPose;
226  rt2ControlData.currentTwistObj.setZero();
227  rt2ControlData.currentPoseObj = leftInitialPose;
228  rt2ControlData.currentTwistObj.setZero();
229  rt2ControlData.currentPoseObj = rightInitialPose;
230  rt2ControlData.currentTwistObj.setZero();
231  rt2ControlBuffer.reinitAllBuffers(rt2ControlData);
232 
233  // DMP (or interface) to RT Target
234  NJointBimanualObjLevelMultiMPControlData initData;
235  initData.objTargetPose = objInitialPose;
236  initData.objTargetTwist.setZero(6);
237 
238  initData.leftTargetPoseInObj = leftInitialPose;
239  initData.leftTargetTwistInObj.setZero(6);
240 
241  initData.rightTargetPoseInObj = rightInitialPose;
242  initData.rightTargetTwistInObj.setZero(6);
243  reinitTripleBuffer(initData);
244 
245  // Control interface: RT to interface
246  RT2InterfaceData rt2InterfaceData;
247  rt2InterfaceData.currentLeftPoseInObjFrame = leftInitialPose;
248  rt2InterfaceData.currentRightPoseInObjFrame = rightInitialPose;
249  rt2InterfaceData.currentObjPose = objInitialPose;
250  rt2InterfaceData.currentObjForce.setZero();
251  rt2InterfaceData.currentObjVel.setZero();
252  rt2InterfaceBuffer.reinitAllBuffers(rt2InterfaceData);
253 
254 
255  /* ------------- initialize PID force controller ------------------- */
256  forcePIDControllers.resize(12);
257  for (size_t i = 0; i < 6; i++)
258  {
259  forcePIDControllers.at(i).reset(new PIDController(
260  cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
261  forcePIDControllers.at(i + 6).reset(new PIDController(
262  cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
263  forcePIDControllers.at(i)->reset();
264  forcePIDControllers.at(i + 6)->reset();
265  }
266 
267  /* ------------- initialize force torque sensor related ------------------- */
268  // sensor frame to tcp frame transformation
269  sensorFrame2TcpFrameLeft.setZero();
270  sensorFrame2TcpFrameRight.setZero();
271 
272  // filter
273  filterCoeff = cfg->filterCoeff;
274  filteredOldValue.setZero(12);
275 
276  // first loop calibrate
277  ftcalibrationTimer = 0;
278  ftOffset.setZero(12);
279 
280  // target wrench
281  targetWrench.setZero(cfg->targetWrench.size());
282  for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
283  {
284  targetWrench(i) = cfg->targetWrench[i];
285  }
286 
287  /* ------------- static compensation ------------------- */
288  massLeft = cfg->massLeft;
289  CoMVecLeft << cfg->CoMVecLeft[0], cfg->CoMVecLeft[1], cfg->CoMVecLeft[2];
290  forceOffsetLeft << cfg->forceOffsetLeft[0], cfg->forceOffsetLeft[1],
291  cfg->forceOffsetLeft[2];
292  torqueOffsetLeft << cfg->torqueOffsetLeft[0], cfg->torqueOffsetLeft[1],
293  cfg->torqueOffsetLeft[2];
294 
295  massRight = cfg->massRight;
296  CoMVecRight << cfg->CoMVecRight[0], cfg->CoMVecRight[1], cfg->CoMVecRight[2];
297  forceOffsetRight << cfg->forceOffsetRight[0], cfg->forceOffsetRight[1],
298  cfg->forceOffsetRight[2];
299  torqueOffsetRight << cfg->torqueOffsetRight[0], cfg->torqueOffsetRight[1],
300  cfg->torqueOffsetRight[2];
301 
302  /* ------------- Pose Offset ------------------- */
303  fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
304  objCom2TCPLeftInObjFrame.setZero();
305  objCom2TCPRightInObjFrame.setZero();
306 
307  /* ------------- setup flags ------------------- */
308  firstLoop = true;
309  dmpStarted = false;
310  ARMARX_IMPORTANT << "finished construction!";
311  }
312 
313  void
314  NJointBimanualObjLevelMultiMPController::rtPreActivateController()
315  {
316  }
317 
318  std::string
319  NJointBimanualObjLevelMultiMPController::getClassName(const Ice::Current&) const
320  {
321  return "NJointBimanualObjLevelMultiMPController";
322  }
323 
324  void
325  NJointBimanualObjLevelMultiMPController::controllerRun()
326  {
327  if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
328  {
329  return;
330  }
331 
332  // get status from RT loop
333  double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
334  Eigen::Matrix4f currentPoseObj = rt2ControlBuffer.getReadBuffer().currentPoseObj;
335  Eigen::VectorXf currentTwistObj = rt2ControlBuffer.getReadBuffer().currentTwistObj;
336  Eigen::Matrix4f currentPoseLeftInObj =
337  rt2ControlBuffer.getReadBuffer().currentPoseLeftInObj;
338  Eigen::VectorXf currentTwistLeftInObj =
339  rt2ControlBuffer.getReadBuffer().currentTwistLeftInObj;
340  Eigen::Matrix4f currentPoseRightInObj =
341  rt2ControlBuffer.getReadBuffer().currentPoseRightInObj;
342  Eigen::VectorXf currentTwistRightInObj =
343  rt2ControlBuffer.getReadBuffer().currentTwistRightInObj;
344 
345  // set can val from outside
346  rightTCPInObjDMP->flow(deltaT, currentPoseRightInObj, currentTwistRightInObj);
347 
348  // DMP flow control
349  bool objectDMPFinished = false;
350  bool leftTCPInObjDMPFinished = false;
351  bool rightTCPInObjDMPFinished = false;
352 
353  if (objectDMP->canVal > 1e-8 || objectDMP->config.DMPStyle == "Periodic")
354  {
355  objectDMP->flow(deltaT, currentPoseObj, currentTwistObj);
356  getWriterControlStruct().objTargetPose = objectDMP->getTargetPoseMat();
357  getWriterControlStruct().objTargetTwist = objectDMP->getTargetVelocity();
358  }
359  else
360  {
361  objectDMPFinished = true;
362  }
363 
364 
365  if (leftTCPInObjDMP->canVal > 1e-8 || leftTCPInObjDMP->config.DMPStyle == "Periodic")
366  {
367  leftTCPInObjDMP->flow(deltaT, currentPoseLeftInObj, currentTwistLeftInObj);
368  getWriterControlStruct().leftTargetPoseInObj = leftTCPInObjDMP->getTargetPoseMat();
369  getWriterControlStruct().leftTargetTwistInObj = leftTCPInObjDMP->getTargetVelocity();
370  }
371  else
372  {
373  leftTCPInObjDMPFinished = true;
374  }
375 
376  if (rightTCPInObjDMP->canVal > 1e-8 || rightTCPInObjDMP->config.DMPStyle == "Periodic")
377  {
378  rightTCPInObjDMP->flow(deltaT, currentPoseRightInObj, currentTwistRightInObj);
379  getWriterControlStruct().rightTargetPoseInObj = rightTCPInObjDMP->getTargetPoseMat();
380  getWriterControlStruct().rightTargetTwistInObj = rightTCPInObjDMP->getTargetVelocity();
381  }
382  else
383  {
384  rightTCPInObjDMPFinished = true;
385  }
386 
387 
388  if (objectDMPFinished && leftTCPInObjDMPFinished && rightTCPInObjDMPFinished)
389  {
390  finished = true;
391  dmpStarted = false;
392  }
393 
394  // set target to RT loop
395  LockGuardType guard{controlDataMutex};
396  writeControlStruct();
397  }
398 
399  void
400  NJointBimanualObjLevelMultiMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
401  const IceUtil::Time& timeSinceLastIteration)
402  {
403  // ------------------------------------------- current tcp pose -------------------------------------------
404  Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
405  Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
406 
407  currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
408  currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
409 
410  double deltaT = timeSinceLastIteration.toSecondsDouble();
411  ftcalibrationTimer += deltaT;
412 
413  if (firstLoop)
414  {
415  Eigen::Matrix4f leftPose = currentLeftPose;
416  Eigen::Matrix4f rightPose = currentRightPose;
417 
418  Eigen::Vector3f objCom2TCPLeftInGlobal =
419  leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
420  Eigen::Vector3f objCom2TCPRightInGlobal =
421  rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
422 
423  // Temporary (until we could detect and set object pose by perception)
424  // T_obj = T_{leftHand} * T_{objTransMatrixInAnchor}
425  objTransMatrixInAnchor.block<3, 3>(0, 0) =
426  leftPose.block<3, 3>(0, 0).transpose() * objInitialPose.block<3, 3>(0, 0);
427  objTransMatrixInAnchor.block<3, 1>(0, 3) =
428  leftPose.block<3, 3>(0, 0).transpose() *
429  (objInitialPose.block<3, 1>(0, 3) - leftPose.block<3, 1>(0, 3));
430 
431  objCom2TCPLeftInObjFrame =
432  virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
433  objCom2TCPRightInObjFrame =
434  virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
435 
436 
437  Eigen::Matrix4f leftSensorFrame =
438  leftRNS->getRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
439  Eigen::Matrix4f rightSensorFrame =
440  rightRNS->getRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
441  leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
442  rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
443 
444  // sensor frame 2 tcp frame transformation
445  sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) =
446  leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
447  sensorFrame2TcpFrameRight.block<3, 3>(0, 0) =
448  rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
449  CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
450  CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
451  firstLoop = false;
452  }
453 
454  Eigen::Matrix4f currentLeftPoseInObjFrame = Eigen::Matrix4f::Identity();
455  Eigen::Matrix4f currentRightPoseInObjFrame = Eigen::Matrix4f::Identity();
456  currentLeftPoseInObjFrame.block<3, 3>(0, 0) =
457  virtualPose.block<3, 3>(0, 0).transpose() * currentLeftPose.block<3, 3>(0, 0);
458  currentLeftPoseInObjFrame.block<3, 1>(0, 3) =
459  virtualPose.block<3, 3>(0, 0).transpose() *
460  (currentLeftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3));
461  currentRightPoseInObjFrame.block<3, 3>(0, 0) =
462  virtualPose.block<3, 3>(0, 0).transpose() * currentRightPose.block<3, 3>(0, 0);
463  currentRightPoseInObjFrame.block<3, 1>(0, 3) =
464  virtualPose.block<3, 3>(0, 0).transpose() *
465  (currentRightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3));
466  rt2InterfaceBuffer.getWriteBuffer().currentLeftPoseInObjFrame = currentLeftPoseInObjFrame;
467  rt2InterfaceBuffer.getWriteBuffer().currentRightPoseInObjFrame = currentRightPoseInObjFrame;
468 
469 
470  // --------------------------------------------- get control parameters ---------------------------------------
471  KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
472  KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
473  KmAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KmAdmittance;
474  KpAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KpAdmittance;
475  KdAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KdAdmittance;
476 
477  if (ftcalibrationTimer < cfg->ftCalibrationTime)
478  {
479  ftOffset.block<3, 1>(0, 0) =
480  0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
481  ftOffset.block<3, 1>(3, 0) =
482  0.5 * ftOffset.block<3, 1>(3, 0) + 0.5 * leftForceTorque->torque;
483  ftOffset.block<3, 1>(6, 0) =
484  0.5 * ftOffset.block<3, 1>(6, 0) + 0.5 * rightForceTorque->force;
485  ftOffset.block<3, 1>(9, 0) =
486  0.5 * ftOffset.block<3, 1>(9, 0) + 0.5 * rightForceTorque->torque;
487 
488  for (int i = 0; i < KmAdmittance.size(); ++i)
489  {
490  KmAdmittance(i) = 0;
491  }
492  }
493  else
494  {
495  for (int i = 0; i < KmAdmittance.size(); ++i)
496  {
497  KmAdmittance(i) = cfg->KmAdmittance.at(i);
498  }
499  }
500 
501  // -------------------------------------------- target wrench ---------------------------------------------
502  Eigen::VectorXf deltaPoseForWrenchControl;
503  deltaPoseForWrenchControl.setZero(12);
504  for (size_t i = 0; i < 12; ++i)
505  {
506  if (KpImpedance(i) == 0)
507  {
508  deltaPoseForWrenchControl(i) = 0;
509  }
510  else
511  {
512  deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
513  if (deltaPoseForWrenchControl(i) > 0.1)
514  {
515  deltaPoseForWrenchControl(i) = 0.1;
516  }
517  else if (deltaPoseForWrenchControl(i) < -0.1)
518  {
519  deltaPoseForWrenchControl(i) = -0.1;
520  }
521  // deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
522  }
523  }
524 
525 
526  // --------------------------------------------- grasp matrix ---------------------------------------------
527 
528  Eigen::Vector3f objCom2TCPLeftInGlobal =
529  currentLeftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
530  Eigen::Vector3f objCom2TCPRightInGlobal =
531  currentRightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
532 
533  Eigen::MatrixXf graspMatrix;
534  graspMatrix.setZero(6, 12);
535  graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
536  graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
537  // graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
538  // graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
539 
540  // Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
541  graspMatrix.block<3, 3>(3, 0) = skew(objCom2TCPLeftInGlobal);
542 
543  // rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
544  graspMatrix.block<3, 3>(3, 6) = skew(objCom2TCPRightInGlobal);
545 
546  // // projection of grasp matrix
547  // Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
548  // Eigen::MatrixXf G_range = pinvG * graspMatrix;
549  // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
550  float lambda = 1;
551  Eigen::MatrixXf pinvGraspMatrixT =
552  leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
553 
554  // ---------------------------------------------- object pose ----------------------------------------------
555  Eigen::Matrix4f objCurrentPose = currentLeftPose * objTransMatrixInAnchor;
556  Eigen::VectorXf objCurrentTwist;
557  // getObjStatus(objCurrentPose, objCurrentTwist);
558  objCurrentTwist.setZero(6);
559 
560  // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
561  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
562  // Jacobian matrices
563  Eigen::MatrixXf jacobiL =
564  leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
565  Eigen::MatrixXf jacobiR =
566  rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
567  jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
568  jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
569 
570  // qpos, qvel
571  Eigen::VectorXf leftqpos;
572  Eigen::VectorXf leftqvel;
573  leftqpos.resize(leftPositionSensors.size());
574  leftqvel.resize(leftVelocitySensors.size());
575  for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
576  {
577  leftqpos(i) = leftPositionSensors[i]->position;
578  leftqvel(i) = leftVelocitySensors[i]->velocity;
579  }
580 
581  Eigen::VectorXf rightqpos;
582  Eigen::VectorXf rightqvel;
583  rightqpos.resize(rightPositionSensors.size());
584  rightqvel.resize(rightVelocitySensors.size());
585 
586  for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
587  {
588  rightqpos(i) = rightPositionSensors[i]->position;
589  rightqvel(i) = rightVelocitySensors[i]->velocity;
590  }
591 
592  // -------------------------------------- compute TCP and object velocity -------------------------------------
593  Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
594  Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
595 
596  Eigen::VectorXf currentTwist(12);
597  currentTwist << currentLeftTwist, currentRightTwist;
598  objCurrentTwist = pinvGraspMatrixT * currentTwist;
599 
600  rt2ControlBuffer.getWriteBuffer().currentPoseObj = objCurrentPose;
601  rt2ControlBuffer.getWriteBuffer().currentTwistObj = objCurrentTwist;
602  rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
603  rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
604  rt2ControlBuffer.commitWrite();
605 
606  // --------------------------------------------- get ft sensor ---------------------------------------------
607  // static compensation
608  Eigen::Vector3f gravity;
609  gravity << 0.0, 0.0, -9.8;
610  Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
611  Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
612  Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
613 
614  Eigen::Vector3f localGravityRight =
615  currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
616  Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
617  Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
618 
619  // mapping of measured wrenches
620  Eigen::VectorXf wrenchesMeasured(12);
621  wrenchesMeasured << leftForceTorque->force - forceOffsetLeft,
622  leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight,
623  rightForceTorque->torque - torqueOffsetRight;
624  for (size_t i = 0; i < 12; i++)
625  {
626  wrenchesMeasured(i) =
627  (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
628  }
629  filteredOldValue = wrenchesMeasured;
630  wrenchesMeasured.block<3, 1>(0, 0) =
631  sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) -
632  localForceVecLeft;
633  wrenchesMeasured.block<3, 1>(3, 0) =
634  sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) -
635  localTorqueVecLeft;
636  wrenchesMeasured.block<3, 1>(6, 0) =
637  sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) -
638  localForceVecRight;
639  wrenchesMeasured.block<3, 1>(9, 0) =
640  sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) -
641  localTorqueVecRight;
642  // if (wrenchesMeasured.norm() < cfg->forceThreshold)
643  // {
644  // wrenchesMeasured.setZero();
645  // }
646 
647  Eigen::VectorXf forceTorqueMeasurementInRoot(12);
648  forceTorqueMeasurementInRoot.block<3, 1>(0, 0) =
649  currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
650  forceTorqueMeasurementInRoot.block<3, 1>(3, 0) =
651  currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
652  forceTorqueMeasurementInRoot.block<3, 1>(6, 0) =
653  currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
654  forceTorqueMeasurementInRoot.block<3, 1>(9, 0) =
655  currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
656 
657 
658  // map to object
659  Eigen::VectorXf objFTValue = graspMatrix * forceTorqueMeasurementInRoot;
660  for (size_t i = 0; i < 6; i++)
661  {
662  if (fabs(objFTValue(i)) < cfg->forceThreshold.at(i))
663  {
664  objFTValue(i) = 0;
665  }
666  else
667  {
668  objFTValue(i) -= cfg->forceThreshold.at(i) * objFTValue(i) / fabs(objFTValue(i));
669  }
670  }
671 
672  // pass sensor value to statechart
673  rt2InterfaceBuffer.getWriteBuffer().currentObjPose = objCurrentPose;
674  rt2InterfaceBuffer.getWriteBuffer().currentObjVel = objCurrentTwist.head<3>();
675  rt2InterfaceBuffer.getWriteBuffer().currentObjForce = objFTValue.head<3>();
676  rt2InterfaceBuffer.commitWrite();
677 
678 
679  // --------------------------------------------- get MP target ---------------------------------------------
680  Eigen::Matrix4f objTargetPose = rtGetControlStruct().objTargetPose;
681  Eigen::VectorXf objTargetTwist = rtGetControlStruct().objTargetTwist;
682 
683  Eigen::Matrix4f leftPoseInObj = rtGetControlStruct().leftTargetPoseInObj;
684  Eigen::VectorXf leftTwistInObj = rtGetControlStruct().leftTargetTwistInObj;
685 
686  Eigen::Matrix4f rightPoseInObj = rtGetControlStruct().rightTargetPoseInObj;
687  Eigen::VectorXf rightTwistInObj = rtGetControlStruct().rightTargetTwistInObj;
688 
689  // integrate mp target
690  integrateVel2Pose(deltaT, objTargetTwist, integratedPoseObj);
691  integrateVel2Pose(deltaT, leftTwistInObj, integratedPoseLeft);
692  integrateVel2Pose(deltaT, rightTwistInObj, integratedPoseRight);
693 
694  // --------------------------------------------- obj admittance control ---------------------------------------------
695  // do admittance control on the object, calculate the virtual pose as attractor for the impedance controller
696  Eigen::VectorXf objPoseError(6);
697  objPoseError.head<3>() = virtualPose.block<3, 1>(0, 3) - objTargetPose.block<3, 1>(0, 3);
698  Eigen::Matrix3f objDiffMat =
699  virtualPose.block<3, 3>(0, 0) * objTargetPose.block<3, 3>(0, 0).transpose();
700  objPoseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
701 
702 
703  Eigen::VectorXf objAcc;
704  Eigen::VectorXf objVel;
705  objAcc.setZero(6);
706  objVel.setZero(6);
707  for (size_t i = 0; i < 6; i++)
708  {
709  // objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * (virtualVel(i) - boxTwist(i));
710  objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) -
711  KdAdmittance(i) * virtualVel(i);
712  }
713  objVel = virtualVel + 0.5 * deltaT * (objAcc + virtualAcc);
714  Eigen::VectorXf deltaObjPose = 0.5 * deltaT * (objVel + virtualVel);
715  virtualAcc = objAcc;
716  virtualVel = objVel;
717  virtualPose.block<3, 1>(0, 3) += deltaObjPose.head<3>();
718  virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(
719  deltaObjPose(3), deltaObjPose(4), deltaObjPose(5)) *
720  virtualPose.block<3, 3>(0, 0);
721 
722  // --------------------------------------------- convert to tcp pose ---------------------------------------------
723  // [R_v, P_v // 0, 1] * [R_l P_l// 0, 1] = [R_v*R_l R_v * P_l + P_v]
724  Eigen::Matrix4f tcpTargetPoseLeft = virtualPose * leftPoseInObj;
725  Eigen::Matrix4f tcpTargetPoseRight = virtualPose * rightPoseInObj;
726  // tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeftInObjFrame - deltaPoseForWrenchControl.block<3, 1>(0, 0));
727  // tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPRightInObjFrame - deltaPoseForWrenchControl.block<3, 1>(6, 0));
728 
729 
730  // --------------------------------------------- Impedance control ---------------------------------------------
731  Eigen::VectorXf poseError(12);
732  Eigen::Matrix3f diffMat =
733  tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
734  poseError.block<3, 1>(0, 0) =
735  tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
736  poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
737 
738  diffMat =
739  tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
740  poseError.block<3, 1>(6, 0) =
741  tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
742  poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
743 
744  Eigen::VectorXf forceImpedance(12);
745  for (size_t i = 0; i < 12; i++)
746  {
747  forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
748  }
749 
750  // --------------------------------------------- Nullspace control ---------------------------------------------
751  Eigen::VectorXf leftNullspaceTorque =
752  cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
753  Eigen::VectorXf rightNullspaceTorque =
754  cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
755 
756  // --------------------------------------------- Set Torque Control Command ---------------------------------------------
757  // float lambda = 1;
758  Eigen::MatrixXf jtpinvL =
759  leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
760  Eigen::MatrixXf jtpinvR =
761  rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
762  Eigen::VectorXf leftJointDesiredTorques =
763  jacobiL.transpose() * forceImpedance.head<6>() +
764  (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
765  Eigen::VectorXf rightJointDesiredTorques =
766  jacobiR.transpose() * forceImpedance.tail<6>() +
767  (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
768 
769  // torque limit
770  for (size_t i = 0; i < leftTargets.size(); ++i)
771  {
772  float desiredTorque = leftJointDesiredTorques(i);
773  if (isnan(desiredTorque))
774  {
775  desiredTorque = 0;
776  }
777  desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
778  desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
779  rt2DebugBuffer.getWriteBuffer().desired_torques[leftJointNames[i]] =
780  leftJointDesiredTorques(i);
781  leftTargets.at(i)->torque = desiredTorque;
782  }
783 
784  for (size_t i = 0; i < rightTargets.size(); ++i)
785  {
786  float desiredTorque = rightJointDesiredTorques(i);
787  if (isnan(desiredTorque))
788  {
789  desiredTorque = 0;
790  }
791  desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
792  desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
793  rt2DebugBuffer.getWriteBuffer().desired_torques[rightJointNames[i]] =
794  rightJointDesiredTorques(i);
795  rightTargets.at(i)->torque = desiredTorque;
796  }
797 
798  // --------------------------------------------- debug output ---------------------------------------------
799  // impedance control, target TS force
800  rt2DebugBuffer.getWriteBuffer().forceImpedance = forceImpedance;
801  rt2DebugBuffer.getWriteBuffer().poseError = poseError;
802  rt2DebugBuffer.getWriteBuffer().forceTorqueMeasurementInRoot = forceTorqueMeasurementInRoot;
803 
804  // object current pose and current twist
805  rt2DebugBuffer.getWriteBuffer().objPoseVec = eigenPose2EigenVec(objCurrentPose);
806  rt2DebugBuffer.getWriteBuffer().objCurrentTwist = objCurrentTwist;
807 
808  // object force and torque
809  rt2DebugBuffer.getWriteBuffer().objForceTorque = objFTValue;
810 
811  // virtual pose, vel and acc
812  rt2DebugBuffer.getWriteBuffer().virtualPoseVec = eigenPose2EigenVec(virtualPose);
813  rt2DebugBuffer.getWriteBuffer().virtualVel = virtualVel;
814  rt2DebugBuffer.getWriteBuffer().virtualAcc = virtualAcc;
815 
816  // integrated pose
817  rt2DebugBuffer.getWriteBuffer().integratedPoseObjVec =
818  eigenPose2EigenVec(integratedPoseObj);
819  rt2DebugBuffer.getWriteBuffer().integratedPoseLeftVec =
820  eigenPose2EigenVec(integratedPoseLeft);
821  rt2DebugBuffer.getWriteBuffer().integratedPoseRightVec =
822  eigenPose2EigenVec(integratedPoseRight);
823 
824 
825  // hand poses
826  rt2DebugBuffer.getWriteBuffer().targetHandPoseInRootLeft =
827  eigenPose2EigenVec(tcpTargetPoseLeft);
828  rt2DebugBuffer.getWriteBuffer().targetHandPoseInRootRight =
829  eigenPose2EigenVec(tcpTargetPoseRight);
830  rt2DebugBuffer.getWriteBuffer().currentHandPoseInRootLeft =
831  eigenPose2EigenVec(currentLeftPose);
832  rt2DebugBuffer.getWriteBuffer().currentHandPoseInRootRight =
833  eigenPose2EigenVec(currentRightPose);
834 
835  // dmp targets
836  rt2DebugBuffer.getWriteBuffer().objTargetPoseVec = eigenPose2EigenVec(objTargetPose);
837  rt2DebugBuffer.getWriteBuffer().leftPoseVecInObj = eigenPose2EigenVec(leftPoseInObj);
838  rt2DebugBuffer.getWriteBuffer().rightPoseVecInObj = eigenPose2EigenVec(rightPoseInObj);
839  rt2DebugBuffer.getWriteBuffer().objTargetTwist = objTargetTwist;
840 
841  // parameters
842  rt2DebugBuffer.getWriteBuffer().KpImpedance = KpImpedance;
843  rt2DebugBuffer.getWriteBuffer().KdImpedance = KdImpedance;
844  rt2DebugBuffer.getWriteBuffer().KpAdmittance = KpAdmittance;
845  rt2DebugBuffer.getWriteBuffer().KdAdmittance = KdAdmittance;
846  rt2DebugBuffer.getWriteBuffer().KmAdmittance = KmAdmittance;
847  rt2DebugBuffer.getWriteBuffer().KmPID = KmPID;
848 
849  rt2DebugBuffer.commitWrite();
850  }
851 
852  void
853  NJointBimanualObjLevelMultiMPController::getObjStatus(Eigen::Matrix4f& pose,
854  Eigen::VectorXf& twist)
855  {
856  // this is a temporary function to get status of the object,
857  // in fact, this should be set via the interface function.
858  Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
859  leftPose.block<3, 1>(0, 3) *= 0.001;
860  pose = leftPose * objTransMatrixInAnchor;
861  twist.setZero(6);
862  }
863 
864  // TODO
865  // void NJointBimanualObjLevelMultiMPController::setObjStatus()
866  // {
867  // // ice interface function, create a buffer for this
868  // }
869 
870 
871  void
872  NJointBimanualObjLevelMultiMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
873  const Ice::Current&)
874  {
875  objectDMP->learnDMPFromFiles(fileNames);
876  }
877 
878  void
879  NJointBimanualObjLevelMultiMPController::learnMultiDMPFromFiles(
880  const Ice::StringSeq& objFileNames,
881  const Ice::StringSeq& leftFileNames,
882  const Ice::StringSeq& rightFileNames,
883  const Ice::Current&)
884  {
885  objectDMP->learnDMPFromFiles(objFileNames);
886  leftTCPInObjDMP->learnDMPFromFiles(leftFileNames);
887  rightTCPInObjDMP->learnDMPFromFiles(rightFileNames);
888  }
889 
890  void
891  NJointBimanualObjLevelMultiMPController::setGoals(const Ice::DoubleSeq& goals,
892  const Ice::Current& ice)
893  {
894  LockGuardType guard(controllerMutex);
895  objectDMP->setGoalPoseVec(goals);
896  }
897 
898  void
899  NJointBimanualObjLevelMultiMPController::setMultiMPGoals(const Ice::DoubleSeq& goalObj,
900  const Ice::DoubleSeq& goalLeft,
901  const Ice::DoubleSeq& goalRight,
902  const Ice::Current& ice)
903  {
904  LockGuardType guard(controllerMutex);
905  objectDMP->setGoalPoseVec(goalObj);
906  leftTCPInObjDMP->setGoalPoseVec(goalLeft);
907  rightTCPInObjDMP->setGoalPoseVec(goalRight);
908  }
909 
910  void
911  NJointBimanualObjLevelMultiMPController::integrateVel2Pose(const double deltaT,
912  Eigen::VectorXf& vel,
913  Eigen::Matrix4f& pose)
914  {
915  Eigen::Matrix3f deltaRot =
916  VirtualRobot::MathTools::rpy2eigen3f(vel.tail<3>() * static_cast<float>(deltaT));
917  pose.block<3, 3>(0, 0) = deltaRot * pose.block<3, 3>(0, 0);
918  pose.block<3, 1>(0, 3) += vel.head<3>() * static_cast<float>(deltaT);
919  }
920 
921  std::vector<double>
922  NJointBimanualObjLevelMultiMPController::eigenPose2Vec(const Eigen::Matrix4f& pose)
923  {
924  VirtualRobot::MathTools::Quaternion ori = VirtualRobot::MathTools::eigen4f2quat(pose);
925  std::vector<double> poseVec{pose(0, 3), pose(1, 3), pose(2, 3), ori.w, ori.x, ori.y, ori.z};
926  return poseVec;
927  }
928 
929  Eigen::VectorXf
930  NJointBimanualObjLevelMultiMPController::eigenPose2EigenVec(const Eigen::Matrix4f& pose)
931  {
932  VirtualRobot::MathTools::Quaternion ori = VirtualRobot::MathTools::eigen4f2quat(pose);
933  Eigen::VectorXf poseVec;
934  poseVec.setZero(7);
935  poseVec(0) = pose(0, 3);
936  poseVec(1) = pose(1, 3);
937  poseVec(2) = pose(2, 3);
938  poseVec(3) = ori.w;
939  poseVec(4) = ori.x;
940  poseVec(5) = ori.y;
941  poseVec(6) = ori.z;
942  return poseVec;
943  }
944 
945  void
946  NJointBimanualObjLevelMultiMPController::runDMP(const Ice::DoubleSeq& goalObj,
947  const Ice::DoubleSeq& goalLeft,
948  const Ice::DoubleSeq& goalRight,
949  Ice::Double timeDuration,
950  const Ice::Current&)
951  {
952  while (!rt2InterfaceBuffer.updateReadBuffer())
953  {
954  usleep(1000);
955  }
956 
957  Eigen::Matrix4f leftInitPose =
958  rt2InterfaceBuffer.getUpToDateReadBuffer().currentLeftPoseInObjFrame;
959  Eigen::Matrix4f rightInitPose =
960  rt2InterfaceBuffer.getUpToDateReadBuffer().currentRightPoseInObjFrame;
961  Eigen::Matrix4f objInitPose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjPose;
962  std::vector<double> objInitPoseVec = eigenPose2Vec(objInitPose);
963  std::vector<double> leftInitPoseVec = eigenPose2Vec(leftInitPose);
964  std::vector<double> rightInitPoseVec = eigenPose2Vec(rightInitPose);
965  ARMARX_INFO << "get init poses: \n"
966  << VAROUT(objInitPoseVec) << "\n"
967  << VAROUT(leftInitPoseVec) << "\n"
968  << VAROUT(rightInitPoseVec);
969 
970  // set the integrated left/right pose when start to run dmp
971  integratedPoseObj = objInitPose;
972  integratedPoseLeft = leftInitPose;
973  integratedPoseRight = rightInitPose;
974 
975  objectDMP->prepareExecution(objInitPoseVec, goalObj);
976  objectDMP->canVal = timeDuration;
977  objectDMP->config.motionTimeDuration = timeDuration;
978 
979  leftTCPInObjDMP->prepareExecution(leftInitPose, getLocalPose(goalObj, goalLeft));
980  leftTCPInObjDMP->canVal = timeDuration;
981  leftTCPInObjDMP->config.motionTimeDuration = timeDuration;
982 
983  rightTCPInObjDMP->prepareExecution(rightInitPose, getLocalPose(goalObj, goalRight));
984  rightTCPInObjDMP->canVal = timeDuration;
985  rightTCPInObjDMP->config.motionTimeDuration = timeDuration;
986  ARMARX_INFO << "DMP prepare execution is done";
987 
988  finished = false;
989  dmpStarted = true;
990  }
991 
992  Eigen::Matrix4f
993  NJointBimanualObjLevelMultiMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate,
994  const Eigen::Matrix4f& globalTargetPose)
995  {
996  Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
997 
998  localPose.block<3, 3>(0, 0) =
999  newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
1000  localPose.block<3, 1>(0, 3) =
1001  newCoordinate.block<3, 3>(0, 0).inverse() *
1002  (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
1003 
1004 
1005  return localPose;
1006  }
1007 
1008  Eigen::Matrix4f
1009  NJointBimanualObjLevelMultiMPController::getLocalPose(
1010  const std::vector<double>& newCoordinateVec,
1011  const std::vector<double>& globalTargetPoseVec)
1012  {
1013  Eigen::Matrix4f newCoordinate =
1014  VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4),
1015  newCoordinateVec.at(5),
1016  newCoordinateVec.at(6),
1017  newCoordinateVec.at(3));
1018  newCoordinate(0, 3) = newCoordinateVec.at(0);
1019  newCoordinate(1, 3) = newCoordinateVec.at(1);
1020  newCoordinate(2, 3) = newCoordinateVec.at(2);
1021 
1022  Eigen::Matrix4f globalTargetPose =
1023  VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4),
1024  globalTargetPoseVec.at(5),
1025  globalTargetPoseVec.at(6),
1026  globalTargetPoseVec.at(3));
1027  globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
1028  globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
1029  globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
1030 
1031  return getLocalPose(newCoordinate, globalTargetPose);
1032  }
1033 
1034  void
1035  NJointBimanualObjLevelMultiMPController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts,
1036  const Ice::DoubleSeq& goals,
1037  Ice::Double timeDuration,
1038  const Ice::Current&)
1039  {
1040  ARMARX_ERROR << "implementation not complete!!!";
1041  while (!rt2InterfaceBuffer.updateReadBuffer())
1042  {
1043  usleep(1000);
1044  }
1045  ARMARX_IMPORTANT << "obj level control: setup dmp ...";
1046  objectDMP->prepareExecution(starts, goals);
1047  objectDMP->canVal = timeDuration;
1048  objectDMP->config.motionTimeDuration = timeDuration;
1049 
1050  finished = false;
1051  dmpStarted = true;
1052 
1053  ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
1054  }
1055 
1056  void
1057  NJointBimanualObjLevelMultiMPController::setViaPoints(Ice::Double u,
1058  const Ice::DoubleSeq& viapoint,
1059  const Ice::Current&)
1060  {
1061  // LockGuardType guard(controllerMutex);
1062  ARMARX_INFO << "setting via points ";
1063  objectDMP->setViaPose(u, viapoint);
1064  }
1065 
1066  void
1067  NJointBimanualObjLevelMultiMPController::removeAllViaPoints(const Ice::Current&)
1068  {
1069  objectDMP->removeAllViaPoints();
1070  }
1071 
1072  void
1073  NJointBimanualObjLevelMultiMPController::setKpImpedance(const Ice::FloatSeq& value,
1074  const Ice::Current&)
1075  {
1076 
1077  Eigen::VectorXf setpoint;
1078  setpoint.setZero(value.size());
1079  ARMARX_CHECK_EQUAL(value.size(), 12);
1080 
1081  for (size_t i = 0; i < value.size(); ++i)
1082  {
1083  setpoint(i) = value.at(i);
1084  }
1085 
1086  LockGuardType guard{interfaceDataMutex};
1087  interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
1088  interface2rtBuffer.commitWrite();
1089  }
1090 
1091  void
1092  NJointBimanualObjLevelMultiMPController::setAmplitude(Ice::Double amp, const Ice::Current&)
1093  {
1094  LockGuardType guard{controllerMutex};
1095  if (objectDMP->config.DMPStyle == "Periodic")
1096  {
1097  objectDMP->setAmplitude(amp);
1098  }
1099 
1100  if (leftTCPInObjDMP->config.DMPStyle == "Periodic")
1101  {
1102  leftTCPInObjDMP->setAmplitude(amp);
1103  }
1104 
1105  if (rightTCPInObjDMP->config.DMPStyle == "Periodic")
1106  {
1107  rightTCPInObjDMP->setAmplitude(amp);
1108  }
1109  }
1110 
1111  void
1112  NJointBimanualObjLevelMultiMPController::setKdImpedance(const Ice::FloatSeq& value,
1113  const Ice::Current&)
1114  {
1115  Eigen::VectorXf setpoint;
1116  setpoint.setZero(value.size());
1117  ARMARX_CHECK_EQUAL(value.size(), 12);
1118 
1119  for (size_t i = 0; i < value.size(); ++i)
1120  {
1121  setpoint(i) = value.at(i);
1122  }
1123 
1124  LockGuardType guard{interfaceDataMutex};
1125  interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
1126  interface2rtBuffer.commitWrite();
1127  }
1128 
1129  void
1130  NJointBimanualObjLevelMultiMPController::setKpAdmittance(const Ice::FloatSeq& value,
1131  const Ice::Current&)
1132  {
1133  Eigen::VectorXf setpoint;
1134  setpoint.setZero(value.size());
1135  ARMARX_CHECK_EQUAL(value.size(), 6);
1136 
1137  for (size_t i = 0; i < value.size(); ++i)
1138  {
1139  setpoint(i) = value.at(i);
1140  }
1141 
1142  LockGuardType guard{interfaceDataMutex};
1143  interface2rtBuffer.getWriteBuffer().KpAdmittance = setpoint;
1144  interface2rtBuffer.commitWrite();
1145  }
1146 
1147  void
1148  NJointBimanualObjLevelMultiMPController::setKdAdmittance(const Ice::FloatSeq& value,
1149  const Ice::Current&)
1150  {
1151  Eigen::VectorXf setpoint;
1152  setpoint.setZero(value.size());
1153  ARMARX_CHECK_EQUAL(value.size(), 6);
1154 
1155  for (size_t i = 0; i < value.size(); ++i)
1156  {
1157  setpoint(i) = value.at(i);
1158  }
1159 
1160  LockGuardType guard{interfaceDataMutex};
1161  interface2rtBuffer.getWriteBuffer().KdAdmittance = setpoint;
1162  interface2rtBuffer.commitWrite();
1163  }
1164 
1165  void
1166  NJointBimanualObjLevelMultiMPController::setKmAdmittance(const Ice::FloatSeq& value,
1167  const Ice::Current&)
1168  {
1169  Eigen::VectorXf setpoint;
1170  setpoint.setZero(value.size());
1171  ARMARX_CHECK_EQUAL(value.size(), 6);
1172 
1173  for (size_t i = 0; i < value.size(); ++i)
1174  {
1175  setpoint(i) = value.at(i);
1176  }
1177 
1178  LockGuardType guard{interfaceDataMutex};
1179  interface2rtBuffer.getWriteBuffer().KmAdmittance = setpoint;
1180  interface2rtBuffer.commitWrite();
1181  }
1182 
1183  std::vector<float>
1184  NJointBimanualObjLevelMultiMPController::getCurrentObjVel(const Ice::Current&)
1185  {
1186  Eigen::Vector3f tvel = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
1187  std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
1188  return tvelvec;
1189  }
1190 
1191  std::vector<float>
1192  NJointBimanualObjLevelMultiMPController::getCurrentObjForce(const Ice::Current&)
1193  {
1194  Eigen::Vector3f fvel = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjForce;
1195  std::vector<float> fvelvec = {fvel(0), fvel(1), fvel(2)};
1196  return fvelvec;
1197  }
1198 
1199  void
1200  NJointBimanualObjLevelMultiMPController::publishVec(const Eigen::VectorXf& bufferVec,
1201  const std::string name,
1202  StringVariantBaseMap& datafields)
1203  {
1204  for (int i = 0; i < bufferVec.rows(); ++i)
1205  {
1206  std::string data_name = name + "_" + std::to_string(i);
1207  datafields[data_name] = new Variant(bufferVec(i));
1208  }
1209  }
1210 
1211  void
1212  NJointBimanualObjLevelMultiMPController::onPublish(const SensorAndControl&,
1213  const DebugDrawerInterfacePrx&,
1214  const DebugObserverInterfacePrx& debugObs)
1215  {
1216 
1217  StringVariantBaseMap datafields;
1218  auto values = rt2DebugBuffer.getUpToDateReadBuffer().desired_torques;
1219  for (auto& pair : values)
1220  {
1221  datafields[pair.first] = new Variant(pair.second);
1222  }
1223 
1224  publishVec(
1225  rt2DebugBuffer.getUpToDateReadBuffer().forceImpedance, "forceImpedance", datafields);
1226  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forcePID, "forcePID", datafields);
1227  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().poseError, "poseError", datafields);
1228 
1229  // ------------------ force torque measurement of hands ------------------
1230  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forceTorqueMeasurementInRoot,
1231  "forceTorqueMeasurementInRoot",
1232  datafields);
1233 
1234  // ------------------ object force torques ------------------
1235  publishVec(
1236  rt2DebugBuffer.getUpToDateReadBuffer().objForceTorque, "objForceTorque", datafields);
1237 
1238  // ------------------ virtual pose, vel and acc ------------------
1239  publishVec(
1240  rt2DebugBuffer.getUpToDateReadBuffer().virtualPoseVec, "virtualPoseVec", datafields);
1241  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().virtualVel, "virtualVel", datafields);
1242 
1243  // ------------------ object pose vec, vel ------------------
1244  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objPoseVec, "objPoseVec", datafields);
1245  publishVec(
1246  rt2DebugBuffer.getUpToDateReadBuffer().objCurrentTwist, "objCurrentTwist", datafields);
1247 
1248  // ------------------ hand poses ------------------
1249  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootLeft,
1250  "targetHandPoseInRootLeft",
1251  datafields);
1252  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootRight,
1253  "targetHandPoseInRootRight",
1254  datafields);
1255  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootLeft,
1256  "currentHandPoseInRootLeft",
1257  datafields);
1258  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootRight,
1259  "currentHandPoseInRootRight",
1260  datafields);
1261 
1262  // ------------------ dmp targets ------------------
1263  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objTargetPoseVec,
1264  "objTargetPoseVec",
1265  datafields);
1266  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().leftPoseVecInObj,
1267  "leftPoseVecInObj",
1268  datafields);
1269  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().rightPoseVecInObj,
1270  "rightPoseVecInObj",
1271  datafields);
1272  publishVec(
1273  rt2DebugBuffer.getUpToDateReadBuffer().objTargetTwist, "objTargetTwist", datafields);
1274 
1275  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseObjVec,
1276  "integratedPoseObjVec",
1277  datafields);
1278  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseLeftVec,
1279  "integratedPoseLeftVec",
1280  datafields);
1281  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseRightVec,
1282  "integratedPoseRightVec",
1283  datafields);
1284 
1285  // ------------------ controller parameters ------------------
1286  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KpImpedance, "KpImpedance", datafields);
1287  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KdImpedance, "KdImpedance", datafields);
1288  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KpAdmittance, "KpAdmittance", datafields);
1289  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KdAdmittance, "KdAdmittance", datafields);
1290  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KmAdmittance, "KmAdmittance", datafields);
1291  publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KmPID, "KmPID", datafields);
1292 
1293  // Eigen::VectorXf forceImpedance = rt2DebugBuffer.getUpToDateReadBuffer().forceImpedance;
1294  // for (int i = 0; i < forceImpedance.rows(); ++i)
1295  // {
1296  // std::stringstream ss;
1297  // ss << i;
1298  // std::string data_name = "forceImpedance_" + ss.str();
1299  // datafields[data_name] = new Variant(forceImpedance(i));
1300  // }
1301 
1302  // Eigen::VectorXf forcePID = rt2DebugBuffer.getUpToDateReadBuffer().forcePID;
1303  // for (int i = 0; i < forcePID.rows(); ++i)
1304  // {
1305  // std::stringstream ss;
1306  // ss << i;
1307  // std::string data_name = "forcePID_" + ss.str();
1308  // datafields[data_name] = new Variant(forcePID(i));
1309  // }
1310 
1311 
1312  // Eigen::VectorXf poseError = rt2DebugBuffer.getUpToDateReadBuffer().poseError;
1313  // for (int i = 0; i < poseError.rows(); ++i)
1314  // {
1315  // std::stringstream ss;
1316  // ss << i;
1317  // std::string data_name = "poseError_" + ss.str();
1318  // datafields[data_name] = new Variant(poseError(i));
1319  // }
1320 
1321  // // ------------------ force torque measurement of hands ------------------
1322  // Eigen::VectorXf forceTorqueMeasurementInRoot = rt2DebugBuffer.getUpToDateReadBuffer().forceTorqueMeasurementInRoot;
1323  // for (int i = 0; i < forceTorqueMeasurementInRoot.rows(); ++i)
1324  // {
1325  // std::stringstream ss;
1326  // ss << i;
1327  // std::string data_name = "forceTorqueMeasurementInRoot_" + ss.str();
1328  // datafields[data_name] = new Variant(forceTorqueMeasurementInRoot(i));
1329  // }
1330 
1331  // ------------------ object force torques ------------------
1332  // Eigen::VectorXf objForceTorque = rt2DebugBuffer.getUpToDateReadBuffer().objForceTorque;
1333  // for (int i = 0; i < objForceTorque.rows(); ++i)
1334  // {
1335  // std::stringstream ss;
1336  // ss << i;
1337  // std::string data_name = "objForceTorque_" + ss.str();
1338  // datafields[data_name] = new Variant(objForceTorque(i));
1339  // }
1340 
1341  // ------------------ virtual pose, vel and acc ------------------
1342  // Eigen::VectorXf virtualPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().virtualPoseVec;
1343  // for (int i = 0; i < virtualPoseVec.rows(); ++i)
1344  // {
1345  // std::stringstream ss;
1346  // ss << i;
1347  // std::string data_name = "virtualPoseVec_" + ss.str();
1348  // datafields[data_name] = new Variant(virtualPoseVec(i));
1349  // }
1350 
1351  // Eigen::VectorXf virtualVel = rt2DebugBuffer.getUpToDateReadBuffer().virtualVel;
1352  // for (int i = 0; i < virtualVel.rows(); ++i)
1353  // {
1354  // std::stringstream ss;
1355  // ss << i;
1356  // std::string data_name = "virtualVel_" + ss.str();
1357  // datafields[data_name] = new Variant(virtualVel(i));
1358  // }
1359 
1360  // ------------------ object pose vec, vel ------------------
1361  // Eigen::VectorXf objPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().objPoseVec;
1362  // for (int i = 0; i < objPoseVec.rows(); ++i)
1363  // {
1364  // std::stringstream ss;
1365  // ss << i;
1366  // std::string data_name = "objPoseVec_" + ss.str();
1367  // datafields[data_name] = new Variant(objPoseVec(i));
1368  // }
1369 
1370  // Eigen::VectorXf objCurrentTwist = rt2DebugBuffer.getUpToDateReadBuffer().objCurrentTwist;
1371  // for (int i = 0; i < objCurrentTwist.rows(); ++i)
1372  // {
1373  // std::stringstream ss;
1374  // ss << i;
1375  // std::string data_name = "objCurrentTwist_" + ss.str();
1376  // datafields[data_name] = new Variant(objCurrentTwist(i));
1377  // }
1378 
1379  // ------------------ hand poses ------------------
1380  // Eigen::VectorXf targetHandPoseInRootLeft = rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootLeft;
1381  // for (int i = 0; i < targetHandPoseInRootLeft.rows(); ++i)
1382  // {
1383  // std::stringstream ss;
1384  // ss << i;
1385  // std::string data_name = "targetHandPoseInRootLeft_" + ss.str();
1386  // datafields[data_name] = new Variant(targetHandPoseInRootLeft(i));
1387  // }
1388  // Eigen::VectorXf targetHandPoseInRootRight = rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootRight;
1389  // for (int i = 0; i < targetHandPoseInRootRight.rows(); ++i)
1390  // {
1391  // std::stringstream ss;
1392  // ss << i;
1393  // std::string data_name = "targetHandPoseInRootRight_" + ss.str();
1394  // datafields[data_name] = new Variant(targetHandPoseInRootRight(i));
1395  // }
1396  // Eigen::VectorXf currentHandPoseInRootLeft = rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootLeft;
1397  // for (int i = 0; i < currentHandPoseInRootLeft.rows(); ++i)
1398  // {
1399  // std::stringstream ss;
1400  // ss << i;
1401  // std::string data_name = "currentHandPoseInRootLeft_" + ss.str();
1402  // datafields[data_name] = new Variant(currentHandPoseInRootLeft(i));
1403  // }
1404  // Eigen::VectorXf currentHandPoseInRootRight = rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootRight;
1405  // for (int i = 0; i < currentHandPoseInRootRight.rows(); ++i)
1406  // {
1407  // std::stringstream ss;
1408  // ss << i;
1409  // std::string data_name = "currentHandPoseInRootRight_" + ss.str();
1410  // datafields[data_name] = new Variant(currentHandPoseInRootRight(i));
1411  // }
1412  // ------------------ dmp targets ------------------
1413  // Eigen::VectorXf objTargetPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().objTargetPoseVec;
1414  // for (int i = 0; i < objTargetPoseVec.rows(); ++i)
1415  // {
1416  // std::stringstream ss;
1417  // ss << i;
1418  // std::string data_name = "objTargetPoseVec_" + ss.str();
1419  // datafields[data_name] = new Variant(objTargetPoseVec(i));
1420  // }
1421  // Eigen::VectorXf leftPoseVecInObj = rt2DebugBuffer.getUpToDateReadBuffer().leftPoseVecInObj;
1422  // for (int i = 0; i < leftPoseVecInObj.rows(); ++i)
1423  // {
1424  // std::stringstream ss;
1425  // ss << i;
1426  // std::string data_name = "leftPoseVecInObj_" + ss.str();
1427  // datafields[data_name] = new Variant(leftPoseVecInObj(i));
1428  // }
1429  // Eigen::VectorXf rightPoseVecInObj = rt2DebugBuffer.getUpToDateReadBuffer().rightPoseVecInObj;
1430  // for (int i = 0; i < rightPoseVecInObj.rows(); ++i)
1431  // {
1432  // std::stringstream ss;
1433  // ss << i;
1434  // std::string data_name = "rightPoseVecInObj_" + ss.str();
1435  // datafields[data_name] = new Variant(rightPoseVecInObj(i));
1436  // }
1437  // Eigen::VectorXf objTargetTwist = rt2DebugBuffer.getUpToDateReadBuffer().objTargetTwist;
1438  // for (int i = 0; i < objTargetTwist.rows(); ++i)
1439  // {
1440  // std::stringstream ss;
1441  // ss << i;
1442  // std::string data_name = "objTargetTwist_" + ss.str();
1443  // datafields[data_name] = new Variant(objTargetTwist(i));
1444  // }
1445 
1446  // parameters
1447  // Eigen::VectorXf KpImpedance = rt2DebugBuffer.getUpToDateReadBuffer().KpImpedance;
1448  // for (int i = 0; i < KpImpedance.rows(); ++i)
1449  // {
1450  // std::stringstream ss;
1451  // ss << i;
1452  // std::string data_name = "KpImpedance_" + ss.str();
1453  // datafields[data_name] = new Variant(KpImpedance(i));
1454  // }
1455  // Eigen::VectorXf KdImpedance = rt2DebugBuffer.getUpToDateReadBuffer().KdImpedance;
1456  // for (int i = 0; i < KdImpedance.rows(); ++i)
1457  // {
1458  // std::stringstream ss;
1459  // ss << i;
1460  // std::string data_name = "KdImpedance_" + ss.str();
1461  // datafields[data_name] = new Variant(KdImpedance(i));
1462  // }
1463  // Eigen::VectorXf KpAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KpAdmittance;
1464  // for (int i = 0; i < KpAdmittance.rows(); ++i)
1465  // {
1466  // std::stringstream ss;
1467  // ss << i;
1468  // std::string data_name = "KpAdmittance_" + ss.str();
1469  // datafields[data_name] = new Variant(KpAdmittance(i));
1470  // }
1471  // Eigen::VectorXf KdAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KdAdmittance;
1472  // for (int i = 0; i < KdAdmittance.rows(); ++i)
1473  // {
1474  // std::stringstream ss;
1475  // ss << i;
1476  // std::string data_name = "KdAdmittance_" + ss.str();
1477  // datafields[data_name] = new Variant(KdAdmittance(i));
1478  // }
1479  // Eigen::VectorXf KmAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KmAdmittance;
1480  // for (int i = 0; i < KmAdmittance.rows(); ++i)
1481  // {
1482  // std::stringstream ss;
1483  // ss << i;
1484  // std::string data_name = "KmAdmittance_" + ss.str();
1485  // datafields[data_name] = new Variant(KmAdmittance(i));
1486  // }
1487  // Eigen::VectorXf KmPID = rt2DebugBuffer.getUpToDateReadBuffer().KmPID;
1488  // for (int i = 0; i < KmPID.rows(); ++i)
1489  // {
1490  // std::stringstream ss;
1491  // ss << i;
1492  // std::string data_name = "KmPID_" + ss.str();
1493  // datafields[data_name] = new Variant(KmPID(i));
1494  // }
1495 
1496  debugObs->setDebugChannel("BimanualForceController", datafields);
1497  }
1498 
1499  void
1500  NJointBimanualObjLevelMultiMPController::onInitNJointController()
1501  {
1502  ARMARX_INFO << "====== init bimanual multi mp controller ======";
1503  runTask("NJointBimanualObjLevelMultiMPController",
1504  [&]
1505  {
1506  CycleUtil c(1);
1507  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
1508  while (getState() == eManagedIceObjectStarted)
1509  {
1510  if (isControllerActive())
1511  {
1512  controllerRun();
1513  }
1514  c.waitForCycleDuration();
1515  }
1516  });
1517  }
1518 
1519  void
1520  NJointBimanualObjLevelMultiMPController::onDisconnectNJointController()
1521  {
1522  ARMARX_INFO << "====== stop bimanual multi mp controller ======";
1523  }
1524 } // namespace armarx::control::deprecated_njoint_mp_controller::bimanual
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelMultiMPControlData >::reinitTripleBuffer
void reinitTripleBuffer(const NJointBimanualObjLevelMultiMPControlData &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::bimanual::registrationControllerNJointBimanualObjLevelMultiMPController
NJointControllerRegistration< NJointBimanualObjLevelMultiMPController > registrationControllerNJointBimanualObjLevelMultiMPController("NJointBimanualObjLevelMultiMPController")
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kori
float Kori
Definition: TaskSpaceVMP.h:54
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::getClassName
std::string getClassName(const Ice::Current &) const
Definition: NJointBimanualObjLevelMultiMPController.cpp:319
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::eigenPose2Vec
std::vector< double > eigenPose2Vec(const Eigen::Matrix4f &pose)
Definition: NJointBimanualObjLevelMultiMPController.cpp:922
MathUtils.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setKpAdmittance
void setKpAdmittance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:1130
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dpos
float Dpos
Definition: TaskSpaceVMP.h:53
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:92
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData
Definition: NJointBimanualObjLevelMultiMPController.h:28
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelMultiMPControlData >::rtGetControlStruct
const NJointBimanualObjLevelMultiMPControlData & 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::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::getLocalPose
Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f &newCoordinate, const Eigen::Matrix4f &globalTargetPose)
Definition: NJointBimanualObjLevelMultiMPController.cpp:993
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setAmplitude
void setAmplitude(Ice::Double amp, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:1092
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::NJointBimanualObjLevelMultiMPController
NJointBimanualObjLevelMultiMPController(const RobotUnitPtr &, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:29
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::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:163
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::NJointControllerBase::useControlTarget
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition: NJointController.cpp:410
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::onDisconnectNJointController
void onDisconnectNJointController()
Definition: NJointBimanualObjLevelMultiMPController.cpp:1520
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData::rightTargetTwistInObj
Eigen::VectorXf rightTargetTwistInObj
Definition: NJointBimanualObjLevelMultiMPController.h:39
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
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::eigenPose2EigenVec
Eigen::VectorXf eigenPose2EigenVec(const Eigen::Matrix4f &pose)
Definition: NJointBimanualObjLevelMultiMPController.cpp:930
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setKpImpedance
void setKpImpedance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:1073
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig
Definition: TaskSpaceVMP.h:59
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::phaseStopParas
PhaseStopParams phaseStopParas
Definition: TaskSpaceVMP.h:67
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelMultiMPControlData >::getWriterControlStruct
NJointBimanualObjLevelMultiMPControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition: TaskSpaceVMP.h:48
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelMultiMPControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:1212
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData::objTargetPose
Eigen::Matrix4f objTargetPose
Definition: NJointBimanualObjLevelMultiMPController.h:32
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::bimanual::NJointBimanualObjLevelMultiMPController::learnMultiDMPFromFiles
void learnMultiDMPFromFiles(const Ice::StringSeq &objFileNames, const Ice::StringSeq &leftFileNames, const Ice::StringSeq &rightFileNames, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:879
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::bimanual::NJointBimanualObjLevelMultiMPController::publishVec
void publishVec(const Eigen::VectorXf &bufferVec, const std::string name, StringVariantBaseMap &datafields)
Definition: NJointBimanualObjLevelMultiMPController.cpp:1200
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::maxValue
float maxValue
Definition: TaskSpaceVMP.h:50
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:920
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::integrateVel2Pose
void integrateVel2Pose(const double deltaT, Eigen::VectorXf &vel, Eigen::Matrix4f &pose)
Definition: NJointBimanualObjLevelMultiMPController.cpp:911
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::bimanual::NJointBimanualObjLevelMultiMPControlData::leftTargetPoseInObj
Eigen::Matrix4f leftTargetPoseInObj
Definition: NJointBimanualObjLevelMultiMPController.h:35
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:855
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setKdImpedance
void setKdImpedance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:1112
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition: NJointControllerBase.h:778
armarx::NJointBimanualObjLevelMultiMPControllerInterface::getCurrentObjForce
Ice::FloatSeq getCurrentObjForce()
armarx::PIDController
Definition: PIDController.h:43
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::NJointBimanualObjLevelMultiMPControllerInterface::getCurrentObjVel
Ice::FloatSeq getCurrentObjVel()
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setMultiMPGoals
void setMultiMPGoals(const Ice::DoubleSeq &goalObj, const Ice::DoubleSeq &goalLeft, const Ice::DoubleSeq &goalRight, const Ice::Current &ice)
Definition: NJointBimanualObjLevelMultiMPController.cpp:899
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelMultiMPControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:108
armarx::SensorValueForceTorque::torque
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION Eigen::Vector3f torque
Definition: SensorValueForceTorque.h:35
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData::objTargetTwist
Eigen::VectorXf objTargetTwist
Definition: NJointBimanualObjLevelMultiMPController.h:33
ArmarXObjectScheduler.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:1057
ControlTarget1DoFActuator.h
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelMultiMPControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition: NJointBimanualObjLevelMultiMPController.cpp:400
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:891
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::controllerRun
void controllerRun()
Definition: NJointBimanualObjLevelMultiMPController.cpp:325
NJointBimanualObjLevelMultiMPController.h
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition: TaskSpaceVMP.h:64
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::runDMP
void runDMP(const Ice::DoubleSeq &goalObj, const Ice::DoubleSeq &goalLeft, const Ice::DoubleSeq &goalRight, Ice::Double timeDuration, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:946
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::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition: TaskSpaceVMP.h:52
CycleUtil.h
PIDController.h
NJointController.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData::rightTargetPoseInObj
Eigen::Matrix4f rightTargetPoseInObj
Definition: NJointBimanualObjLevelMultiMPController.h:38
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setKdAdmittance
void setKdAdmittance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:1148
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition: NJointBimanualObjLevelMultiMPController.cpp:314
IceUtil::Handle< class RobotUnit >
armarx::NJointBimanualObjLevelMultiMPControllerInterface::removeAllViaPoints
void removeAllViaPoints()
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::skew
Eigen::Matrix3f skew(Eigen::Vector3f vec)
Definition: NJointBimanualObjLevelMultiMPController.h:74
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::firstLoop
bool firstLoop
Definition: NJointBimanualObjLevelMultiMPController.h:326
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::bimanual::NJointBimanualObjLevelMultiMPController::onInitNJointController
void onInitNJointController()
Definition: NJointBimanualObjLevelMultiMPController.cpp:1500
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::getObjStatus
void getObjStatus(Eigen::Matrix4f &pose, Eigen::VectorXf &twist)
Definition: NJointBimanualObjLevelMultiMPController.cpp:853
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::learnDMPFromFiles
void learnDMPFromFiles(const Ice::StringSeq &fileNames, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:872
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition: TaskSpaceVMP.h:62
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData::leftTargetTwistInObj
Eigen::VectorXf leftTargetTwistInObj
Definition: NJointBimanualObjLevelMultiMPController.h:36
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition: TaskSpaceVMP.h:56
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
armarx::TripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:116
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::SensorValueForceTorque
Definition: SensorValueForceTorque.h:32
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:772
armarx::TripleBuffer::reinitAllBuffers
std::enable_if< std::is_copy_constructible< U >::value >::type reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:171
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition: TaskSpaceVMP.h:90
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:776
SensorValue1DoFActuator.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setKmAdmittance
void setKmAdmittance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:1166
armarx::NJointControllerBase::useSensorValue
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
Definition: NJointController.cpp:383
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
armarx::control::deprecated_njoint_mp_controller::bimanual
Definition: NJointBimanualCartesianAdmittanceController.cpp:30
armarx::TripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:143
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::runDMPWithVirtualStart
void runDMPWithVirtualStart(const Ice::DoubleSeq &starts, const Ice::DoubleSeq &goals, Ice::Double timeDuration, const Ice::Current &)
Definition: NJointBimanualObjLevelMultiMPController.cpp:1035