NJointBimanualCCDMPController.cpp
Go to the documentation of this file.
1 #include "NJointBimanualCCDMPController.h"
2 
3 
4 // Simox
5 #include <VirtualRobot/IK/DifferentialIK.h>
6 #include <VirtualRobot/MathTools.h>
7 #include <VirtualRobot/Nodes/RobotNode.h>
8 #include <VirtualRobot/Robot.h>
9 #include <VirtualRobot/RobotNodeSet.h>
10 
11 // DMP
12 #include <dmp/representation/dmp/umitsmp.h>
13 
14 // armarx
15 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
16 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
17 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
18 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
19 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
20 #include <RobotAPI/libraries/core/CartesianVelocityController.h>
21 #include <RobotAPI/libraries/core/PIDController.h>
22 #include <RobotAPI/libraries/core/math/MathUtils.h>
23 
24 // control
25 
26 
27 namespace armarx::control::deprecated_njoint_mp_controller::bimanual
28 {
29  NJointControllerRegistration<NJointBimanualCCDMPController>
30  registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController");
31 
32  NJointBimanualCCDMPController::NJointBimanualCCDMPController(
33  const RobotUnitPtr& robotUnit,
34  const armarx::NJointControllerConfigPtr& config,
35  const VirtualRobot::RobotPtr&)
36  {
37  ARMARX_INFO << "Preparing ... ";
38  cfg = NJointBimanualCCDMPControllerConfigPtr::dynamicCast(config);
39 
40  ARMARX_CHECK_EXPRESSION(robotUnit);
41  useSynchronizedRtRobot();
42 
43  leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
44  leftNullSpaceCoefs.resize(leftRNS->getSize());
45  leftNullSpaceCoefs.setOnes();
46 
47  for (size_t i = 0; i < leftRNS->getSize(); ++i)
48  {
49  std::string jointName = leftRNS->getNode(i)->getName();
50 
51  if (leftRNS->getNode(i)->isLimitless())
52  {
53  leftNullSpaceCoefs(i) = 1;
54  }
55 
56  leftJointNames.push_back(jointName);
57  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
58  const SensorValueBase* sv = useSensorValue(jointName);
59  leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
60  const SensorValue1DoFActuatorVelocity* velocitySensor =
61  sv->asA<SensorValue1DoFActuatorVelocity>();
62  const SensorValue1DoFActuatorPosition* positionSensor =
63  sv->asA<SensorValue1DoFActuatorPosition>();
64  const SensorValue1DoFActuatorAcceleration* accelerationSensor =
65  sv->asA<SensorValue1DoFActuatorAcceleration>();
66 
67  if (!velocitySensor)
68  {
69  ARMARX_WARNING << "No velocitySensor available for " << jointName;
70  }
71  if (!positionSensor)
72  {
73  ARMARX_WARNING << "No positionSensor available for " << jointName;
74  }
75 
76  if (!accelerationSensor)
77  {
78  ARMARX_WARNING << "No accelerationSensor available for " << jointName;
79  }
80 
81 
82  leftVelocitySensors.push_back(velocitySensor);
83  leftPositionSensors.push_back(positionSensor);
84  leftAccelerationSensors.push_back(accelerationSensor);
85  };
86  rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
87 
88  rightNullSpaceCoefs.resize(rightRNS->getSize());
89  rightNullSpaceCoefs.setOnes();
90  for (size_t i = 0; i < rightRNS->getSize(); ++i)
91  {
92  std::string jointName = rightRNS->getNode(i)->getName();
93 
94  if (rightRNS->getNode(i)->isLimitless())
95  {
96  rightNullSpaceCoefs(i) = 1;
97  }
98 
99  rightJointNames.push_back(jointName);
100  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
101  const SensorValueBase* sv = useSensorValue(jointName);
102  rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
103  const SensorValue1DoFActuatorVelocity* velocitySensor =
104  sv->asA<SensorValue1DoFActuatorVelocity>();
105  const SensorValue1DoFActuatorPosition* positionSensor =
106  sv->asA<SensorValue1DoFActuatorPosition>();
107  const SensorValue1DoFActuatorAcceleration* accelerationSensor =
108  sv->asA<SensorValue1DoFActuatorAcceleration>();
109 
110 
111  if (!velocitySensor)
112  {
113  ARMARX_WARNING << "No velocitySensor available for " << jointName;
114  }
115  if (!positionSensor)
116  {
117  ARMARX_WARNING << "No positionSensor available for " << jointName;
118  }
119  if (!accelerationSensor)
120  {
121  ARMARX_WARNING << "No accelerationSensor available for " << jointName;
122  }
123 
124  rightVelocitySensors.push_back(velocitySensor);
125  rightPositionSensors.push_back(positionSensor);
126  rightAccelerationSensors.push_back(accelerationSensor);
127  };
128 
129 
130  const SensorValueBase* svlf = useSensorValue("FT L");
131  leftForceTorque = svlf->asA<SensorValueForceTorque>();
132  const SensorValueBase* svrf = useSensorValue("FT R");
133  rightForceTorque = svrf->asA<SensorValueForceTorque>();
134 
135  leftIK.reset(new VirtualRobot::DifferentialIK(
136  leftRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
137  rightIK.reset(new VirtualRobot::DifferentialIK(
138  rightRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
139 
140 
141  tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
142  taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
143  taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
144  taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
145  taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
146  taskSpaceDMPConfig.DMPAmplitude = 1.0;
147  taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
148  taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
149  taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
150  taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
151  taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
152  taskSpaceDMPConfig.phaseStopParas.Dori = 0;
153  taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
154  taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
155  taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
156 
157 
158  tsvmp::TaskSpaceDMPControllerPtr leftLeader(
159  new tsvmp::TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig, false));
160  tsvmp::TaskSpaceDMPControllerPtr leftFollower(
161  new tsvmp::TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig, false));
162  tsvmp::TaskSpaceDMPControllerPtr rightLeader(
163  new tsvmp::TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig, false));
164  tsvmp::TaskSpaceDMPControllerPtr rightFollower(
165  new tsvmp::TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig, false));
166 
167  leftGroup.push_back(leftLeader);
168  leftGroup.push_back(rightFollower);
169 
170  rightGroup.push_back(rightLeader);
171  rightGroup.push_back(leftFollower);
172 
173  bothLeaderGroup.push_back(leftLeader);
174  bothLeaderGroup.push_back(rightLeader);
175 
176 
177  tcpLeft = leftRNS->getTCP();
178  tcpRight = rightRNS->getTCP();
179 
180 
181  leaderName = cfg->defautLeader;
182 
183 
184  leftDesiredJointValues.resize(leftTargets.size());
185  ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
186 
187  for (size_t i = 0; i < leftTargets.size(); ++i)
188  {
189  leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
190  }
191 
192  rightDesiredJointValues.resize(rightTargets.size());
193  ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
194 
195  for (size_t i = 0; i < rightTargets.size(); ++i)
196  {
197  rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
198  }
199 
200 
201  leftKpos << cfg->leftKpos[0], cfg->leftKpos[1], cfg->leftKpos[2];
202  leftDpos << cfg->leftDpos[0], cfg->leftDpos[1], cfg->leftDpos[2];
203  leftKori << cfg->leftKori[0], cfg->leftKori[1], cfg->leftKori[2];
204  leftDori << cfg->leftDori[0], cfg->leftDori[1], cfg->leftDori[2];
205 
206  rightKpos << cfg->rightKpos[0], cfg->rightKpos[1], cfg->rightKpos[2];
207  rightDpos << cfg->rightDpos[0], cfg->rightDpos[1], cfg->rightDpos[2];
208  rightKori << cfg->rightKori[0], cfg->rightKori[1], cfg->rightKori[2];
209  rightDori << cfg->rightDori[0], cfg->rightDori[1], cfg->rightDori[2];
210 
211  knull = cfg->knull;
212  dnull = cfg->dnull;
213 
214 
215  torqueLimit = cfg->torqueLimit;
216 
217 
218  maxLinearVel = cfg->maxLinearVel;
219  maxAngularVel = cfg->maxAngularVel;
220 
221  torqueFactor = 1.0;
222  startReduceTorque = cfg->startReduceTorque;
223 
224  NJointBimanualCCDMPControllerInterfaceData initInterfaceData;
225  initInterfaceData.currentLeftPose = Eigen::Matrix4f::Identity();
226  initInterfaceData.currentRightPose = Eigen::Matrix4f::Identity();
227  interfaceData.reinitAllBuffers(initInterfaceData);
228 
229  NJointBimanualCCDMPControllerSensorData initSensorData;
230  initSensorData.deltaT = 0;
231  initSensorData.currentTime = 0;
232  initSensorData.currentLeftPose = Eigen::Matrix4f::Identity();
233  initSensorData.currentRightPose = Eigen::Matrix4f::Identity();
234  controllerSensorData.reinitAllBuffers(initSensorData);
235  }
236 
237  std::string
238  NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
239  {
240  return "NJointBimanualCCDMPController";
241  }
242 
243  void
244  NJointBimanualCCDMPController::rtPreActivateController()
245  {
246  NJointBimanualCCDMPControllerControlData initData;
247  initData.leftTargetVel.resize(6);
248  initData.leftTargetVel.setZero();
249  initData.rightTargetVel.resize(6);
250  initData.rightTargetVel.setZero();
251  initData.leftTargetPose = tcpLeft->getPoseInRootFrame();
252  initData.rightTargetPose = tcpRight->getPoseInRootFrame();
253  initData.virtualTime = cfg->timeDuration;
254  reinitTripleBuffer(initData);
255  }
256 
257  void
258  NJointBimanualCCDMPController::controllerRun()
259  {
260  if (!controllerSensorData.updateReadBuffer())
261  {
262  return;
263  }
264 
265  double deltaT = controllerSensorData.getReadBuffer().deltaT;
266 
267 
268  Eigen::VectorXf leftTargetVel;
269  leftTargetVel.resize(6);
270  leftTargetVel.setZero();
271  Eigen::VectorXf rightTargetVel;
272  rightTargetVel.resize(6);
273  rightTargetVel.setZero();
274 
275  std::vector<tsvmp::TaskSpaceDMPControllerPtr> currentControlGroup;
276  Eigen::Matrix4f currentLeaderPose;
277  Eigen::Matrix4f currentFollowerPose;
278  Eigen::VectorXf currentLeaderTwist;
279  Eigen::VectorXf currentFollowerTwist;
280  if (leaderName == "Left")
281  {
282  currentControlGroup = leftGroup;
283  currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
284  currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
285  currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
286  currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
287  }
288  else if (leaderName == "right")
289  {
290  currentControlGroup = rightGroup;
291  currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
292  currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
293  currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
294  currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
295  }
296  else
297  {
298  currentControlGroup = bothLeaderGroup;
299 
300  tsvmp::TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
301  tsvmp::TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
302  leaderDMPleft->flow(deltaT,
303  controllerSensorData.getReadBuffer().currentLeftPose,
304  controllerSensorData.getReadBuffer().currentLeftTwist);
305  leaderDMPright->flow(deltaT,
306  controllerSensorData.getReadBuffer().currentRightPose,
307  controllerSensorData.getReadBuffer().currentRightTwist);
308 
309  Eigen::VectorXf leftTargetVel = leaderDMPleft->getTargetVelocity();
310  Eigen::Matrix4f leftTargetPose = leaderDMPleft->getTargetPoseMat();
311  Eigen::VectorXf rightTargetVel = leaderDMPright->getTargetVelocity();
312  Eigen::Matrix4f rightTargetPose = leaderDMPright->getTargetPoseMat();
313 
314  virtualtimer = leaderDMPleft->canVal;
315  LockGuardType guard{controlDataMutex};
316  getWriterControlStruct().leftTargetVel = leftTargetVel;
317  getWriterControlStruct().rightTargetVel = rightTargetVel;
318  getWriterControlStruct().leftTargetPose = leftTargetPose;
319  getWriterControlStruct().rightTargetPose = rightTargetPose;
320  writeControlStruct();
321 
322  return;
323  }
324 
325  tsvmp::TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
326  tsvmp::TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
327  virtualtimer = leaderDMP->canVal;
328 
329  if (virtualtimer < 1e-8)
330  {
331  finished = true;
332  }
333 
334 
335  leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
336 
337  Eigen::Matrix4f currentFollowerLocalPose;
338  currentFollowerLocalPose.block<3, 3>(0, 0) =
339  currentLeaderPose.block<3, 3>(0, 0).inverse() * currentFollowerPose.block<3, 3>(0, 0);
340  currentFollowerLocalPose.block<3, 1>(0, 3) =
341  currentLeaderPose.block<3, 3>(0, 0).inverse() *
342  (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
343  followerDMP->flow(deltaT, currentFollowerLocalPose, currentFollowerTwist);
344 
345  Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
346  Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
347  Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
348  std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
349 
350  Eigen::Matrix4f followerTargetPose;
351  followerTargetPose.block<3, 3>(0, 0) =
352  currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 3>(0, 0);
353  followerTargetPose.block<3, 1>(0, 3) =
354  currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) +
355  currentLeaderPose.block<3, 1>(0, 3);
356 
357 
358  Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
359  Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
360  followerTargetVel.setZero();
361 
362  // followerTargetVel.block<3, 1>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetVel.block<3, 1>(0, 0) + leaderTargetVel.block<3, 1>(0, 0);
363  // followerTargetVel.block<3, 1>(0, 0) = KposFollower * (followerTargetPose.block<3, 1>(0, 3) - currentFollowerPose.block<3, 1>(0, 3));
364  // Eigen::Matrix3f followerDiffMat = followerTargetPose.block<3, 3>(0, 0) * currentFollowerPose.block<3, 3>(0, 0).inverse();
365  // Eigen::Vector3f followerDiffRPY = KoriFollower * VirtualRobot::MathTools::eigen3f2rpy(followerDiffMat);
366  // followerTargetVel(3) = followerDiffRPY(0);
367  // followerTargetVel(4) = followerDiffRPY(1);
368  // followerTargetVel(5) = followerDiffRPY(2);
369 
370 
371  std::vector<double> leftDMPTarget;
372  std::vector<double> rightDMPTarget;
373 
374  Eigen::Matrix4f leftTargetPose;
375  Eigen::Matrix4f rightTargetPose;
376 
377  // float leftKratio = 1.0;
378  // float rightKratio = 1.0;
379 
380  if (leaderName == "Left")
381  {
382  leftTargetVel = leaderTargetVel;
383  rightTargetVel = followerTargetVel;
384 
385  leftDMPTarget = leaderDMP->getTargetPose();
386  rightDMPTarget = followerLocalTargetPoseVec;
387 
388 
389  leftTargetPose = leaderTargetPose;
390  rightTargetPose = followerTargetPose;
391  }
392  else if (leaderName == "right")
393  {
394  rightTargetVel = leaderTargetVel;
395  leftTargetVel = followerTargetVel;
396 
397  rightDMPTarget = leaderDMP->getTargetPose();
398  leftDMPTarget = followerLocalTargetPoseVec;
399 
400  rightTargetPose = leaderTargetPose;
401  leftTargetPose = followerTargetPose;
402  }
403 
404  LockGuardType guard{controlDataMutex};
405  getWriterControlStruct().leftTargetVel = leftTargetVel;
406  getWriterControlStruct().rightTargetVel = rightTargetVel;
407  getWriterControlStruct().leftTargetPose = leftTargetPose;
408  getWriterControlStruct().rightTargetPose = rightTargetPose;
409  getWriterControlStruct().virtualTime = virtualtimer;
410 
411  writeControlStruct();
412  }
413 
414  Eigen::VectorXf
415  NJointBimanualCCDMPController::getControlWrench(const Eigen::VectorXf& tcptwist,
416  const Eigen::Matrix4f& currentPose,
417  const Eigen::Matrix4f& targetPose)
418  {
419  // Eigen::Vector3f currentTCPLinearVelocity;
420  // currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
421  // Eigen::Vector3f currentTCPAngularVelocity;
422  // currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
423 
424  // Eigen::Vector3f currentTCPPosition = currentPose.block<3,1>(0,3);
425  // Eigen::Vector3f desiredPosition = targetPose.block<3,1>(0,3);
426  // Eigen::Vector3f tcpForces = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition);
427  // Eigen::Vector3f tcpDesiredForce = tcpForces - dpos.cwiseProduct(currentTCPLinearVelocity);
428 
429  // Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
430  // Eigen::Matrix3f diffMat = targetPose.block<3,3>(0,0) * currentRotMat.inverse();
431  // Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
432 
433  // Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
434  // Eigen::Vector6f tcpDesiredWrench;
435  // tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
436 
437  // return tcpDesiredWrench;
438  return Eigen::Vector6f::Zero();
439  }
440 
441  void
442  NJointBimanualCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
443  const IceUtil::Time& timeSinceLastIteration)
444  {
445  double deltaT = timeSinceLastIteration.toSecondsDouble();
446 
447 
448  interfaceData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
449  interfaceData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
450  interfaceData.commitWrite();
451 
452  controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
453  controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
454  controllerSensorData.getWriteBuffer().deltaT = deltaT;
455  controllerSensorData.getWriteBuffer().currentTime += deltaT;
456 
457  // cartesian vel controller
458  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
459 
460  Eigen::MatrixXf jacobiL =
461  leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
462 
463  Eigen::VectorXf leftqpos;
464  Eigen::VectorXf leftqvel;
465  leftqpos.resize(leftPositionSensors.size());
466  leftqvel.resize(leftVelocitySensors.size());
467  for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
468  {
469  leftqpos(i) = leftPositionSensors[i]->position;
470  leftqvel(i) = leftVelocitySensors[i]->velocity;
471  }
472 
473  Eigen::MatrixXf jacobiR =
474  rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
475 
476 
477  Eigen::VectorXf rightqpos;
478  Eigen::VectorXf rightqvel;
479  rightqpos.resize(rightPositionSensors.size());
480  rightqvel.resize(rightVelocitySensors.size());
481 
482  for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
483  {
484  rightqpos(i) = rightPositionSensors[i]->position;
485  rightqvel(i) = rightVelocitySensors[i]->velocity;
486  }
487 
488  Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
489  Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
490 
491  controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
492  controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
493  controllerSensorData.commitWrite();
494 
495 
496  jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
497  jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
498 
499 
500  Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
501  Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
502  double virtualtime = rtGetControlStruct().virtualTime;
503  Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
504  Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
505 
506  Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
507  float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
508  if (normLinearVelocity > maxLinearVel)
509  {
510  leftTargetVel.block<3, 1>(0, 0) =
511  maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
512  }
513  float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
514  if (normAngularVelocity > maxAngularVel)
515  {
516  leftTargetVel.block<3, 1>(3, 0) =
517  maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
518  }
519 
520  Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
521  normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
522  if (normLinearVelocity > maxLinearVel)
523  {
524  rightTargetVel.block<3, 1>(0, 0) =
525  maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
526  }
527  normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
528  if (normAngularVelocity > maxAngularVel)
529  {
530  rightTargetVel.block<3, 1>(3, 0) =
531  maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
532  }
533 
534 
535  // unconstrained space controller
536  Eigen::Vector6f leftJointControlWrench;
537  {
538 
539 
540  Eigen::Vector3f targetTCPLinearVelocity;
541  targetTCPLinearVelocity << 0.001 * leftTargetVel(0), 0.001 * leftTargetVel(1),
542  0.001 * leftTargetVel(2);
543 
544  Eigen::Vector3f currentTCPLinearVelocity;
545  currentTCPLinearVelocity << 0.001 * currentLeftTwist(0), 0.001 * currentLeftTwist(1),
546  0.001 * currentLeftTwist(2);
547  Eigen::Vector3f currentTCPAngularVelocity;
548  currentTCPAngularVelocity << currentLeftTwist(3), currentLeftTwist(4),
549  currentLeftTwist(5);
550  Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
551  Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
552 
553  Eigen::Vector3f tcpDesiredForce =
554  0.001 * leftKpos.cwiseProduct(desiredPosition - currentTCPPosition) +
555  leftDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
556  Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
557  Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
558  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
559  Eigen::Vector3f tcpDesiredTorque =
560  leftKori.cwiseProduct(rpy) - leftDori.cwiseProduct(currentTCPAngularVelocity);
561  leftJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
562  }
563 
564  Eigen::Vector6f rightJointControlWrench;
565  {
566 
567  Eigen::Vector3f targetTCPLinearVelocity;
568  targetTCPLinearVelocity << 0.001 * rightTargetVel(0), 0.001 * rightTargetVel(1),
569  0.001 * rightTargetVel(2);
570 
571  Eigen::Vector3f currentTCPLinearVelocity;
572  currentTCPLinearVelocity << 0.001 * currentRightTwist(0), 0.001 * currentRightTwist(1),
573  0.001 * currentRightTwist(2);
574  Eigen::Vector3f currentTCPAngularVelocity;
575  currentTCPAngularVelocity << currentRightTwist(3), currentRightTwist(4),
576  currentRightTwist(5);
577  Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
578  Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
579 
580  Eigen::Vector3f tcpDesiredForce =
581  0.001 * rightKpos.cwiseProduct(desiredPosition - currentTCPPosition) +
582  rightDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
583  Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
584  Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
585  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
586  Eigen::Vector3f tcpDesiredTorque =
587  rightKori.cwiseProduct(rpy) - rightDori.cwiseProduct(currentTCPAngularVelocity);
588  rightJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
589  }
590 
591 
592  // Eigen::VectorXf leftJointLimitAvoidance(leftRNS->getSize());
593  // for (size_t i = 0; i < leftRNS->getSize(); i++)
594  // {
595  // VirtualRobot::RobotNodePtr rn = leftRNS->getNode(i);
596  // if (rn->isLimitless())
597  // {
598  // leftJointLimitAvoidance(i) = 0;
599  // }
600  // else
601  // {
602  // float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
603  // leftJointLimitAvoidance(i) = cos(f * M_PI);
604  // }
605  // }
606  // Eigen::VectorXf leftNullspaceTorque = knull * leftJointLimitAvoidance - dnull * leftqvel;
607  // Eigen::VectorXf rightJointLimitAvoidance(rightRNS->getSize());
608  // for (size_t i = 0; i < rightRNS->getSize(); i++)
609  // {
610  // VirtualRobot::RobotNodePtr rn = rightRNS->getNode(i);
611  // if (rn->isLimitless())
612  // {
613  // rightJointLimitAvoidance(i) = 0;
614  // }
615  // else
616  // {
617  // float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
618  // rightJointLimitAvoidance(i) = cos(f * M_PI);
619  // }
620  // }
621  // Eigen::VectorXf rightNullspaceTorque = knull * rightJointLimitAvoidance - dnull * rightqvel;
622 
623 
624  Eigen::VectorXf leftNullspaceTorque =
625  knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
626  Eigen::VectorXf rightNullspaceTorque =
627  knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
628 
629  float lambda = 2;
630 
631  Eigen::MatrixXf jtpinvL =
632  leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
633  Eigen::MatrixXf jtpinvR =
634  rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
635  Eigen::VectorXf leftJointDesiredTorques =
636  jacobiL.transpose() * leftJointControlWrench +
637  (I - jacobiL.transpose() * jtpinvL) *
638  leftNullSpaceCoefs.cwiseProduct(leftNullspaceTorque);
639  Eigen::VectorXf rightJointDesiredTorques =
640  jacobiR.transpose() * rightJointControlWrench +
641  (I - jacobiR.transpose() * jtpinvR) *
642  rightNullSpaceCoefs.cwiseProduct(rightNullspaceTorque);
643 
644 
645  // torque limit
646  for (size_t i = 0; i < leftTargets.size(); ++i)
647  {
648  float desiredTorque = leftJointDesiredTorques(i);
649 
650  if (isnan(desiredTorque))
651  {
652  desiredTorque = 0;
653  }
654 
655  desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
656  desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
657 
658  debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] = desiredTorque;
659 
660  leftTargets.at(i)->torque = desiredTorque;
661  }
662 
663 
664  for (size_t i = 0; i < rightTargets.size(); ++i)
665  {
666  float desiredTorque = rightJointDesiredTorques(i);
667 
668  if (isnan(desiredTorque))
669  {
670  desiredTorque = 0;
671  }
672 
673  desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
674  desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
675 
676  debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] = desiredTorque;
677 
678  rightTargets.at(i)->torque = desiredTorque;
679  }
680  debugDataInfo.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
681 
682  debugDataInfo.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
683  debugDataInfo.getWriteBuffer().leftControlSignal_y = leftJointControlWrench(1);
684  debugDataInfo.getWriteBuffer().leftControlSignal_z = leftJointControlWrench(2);
685  debugDataInfo.getWriteBuffer().leftControlSignal_ro = leftJointControlWrench(3);
686  debugDataInfo.getWriteBuffer().leftControlSignal_pi = leftJointControlWrench(4);
687  debugDataInfo.getWriteBuffer().leftControlSignal_ya = leftJointControlWrench(5);
688 
689  debugDataInfo.getWriteBuffer().rightControlSignal_x = rightJointControlWrench(0);
690  debugDataInfo.getWriteBuffer().rightControlSignal_y = rightJointControlWrench(1);
691  debugDataInfo.getWriteBuffer().rightControlSignal_z = rightJointControlWrench(2);
692  debugDataInfo.getWriteBuffer().rightControlSignal_ro = rightJointControlWrench(3);
693  debugDataInfo.getWriteBuffer().rightControlSignal_pi = rightJointControlWrench(4);
694  debugDataInfo.getWriteBuffer().rightControlSignal_ya = rightJointControlWrench(5);
695  // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_x = tcpDesiredTorque(0);
696  // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_y = tcpDesiredTorque(1);
697  // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
698  // debugDataInfo.getWriteBuffer().quatError = errorAngle;
699  // debugDataInfo.getWriteBuffer().posiError = posiError;
700  debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
701  debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
702  debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
703  debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
704  debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
705  debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
706 
707 
708  debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
709  debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
710  debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
711 
712  debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
713  debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
714  debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
715  debugDataInfo.getWriteBuffer().virtualTime = virtualtime;
716 
717  debugDataInfo.commitWrite();
718 
719 
720  // Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
721  // Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
722  // Eigen::VectorXf jnvL = jtpinvL * leftTargetVel + (I - jtpinvL * jacobiL) * leftNullSpaceJointVelocity;
723 
724  // for (size_t i = 0; i < leftTargets.size(); ++i)
725  // {
726  // leftTargets.at(i)->velocity = jnvL(i);
727  // }
728  // Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
729  // Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
730  // Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
731 
732  // for (size_t i = 0; i < rightTargets.size(); ++i)
733  // {
734  // rightTargets.at(i)->velocity = jnvR(i);
735  // }
736  }
737 
738  void
739  NJointBimanualCCDMPController::learnDMPFromFiles(const std::string& name,
740  const Ice::StringSeq& fileNames,
741  const Ice::Current&)
742  {
743  if (name == "LeftLeader")
744  {
745  leftGroup.at(0)->learnDMPFromFiles(fileNames);
746  }
747  else if (name == "LeftFollower")
748  {
749  rightGroup.at(1)->learnDMPFromFiles(fileNames);
750  }
751  else if (name == "RightLeader")
752  {
753  rightGroup.at(0)->learnDMPFromFiles(fileNames);
754  }
755  else
756  {
757  leftGroup.at(1)->learnDMPFromFiles(fileNames);
758  }
759  }
760 
761  void
762  NJointBimanualCCDMPController::setViaPoints(Ice::Double u,
763  const Ice::DoubleSeq& viapoint,
764  const Ice::Current&)
765  {
766  LockGuardType guard(controllerMutex);
767  if (leaderName == "Left")
768  {
769  leftGroup.at(0)->setViaPose(u, viapoint);
770  }
771  else
772  {
773  rightGroup.at(0)->setViaPose(u, viapoint);
774  }
775  }
776 
777  void
778  NJointBimanualCCDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
779  {
780  LockGuardType guard(controllerMutex);
781  if (leaderName == "Left")
782  {
783  leftGroup.at(0)->setGoalPoseVec(goals);
784  }
785  else
786  {
787  rightGroup.at(0)->setGoalPoseVec(goals);
788  }
789  }
790 
791  void
792  NJointBimanualCCDMPController::changeLeader(const Ice::Current&)
793  {
794  LockGuardType guard(controllerMutex);
795 
796  if (leaderName == "Left")
797  {
798  leaderName = "Right";
799  rightGroup.at(0)->canVal = virtualtimer;
800  rightGroup.at(1)->canVal = virtualtimer;
801  }
802  else
803  {
804  leaderName = "Left";
805  leftGroup.at(0)->canVal = virtualtimer;
806  leftGroup.at(1)->canVal = virtualtimer;
807  }
808  }
809 
810  void
811  NJointBimanualCCDMPController::runDMP(const Ice::DoubleSeq& leftGoals,
812  const Ice::DoubleSeq& rightGoals,
813  const Ice::Current&)
814  {
815 
816  while (!interfaceData.updateReadBuffer())
817  {
818  usleep(100);
819  }
820  Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().currentLeftPose;
821  Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().currentRightPose;
822 
823  ARMARX_IMPORTANT << VAROUT(leftPose);
824  ARMARX_IMPORTANT << VAROUT(rightPose);
825 
826  virtualtimer = cfg->timeDuration;
827 
828  leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
829  rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
830 
831 
832  ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
833 
834  leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose),
835  getLocalPose(leftGoals, rightGoals));
836  rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose),
837  getLocalPose(rightGoals, leftGoals));
838 
839  finished = false;
840  controllerTask->start();
841  }
842 
843  Eigen::Matrix4f
844  NJointBimanualCCDMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate,
845  const Eigen::Matrix4f& globalTargetPose)
846  {
847  Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
848 
849  localPose.block<3, 3>(0, 0) =
850  newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
851  localPose.block<3, 1>(0, 3) =
852  newCoordinate.block<3, 3>(0, 0).inverse() *
853  (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
854 
855 
856  return localPose;
857  }
858 
859  void
860  NJointBimanualCCDMPController::onPublish(const SensorAndControl&,
861  const DebugDrawerInterfacePrx&,
862  const DebugObserverInterfacePrx& debugObs)
863  {
864 
865  StringVariantBaseMap datafields;
866  auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
867  for (auto& pair : values)
868  {
869  datafields[pair.first] = new Variant(pair.second);
870  }
871 
872  auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
873  for (auto& pair : constrained_force)
874  {
875  datafields[pair.first] = new Variant(pair.second);
876  }
877 
878 
879  datafields["leftTargetPose_x"] =
880  new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
881  datafields["leftTargetPose_y"] =
882  new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
883  datafields["leftTargetPose_z"] =
884  new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
885  datafields["rightTargetPose_x"] =
886  new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
887  datafields["rightTargetPose_y"] =
888  new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
889  datafields["rightTargetPose_z"] =
890  new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
891 
892 
893  datafields["leftCurrentPose_x"] =
894  new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
895  datafields["leftCurrentPose_y"] =
896  new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
897  datafields["leftCurrentPose_z"] =
898  new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
899  datafields["rightCurrentPose_x"] =
900  new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
901  datafields["rightCurrentPose_y"] =
902  new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
903  datafields["rightCurrentPose_z"] =
904  new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
905 
906  datafields["leftControlSignal_x"] =
907  new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_x);
908  datafields["leftControlSignal_y"] =
909  new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_y);
910  datafields["leftControlSignal_z"] =
911  new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_z);
912  datafields["leftControlSignal_ro"] =
913  new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ro);
914  datafields["leftControlSignal_pi"] =
915  new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_pi);
916  datafields["leftControlSignal_ya"] =
917  new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ya);
918 
919 
920  datafields["rightControlSignal_x"] =
921  new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_x);
922  datafields["rightControlSignal_y"] =
923  new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_y);
924  datafields["rightControlSignal_z"] =
925  new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_z);
926  datafields["rightControlSignal_ro"] =
927  new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ro);
928  datafields["rightControlSignal_pi"] =
929  new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_pi);
930  datafields["rightControlSignal_ya"] =
931  new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ya);
932 
933  datafields["virtual_timer"] =
934  new Variant(debugDataInfo.getUpToDateReadBuffer().virtualTime);
935 
936 
937  debugObs->setDebugChannel("DMPController", datafields);
938  }
939 
940  void
941  NJointBimanualCCDMPController::onInitNJointController()
942  {
943  ARMARX_INFO << "init ...";
944  controllerTask = new PeriodicTask<NJointBimanualCCDMPController>(
945  this, &NJointBimanualCCDMPController::controllerRun, 0.3);
946  }
947 
948  void
949  NJointBimanualCCDMPController::onDisconnectNJointController()
950  {
951  controllerTask->stop();
952  ARMARX_INFO << "stopped ...";
953  }
954 
955  Eigen::Matrix4f
956  NJointBimanualCCDMPController::getLocalPose(const std::vector<double>& newCoordinateVec,
957  const std::vector<double>& globalTargetPoseVec)
958  {
959  Eigen::Matrix4f newCoordinate =
960  VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4),
961  newCoordinateVec.at(5),
962  newCoordinateVec.at(6),
963  newCoordinateVec.at(3));
964  newCoordinate(0, 3) = newCoordinateVec.at(0);
965  newCoordinate(1, 3) = newCoordinateVec.at(1);
966  newCoordinate(2, 3) = newCoordinateVec.at(2);
967 
968  Eigen::Matrix4f globalTargetPose =
969  VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4),
970  globalTargetPoseVec.at(5),
971  globalTargetPoseVec.at(6),
972  globalTargetPoseVec.at(3));
973  globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
974  globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
975  globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
976 
977  return getLocalPose(newCoordinate, globalTargetPose);
978  }
979 
980  double
981  NJointBimanualCCDMPController::getVirtualTime(const Ice::Current&)
982  {
983  return virtualtimer;
984  }
985 
986  std::string
987  NJointBimanualCCDMPController::getLeaderName(const Ice::Current&)
988  {
989  return leaderName;
990  }
991 
992  bool
993  NJointBimanualCCDMPController::isFinished(const Ice::Current&)
994  {
995  return finished;
996  }
997 } // namespace armarx::control::deprecated_njoint_mp_controller::bimanual
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const NJointBimanualCCDMPControllerControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:223
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kori
float Kori
Definition: TaskSpaceVMP.h:54
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPControllerControlData::leftTargetPose
Eigen::Matrix4f leftTargetPose
Definition: NJointBimanualCCDMPController.h:47
MathUtils.h
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dpos
float Dpos
Definition: TaskSpaceVMP.h:53
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPControllerControlData::rightTargetPose
Eigen::Matrix4f rightTargetPose
Definition: NJointBimanualCCDMPController.h:48
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:92
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::rtGetControlStruct
const NJointBimanualCCDMPControllerControlData & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPKernelSize
int DMPKernelSize
Definition: TaskSpaceVMP.h:61
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::NJointBimanualCCDMPControllerInterface::getLeaderName
string getLeaderName()
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::NJointBimanualCCDMPController
NJointBimanualCCDMPController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: NJointBimanualCCDMPController.cpp:860
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::controllerRun
void controllerRun()
Definition: NJointBimanualCCDMPController.cpp:258
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::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPControllerControlData::leftTargetVel
Eigen::VectorXf leftTargetVel
Definition: NJointBimanualCCDMPController.h:44
armarx::NJointBimanualCCDMPControllerInterface::getVirtualTime
double getVirtualTime()
armarx::NJointControllerBase::useControlTarget
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition: NJointController.cpp:410
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::onInitNJointController
void onInitNJointController() override
Definition: NJointBimanualCCDMPController.cpp:941
armarx::NJointBimanualCCDMPControllerControlData::leftTargetPose
Eigen::Matrix4f leftTargetPose
Definition: NJointBimanualDMPForceController.h:40
armarx::NJointBimanualCCDMPControllerInterface::isFinished
bool isFinished()
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::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: NJointBimanualCCDMPController.cpp:442
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig
Definition: TaskSpaceVMP.h:59
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::phaseStopParas
PhaseStopParams phaseStopParas
Definition: TaskSpaceVMP.h:67
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::getWriterControlStruct
NJointBimanualCCDMPControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition: TaskSpaceVMP.h:48
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &) override
Definition: NJointBimanualCCDMPController.cpp:778
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::learnDMPFromFiles
void learnDMPFromFiles(const std::string &, const Ice::StringSeq &, const Ice::Current &) override
Definition: NJointBimanualCCDMPController.cpp:739
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::motionTimeDuration
float motionTimeDuration
Definition: TaskSpaceVMP.h:66
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:846
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::backDist
float backDist
Definition: TaskSpaceVMP.h:49
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::maxValue
float maxValue
Definition: TaskSpaceVMP.h:50
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::runDMP
void runDMP(const Ice::DoubleSeq &leftGoals, const Ice::DoubleSeq &rightGoals, const Ice::Current &) override
Definition: NJointBimanualCCDMPController.cpp:811
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:920
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dori
float Dori
Definition: TaskSpaceVMP.h:55
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: NJointBimanualCCDMPController.cpp:238
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:108
ControlTarget1DoFActuator.h
armarx::NJointControllerWithTripleBuffer< NJointBimanualCCDMPControllerControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition: TaskSpaceVMP.h:64
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::slop
float slop
Definition: TaskSpaceVMP.h:51
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition: TaskSpaceVMP.h:52
CartesianVelocityController.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPControllerControlData::virtualTime
double virtualTime
Definition: NJointBimanualCCDMPController.h:50
PIDController.h
NJointController.h
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerPtr
std::shared_ptr< TaskSpaceDMPController > TaskSpaceDMPControllerPtr
Definition: NJointTaskSpaceImpedanceDMPController.h:31
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPControllerControlData
Definition: NJointBimanualCCDMPController.h:41
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
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &) override
Definition: NJointBimanualCCDMPController.cpp:762
armarx::control::deprecated_njoint_mp_controller::bimanual::registrationControllerNJointBimanualCCDMPController
NJointControllerRegistration< NJointBimanualCCDMPController > registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController")
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPControllerControlData::rightTargetVel
Eigen::VectorXf rightTargetVel
Definition: NJointBimanualCCDMPController.h:45
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPStyle
std::string DMPStyle
Definition: TaskSpaceVMP.h:63
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition: TaskSpaceVMP.h:62
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: NJointBimanualCCDMPController.cpp:244
Eigen::Matrix< float, 6, 1 >
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition: TaskSpaceVMP.h:56
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPController::onDisconnectNJointController
void onDisconnectNJointController() override
Definition: NJointBimanualCCDMPController.cpp:949
armarx::PeriodicTask
Definition: ArmarXManager.h:70
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::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition: TaskSpaceVMP.h:90
armarx::NJointBimanualCCDMPControllerControlData::rightTargetPose
Eigen::Matrix4f rightTargetPose
Definition: NJointBimanualDMPForceController.h:41
SensorValue1DoFActuator.h
armarx::NJointControllerBase::useSensorValue
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
Definition: NJointController.cpp:383
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
armarx::control::deprecated_njoint_mp_controller::bimanual
Definition: NJointBimanualCartesianAdmittanceController.cpp:30
armarx::TripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:143
armarx::NJointBimanualCCDMPControllerControlData::rightTargetVel
Eigen::VectorXf rightTargetVel
Definition: NJointBimanualDMPForceController.h:38
norm
double norm(const Point &a)
Definition: point.hpp:102