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