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