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