NJointBimanualForceMPController.cpp
Go to the documentation of this file.
1 #include "NJointBimanualForceMPController.h"
2 
3 #include <random>
4 
5 // Simox
6 #include <VirtualRobot/IK/DifferentialIK.h>
7 #include <VirtualRobot/MathTools.h>
8 #include <VirtualRobot/Nodes/RobotNode.h>
9 #include <VirtualRobot/Robot.h>
10 #include <VirtualRobot/RobotNodeSet.h>
11 
12 // DMP
13 #include <dmp/representation/dmp/umitsmp.h>
14 
15 // armarx
16 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
17 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
18 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
19 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
20 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
21 #include <RobotAPI/libraries/core/CartesianVelocityController.h>
22 #include <RobotAPI/libraries/core/PIDController.h>
23 
24 // control
25 
26 
27 namespace armarx::control::deprecated_njoint_mp_controller::bimanual
28 {
29  NJointControllerRegistration<NJointBimanualForceMPController>
30  registrationControllerNJointBimanualForceMPController("NJointBimanualForceMPController");
31 
32  NJointBimanualForceMPController::NJointBimanualForceMPController(
33  const RobotUnitPtr& robUnit,
34  const armarx::NJointControllerConfigPtr& config,
35  const VirtualRobot::RobotPtr&)
36  {
37  useSynchronizedRtRobot();
38  cfg = NJointBimanualForceMPControllerConfigPtr::dynamicCast(config);
39 
40  VirtualRobot::RobotNodeSetPtr lrns = rtGetRobot()->getRobotNodeSet("LeftArm");
41  for (size_t i = 0; i < lrns->getSize(); ++i)
42  {
43  std::string jointName = lrns->getNode(i)->getName();
44  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
45  const SensorValueBase* sv = useSensorValue(jointName);
46  leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
47  const SensorValue1DoFActuatorVelocity* velocitySensor =
48  sv->asA<SensorValue1DoFActuatorVelocity>();
49  const SensorValue1DoFActuatorPosition* positionSensor =
50  sv->asA<SensorValue1DoFActuatorPosition>();
51 
52  if (!velocitySensor)
53  {
54  ARMARX_WARNING << "No velocity sensor available for " << jointName;
55  }
56  if (!positionSensor)
57  {
58  ARMARX_WARNING << "No position sensor available for " << jointName;
59  }
60 
61  leftVelocitySensors.push_back(velocitySensor);
62  leftPositionSensors.push_back(positionSensor);
63  };
64 
65  VirtualRobot::RobotNodeSetPtr rrns = rtGetRobot()->getRobotNodeSet("RightArm");
66  for (size_t i = 0; i < rrns->getSize(); ++i)
67  {
68  std::string jointName = rrns->getNode(i)->getName();
69  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
70  const SensorValueBase* sv = useSensorValue(jointName);
71  rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
72  const SensorValue1DoFActuatorVelocity* velocitySensor =
73  sv->asA<SensorValue1DoFActuatorVelocity>();
74  const SensorValue1DoFActuatorPosition* positionSensor =
75  sv->asA<SensorValue1DoFActuatorPosition>();
76 
77  if (!velocitySensor)
78  {
79  ARMARX_WARNING << "No velocity sensor available for " << jointName;
80  }
81  if (!positionSensor)
82  {
83  ARMARX_WARNING << "No position sensor available for " << jointName;
84  }
85 
86  rightVelocitySensors.push_back(velocitySensor);
87  rightPositionSensors.push_back(positionSensor);
88  };
89  const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
90  leftForceTorque = svlf->asA<SensorValueForceTorque>();
91  const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
92  rightForceTorque = svrf->asA<SensorValueForceTorque>();
93 
94  leftIK.reset(new VirtualRobot::DifferentialIK(
95  lrns, lrns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
96  rightIK.reset(new VirtualRobot::DifferentialIK(
97  rrns, rrns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
98 
99  leftTCP = lrns->getTCP();
100  rightTCP = rrns->getTCP();
101 
102  leftLearned = false;
103  rightLearned = false;
104 
105  // set tcp controller
106  leftTCPController.reset(new CartesianVelocityController(lrns, leftTCP));
107  rightTCPController.reset(new CartesianVelocityController(rrns, rightTCP));
108 
109 
110  finished = false;
111  tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
112  taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
113  taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
114  taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
115  taskSpaceDMPConfig.DMPStyle = cfg->dmpStyle;
116  taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
117  taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
118  taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
119  taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
120  taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
121  taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
122  taskSpaceDMPConfig.phaseStopParas.Dori = 0;
123  taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
124  taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
125  taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
126  leftDMPController.reset(
127  new tsvmp::TaskSpaceDMPController("Left", taskSpaceDMPConfig, false));
128  rightDMPController.reset(
129  new tsvmp::TaskSpaceDMPController("Right", taskSpaceDMPConfig, false));
130 
131 
132  // initialize tcp position and orientation
133  NJointBimanualForceMPControllerSensorData initSensorData;
134  initSensorData.leftPoseInRootFrame = leftTCP->getPoseInRootFrame();
135  initSensorData.leftTwistInRootFrame.setZero();
136  initSensorData.rightPoseInRootFrame = rightTCP->getPoseInRootFrame();
137  initSensorData.rightTwistInRootFrame.setZero();
138  initSensorData.leftForceInRootFrame.setZero();
139  initSensorData.rightForceInRootFrame.setZero();
140  controllerSensorData.reinitAllBuffers(initSensorData);
141 
142  NJointBimanualForceMPControllerControlData initData;
143  initData.leftTargetPose = leftTCP->getPoseInRootFrame();
144  initData.rightTargetPose = rightTCP->getPoseInRootFrame();
145  reinitTripleBuffer(initData);
146 
147 
148  Kp_LinearVel = cfg->Kp_LinearVel;
149  Kp_AngularVel = cfg->Kp_AngularVel;
150  Kd_LinearVel = cfg->Kd_LinearVel;
151  Kd_AngularVel = cfg->Kd_AngularVel;
152 
153  forceIterm = 0;
154  I_decay = 0.9;
155  xvel = 0;
156 
157  leftFilteredValue.setZero();
158  rightFilteredValue.setZero();
159 
160  for (size_t i = 0; i < 3; ++i)
161  {
162  leftForceOffset(i) = cfg->leftForceOffset.at(i);
163  }
164  for (size_t i = 0; i < 3; ++i)
165  {
166  rightForceOffset(i) = cfg->rightForceOffset.at(i);
167  }
168 
169 
170  targetSupportForce = cfg->targetSupportForce;
171 
172  NJointBimanualForceMPControllerInterfaceData initInterfaceData;
173  initInterfaceData.leftTcpPoseInRootFrame = leftTCP->getPoseInRootFrame();
174  initInterfaceData.rightTcpPoseInRootFrame = rightTCP->getPoseInRootFrame();
175  interfaceData.reinitAllBuffers(initInterfaceData);
176  }
177 
178  std::string
179  NJointBimanualForceMPController::getClassName(const Ice::Current&) const
180  {
181  return "NJointBimanualForceMPController";
182  }
183 
184  void
185  NJointBimanualForceMPController::controllerRun()
186  {
187  if (!controllerSensorData.updateReadBuffer() || !leftDMPController || !rightDMPController)
188  {
189  return;
190  }
191  double deltaT = controllerSensorData.getReadBuffer().deltaT;
192  Eigen::Matrix4f leftPose = controllerSensorData.getReadBuffer().leftPoseInRootFrame;
193  Eigen::Matrix4f rightPose = controllerSensorData.getReadBuffer().rightPoseInRootFrame;
194  Eigen::Vector6f leftTwist = controllerSensorData.getReadBuffer().leftTwistInRootFrame;
195  Eigen::Vector6f rightTwist = controllerSensorData.getReadBuffer().rightTwistInRootFrame;
196  Eigen::Vector3f leftForce = controllerSensorData.getReadBuffer().leftForceInRootFrame;
197  Eigen::Vector3f rightForce = controllerSensorData.getReadBuffer().rightForceInRootFrame;
198 
199  float forceOnHands = (leftForce + rightForce)(2);
200 
201  xvel =
202  cfg->forceP * (targetSupportForce + forceOnHands) + I_decay * cfg->forceI * forceIterm;
203 
204  forceIterm += targetSupportForce - forceOnHands;
205 
206  canVal = canVal + forceSign * xvel * deltaT;
207  // canVal = cfg->timeDuration + forceSign * xvel;
208 
209 
210  if (canVal > cfg->timeDuration)
211  {
212  canVal = cfg->timeDuration;
213  }
214 
215 
216  leftDMPController->canVal = canVal;
217  rightDMPController->canVal = canVal;
218 
219  leftDMPController->flow(deltaT, leftPose, leftTwist);
220  rightDMPController->flow(deltaT, rightPose, rightTwist);
221 
222  if (canVal < 1e-8)
223  {
224  finished = true;
225  }
226 
227 
228  std::vector<double> leftTargetState = leftDMPController->getTargetPose();
229  debugOutputData.getWriteBuffer().dmpTargets["leftTarget_x"] = leftTargetState[0];
230  debugOutputData.getWriteBuffer().dmpTargets["leftTarget_y"] = leftTargetState[1];
231  debugOutputData.getWriteBuffer().dmpTargets["leftTarget_z"] = leftTargetState[2];
232  debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qw"] = leftTargetState[3];
233  debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qx"] = leftTargetState[4];
234  debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qy"] = leftTargetState[5];
235  debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qz"] = leftTargetState[6];
236  std::vector<double> rightTargetState = rightDMPController->getTargetPose();
237  debugOutputData.getWriteBuffer().dmpTargets["rightTarget_x"] = rightTargetState[0];
238  debugOutputData.getWriteBuffer().dmpTargets["rightTarget_y"] = rightTargetState[1];
239  debugOutputData.getWriteBuffer().dmpTargets["rightTarget_z"] = rightTargetState[2];
240  debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qw"] = rightTargetState[3];
241  debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qx"] = rightTargetState[4];
242  debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qy"] = rightTargetState[5];
243  debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qz"] = rightTargetState[6];
244 
245 
246  {
247  debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = leftPose(0, 3);
248  debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = leftPose(1, 3);
249  debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = leftPose(2, 3);
250  VirtualRobot::MathTools::Quaternion leftQuat =
251  VirtualRobot::MathTools::eigen4f2quat(leftPose);
252  debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = leftQuat.w;
253  debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = leftQuat.x;
254  debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = leftQuat.y;
255  debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = leftQuat.z;
256  }
257  {
258  debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = rightPose(0, 3);
259  debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = rightPose(1, 3);
260  debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = rightPose(2, 3);
261  VirtualRobot::MathTools::Quaternion rightQuat =
262  VirtualRobot::MathTools::eigen4f2quat(rightPose);
263  debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = rightQuat.w;
264  debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = rightQuat.x;
265  debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = rightQuat.y;
266  debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = rightQuat.z;
267  }
268 
269  debugOutputData.getWriteBuffer().canVal = canVal;
270  debugOutputData.getWriteBuffer().forceOnHands = forceOnHands;
271  debugOutputData.commitWrite();
272 
273 
274  getWriterControlStruct().leftTargetPose = leftDMPController->getTargetPoseMat();
275  getWriterControlStruct().rightTargetPose = rightDMPController->getTargetPoseMat();
276  writeControlStruct();
277  }
278 
279  void
280  NJointBimanualForceMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
281  const IceUtil::Time& timeSinceLastIteration)
282  {
283  Eigen::Matrix4f leftPose = leftTCP->getPoseInRootFrame();
284  Eigen::Matrix4f rightPose = rightTCP->getPoseInRootFrame();
285  Eigen::MatrixXf leftJacobi =
286  leftIK->getJacobianMatrix(leftTCP, VirtualRobot::IKSolver::CartesianSelection::All);
287  Eigen::MatrixXf rightJacobi =
288  leftIK->getJacobianMatrix(leftTCP, VirtualRobot::IKSolver::CartesianSelection::All);
289 
290  Eigen::VectorXf qvel;
291  qvel.resize(leftVelocitySensors.size());
292  for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
293  {
294  qvel(i) = leftVelocitySensors[i]->velocity;
295  }
296  Eigen::Vector6f leftTwist = leftJacobi * qvel;
297  for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
298  {
299  qvel(i) = rightVelocitySensors[i]->velocity;
300  }
301  Eigen::Vector6f rightTwist = rightJacobi * qvel;
302 
303  leftFilteredValue = (1 - cfg->filterCoeff) * (leftForceTorque->force - leftForceOffset) +
304  cfg->filterCoeff * leftFilteredValue;
305  rightFilteredValue = (1 - cfg->filterCoeff) * (rightForceTorque->force - rightForceOffset) +
306  cfg->filterCoeff * rightFilteredValue;
307 
308  Eigen::Matrix4f leftSensorFrame =
309  rtGetRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
310  Eigen::Matrix4f rightSensorFrame =
311  rtGetRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
312 
313  Eigen::Vector3f leftForceInRootFrame = leftPose.block<3, 3>(0, 0).transpose() *
314  leftSensorFrame.block<3, 3>(0, 0) *
315  leftFilteredValue;
316  Eigen::Vector3f rightForceInRootFrame = rightPose.block<3, 3>(0, 0).transpose() *
317  rightSensorFrame.block<3, 3>(0, 0) *
318  rightFilteredValue;
319 
320  controllerSensorData.getWriteBuffer().leftPoseInRootFrame = leftPose;
321  controllerSensorData.getWriteBuffer().rightPoseInRootFrame = rightPose;
322  controllerSensorData.getWriteBuffer().leftTwistInRootFrame = leftTwist;
323  controllerSensorData.getWriteBuffer().rightTwistInRootFrame = rightTwist;
324  controllerSensorData.getWriteBuffer().leftForceInRootFrame = leftForceInRootFrame;
325  controllerSensorData.getWriteBuffer().rightForceInRootFrame = rightForceInRootFrame;
326  controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
327  controllerSensorData.commitWrite();
328 
329  interfaceData.getWriteBuffer().leftTcpPoseInRootFrame = leftPose;
330  interfaceData.getWriteBuffer().rightTcpPoseInRootFrame = rightPose;
331  interfaceData.commitWrite();
332 
333 
334  Eigen::Matrix4f targetPose = rtGetControlStruct().leftTargetPose;
335  Eigen::Matrix4f currentPose = leftPose;
336  Eigen::Vector6f leftVel;
337 
338  {
339  Eigen::Matrix3f diffMat =
340  targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
341  Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
342 
343  Eigen::Vector6f rtTargetVel;
344  rtTargetVel.block<3, 1>(0, 0) =
345  Kp_LinearVel * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) +
346  Kd_LinearVel * (-leftTwist.block<3, 1>(0, 0));
347  rtTargetVel.block<3, 1>(3, 0) =
348  Kp_AngularVel * errorRPY + Kd_AngularVel * (-leftTwist.block<3, 1>(3, 0));
349 
350  float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
351  if (normLinearVelocity > cfg->maxLinearVel)
352  {
353  rtTargetVel.block<3, 1>(0, 0) =
354  cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
355  }
356 
357  float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
358  if (normAngularVelocity > cfg->maxAngularVel)
359  {
360  rtTargetVel.block<3, 1>(3, 0) =
361  cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
362  }
363 
364  for (size_t i = 0; i < 6; i++)
365  {
366  leftVel(i) = rtTargetVel(i);
367  }
368  }
369 
370  // cartesian vel controller
371 
372 
373  targetPose = rtGetControlStruct().rightTargetPose;
374  currentPose = rightPose;
375  Eigen::Vector6f rightVel;
376 
377  {
378  Eigen::Matrix3f diffMat =
379  targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
380  Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
381 
382  Eigen::Vector6f rtTargetVel;
383  rtTargetVel.block<3, 1>(0, 0) =
384  Kp_LinearVel * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) +
385  Kd_LinearVel * (-rightTwist.block<3, 1>(0, 0));
386  rtTargetVel.block<3, 1>(3, 0) =
387  Kp_AngularVel * errorRPY + Kd_AngularVel * (-rightTwist.block<3, 1>(3, 0));
388 
389  float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
390  if (normLinearVelocity > cfg->maxLinearVel)
391  {
392  rtTargetVel.block<3, 1>(0, 0) =
393  cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
394  }
395 
396  float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
397  if (normAngularVelocity > cfg->maxAngularVel)
398  {
399  rtTargetVel.block<3, 1>(3, 0) =
400  cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
401  }
402 
403  for (size_t i = 0; i < 6; i++)
404  {
405  rightVel(i) = rtTargetVel(i);
406  }
407  }
408 
409 
410  Eigen::VectorXf leftJTV =
411  leftTCPController->calculate(leftVel,
412  cfg->KpJointLimitAvoidanceScale,
413  VirtualRobot::IKSolver::CartesianSelection::All);
414  for (size_t i = 0; i < leftTargets.size(); ++i)
415  {
416  leftTargets.at(i)->velocity = leftJTV(i);
417  if (!leftTargets.at(i)->isValid())
418  {
419  ARMARX_IMPORTANT
420  << deactivateSpam(1, std::to_string(i))
421  << "Velocity controller target is invalid - setting to zero! set value: "
422  << leftTargets.at(i)->velocity;
423  leftTargets.at(i)->velocity = 0.0f;
424  }
425  }
426 
427  Eigen::VectorXf rightJTV =
428  rightTCPController->calculate(rightVel,
429  cfg->KpJointLimitAvoidanceScale,
430  VirtualRobot::IKSolver::CartesianSelection::All);
431  for (size_t i = 0; i < rightTargets.size(); ++i)
432  {
433  rightTargets.at(i)->velocity = rightJTV(i);
434  if (!rightTargets.at(i)->isValid())
435  {
436  ARMARX_IMPORTANT
437  << deactivateSpam(1, std::to_string(i))
438  << "Velocity controller target is invalid - setting to zero! set value: "
439  << rightTargets.at(i)->velocity;
440  rightTargets.at(i)->velocity = 0.0f;
441  }
442  }
443  }
444 
445  void
446  NJointBimanualForceMPController::learnDMPFromFiles(const std::string& whichDMP,
447  const Ice::StringSeq& fileNames,
448  const Ice::Current&)
449  {
450  ARMARX_IMPORTANT << "Learn DMP " << whichDMP;
451  if (whichDMP == "Left")
452  {
453  leftDMPController->learnDMPFromFiles(fileNames);
454  leftLearned = true;
455  ARMARX_INFO << "Left Learned";
456  }
457  if (whichDMP == "Right")
458  {
459  rightDMPController->learnDMPFromFiles(fileNames);
460  rightLearned = true;
461  ARMARX_INFO << "Right Learned";
462  }
463  }
464 
465  void
466  NJointBimanualForceMPController::runDMP(const Ice::DoubleSeq& leftGoals,
467  const Ice::DoubleSeq& rightGoals,
468  const Ice::Current&)
469  {
470  if (!leftLearned)
471  {
472  ARMARX_ERROR << "Left DMP is not learned, aborted";
473  return;
474  }
475 
476  if (!rightLearned)
477  {
478  ARMARX_ERROR << "Right DMP is not learned, aborted";
479  return;
480  }
481 
482  while (!interfaceData.updateReadBuffer())
483  {
484  usleep(100);
485  }
486 
487  Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().leftTcpPoseInRootFrame;
488  Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().rightTcpPoseInRootFrame;
489 
490  forceSign = leftGoals.at(2) - leftPose(2, 3) > 0 ? -1 : 1;
491  // Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
492  ARMARX_IMPORTANT << "leftGoals dim: " << leftGoals.size();
493  ARMARX_IMPORTANT << "rightGoals dim: " << rightGoals.size();
494 
495  leftDMPController->prepareExecution(leftDMPController->eigen4f2vec(leftPose), leftGoals);
496  rightDMPController->prepareExecution(rightDMPController->eigen4f2vec(rightPose),
497  rightGoals);
498 
499  canVal = cfg->timeDuration;
500  finished = false;
501 
502  ARMARX_INFO << "run DMP";
503  controllerTask->start();
504  }
505 
506  void
507  NJointBimanualForceMPController::setViaPoints(const std::string& whichDMP,
508  double u,
509  const Ice::DoubleSeq& viaPoint,
510  const Ice::Current&)
511  {
512  if (whichDMP == "Left")
513  {
514  leftDMPController->setViaPose(u, viaPoint);
515  }
516  if (whichDMP == "Right")
517  {
518  rightDMPController->setViaPose(u, viaPoint);
519  }
520  }
521 
522  void
523  NJointBimanualForceMPController::onPublish(const SensorAndControl&,
524  const DebugDrawerInterfacePrx&,
525  const DebugObserverInterfacePrx& debugObs)
526  {
527  std::string debugName = cfg->debugName;
528  std::string datafieldName = debugName;
529  StringVariantBaseMap datafields;
530 
531  auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
532  for (auto& pair : dmpTargets)
533  {
534  datafieldName = pair.first + "_" + debugName;
535  datafields[datafieldName] = new Variant(pair.second);
536  }
537 
538  auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
539  for (auto& pair : currentPose)
540  {
541  datafieldName = pair.first + "_" + debugName;
542  datafields[datafieldName] = new Variant(pair.second);
543  }
544 
545  datafieldName = "canVal_" + debugName;
546  datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().canVal);
547 
548  datafieldName = "forceOnHands_" + debugName;
549  datafields[datafieldName] =
550  new Variant(debugOutputData.getUpToDateReadBuffer().forceOnHands);
551  datafieldName = "DMPController_" + debugName;
552 
553  debugObs->setDebugChannel(datafieldName, datafields);
554  }
555 
556  void
557  NJointBimanualForceMPController::onInitNJointController()
558  {
559  ARMARX_INFO << "init ...";
560  controllerTask = new PeriodicTask<NJointBimanualForceMPController>(
561  this, &NJointBimanualForceMPController::controllerRun, 0.3);
562  }
563 
564  void
565  NJointBimanualForceMPController::onDisconnectNJointController()
566  {
567  ARMARX_INFO << "stopped ...";
568  controllerTask->stop();
569  }
570 
571 
572 } // namespace armarx::control::deprecated_njoint_mp_controller::bimanual
armarx::NJointControllerWithTripleBuffer< NJointBimanualForceMPControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const NJointBimanualForceMPControllerControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPController::learnDMPFromFiles
void learnDMPFromFiles(const std::string &whichMP, const Ice::StringSeq &fileNames, const Ice::Current &) override
Definition: NJointBimanualForceMPController.cpp:446
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::tsvmp::PhaseStopParams::Kori
float Kori
Definition: TaskSpaceVMP.h:54
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: NJointBimanualForceMPController.cpp:179
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dpos
float Dpos
Definition: TaskSpaceVMP.h:53
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< NJointBimanualForceMPControllerControlData >::rtGetControlStruct
const NJointBimanualForceMPControllerControlData & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPKernelSize
int DMPKernelSize
Definition: TaskSpaceVMP.h:61
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::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
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::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPControllerControlData
Definition: NJointBimanualForceMPController.h:35
SensorValueForceTorque.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPControllerControlData::leftTargetPose
Eigen::Matrix4f leftTargetPose
Definition: NJointBimanualForceMPController.h:38
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig
Definition: TaskSpaceVMP.h:59
armarx::CartesianVelocityController
Definition: CartesianVelocityController.h:36
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::phaseStopParas
PhaseStopParams phaseStopParas
Definition: TaskSpaceVMP.h:67
armarx::NJointControllerWithTripleBuffer< NJointBimanualForceMPControllerControlData >::getWriterControlStruct
NJointBimanualForceMPControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition: TaskSpaceVMP.h:48
armarx::control::deprecated_njoint_mp_controller::bimanual::registrationControllerNJointBimanualForceMPController
NJointControllerRegistration< NJointBimanualForceMPController > registrationControllerNJointBimanualForceMPController("NJointBimanualForceMPController")
armarx::SensorValueForceTorque::force
Eigen::Vector3f force
Definition: SensorValueForceTorque.h:36
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::motionTimeDuration
float motionTimeDuration
Definition: TaskSpaceVMP.h:66
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:846
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::backDist
float backDist
Definition: TaskSpaceVMP.h:49
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::maxValue
float maxValue
Definition: TaskSpaceVMP.h:50
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dori
float Dori
Definition: TaskSpaceVMP.h:55
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: NJointBimanualForceMPController.cpp:280
if
if(!yyvaluep)
Definition: Grammar.cpp:645
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPControllerControlData::rightTargetPose
Eigen::Matrix4f rightTargetPose
Definition: NJointBimanualForceMPController.h:39
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPController::setViaPoints
void setViaPoints(const std::string &whichDMP, double canVal, const Ice::DoubleSeq &viaPoint, const Ice::Current &) override
Definition: NJointBimanualForceMPController.cpp:507
armarx::NJointControllerWithTripleBuffer< NJointBimanualForceMPControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:108
ControlTarget1DoFActuator.h
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition: TaskSpaceVMP.h:64
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::slop
float slop
Definition: TaskSpaceVMP.h:51
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition: TaskSpaceVMP.h:52
CartesianVelocityController.h
PIDController.h
NJointController.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPController::onInitNJointController
void onInitNJointController() override
Definition: NJointBimanualForceMPController.cpp:557
NJointBimanualForceMPController.h
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::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition: TaskSpaceVMP.h:62
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:99
Eigen::Matrix< float, 6, 1 >
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition: TaskSpaceVMP.h:56
armarx::PeriodicTask
Definition: ArmarXManager.h:70
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
armarx::TripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:116
armarx::SensorValueForceTorque
Definition: SensorValueForceTorque.h:32
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:772
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPController::runDMP
void runDMP(const Ice::DoubleSeq &leftGoals, const Ice::DoubleSeq &rightGoals, const Ice::Current &) override
Definition: NJointBimanualForceMPController.cpp:466
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: NJointBimanualForceMPController.cpp:523
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPController::controllerRun
void controllerRun()
Definition: NJointBimanualForceMPController.cpp:185
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
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
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPController::onDisconnectNJointController
void onDisconnectNJointController() override
Definition: NJointBimanualForceMPController.cpp:565
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceMPController::NJointBimanualForceMPController
NJointBimanualForceMPController(const RobotUnitPtr &, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointBimanualForceMPController.cpp:32
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
norm
double norm(const Point &a)
Definition: point.hpp:102