NJointBimanualDMPForceController.cpp
Go to the documentation of this file.
1 #include <VirtualRobot/IK/DifferentialIK.h>
2 #include <VirtualRobot/Robot.h>
3 
4 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
5 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
6 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
7 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
8 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
9 #include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
10 #include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
11 #include <RobotAPI/libraries/core/CartesianVelocityController.h>
12 #include <RobotAPI/libraries/core/PIDController.h>
13 
14 #include "NJointBimanualCCDMPController.h"
15 #include <dmp/representation/dmp/umitsmp.h>
16 
17 namespace armarx
18 {
19  NJointControllerRegistration<NJointBimanualCCDMPController>
20  registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController");
21 
22  NJointBimanualCCDMPController::NJointBimanualCCDMPController(
23  armarx::NJointControllerDescriptionProviderInterfacePtr prov,
24  const armarx::NJointControllerConfigPtr& config,
25  const VirtualRobot::RobotPtr&)
26  {
27  ARMARX_INFO << "Preparing ... ";
28  cfg = NJointBimanualCCDMPControllerConfigPtr::dynamicCast(config);
29  ARMARX_CHECK_EXPRESSION(prov);
30  RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
31  ARMARX_CHECK_EXPRESSION(robotUnit);
32 
33  leftRNS = robotUnit->getRtRobot()->getRobotNodeSet("LeftArm");
34  rnsLeftBody = robotUnit->getRtRobot()->getRobotNodeSet("LeftArmCol");
35  for (size_t i = 0; i < leftRNS->getSize(); ++i)
36  {
37  std::string jointName = leftRNS->getNode(i)->getName();
38 
39  leftJointNames.push_back(jointName);
40  ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
41  const SensorValueBase* sv = prov->getSensorValue(jointName);
42  leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
43  const SensorValue1DoFActuatorVelocity* velocitySensor =
44  sv->asA<SensorValue1DoFActuatorVelocity>();
45  const SensorValue1DoFActuatorPosition* positionSensor =
46  sv->asA<SensorValue1DoFActuatorPosition>();
47  const SensorValue1DoFActuatorAcceleration* accelerationSensor =
48  sv->asA<SensorValue1DoFActuatorAcceleration>();
49 
50  if (!velocitySensor)
51  {
52  ARMARX_WARNING << "No velocitySensor available for " << jointName;
53  }
54  if (!positionSensor)
55  {
56  ARMARX_WARNING << "No positionSensor available for " << jointName;
57  }
58 
59  if (!accelerationSensor)
60  {
61  ARMARX_WARNING << "No accelerationSensor available for " << jointName;
62  }
63 
64 
65  leftVelocitySensors.push_back(velocitySensor);
66  leftPositionSensors.push_back(positionSensor);
67  leftAccelerationSensors.push_back(accelerationSensor);
68  };
69 
70 
71  rnsRightBody = robotUnit->getRtRobot()->getRobotNodeSet("RightArmCol");
72 
73  rightRNS = robotUnit->getRtRobot()->getRobotNodeSet("RightArm");
74  for (size_t i = 0; i < rightRNS->getSize(); ++i)
75  {
76  std::string jointName = rightRNS->getNode(i)->getName();
77  rightJointNames.push_back(jointName);
78  ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
79  const SensorValueBase* sv = prov->getSensorValue(jointName);
80  rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
81  const SensorValue1DoFActuatorVelocity* velocitySensor =
82  sv->asA<SensorValue1DoFActuatorVelocity>();
83  const SensorValue1DoFActuatorPosition* positionSensor =
84  sv->asA<SensorValue1DoFActuatorPosition>();
85  const SensorValue1DoFActuatorAcceleration* accelerationSensor =
86  sv->asA<SensorValue1DoFActuatorAcceleration>();
87 
88 
89  if (!velocitySensor)
90  {
91  ARMARX_WARNING << "No velocitySensor available for " << jointName;
92  }
93  if (!positionSensor)
94  {
95  ARMARX_WARNING << "No positionSensor available for " << jointName;
96  }
97  if (!accelerationSensor)
98  {
99  ARMARX_WARNING << "No accelerationSensor available for " << jointName;
100  }
101 
102  rightVelocitySensors.push_back(velocitySensor);
103  rightPositionSensors.push_back(positionSensor);
104  rightAccelerationSensors.push_back(accelerationSensor);
105  };
106 
107 
108  const SensorValueBase* svlf = prov->getSensorValue("FT L");
109  leftForceTorque = svlf->asA<SensorValueForceTorque>();
110  const SensorValueBase* svrf = prov->getSensorValue("FT R");
111  rightForceTorque = svrf->asA<SensorValueForceTorque>();
112 
113  leftIK.reset(new VirtualRobot::DifferentialIK(
114  leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
115  rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS,
116  rightRNS->getRobot()->getRootNode(),
117  VirtualRobot::JacobiProvider::eSVDDamped));
118 
119 
120  TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
121  taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
122  taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
123  taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
124  taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
125  taskSpaceDMPConfig.DMPAmplitude = 1.0;
126  taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
127  taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
128  taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
129  taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
130  taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
131  taskSpaceDMPConfig.phaseStopParas.Dori = 0;
132  taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
133  taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
134  taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
135 
136 
137  TaskSpaceDMPControllerPtr leftLeader(
138  new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig));
139  TaskSpaceDMPControllerPtr leftFollower(
140  new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig));
141  TaskSpaceDMPControllerPtr rightLeader(
142  new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig));
143  TaskSpaceDMPControllerPtr rightFollower(
144  new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig));
145 
146  leftGroup.push_back(leftLeader);
147  leftGroup.push_back(rightFollower);
148 
149  rightGroup.push_back(rightLeader);
150  rightGroup.push_back(leftFollower);
151 
152  bothLeaderGroup.push_back(leftLeader);
153  bothLeaderGroup.push_back(rightLeader);
154 
155 
156  tcpLeft = leftRNS->getTCP();
157  tcpRight = rightRNS->getTCP();
158  NJointBimanualCCDMPControllerSensorData initSensorData;
159  initSensorData.deltaT = 0;
160  initSensorData.currentTime = 0;
161  initSensorData.currentLeftPose = leftRNS->getTCP()->getPoseInRootFrame();
162  initSensorData.currentRightPose = rightRNS->getTCP()->getPoseInRootFrame();
163  controllerSensorData.reinitAllBuffers(initSensorData);
164 
165  leaderName = "both";
166 
167  NJointBimanualCCDMPControllerControlData initData;
168  initData.leftTargetVel.resize(6);
169  initData.leftTargetVel.setZero();
170  initData.rightTargetVel.resize(6);
171  initData.rightTargetVel.setZero();
172  reinitTripleBuffer(initData);
173 
174  leftDesiredJointValues.resize(leftTargets.size());
175  ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
176 
177  for (size_t i = 0; i < leftTargets.size(); ++i)
178  {
179  leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
180  }
181 
182  rightDesiredJointValues.resize(rightTargets.size());
183  ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
184 
185  for (size_t i = 0; i < rightTargets.size(); ++i)
186  {
187  rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
188  }
189 
190  KoriFollower = cfg->KoriFollower;
191  KposFollower = cfg->KposFollower;
192 
193  kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
194  dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
195  kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
196  dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
197 
198 
199  knull = cfg->knull;
200  dnull = cfg->dnull;
201 
202  torqueLimit = cfg->torqueLimit;
203 
204  kpf.resize(12);
205  kif.resize(12);
206  forceC_des.resize(12);
207 
208  ARMARX_CHECK_EQUAL(cfg->Kpf.size(), kpf.rows());
209  ARMARX_CHECK_EQUAL(cfg->Kif.size(), kif.rows());
210  ARMARX_CHECK_EQUAL(cfg->DesiredForce.size(), forceC_des.rows());
211 
212 
213  for (size_t i = 0; i < 12; i++)
214  {
215  kpf(i) = cfg->Kpf.at(i);
216  kif(i) = cfg->Kif.at(i);
217  forceC_des(i) = cfg->DesiredForce.at(i);
218  }
219 
220  boxWidth = cfg->BoxWidth;
221 
222  filtered_leftForce.setZero();
223  filtered_leftTorque.setZero();
224  filtered_rightForce.setZero();
225  filtered_rightTorque.setZero();
226 
227  // offset_leftForce.setZero();
228  // offset_leftTorque.setZero();
229  // offset_rightForce.setZero();
230  // offset_rightTorque.setZero();
231 
232  offset_leftForce(0) = -4.34;
233  offset_leftForce(1) = -8.21;
234  offset_leftForce(2) = 15.59;
235 
236  offset_leftTorque(0) = 1.42;
237  offset_leftTorque(1) = 0.29;
238  offset_leftTorque(2) = 0.09;
239 
240  offset_rightForce(0) = 2.88;
241  offset_rightForce(1) = 11.15;
242  offset_rightForce(2) = -20.18;
243 
244  offset_rightTorque(0) = -1.83;
245  offset_rightTorque(1) = 0.34;
246  offset_rightTorque(2) = -0.01;
247 
248 
249  filterTimeConstant = cfg->FilterTimeConstant;
250 
251  ftPIDController.resize(12);
252  for (size_t i = 0; i < ftPIDController.size(); i++)
253  {
254  ftPIDController[i].reset(new PIDController(kpf(i), kif(i), 0.0, 10, 10));
255  }
256 
257  isForceCompensateDone = false;
258  }
259 
260  std::string
261  NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
262  {
263  return "NJointBimanualCCDMPController";
264  }
265 
266  void
267  NJointBimanualCCDMPController::controllerRun()
268  {
269  if (!controllerSensorData.updateReadBuffer())
270  {
271  return;
272  }
273 
274  double deltaT = controllerSensorData.getReadBuffer().deltaT;
275 
276 
277  Eigen::VectorXf leftTargetVel;
278  leftTargetVel.resize(6);
279  leftTargetVel.setZero();
280  Eigen::VectorXf rightTargetVel;
281  rightTargetVel.resize(6);
282  rightTargetVel.setZero();
283 
284  std::vector<TaskSpaceDMPControllerPtr> currentControlGroup;
285  Eigen::Matrix4f currentLeaderPose;
286  Eigen::Matrix4f currentFollowerPose;
287  Eigen::VectorXf currentLeaderTwist;
288  Eigen::VectorXf currentFollowerTwist;
289  if (leaderName == "Left")
290  {
291  currentControlGroup = leftGroup;
292  currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
293  currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
294  currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
295  currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
296  }
297  else if (leaderName == "right")
298  {
299  currentControlGroup = rightGroup;
300  currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
301  currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
302  currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
303  currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
304  }
305  else
306  {
307  currentControlGroup = bothLeaderGroup;
308 
309  TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
310  TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
311  leaderDMPleft->flow(deltaT,
312  controllerSensorData.getReadBuffer().currentLeftPose,
313  controllerSensorData.getReadBuffer().currentLeftTwist);
314  leaderDMPright->flow(deltaT,
315  controllerSensorData.getReadBuffer().currentRightPose,
316  controllerSensorData.getReadBuffer().currentRightTwist);
317 
318  Eigen::VectorXf leftTargetVel = leaderDMPleft->getTargetVelocity();
319  Eigen::Matrix4f leftTargetPose = leaderDMPleft->getTargetPoseMat();
320  Eigen::VectorXf rightTargetVel = leaderDMPright->getTargetVelocity();
321  Eigen::Matrix4f rightTargetPose = leaderDMPright->getTargetPoseMat();
322 
323  virtualtimer = leaderDMPleft->canVal;
324  LockGuardType guard{controlDataMutex};
325  getWriterControlStruct().leftTargetVel = leftTargetVel;
326  getWriterControlStruct().rightTargetVel = rightTargetVel;
327  getWriterControlStruct().leftTargetPose = leftTargetPose;
328  getWriterControlStruct().rightTargetPose = rightTargetPose;
329  writeControlStruct();
330 
331  return;
332  }
333 
334  TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
335  TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
336 
337 
338  leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
339 
340  Eigen::Matrix4f currentFollowerLocalPose;
341  currentFollowerLocalPose.block<3, 3>(0, 0) =
342  currentFollowerPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0).inverse();
343  currentFollowerLocalPose.block<3, 1>(0, 3) =
344  currentFollowerLocalPose.block<3, 3>(0, 0) *
345  (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
346  followerDMP->flow(deltaT, currentFollowerPose, currentFollowerTwist);
347 
348  Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
349  Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
350  Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
351  std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
352 
353  Eigen::Matrix4f followerTargetPose;
354  followerTargetPose.block<3, 3>(0, 0) =
355  followerLocalTargetPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0);
356  followerTargetPose.block<3, 1>(0, 3) =
357  currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) +
358  currentLeaderPose.block<3, 1>(0, 3);
359 
360 
361  Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
362  Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
363  // followerTargetVel.block<3, 1>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetVel.block<3, 1>(0, 0) + leaderTargetVel.block<3, 1>(0, 0);
364  followerTargetVel.block<3, 1>(0, 0) =
365  KposFollower *
366  (followerTargetPose.block<3, 1>(0, 3) - currentFollowerPose.block<3, 1>(0, 3));
367 
368 
369  Eigen::Matrix3f followerDiffMat =
370  followerTargetPose.block<3, 3>(0, 0) * currentFollowerPose.block<3, 3>(0, 0).inverse();
371  Eigen::Vector3f followerDiffRPY =
372  KoriFollower * VirtualRobot::MathTools::eigen3f2rpy(followerDiffMat);
373  followerTargetVel(3) = followerDiffRPY(0);
374  followerTargetVel(4) = followerDiffRPY(1);
375  followerTargetVel(5) = followerDiffRPY(2);
376 
377  virtualtimer = leaderDMP->canVal;
378 
379 
380  std::vector<double> leftDMPTarget;
381  std::vector<double> rightDMPTarget;
382 
383  Eigen::Matrix4f leftTargetPose;
384  Eigen::Matrix4f rightTargetPose;
385 
386  if (leaderName == "Left")
387  {
388  leftTargetVel = leaderTargetVel;
389  rightTargetVel = followerTargetVel;
390 
391  leftDMPTarget = leaderDMP->getTargetPose();
392  rightDMPTarget = followerLocalTargetPoseVec;
393 
394 
395  leftTargetPose = leaderTargetPose;
396  rightTargetPose = followerLocalTargetPose;
397  }
398  else if (leaderName == "right")
399  {
400  rightTargetVel = leaderTargetVel;
401  leftTargetVel = followerTargetVel;
402 
403  rightDMPTarget = leaderDMP->getTargetPose();
404  leftDMPTarget = followerLocalTargetPoseVec;
405 
406  rightTargetPose = leaderTargetPose;
407  leftTargetPose = followerLocalTargetPose;
408  }
409 
410 
411  // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_xvel"] = leftTargetVel(0);
412  // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yvel"] = leftTargetVel(1);
413  // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_zvel"] = leftTargetVel(2);
414  // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_rollvel"] = leftTargetVel(3);
415  // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_pitchvel"] = leftTargetVel(4);
416  // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yawvel"] = leftTargetVel(5);
417 
418  // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_xvel"] = rightTargetVel(0);
419  // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yvel"] = rightTargetVel(1);
420  // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_zvel"] = rightTargetVel(2);
421  // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_rollvel"] = rightTargetVel(3);
422  // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_pitchvel"] = rightTargetVel(4);
423  // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yawvel"] = rightTargetVel(5);
424 
425 
426  // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_x"] = leftDMPTarget[0];
427  // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_y"] = leftDMPTarget[1];
428  // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_z"] = leftDMPTarget[2];
429  // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qw"] = leftDMPTarget[3];
430  // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qx"] = leftDMPTarget[4];
431  // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qy"] = leftDMPTarget[5];
432  // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qz"] = leftDMPTarget[6];
433 
434  // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_x"] = rightDMPTarget[0];
435  // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_y"] = rightDMPTarget[1];
436  // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_z"] = rightDMPTarget[2];
437  // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qw"] = rightDMPTarget[3];
438  // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qx"] = rightDMPTarget[4];
439  // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qy"] = rightDMPTarget[5];
440  // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qz"] = rightDMPTarget[6];
441 
442  // std::vector<double> currentLeftPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentLeftPose);
443  // std::vector<double> currentRightPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentRightPose);
444 
445 
446  // debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = currentLeftPoseVec[0];
447  // debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = currentLeftPoseVec[1];
448  // debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = currentLeftPoseVec[2];
449  // debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = currentLeftPoseVec[3];
450  // debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = currentLeftPoseVec[4];
451  // debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = currentLeftPoseVec[5];
452  // debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = currentLeftPoseVec[6];
453 
454  // debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = currentRightPoseVec[0];
455  // debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = currentRightPoseVec[1];
456  // debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = currentRightPoseVec[2];
457  // debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = currentRightPoseVec[3];
458  // debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = currentRightPoseVec[4];
459  // debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = currentRightPoseVec[5];
460  // debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = currentRightPoseVec[6];
461 
462 
463  // debugOutputData.getWriteBuffer().leaderCanVal = leaderDMP->debugData.canVal;
464  // debugOutputData.getWriteBuffer().leadermpcFactor = leaderDMP->debugData.mpcFactor;
465  // debugOutputData.getWriteBuffer().leadererror = leaderDMP->debugData.poseError;
466  // debugOutputData.getWriteBuffer().leaderposError = leaderDMP->debugData.posiError;
467  // debugOutputData.getWriteBuffer().leaderoriError = leaderDMP->debugData.oriError;
468 
469  // debugOutputData.getWriteBuffer().followerCanVal = followerDMP->debugData.canVal;
470  // debugOutputData.getWriteBuffer().followermpcFactor = followerDMP->debugData.mpcFactor;
471  // debugOutputData.getWriteBuffer().followererror = followerDMP->debugData.poseError;
472  // debugOutputData.getWriteBuffer().followerposError = followerDMP->debugData.posiError;
473  // debugOutputData.getWriteBuffer().followeroriError = followerDMP->debugData.oriError;
474 
475  // debugOutputData.commitWrite();
476 
477 
478  LockGuardType guard{controlDataMutex};
479  getWriterControlStruct().leftTargetVel = leftTargetVel;
480  getWriterControlStruct().rightTargetVel = rightTargetVel;
481  getWriterControlStruct().leftTargetPose = leftTargetPose;
482  getWriterControlStruct().rightTargetPose = rightTargetPose;
483  writeControlStruct();
484  }
485 
486  Eigen::VectorXf
487  NJointBimanualCCDMPController::getControlWrench(const Eigen::VectorXf& tcptwist,
488  const Eigen::Matrix4f& currentPose,
489  const Eigen::Matrix4f& targetPose)
490  {
491  // Eigen::Vector3f currentTCPLinearVelocity;
492  // currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
493  // Eigen::Vector3f currentTCPAngularVelocity;
494  // currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
495 
496  // Eigen::Vector3f currentTCPPosition = currentPose.block<3,1>(0,3);
497  // Eigen::Vector3f desiredPosition = targetPose.block<3,1>(0,3);
498  // Eigen::Vector3f tcpForces = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition);
499  // Eigen::Vector3f tcpDesiredForce = tcpForces - dpos.cwiseProduct(currentTCPLinearVelocity);
500 
501  // Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
502  // Eigen::Matrix3f diffMat = targetPose.block<3,3>(0,0) * currentRotMat.inverse();
503  // Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
504 
505  // Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
506  // Eigen::Vector6f tcpDesiredWrench;
507  // tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
508 
509  // return tcpDesiredWrench;
510  }
511 
512  void
513  NJointBimanualCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
514  const IceUtil::Time& timeSinceLastIteration)
515  {
516  double deltaT = timeSinceLastIteration.toSecondsDouble();
517  controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
518  controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
519  controllerSensorData.getWriteBuffer().deltaT = deltaT;
520  controllerSensorData.getWriteBuffer().currentTime += deltaT;
521 
522 
523  // if(controllerSensorData.getWriteBuffer().currentTime > 0.3 && !isForceCompensateDone)
524  // {
525  // offset_leftForce = filtered_leftForce;
526  // offset_leftTorque = filtered_leftTorque;
527  // offset_rightForce = filtered_rightForce;
528  // offset_rightTorque = filtered_rightTorque;
529  // isForceCompensateDone = true;
530 
531 
532  // filtered_leftForce.setZero();
533  // filtered_leftTorque.setZero();
534  // filtered_rightForce.setZero();
535  // filtered_rightTorque.setZero();
536  // }
537 
538  // if(controllerSensorData.getWriteBuffer().currentTime <= 0.3)
539  // {
540  // // torque limit
541  // for (size_t i = 0; i < leftTargets.size(); ++i)
542  // {
543  // leftTargets.at(i)->torque = 0;
544  // }
545 
546  // for (size_t i = 0; i < rightTargets.size(); ++i)
547  // {
548  // rightTargets.at(i)->torque = 0;
549  // }
550 
551  // return;
552  // }
553 
554  // cartesian vel controller
555  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
556 
557  Eigen::MatrixXf jacobiL =
558  leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
559  jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
560 
561  Eigen::VectorXf leftqpos;
562  Eigen::VectorXf leftqvel;
563  leftqpos.resize(leftPositionSensors.size());
564  leftqvel.resize(leftVelocitySensors.size());
565  for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
566  {
567  leftqpos(i) = leftPositionSensors[i]->position;
568  leftqvel(i) = leftVelocitySensors[i]->velocity;
569  }
570 
571  Eigen::MatrixXf jacobiR =
572  rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
573  jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
574 
575 
576  Eigen::VectorXf rightqpos;
577  Eigen::VectorXf rightqvel;
578  rightqpos.resize(rightPositionSensors.size());
579  rightqvel.resize(rightVelocitySensors.size());
580 
581  for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
582  {
583  rightqpos(i) = rightPositionSensors[i]->position;
584  rightqvel(i) = rightVelocitySensors[i]->velocity;
585  }
586 
587  Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
588  Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
589 
590 
591  controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
592  controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
593  controllerSensorData.commitWrite();
594 
595 
596  Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
597  Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
598  Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
599  Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
600  Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
601  Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
602 
603  // Todo: force control
604  int nl = leftRNS->getSize();
605  int nr = rightRNS->getSize();
606  int n = nl + nr;
607  // GraspMatrix
608 
609  // Eigen::Matrix4f poseLeftInRightCoordinate = tcpRight->toLocalCoordinateSystem(tcpLeft->getGlobalPose());
610  // Eigen::Vector3f positionLeftInRightCoordinate;
611  // positionLeftInRightCoordinate << poseLeftInRightCoordinate(0,3), poseLeftInRightCoordinate(1,3), poseLeftInRightCoordinate(2,3);
612  // boxWidth = 0.001 * 0.5 * positionLeftInRightCoordinate.norm();
613 
614 
615  Eigen::Vector3f localDistanceCoM;
616  localDistanceCoM << 0.5 * boxWidth, 0, 0;
617 
618 
619  Eigen::Vector3f rl =
620  -leftCurrentPose.block<3, 3>(0, 0) *
621  localDistanceCoM; //tcpLeft->getRobot()->getRootNode()->toLocalCoordinateSystem(tcpLeft->toGlobalCoordinateSystemVec(localDistanceCoM));
622  Eigen::Vector3f rr = rightCurrentPose.block<3, 3>(0, 0) * localDistanceCoM;
623  Eigen::MatrixXf leftGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
624  Eigen::MatrixXf rightGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
625  leftGraspMatrix.block<3, 3>(3, 0) << 0, -rl(2), rl(1), rl(2), 0, -rl(0), -rl(1), rl(0), 0;
626  rightGraspMatrix.block<3, 3>(3, 0) << 0, -rr(2), rr(1), rr(2), 0, -rr(0), -rr(1), rr(0), 0;
627  Eigen::MatrixXf graspMatrix;
628  graspMatrix.resize(6, 12);
629  graspMatrix << leftGraspMatrix, rightGraspMatrix;
630 
631  // constraint Jacobain and projection matrix
632  Eigen::MatrixXf jacobi;
633  jacobi.resize(12, n);
634  jacobi.setZero(12, n);
635  jacobi.block<6, 8>(0, 0) = jacobiL;
636  jacobi.block<6, 8>(6, 8) = jacobiR;
637 
638  Eigen::MatrixXf pinvGraspMatrix =
639  leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 1);
640  Eigen::MatrixXf proj2GraspNullspace =
641  Eigen::MatrixXf::Identity(12, 12) - pinvGraspMatrix * graspMatrix;
642  Eigen::MatrixXf jacobiConstrained = proj2GraspNullspace * jacobi;
643 
644  Eigen::MatrixXf pinvJacobiC =
645  leftIK->computePseudoInverseJacobianMatrix(jacobiConstrained, 1);
646  Eigen::MatrixXf projection =
647  Eigen::MatrixXf::Identity(n, n) - pinvJacobiC * jacobiConstrained;
648 
649 
650  // calculate inertia matrix
651  Eigen::MatrixXf leftInertialMatrix;
652  Eigen::MatrixXf rightInertialMatrix;
653  leftInertialMatrix.setZero(nl, nl);
654  rightInertialMatrix.setZero(nr, nr);
655 
656  for (size_t i = 0; i < rnsLeftBody->getNodeNames().size(); ++i)
657  {
658  VirtualRobot::RobotNodePtr rnbody = rnsLeftBody->getNode(i);
659  VirtualRobot::RobotNodePtr rnjoint = leftRNS->getNode(i + 1);
660 
661  Eigen::MatrixXf jacobipos_rn =
662  0.001 * leftIK->getJacobianMatrix(
663  rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
664  Eigen::MatrixXf jacobiori_rn = leftIK->getJacobianMatrix(
665  rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
666 
667 
668  float m = rnbody->getMass();
669  Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
670  leftInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn +
671  jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
672  }
673 
674  for (size_t i = 0; i < rnsRightBody->getNodeNames().size(); ++i)
675  {
676  VirtualRobot::RobotNodePtr rnbody = rnsRightBody->getNode(i);
677  VirtualRobot::RobotNodePtr rnjoint = rightRNS->getNode(i + 1);
678 
679  Eigen::MatrixXf jacobipos_rn =
680  0.001 * rightIK->getJacobianMatrix(
681  rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
682  Eigen::MatrixXf jacobiori_rn = rightIK->getJacobianMatrix(
683  rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
684 
685 
686  float m = rnbody->getMass();
687  Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
688 
689  rightInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn +
690  jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
691  }
692  Eigen::MatrixXf M = Eigen::MatrixXf::Zero(n, n);
693  M.topLeftCorner(nl, nl) = leftInertialMatrix;
694  M.bottomRightCorner(nr, nr) = rightInertialMatrix;
695 
696  Eigen::MatrixXf Mc = M + projection * M - M * projection;
697 
698 
699  // force filter and measure
700  double filterFactor = 1 / ((filterTimeConstant / deltaT) + 1);
701 
702  filtered_leftForce = (1 - filterFactor) * filtered_leftForce +
703  filterFactor * (leftForceTorque->force - offset_leftForce);
704  filtered_leftTorque = (1 - filterFactor) * filtered_leftTorque +
705  filterFactor * (leftForceTorque->torque - offset_leftTorque);
706  filtered_rightForce = (1 - filterFactor) * filtered_rightForce +
707  filterFactor * (rightForceTorque->force - offset_rightForce);
708  filtered_rightTorque = (1 - filterFactor) * filtered_rightForce +
709  filterFactor * (rightForceTorque->torque - offset_rightTorque);
710 
711  Eigen::VectorXf filteredForce;
712  filteredForce.resize(12);
713  filteredForce << filtered_leftForce, filtered_leftTorque, filtered_rightForce,
714  filtered_rightTorque;
715  filteredForce.block<3, 1>(0, 0) =
716  leftCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(0, 0);
717  filteredForce.block<3, 1>(3, 0) =
718  leftCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(3, 0);
719  filteredForce.block<3, 1>(6, 0) =
720  rightCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(6, 0);
721  filteredForce.block<3, 1>(9, 0) =
722  rightCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(9, 0);
723 
724 
725  // calculate force control part
726  Eigen::VectorXf constrainedForceMeasure = proj2GraspNullspace * filteredForce;
727 
728  constrainedForceMeasure.block<3, 1>(0, 0) = leftCurrentPose.block<3, 3>(0, 0).transpose() *
729  constrainedForceMeasure.block<3, 1>(0, 0);
730  constrainedForceMeasure.block<3, 1>(3, 0) = leftCurrentPose.block<3, 3>(0, 0).transpose() *
731  constrainedForceMeasure.block<3, 1>(3, 0);
732  constrainedForceMeasure.block<3, 1>(6, 0) = rightCurrentPose.block<3, 3>(0, 0).transpose() *
733  constrainedForceMeasure.block<3, 1>(6, 0);
734  constrainedForceMeasure.block<3, 1>(9, 0) = rightCurrentPose.block<3, 3>(0, 0).transpose() *
735  constrainedForceMeasure.block<3, 1>(9, 0);
736  Eigen::VectorXf forceConstrained = Eigen::VectorXf::Zero(12);
737  for (size_t i = 0; i < 12; i++)
738  {
739  ftPIDController[i]->update(constrainedForceMeasure(i), forceC_des(i));
740  forceConstrained(i) = ftPIDController[i]->getControlValue() + forceC_des(i);
741  // forceConstrained(i) = forceC_des(i) + kpf(i) * (forceC_des(i) - filtered_leftForce(i)); // TODO: add PID controller
742  // forceConstrained(i + 3) = forceC_des(i + 3) + kpf(i + 3) * (forceC_des(i + 3) - filtered_leftTorque(i)); // TODO: add PID controller
743  // forceConstrained(i + 6) = forceC_des(i + 6) + kpf(i + 6) * (forceC_des(i + 6) - filtered_rightForce(i)); // TODO: add PID controller
744  // forceConstrained(i + 9) = forceC_des(i + 9) + kpf(i + 9) * (forceC_des(i + 9) - filtered_rightTorque(i)); // TODO: add PID controller
745  }
746  forceConstrained.block<3, 1>(0, 0) =
747  leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(0, 0);
748  forceConstrained.block<3, 1>(3, 0) =
749  leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(3, 0);
750  forceConstrained.block<3, 1>(6, 0) =
751  rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(6, 0);
752  forceConstrained.block<3, 1>(9, 0) =
753  rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(9, 0);
754 
755 
756  // unconstrained space controller
757  Eigen::Vector6f leftJointControlWrench;
758  {
759  Eigen::Vector3f targetTCPLinearVelocity;
760  targetTCPLinearVelocity << 0.001 * leftTargetVel(0), 0.001 * leftTargetVel(1),
761  0.001 * leftTargetVel(2);
762 
763  Eigen::Vector3f currentTCPLinearVelocity;
764  currentTCPLinearVelocity << currentLeftTwist(0), currentLeftTwist(1),
765  currentLeftTwist(2);
766  Eigen::Vector3f currentTCPAngularVelocity;
767  currentTCPAngularVelocity << currentLeftTwist(3), currentLeftTwist(4),
768  currentLeftTwist(5);
769  Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
770  Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
771 
772  Eigen::Vector3f tcpDesiredForce =
773  0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) +
774  dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
775  Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
776  Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
777  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
778  Eigen::Vector3f tcpDesiredTorque =
779  kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
780  leftJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
781  }
782 
783 
784  Eigen::Vector6f rightJointControlWrench;
785  {
786  Eigen::Vector3f targetTCPLinearVelocity;
787  targetTCPLinearVelocity << 0.001 * rightTargetVel(0), 0.001 * rightTargetVel(1),
788  0.001 * rightTargetVel(2);
789 
790  Eigen::Vector3f currentTCPLinearVelocity;
791  currentTCPLinearVelocity << currentRightTwist(0), currentRightTwist(1),
792  currentRightTwist(2);
793  Eigen::Vector3f currentTCPAngularVelocity;
794  currentTCPAngularVelocity << currentRightTwist(3), currentRightTwist(4),
795  currentRightTwist(5);
796  Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
797  Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
798 
799  Eigen::Vector3f tcpDesiredForce =
800  0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) -
801  dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
802  Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
803  Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
804  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
805  Eigen::Vector3f tcpDesiredTorque =
806  kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
807  rightJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
808  }
809 
810  Eigen::VectorXf forceUnconstrained(12);
811  forceUnconstrained << leftJointControlWrench, rightJointControlWrench;
812 
813  Eigen::VectorXf unConstrainedForceMeasure = pinvGraspMatrix * graspMatrix * filteredForce;
814 
815  Eigen::MatrixXf taskSpaceInertia = (jacobi * M.inverse() * jacobi.transpose()).inverse();
816 
817 
818  forceUnconstrained =
819  taskSpaceInertia *
820  forceUnconstrained; //+ unConstrainedForceMeasure) - unConstrainedForceMeasure;
821 
822  Eigen::VectorXf leftNullspaceTorque =
823  knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
824  Eigen::VectorXf rightNullspaceTorque =
825  knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
826  float lambda = 2;
827 
828  Eigen::MatrixXf jtpinvL =
829  leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
830  Eigen::MatrixXf jtpinvR =
831  leftIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
832  forceUnconstrained.head<8>()) =
833  forceUnconstrained.head<8>()) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
834  forceUnconstrained.tail<8>() = forceUnconstrained.tail<8>() +
835  (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
836 
837 
838  //
839  //// Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
840  // Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceUnconstrained.head<6>() + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
841 
842  //
843  //// Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
844  // Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceUnconstrained.tail<6>() + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
845 
846  // Eigen::VectorXf torqueUnconstrained(16);
847  // torqueUnconstrained << leftJointDesiredTorques, rightJointDesiredTorques;
848 
849  // constrained space control
850 
851  // Eigen::MatrixXf mu = (Eigen::MatrixXf::Identity(n, n) - projection) * M * Mc.inverse();
852  // Eigen::VectorXf qacc;
853  // qacc.resize(n);
854  // for (size_t i = 0; i < leftAccelerationSensors.size(); ++i)
855  // {
856  // qacc(i) = 0.001 * leftAccelerationSensors[i]->acceleration;
857  // }
858 
859  // for (size_t i = 0; i < rightAccelerationSensors.size(); ++i)
860  // {
861  // qacc(i + leftAccelerationSensors.size()) = 0.001 * rightAccelerationSensors[i]->acceleration;
862  // }
863 
864 
865  // Eigen::VectorXf torqueConstrained = mu * (torqueUnconstrained + (Eigen::MatrixXf::Identity(n, n) - projection) * qacc) - jacobiConstrained.transpose() * forceConstrained;
866 
867 
868  // all torques
869 
870 
871  // Eigen::VectorXf torqueInput = projection * torqueUnconstrained + projection * torqueConstrained;
872  // Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * (Eigen::MatrixXf::Identity(12,12) - proj2GraspNullspace) * forceConstrained;
873  // Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * forceConstrained;
874  Eigen::VectorXf torqueInput = jacobi.transpose() * (forceUnconstrained + forceConstrained);
875 
876  Eigen::VectorXf leftJointDesiredTorques = torqueInput.head(nl);
877  Eigen::VectorXf rightJointDesiredTorques = torqueInput.block<8, 1>(8, 0);
878 
879  // torque limit
880  for (size_t i = 0; i < leftTargets.size(); ++i)
881  {
882  float desiredTorque = leftJointDesiredTorques(i);
883 
884  if (isnan(desiredTorque))
885  {
886  desiredTorque = 0;
887  }
888 
889  desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
890  desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
891 
892  debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] =
893  leftJointDesiredTorques(i);
894 
895  leftTargets.at(i)->torque = desiredTorque;
896  }
897 
898 
899  for (size_t i = 0; i < rightTargets.size(); ++i)
900  {
901  float desiredTorque = rightJointDesiredTorques(i);
902 
903  if (isnan(desiredTorque))
904  {
905  desiredTorque = 0;
906  }
907 
908  desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
909  desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
910 
911  debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] =
912  rightJointDesiredTorques(i);
913 
914  rightTargets.at(i)->torque = desiredTorque;
915  }
916 
917  // debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
918  // debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
919  // debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
920  // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_x = tcpDesiredTorque(0);
921  // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_y = tcpDesiredTorque(1);
922  // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
923  // debugDataInfo.getWriteBuffer().quatError = errorAngle;
924  // debugDataInfo.getWriteBuffer().posiError = posiError;
925  debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
926  debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
927  debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
928  debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
929  debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
930  debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
931 
932 
933  debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
934  debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
935  debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
936 
937  debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
938  debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
939  debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
940 
941 
942  for (size_t i = 0; i < 12; ++i)
943  {
944  std::stringstream ss;
945  ss << i;
946  std::string name = "forceConstrained_" + ss.str();
947  debugDataInfo.getWriteBuffer().constrained_force[name] = filteredForce(i);
948  }
949 
950 
951  debugDataInfo.commitWrite();
952 
953 
954  // Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
955  // float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
956  // if (normLinearVelocity > cfg->maxLinearVel)
957  // {
958  // leftTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
959  // }
960  // float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
961  // if (normAngularVelocity > cfg->maxAngularVel)
962  // {
963  // leftTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
964  // }
965 
966  // Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
967  // Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
968  // Eigen::VectorXf jnvL = jtpinvL * leftTargetVel + (I - jtpinvL * jacobiL) * leftNullSpaceJointVelocity;
969 
970  // for (size_t i = 0; i < leftTargets.size(); ++i)
971  // {
972  // leftTargets.at(i)->velocity = jnvL(i);
973  // }
974 
975  // Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
976  // normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
977  // if (normLinearVelocity > cfg->maxLinearVel)
978  // {
979  // rightTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
980  // }
981  // normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
982  // if (normAngularVelocity > cfg->maxAngularVel)
983  // {
984  // rightTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
985  // }
986  // Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
987  // Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
988  // Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
989 
990  // for (size_t i = 0; i < rightTargets.size(); ++i)
991  // {
992  // rightTargets.at(i)->velocity = jnvR(i);
993  // }
994  }
995 
996  void
997  NJointBimanualCCDMPController::learnDMPFromFiles(const std::string& name,
998  const Ice::StringSeq& fileNames,
999  const Ice::Current&)
1000  {
1001  if (name == "LeftLeader")
1002  {
1003  leftGroup.at(0)->learnDMPFromFiles(fileNames);
1004  }
1005  else if (name == "LeftFollower")
1006  {
1007  rightGroup.at(1)->learnDMPFromFiles(fileNames);
1008  }
1009  else if (name == "RightLeader")
1010  {
1011  rightGroup.at(0)->learnDMPFromFiles(fileNames);
1012  }
1013  else
1014  {
1015  leftGroup.at(1)->learnDMPFromFiles(fileNames);
1016  }
1017  }
1018 
1019  void
1020  NJointBimanualCCDMPController::setViaPoints(Ice::Double u,
1021  const Ice::DoubleSeq& viapoint,
1022  const Ice::Current&)
1023  {
1024  LockGuardType guard(controllerMutex);
1025  if (leaderName == "Left")
1026  {
1027  leftGroup.at(0)->setViaPose(u, viapoint);
1028  }
1029  else
1030  {
1031  rightGroup.at(0)->setViaPose(u, viapoint);
1032  }
1033  }
1034 
1035  void
1036  NJointBimanualCCDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
1037  {
1038  LockGuardType guard(controllerMutex);
1039  if (leaderName == "Left")
1040  {
1041  leftGroup.at(0)->setGoalPoseVec(goals);
1042  }
1043  else
1044  {
1045  rightGroup.at(0)->setGoalPoseVec(goals);
1046  }
1047  }
1048 
1049  void
1050  NJointBimanualCCDMPController::changeLeader(const Ice::Current&)
1051  {
1052  LockGuardType guard(controllerMutex);
1053 
1054  if (leaderName == "Left")
1055  {
1056  leaderName = "Right";
1057  rightGroup.at(0)->canVal = virtualtimer;
1058  rightGroup.at(1)->canVal = virtualtimer;
1059  }
1060  else
1061  {
1062  leaderName = "Left";
1063  leftGroup.at(0)->canVal = virtualtimer;
1064  leftGroup.at(1)->canVal = virtualtimer;
1065  }
1066  }
1067 
1068  void
1069  NJointBimanualCCDMPController::runDMP(const Ice::DoubleSeq& leftGoals,
1070  const Ice::DoubleSeq& rightGoals,
1071  const Ice::Current&)
1072  {
1073  virtualtimer = cfg->timeDuration;
1074 
1075  Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
1076  Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
1077 
1078  leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
1079  rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
1080 
1081 
1082  ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
1083 
1084  leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose),
1085  getLocalPose(leftGoals, rightGoals));
1086  rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose),
1087  getLocalPose(rightGoals, leftGoals));
1088 
1089  controllerTask->start();
1090  }
1091 
1092  Eigen::Matrix4f
1093  NJointBimanualCCDMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate,
1094  const Eigen::Matrix4f& globalTargetPose)
1095  {
1096  Eigen::Matrix4f localPose;
1097  localPose.block<3, 3>(0, 0) =
1098  globalTargetPose.block<3, 3>(0, 0) * newCoordinate.block<3, 3>(0, 0).inverse();
1099  localPose.block<3, 1>(0, 3) =
1100  localPose.block<3, 3>(0, 0) *
1101  (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
1102 
1103  return localPose;
1104  }
1105 
1106  void
1107  NJointBimanualCCDMPController::onPublish(const SensorAndControl&,
1108  const DebugDrawerInterfacePrx&,
1109  const DebugObserverInterfacePrx& debugObs)
1110  {
1111 
1112  StringVariantBaseMap datafields;
1113  auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
1114  for (auto& pair : values)
1115  {
1116  datafields[pair.first] = new Variant(pair.second);
1117  }
1118 
1119  auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
1120  for (auto& pair : constrained_force)
1121  {
1122  datafields[pair.first] = new Variant(pair.second);
1123  }
1124 
1125 
1126  datafields["leftTargetPose_x"] =
1127  new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
1128  datafields["leftTargetPose_y"] =
1129  new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
1130  datafields["leftTargetPose_z"] =
1131  new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
1132  datafields["rightTargetPose_x"] =
1133  new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
1134  datafields["rightTargetPose_y"] =
1135  new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
1136  datafields["rightTargetPose_z"] =
1137  new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
1138 
1139 
1140  datafields["leftCurrentPose_x"] =
1141  new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
1142  datafields["leftCurrentPose_y"] =
1143  new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
1144  datafields["leftCurrentPose_z"] =
1145  new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
1146  datafields["rightCurrentPose_x"] =
1147  new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
1148  datafields["rightCurrentPose_y"] =
1149  new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
1150  datafields["rightCurrentPose_z"] =
1151  new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
1152 
1153  // StringVariantBaseMap datafields;
1154  // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
1155  // for (auto& pair : values)
1156  // {
1157  // datafields[pair.first] = new Variant(pair.second);
1158  // }
1159 
1160  // auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
1161  // for (auto& pair : dmpTargets)
1162  // {
1163  // datafields[pair.first] = new Variant(pair.second);
1164  // }
1165 
1166  // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
1167  // for (auto& pair : currentPose)
1168  // {
1169  // datafields[pair.first] = new Variant(pair.second);
1170  // }
1171 
1172  // datafields["leaderCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderCanVal);
1173  // datafields["leadermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadermpcFactor);
1174  // datafields["leaderError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadererror);
1175  // datafields["leaderposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderposError);
1176  // datafields["leaderoriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderoriError);
1177 
1178  // datafields["followerCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerCanVal);
1179  // datafields["followermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().followermpcFactor);
1180  // datafields["followerError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followererror);
1181  // datafields["followerposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerposError);
1182  // datafields["followeroriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followeroriError);
1183 
1184  debugObs->setDebugChannel("DMPController", datafields);
1185  }
1186 
1187  void
1188  NJointBimanualCCDMPController::onInitComponent()
1189  {
1190  ARMARX_INFO << "init ...";
1191  controllerTask = new PeriodicTask<NJointBimanualCCDMPController>(
1192  this, &NJointBimanualCCDMPController::controllerRun, 0.3);
1193  }
1194 
1195  void
1196  NJointBimanualCCDMPController::onDisconnectComponent()
1197  {
1198  controllerTask->stop();
1199  ARMARX_INFO << "stopped ...";
1200  }
1201 
1202 
1203 } // namespace armarx
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const NJointBimanualCCDMPControllerControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::NJointBimanualCCDMPController::learnDMPFromFiles
void learnDMPFromFiles(const std::string &, const Ice::StringSeq &, const Ice::Current &)
Definition: NJointBimanualDMPForceController.cpp:997
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:223
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:92
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::rtGetControlStruct
const NJointBimanualCCDMPControllerControlData & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
GfxTL::Orientation
ScalarT Orientation(const VectorXD< 2, ScalarT > &p1, const VectorXD< 2, ScalarT > &p2, const VectorXD< 2, ScalarT > &c)
Definition: Orientation.h:10
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
RobotUnit.h
armarx::RobotUnitPtr
IceUtil::Handle< class RobotUnit > RobotUnitPtr
Definition: FTSensor.h:34
armarx::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:163
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::NJointBimanualCCDMPControllerControlData::leftTargetPose
Eigen::Matrix4f leftTargetPose
Definition: NJointBimanualDMPForceController.h:40
armarx::NJointBimanualCCDMPController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &)
Definition: NJointBimanualDMPForceController.cpp:1020
armarx::NJointBimanualCCDMPControllerControlData::leftTargetVel
Eigen::VectorXf leftTargetVel
Definition: NJointBimanualDMPForceController.h:37
armarx::NJointBimanualCCDMPControllerInterface::changeLeader
void changeLeader()
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
NJointBimanualCCDMPController.h
SensorValueForceTorque.h
armarx::NJointBimanualCCDMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition: NJointBimanualDMPForceController.cpp:513
armarx::NJointBimanualCCDMPController::runDMP
void runDMP(const Ice::DoubleSeq &leftGoals, const Ice::DoubleSeq &rightGoals, const Ice::Current &)
Definition: NJointBimanualDMPForceController.cpp:1069
armarx::NJointBimanualCCDMPController::NJointBimanualCCDMPController
NJointBimanualCCDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::getWriterControlStruct
NJointBimanualCCDMPControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::SensorValueForceTorque::force
Eigen::Vector3f force
Definition: SensorValueForceTorque.h:36
armarx::NJointBimanualCCDMPController::onDisconnectComponent
void onDisconnectComponent()
Definition: NJointBimanualDMPForceController.cpp:1196
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:920
armarx::NJointBimanualCCDMPController::onInitComponent
void onInitComponent()
Definition: NJointBimanualDMPForceController.cpp:1188
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:108
armarx::NJointBimanualCCDMPController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition: NJointBimanualDMPForceController.cpp:1036
armarx::SensorValueForceTorque::torque
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION Eigen::Vector3f torque
Definition: SensorValueForceTorque.h:35
armarx::NJointBimanualCCDMPController::getClassName
std::string getClassName(const Ice::Current &) const
Definition: NJointBimanualDMPForceController.cpp:261
ControlTarget1DoFActuator.h
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
armarx::registrationControllerNJointBimanualCCDMPController
NJointControllerRegistration< NJointBimanualCCDMPController > registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController")
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
CartesianVelocityController.h
armarx::NJointBimanualCCDMPController::controllerRun
void controllerRun()
Definition: NJointBimanualDMPForceController.cpp:267
PIDController.h
NJointController.h
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerPtr
std::shared_ptr< TaskSpaceDMPController > TaskSpaceDMPControllerPtr
Definition: NJointTaskSpaceImpedanceDMPController.h:31
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
Eigen::Matrix< float, 6, 1 >
armarx::PeriodicTask
Definition: ArmarXManager.h:70
armarx::NJointBimanualCCDMPController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition: NJointBimanualDMPForceController.cpp:1107
armarx::navigation::core::Position
Eigen::Vector3f Position
Definition: basic_types.h:36
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_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::TripleBuffer::reinitAllBuffers
std::enable_if< std::is_copy_constructible< U >::value >::type reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:171
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::NJointBimanualCCDMPControllerControlData::rightTargetPose
Eigen::Matrix4f rightTargetPose
Definition: NJointBimanualDMPForceController.h:41
SensorValue1DoFActuator.h
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
armarx::TripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:143
armarx::NJointBimanualCCDMPControllerControlData::rightTargetVel
Eigen::VectorXf rightTargetVel
Definition: NJointBimanualDMPForceController.h:38