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