NJointBimanualObjLevelController.cpp
Go to the documentation of this file.
1 #include "NJointBimanualObjLevelController.h"
2 
3 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
4 #include <ArmarXCore/core/time/CycleUtil.h>
5 
6 // Simox
7 #include <VirtualRobot/IK/DifferentialIK.h>
8 #include <VirtualRobot/MathTools.h>
9 #include <VirtualRobot/Nodes/RobotNode.h>
10 #include <VirtualRobot/Nodes/Sensor.h>
11 #include <VirtualRobot/Robot.h>
12 #include <VirtualRobot/RobotNodeSet.h>
13 
14 // armarx
15 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
16 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
17 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
18 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
19 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
20 #include <RobotAPI/libraries/core/PIDController.h>
21 #include <RobotAPI/libraries/core/math/MathUtils.h>
22 
23 namespace armarx::control::deprecated_njoint_mp_controller::bimanual
24 {
25  NJointControllerRegistration<NJointBimanualObjLevelController>
26  registrationControllerNJointBimanualObjLevelController("NJointBimanualObjLevelController");
27 
28  NJointBimanualObjLevelController::NJointBimanualObjLevelController(
29  const RobotUnitPtr& robUnit,
30  const armarx::NJointControllerConfigPtr& config,
31  const VirtualRobot::RobotPtr&)
32  {
33  ARMARX_INFO << "Initializing Bimanual Object Level Controller";
34 
35  useSynchronizedRtRobot();
36  cfg = NJointBimanualObjLevelControllerConfigPtr::dynamicCast(config);
37  // ARMARX_CHECK_EXPRESSION(prov);
38  // RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
39  // ARMARX_CHECK_EXPRESSION(robotUnit);
40  leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
41 
42  for (size_t i = 0; i < leftRNS->getSize(); ++i)
43  {
44  std::string jointName = leftRNS->getNode(i)->getName();
45  leftJointNames.push_back(jointName);
46  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
47  const SensorValueBase* sv = useSensorValue(jointName);
48  leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
49  const SensorValue1DoFActuatorVelocity* velocitySensor =
50  sv->asA<SensorValue1DoFActuatorVelocity>();
51  const SensorValue1DoFActuatorPosition* positionSensor =
52  sv->asA<SensorValue1DoFActuatorPosition>();
53 
54  if (!velocitySensor)
55  {
56  ARMARX_WARNING << "No velocitySensor available for " << jointName;
57  }
58  if (!positionSensor)
59  {
60  ARMARX_WARNING << "No positionSensor available for " << jointName;
61  }
62 
63 
64  leftVelocitySensors.push_back(velocitySensor);
65  leftPositionSensors.push_back(positionSensor);
66  };
67 
68  rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
69 
70  for (size_t i = 0; i < rightRNS->getSize(); ++i)
71  {
72  std::string jointName = rightRNS->getNode(i)->getName();
73  rightJointNames.push_back(jointName);
74  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
75  const SensorValueBase* sv = useSensorValue(jointName);
76  rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
77 
78  const SensorValue1DoFActuatorVelocity* velocitySensor =
79  sv->asA<SensorValue1DoFActuatorVelocity>();
80  const SensorValue1DoFActuatorPosition* positionSensor =
81  sv->asA<SensorValue1DoFActuatorPosition>();
82 
83  if (!velocitySensor)
84  {
85  ARMARX_WARNING << "No velocitySensor available for " << jointName;
86  }
87  if (!positionSensor)
88  {
89  ARMARX_WARNING << "No positionSensor available for " << jointName;
90  }
91 
92  rightVelocitySensors.push_back(velocitySensor);
93  rightPositionSensors.push_back(positionSensor);
94  };
95 
96 
97  // const SensorValueBase* svlf = prov->getSensorValue("FT L");
98  const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
99  leftForceTorque = svlf->asA<SensorValueForceTorque>();
100  // const SensorValueBase* svrf = prov->getSensorValue("FT R");
101  const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
102  rightForceTorque = svrf->asA<SensorValueForceTorque>();
103 
104  leftIK.reset(new VirtualRobot::DifferentialIK(
105  leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
106  rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS,
107  rightRNS->getRobot()->getRootNode(),
108  VirtualRobot::JacobiProvider::eSVDDamped));
109 
110 
111  tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
112  taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
113  taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
114  taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
115  taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
116  taskSpaceDMPConfig.DMPAmplitude = 1.0;
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 
127 
128  objectDMP.reset(new tsvmp::TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
129  ARMARX_IMPORTANT << "dmp finieshed";
130 
131  tcpLeft = leftRNS->getTCP();
132  tcpRight = rightRNS->getTCP();
133 
134  //initialize control parameters
135  KpImpedance.setZero(cfg->KpImpedance.size());
136  ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 12);
137 
138  for (int i = 0; i < KpImpedance.size(); ++i)
139  {
140  KpImpedance(i) = cfg->KpImpedance.at(i);
141  }
142 
143  KdImpedance.setZero(cfg->KdImpedance.size());
144  ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 12);
145 
146  for (int i = 0; i < KdImpedance.size(); ++i)
147  {
148  KdImpedance(i) = cfg->KdImpedance.at(i);
149  }
150 
151  KpAdmittance.setZero(cfg->KpAdmittance.size());
152  ARMARX_CHECK_EQUAL(cfg->KpAdmittance.size(), 6);
153 
154  for (int i = 0; i < KpAdmittance.size(); ++i)
155  {
156  KpAdmittance(i) = cfg->KpAdmittance.at(i);
157  }
158 
159  KdAdmittance.setZero(cfg->KdAdmittance.size());
160  ARMARX_CHECK_EQUAL(cfg->KdAdmittance.size(), 6);
161 
162  for (int i = 0; i < KdAdmittance.size(); ++i)
163  {
164  KdAdmittance(i) = cfg->KdAdmittance.at(i);
165  }
166 
167  KmAdmittance.setZero(cfg->KmAdmittance.size());
168  ARMARX_CHECK_EQUAL(cfg->KmAdmittance.size(), 6);
169 
170  for (int i = 0; i < KmAdmittance.size(); ++i)
171  {
172  KmAdmittance(i) = cfg->KmAdmittance.at(i);
173  }
174 
175 
176  Inferface2rtData initInt2rtData;
177  initInt2rtData.KpImpedance = KpImpedance;
178  initInt2rtData.KdImpedance = KdImpedance;
179  initInt2rtData.KmAdmittance = KmAdmittance;
180  initInt2rtData.KpAdmittance = KpAdmittance;
181  initInt2rtData.KdAdmittance = KdAdmittance;
182  interface2rtBuffer.reinitAllBuffers(initInt2rtData);
183 
184  leftDesiredJointValues.resize(leftTargets.size());
185  ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
186 
187  for (size_t i = 0; i < leftTargets.size(); ++i)
188  {
189  leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
190  }
191 
192  rightDesiredJointValues.resize(rightTargets.size());
193  ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
194 
195  for (size_t i = 0; i < rightTargets.size(); ++i)
196  {
197  rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
198  }
199 
200  KmPID.resize(cfg->KmPID.size());
201  ARMARX_CHECK_EQUAL(cfg->KmPID.size(), 6);
202 
203  for (int i = 0; i < KmPID.size(); ++i)
204  {
205  KmPID(i) = cfg->KmPID.at(i);
206  }
207 
208  virtualAcc.setZero(6);
209  virtualVel.setZero(6);
210  virtualPose = Eigen::Matrix4f::Identity();
211  ARMARX_INFO << "got controller params";
212 
213 
214  boxInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->boxInitialPose[4],
215  cfg->boxInitialPose[5],
216  cfg->boxInitialPose[6],
217  cfg->boxInitialPose[3]);
218  for (int i = 0; i < 3; ++i)
219  {
220  boxInitialPose(i, 3) = cfg->boxInitialPose[i];
221  }
222 
223  rt2ControlData initSensorData;
224  initSensorData.deltaT = 0;
225  initSensorData.currentTime = 0;
226  initSensorData.currentPose = boxInitialPose;
227  initSensorData.currentTwist.setZero();
228  rt2ControlBuffer.reinitAllBuffers(initSensorData);
229 
230 
231  ControlInterfaceData initInterfaceData;
232  initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
233  initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
234  initInterfaceData.currentObjPose = boxInitialPose;
235  initInterfaceData.currentObjForce.setZero();
236  initInterfaceData.currentObjVel.setZero();
237  controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
238 
239 
240  leftInitialPose = tcpLeft->getPoseInRootFrame();
241  rightInitialPose = tcpRight->getPoseInRootFrame();
242  leftInitialPose.block<3, 1>(0, 3) = 0.001 * leftInitialPose.block<3, 1>(0, 3);
243  rightInitialPose.block<3, 1>(0, 3) = 0.001 * rightInitialPose.block<3, 1>(0, 3);
244 
245  // leftInitialPose = boxInitialPose;
246  // leftInitialPose(0, 3) -= cfg->boxWidth * 0.5;
247  // rightInitialPose = boxInitialPose;
248  // rightInitialPose(0, 3) += cfg->boxWidth * 0.5;
249 
250  forcePIDControllers.resize(12);
251  for (size_t i = 0; i < 6; i++)
252  {
253  forcePIDControllers.at(i).reset(new PIDController(
254  cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
255  forcePIDControllers.at(i + 6).reset(new PIDController(
256  cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
257  forcePIDControllers.at(i)->reset();
258  forcePIDControllers.at(i + 6)->reset();
259  }
260 
261  // filter
262  filterCoeff = cfg->filterCoeff;
263  ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
264  filteredOldValue.setZero(12);
265 
266  // static compensation
267  massLeft = cfg->massLeft;
268  CoMVecLeft << cfg->CoMVecLeft[0], cfg->CoMVecLeft[1], cfg->CoMVecLeft[2];
269  forceOffsetLeft << cfg->forceOffsetLeft[0], cfg->forceOffsetLeft[1],
270  cfg->forceOffsetLeft[2];
271  torqueOffsetLeft << cfg->torqueOffsetLeft[0], cfg->torqueOffsetLeft[1],
272  cfg->torqueOffsetLeft[2];
273 
274  massRight = cfg->massRight;
275  CoMVecRight << cfg->CoMVecRight[0], cfg->CoMVecRight[1], cfg->CoMVecRight[2];
276  forceOffsetRight << cfg->forceOffsetRight[0], cfg->forceOffsetRight[1],
277  cfg->forceOffsetRight[2];
278  torqueOffsetRight << cfg->torqueOffsetRight[0], cfg->torqueOffsetRight[1],
279  cfg->torqueOffsetRight[2];
280 
281  sensorFrame2TcpFrameLeft.setZero();
282  sensorFrame2TcpFrameRight.setZero();
283 
284  NJointBimanualObjLevelControlData initData;
285  initData.boxPose = boxInitialPose;
286  initData.boxTwist.setZero(6);
287  reinitTripleBuffer(initData);
288 
289  firstLoop = true;
290  ARMARX_INFO << "left initial pose: \n"
291  << leftInitialPose << "\n right initial pose: \n"
292  << rightInitialPose;
293 
294  ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
295  ARMARX_IMPORTANT << "finished construction!";
296 
297  dmpStarted = false;
298 
299 
300  ftcalibrationTimer = 0;
301  ftOffset.setZero(12);
302 
303  targetWrench.setZero(cfg->targetWrench.size());
304  for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
305  {
306  targetWrench(i) = cfg->targetWrench[i];
307  }
308 
309 
310  fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
311  objCom2TCPLeftInObjFrame.setZero();
312  objCom2TCPRightInObjFrame.setZero();
313  }
314 
315  void
316  NJointBimanualObjLevelController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
317  {
318  objectDMP->setWeights(weights);
319  }
320 
321  DoubleSeqSeq
322  NJointBimanualObjLevelController::getMPWeights(const Ice::Current&)
323  {
324  DMP::DVec2d res = objectDMP->getWeights();
325  DoubleSeqSeq resvec;
326  for (size_t i = 0; i < res.size(); ++i)
327  {
328  std::vector<double> cvec;
329  for (size_t j = 0; j < res[i].size(); ++j)
330  {
331  cvec.push_back(res[i][j]);
332  }
333  resvec.push_back(cvec);
334  }
335 
336  return resvec;
337  }
338 
339  void
340  NJointBimanualObjLevelController::setMPRotWeights(const DoubleSeqSeq& weights,
341  const Ice::Current&)
342  {
343  objectDMP->setRotWeights(weights);
344  }
345 
346  DoubleSeqSeq
347  NJointBimanualObjLevelController::getMPRotWeights(const Ice::Current&)
348  {
349  DMP::DVec2d res = objectDMP->getRotWeights();
350  DoubleSeqSeq resvec;
351  for (size_t i = 0; i < res.size(); ++i)
352  {
353  std::vector<double> cvec;
354  for (size_t j = 0; j < res[i].size(); ++j)
355  {
356  cvec.push_back(res[i][j]);
357  }
358  resvec.push_back(cvec);
359  }
360 
361  return resvec;
362  }
363 
364  void
365  NJointBimanualObjLevelController::rtPreActivateController()
366  {
367  Eigen::Matrix4f boxInitPose = Eigen::Matrix4f::Identity();
368  Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
369  Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
370  leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3) * 0.001;
371  rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3) * 0.001;
372  boxInitPose.block<3, 1>(0, 3) =
373  0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
374  boxInitPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
375 
376  NJointBimanualObjLevelControlData initData;
377  initData.boxPose = boxInitPose;
378  initData.boxTwist.resize(6);
379  reinitTripleBuffer(initData);
380  }
381 
382  std::string
383  NJointBimanualObjLevelController::getClassName(const Ice::Current&) const
384  {
385  return "NJointBimanualObjLevelController";
386  }
387 
388  void
389  NJointBimanualObjLevelController::controllerRun()
390  {
391  if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
392  {
393  return;
394  }
395 
396  double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
397  Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
398  Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
399  //ARMARX_IMPORTANT << "canVal: " << objectDMP->canVal;
400 
401  if (objectDMP->canVal < 1e-8)
402  {
403  finished = true;
404  dmpStarted = false;
405  }
406 
407  objectDMP->flow(deltaT, currentPose, currentTwist);
408 
409  LockGuardType guard{controlDataMutex};
410  getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
411  getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
412  writeControlStruct();
413  }
414 
415  void
416  NJointBimanualObjLevelController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
417  const IceUtil::Time& timeSinceLastIteration)
418  {
419  Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
420  Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
421 
422  controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
423  controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
424  double deltaT = timeSinceLastIteration.toSecondsDouble();
425 
426  ftcalibrationTimer += deltaT;
427 
428  if (firstLoop)
429  {
430  Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
431  Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
432 
433  leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3) * 0.001;
434  rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3) * 0.001;
435 
436  virtualPose.block<3, 1>(0, 3) =
437  0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
438  virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
439  fixedLeftRightRotOffset =
440  virtualPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
441 
442  Eigen::Vector3f objCom2TCPLeftInGlobal =
443  leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
444  Eigen::Vector3f objCom2TCPRightInGlobal =
445  rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
446  // objCom2TCPLeft << -cfg->boxWidth * 0.5, 0, 0;
447  // objCom2TCPRight << cfg->boxWidth * 0.5, 0, 0;
448 
449  objCom2TCPLeftInObjFrame =
450  virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
451  objCom2TCPRightInObjFrame =
452  virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
453 
454 
455  Eigen::Matrix4f leftSensorFrame =
456  rtGetRobot()->getSensor("FT L")->getRobotNode()->getPoseInRootFrame();
457  Eigen::Matrix4f rightSensorFrame =
458  rtGetRobot()->getSensor("FT R")->getRobotNode()->getPoseInRootFrame();
459  leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
460  rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
461 
462  sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) =
463  leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
464  sensorFrame2TcpFrameRight.block<3, 3>(0, 0) =
465  rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
466  CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
467  CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
468  firstLoop = false;
469  ARMARX_INFO << "modified left pose:\n " << leftPose;
470  ARMARX_INFO << "modified right pose:\n " << rightPose;
471  }
472 
473  // --------------------------------------------- get control parameters ---------------------------------------
474  KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
475  KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
476  KmAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KmAdmittance;
477  KpAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KpAdmittance;
478  KdAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KdAdmittance;
479 
480  if (ftcalibrationTimer < cfg->ftCalibrationTime)
481  {
482  ftOffset.block<3, 1>(0, 0) =
483  0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
484  ftOffset.block<3, 1>(3, 0) =
485  0.5 * ftOffset.block<3, 1>(3, 0) + 0.5 * leftForceTorque->torque;
486  ftOffset.block<3, 1>(6, 0) =
487  0.5 * ftOffset.block<3, 1>(6, 0) + 0.5 * rightForceTorque->force;
488  ftOffset.block<3, 1>(9, 0) =
489  0.5 * ftOffset.block<3, 1>(9, 0) + 0.5 * rightForceTorque->torque;
490 
491  for (int i = 0; i < KmAdmittance.size(); ++i)
492  {
493  KmAdmittance(i) = 0;
494  }
495  }
496  else
497  {
498  for (int i = 0; i < KmAdmittance.size(); ++i)
499  {
500  KmAdmittance(i) = cfg->KmAdmittance.at(i);
501  }
502  }
503 
504  // -------------------------------------------- target wrench ---------------------------------------------
505  Eigen::VectorXf deltaPoseForWrenchControl;
506  deltaPoseForWrenchControl.setZero(12);
507  for (size_t i = 0; i < 12; ++i)
508  {
509  if (KpImpedance(i) == 0)
510  {
511  deltaPoseForWrenchControl(i) = 0;
512  }
513  else
514  {
515  deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
516  if (deltaPoseForWrenchControl(i) > 0.1)
517  {
518  deltaPoseForWrenchControl(i) = 0.1;
519  }
520  else if (deltaPoseForWrenchControl(i) < -0.1)
521  {
522  deltaPoseForWrenchControl(i) = -0.1;
523  }
524  // deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
525  }
526  }
527 
528  // ------------------------------------------- current tcp pose -------------------------------------------
529 
530  currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
531  currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
532 
533  // --------------------------------------------- grasp matrix ---------------------------------------------
534 
535  Eigen::MatrixXf graspMatrix;
536  graspMatrix.setZero(6, 12);
537  graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
538  graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
539  // graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
540  // graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
541 
542  Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
543  graspMatrix.block<3, 3>(3, 0) = skew(rvec);
544 
545  rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
546  graspMatrix.block<3, 3>(3, 6) = skew(rvec);
547 
548  // // projection of grasp matrix
549  // Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
550  // Eigen::MatrixXf G_range = pinvG * graspMatrix;
551  // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
552  float lambda = 1;
553  Eigen::MatrixXf pinvGraspMatrixT =
554  leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
555 
556  // ---------------------------------------------- object pose ----------------------------------------------
557  Eigen::Matrix4f boxCurrentPose = currentRightPose;
558  boxCurrentPose.block<3, 1>(0, 3) =
559  0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
560  Eigen::VectorXf boxCurrentTwist;
561  boxCurrentTwist.setZero(6);
562 
563  // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
564  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
565  // Jacobian matrices
566  Eigen::MatrixXf jacobiL =
567  leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
568  Eigen::MatrixXf jacobiR =
569  rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
570  jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
571  jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
572 
573  // qpos, qvel
574  Eigen::VectorXf leftqpos;
575  Eigen::VectorXf leftqvel;
576  leftqpos.resize(leftPositionSensors.size());
577  leftqvel.resize(leftVelocitySensors.size());
578  for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
579  {
580  leftqpos(i) = leftPositionSensors[i]->position;
581  leftqvel(i) = leftVelocitySensors[i]->velocity;
582  }
583 
584  Eigen::VectorXf rightqpos;
585  Eigen::VectorXf rightqvel;
586  rightqpos.resize(rightPositionSensors.size());
587  rightqvel.resize(rightVelocitySensors.size());
588 
589  for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
590  {
591  rightqpos(i) = rightPositionSensors[i]->position;
592  rightqvel(i) = rightVelocitySensors[i]->velocity;
593  }
594 
595  // -------------------------------------- compute TCP and object velocity -------------------------------------
596  Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
597  Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
598 
599  Eigen::VectorXf currentTwist(12);
600  currentTwist << currentLeftTwist, currentRightTwist;
601  boxCurrentTwist = pinvGraspMatrixT * currentTwist;
602 
603  rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
604  rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
605  rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
606  rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
607  rt2ControlBuffer.commitWrite();
608 
609 
610  // --------------------------------------------- get ft sensor ---------------------------------------------
611  // static compensation
612  Eigen::Vector3f gravity;
613  gravity << 0.0, 0.0, -9.8;
614  Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
615  Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
616  Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
617 
618  Eigen::Vector3f localGravityRight =
619  currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
620  Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
621  Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
622 
623  // mapping of measured wrenches
624  Eigen::VectorXf wrenchesMeasured(12);
625  wrenchesMeasured << leftForceTorque->force - forceOffsetLeft,
626  leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight,
627  rightForceTorque->torque - torqueOffsetRight;
628  for (size_t i = 0; i < 12; i++)
629  {
630  wrenchesMeasured(i) =
631  (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
632  }
633  filteredOldValue = wrenchesMeasured;
634  wrenchesMeasured.block<3, 1>(0, 0) =
635  sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) -
636  localForceVecLeft;
637  wrenchesMeasured.block<3, 1>(3, 0) =
638  sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) -
639  localTorqueVecLeft;
640  wrenchesMeasured.block<3, 1>(6, 0) =
641  sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) -
642  localForceVecRight;
643  wrenchesMeasured.block<3, 1>(9, 0) =
644  sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) -
645  localTorqueVecRight;
646  // if (wrenchesMeasured.norm() < cfg->forceThreshold)
647  // {
648  // wrenchesMeasured.setZero();
649  // }
650 
651  Eigen::VectorXf wrenchesMeasuredInRoot(12);
652  wrenchesMeasuredInRoot.block<3, 1>(0, 0) =
653  currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
654  wrenchesMeasuredInRoot.block<3, 1>(3, 0) =
655  currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
656  wrenchesMeasuredInRoot.block<3, 1>(6, 0) =
657  currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
658  wrenchesMeasuredInRoot.block<3, 1>(9, 0) =
659  currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
660 
661 
662  // map to object
663  Eigen::VectorXf objFTValue = graspMatrix * wrenchesMeasuredInRoot;
664  for (size_t i = 0; i < 6; i++)
665  {
666  if (fabs(objFTValue(i)) < cfg->forceThreshold.at(i))
667  {
668  objFTValue(i) = 0;
669  }
670  else
671  {
672  objFTValue(i) -= cfg->forceThreshold.at(i) * objFTValue(i) / fabs(objFTValue(i));
673  }
674  }
675 
676  // pass sensor value to statechart
677  controlInterfaceBuffer.getWriteBuffer().currentObjPose = boxCurrentPose;
678  controlInterfaceBuffer.getWriteBuffer().currentObjVel = boxCurrentTwist.head<3>();
679  controlInterfaceBuffer.getWriteBuffer().currentObjForce = objFTValue.head<3>();
680  controlInterfaceBuffer.commitWrite();
681 
682 
683  // --------------------------------------------- get MP target ---------------------------------------------
684  Eigen::Matrix4f boxPose = rtGetControlStruct().boxPose;
685  Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
686 
687 
688  // --------------------------------------------- obj admittance control ---------------------------------------------
689  // admittance
690  Eigen::VectorXf objPoseError(6);
691  objPoseError.head<3>() = virtualPose.block<3, 1>(0, 3) - boxPose.block<3, 1>(0, 3);
692  Eigen::Matrix3f objDiffMat =
693  virtualPose.block<3, 3>(0, 0) * boxPose.block<3, 3>(0, 0).transpose();
694  objPoseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
695 
696 
697  Eigen::VectorXf objAcc;
698  Eigen::VectorXf objVel;
699  objAcc.setZero(6);
700  objVel.setZero(6);
701  for (size_t i = 0; i < 6; i++)
702  {
703  // objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * (virtualVel(i) - boxTwist(i));
704  objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) -
705  KdAdmittance(i) * virtualVel(i);
706  }
707  objVel = virtualVel + 0.5 * deltaT * (objAcc + virtualAcc);
708  Eigen::VectorXf deltaObjPose = 0.5 * deltaT * (objVel + virtualVel);
709  virtualAcc = objAcc;
710  virtualVel = objVel;
711  virtualPose.block<3, 1>(0, 3) += deltaObjPose.head<3>();
712  virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(
713  deltaObjPose(3), deltaObjPose(4), deltaObjPose(5)) *
714  virtualPose.block<3, 3>(0, 0);
715 
716  // --------------------------------------------- convert to tcp pose ---------------------------------------------
717  Eigen::Matrix4f tcpTargetPoseLeft = virtualPose;
718  Eigen::Matrix4f tcpTargetPoseRight = virtualPose;
719 
720  tcpTargetPoseRight.block<3, 3>(0, 0) =
721  virtualPose.block<3, 3>(0, 0) * fixedLeftRightRotOffset;
722 
723  tcpTargetPoseLeft.block<3, 1>(0, 3) +=
724  virtualPose.block<3, 3>(0, 0) *
725  (objCom2TCPLeftInObjFrame - deltaPoseForWrenchControl.block<3, 1>(0, 0));
726  tcpTargetPoseRight.block<3, 1>(0, 3) +=
727  virtualPose.block<3, 3>(0, 0) *
728  (objCom2TCPRightInObjFrame - deltaPoseForWrenchControl.block<3, 1>(6, 0));
729 
730  // --------------------------------------------- Impedance control ---------------------------------------------
731  Eigen::VectorXf poseError(12);
732  Eigen::Matrix3f diffMat =
733  tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
734  poseError.block<3, 1>(0, 0) =
735  tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
736  poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
737 
738  diffMat =
739  tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
740  poseError.block<3, 1>(6, 0) =
741  tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
742  poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
743 
744  Eigen::VectorXf forceImpedance(12);
745  for (size_t i = 0; i < 12; i++)
746  {
747  forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
748  // forceImpedance(i + 6) = KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
749  }
750 
751  // --------------------------------------------- Nullspace control ---------------------------------------------
752  Eigen::VectorXf leftNullspaceTorque =
753  cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
754  Eigen::VectorXf rightNullspaceTorque =
755  cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
756 
757  // --------------------------------------------- Set Torque Control Command ---------------------------------------------
758  // float lambda = 1;
759  Eigen::MatrixXf jtpinvL =
760  leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
761  Eigen::MatrixXf jtpinvR =
762  rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
763  Eigen::VectorXf leftJointDesiredTorques =
764  jacobiL.transpose() * forceImpedance.head<6>() +
765  (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
766  Eigen::VectorXf rightJointDesiredTorques =
767  jacobiR.transpose() * forceImpedance.tail<6>() +
768  (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
769 
770  // torque limit
771  for (size_t i = 0; i < leftTargets.size(); ++i)
772  {
773  float desiredTorque = leftJointDesiredTorques(i);
774  if (isnan(desiredTorque))
775  {
776  desiredTorque = 0;
777  }
778  desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
779  desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
780  debugOutputData.getWriteBuffer().desired_torques[leftJointNames[i]] =
781  leftJointDesiredTorques(i);
782  leftTargets.at(i)->torque = desiredTorque;
783  }
784 
785  for (size_t i = 0; i < rightTargets.size(); ++i)
786  {
787  float desiredTorque = rightJointDesiredTorques(i);
788  if (isnan(desiredTorque))
789  {
790  desiredTorque = 0;
791  }
792  desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
793  desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
794  debugOutputData.getWriteBuffer().desired_torques[rightJointNames[i]] =
795  rightJointDesiredTorques(i);
796  rightTargets.at(i)->torque = desiredTorque;
797  }
798 
799  // --------------------------------------------- debug output ---------------------------------------------
800  debugOutputData.getWriteBuffer().forceImpedance = forceImpedance;
801  debugOutputData.getWriteBuffer().poseError = poseError;
802  // debugOutputData.getWriteBuffer().wrenchesConstrained = wrenchesConstrained;
803  debugOutputData.getWriteBuffer().wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
804  // debugOutputData.getWriteBuffer().wrenchDMP = wrenchDMP;
805  // debugOutputData.getWriteBuffer().computedBoxWrench = computedBoxWrench;
806 
807  debugOutputData.getWriteBuffer().virtualPose_x = virtualPose(0, 3);
808  debugOutputData.getWriteBuffer().virtualPose_y = virtualPose(1, 3);
809  debugOutputData.getWriteBuffer().virtualPose_z = virtualPose(2, 3);
810 
811  debugOutputData.getWriteBuffer().objPose_x = boxCurrentPose(0, 3);
812  debugOutputData.getWriteBuffer().objPose_y = boxCurrentPose(1, 3);
813  debugOutputData.getWriteBuffer().objPose_z = boxCurrentPose(2, 3);
814 
815  debugOutputData.getWriteBuffer().objForce_x = objFTValue(0);
816  debugOutputData.getWriteBuffer().objForce_y = objFTValue(1);
817  debugOutputData.getWriteBuffer().objForce_z = objFTValue(2);
818  debugOutputData.getWriteBuffer().objTorque_x = objFTValue(3);
819  debugOutputData.getWriteBuffer().objTorque_y = objFTValue(4);
820  debugOutputData.getWriteBuffer().objTorque_z = objFTValue(5);
821 
822  debugOutputData.getWriteBuffer().objVel_x = objVel(0);
823  debugOutputData.getWriteBuffer().objVel_y = objVel(1);
824  debugOutputData.getWriteBuffer().objVel_z = objVel(2);
825  debugOutputData.getWriteBuffer().objVel_rx = objVel(3);
826  debugOutputData.getWriteBuffer().objVel_ry = objVel(4);
827  debugOutputData.getWriteBuffer().objVel_rz = objVel(5);
828 
829  debugOutputData.getWriteBuffer().deltaPose_x = deltaObjPose(0);
830  debugOutputData.getWriteBuffer().deltaPose_y = deltaObjPose(1);
831  debugOutputData.getWriteBuffer().deltaPose_z = deltaObjPose(2);
832  debugOutputData.getWriteBuffer().deltaPose_rx = deltaObjPose(3);
833  debugOutputData.getWriteBuffer().deltaPose_ry = deltaObjPose(4);
834  debugOutputData.getWriteBuffer().deltaPose_rz = deltaObjPose(5);
835 
836  debugOutputData.getWriteBuffer().modifiedPoseRight_x = tcpTargetPoseRight(0, 3);
837  debugOutputData.getWriteBuffer().modifiedPoseRight_y = tcpTargetPoseRight(1, 3);
838  debugOutputData.getWriteBuffer().modifiedPoseRight_z = tcpTargetPoseRight(2, 3);
839 
840  debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
841  debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
842  debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
843 
844 
845  VirtualRobot::MathTools::Quaternion leftQuat =
846  VirtualRobot::MathTools::eigen4f2quat(currentLeftPose);
847  debugOutputData.getWriteBuffer().leftQuat_w = leftQuat.w;
848  debugOutputData.getWriteBuffer().leftQuat_x = leftQuat.x;
849  debugOutputData.getWriteBuffer().leftQuat_y = leftQuat.y;
850  debugOutputData.getWriteBuffer().leftQuat_z = leftQuat.y;
851 
852  debugOutputData.getWriteBuffer().modifiedPoseLeft_x = tcpTargetPoseLeft(0, 3);
853  debugOutputData.getWriteBuffer().modifiedPoseLeft_y = tcpTargetPoseLeft(1, 3);
854  debugOutputData.getWriteBuffer().modifiedPoseLeft_z = tcpTargetPoseLeft(2, 3);
855 
856  debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
857  debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
858  debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
859 
860  VirtualRobot::MathTools::Quaternion rightQuat =
861  VirtualRobot::MathTools::eigen4f2quat(currentRightPose);
862  debugOutputData.getWriteBuffer().rightQuat_w = rightQuat.w;
863  debugOutputData.getWriteBuffer().rightQuat_x = rightQuat.x;
864  debugOutputData.getWriteBuffer().rightQuat_y = rightQuat.y;
865  debugOutputData.getWriteBuffer().rightQuat_z = rightQuat.y;
866 
867 
868  debugOutputData.getWriteBuffer().dmpBoxPose_x = boxPose(0, 3);
869  debugOutputData.getWriteBuffer().dmpBoxPose_y = boxPose(1, 3);
870  debugOutputData.getWriteBuffer().dmpBoxPose_z = boxPose(2, 3);
871 
872  debugOutputData.getWriteBuffer().dmpTwist_x = boxTwist(0);
873  debugOutputData.getWriteBuffer().dmpTwist_y = boxTwist(1);
874  debugOutputData.getWriteBuffer().dmpTwist_z = boxTwist(2);
875  debugOutputData.getWriteBuffer().rx = rvec(0);
876  debugOutputData.getWriteBuffer().ry = rvec(1);
877  debugOutputData.getWriteBuffer().rz = rvec(2);
878 
879  // debugOutputData.getWriteBuffer().modifiedTwist_lx = twistDMP(0);
880  // debugOutputData.getWriteBuffer().modifiedTwist_ly = twistDMP(1);
881  // debugOutputData.getWriteBuffer().modifiedTwist_lz = twistDMP(2);
882  // debugOutputData.getWriteBuffer().modifiedTwist_rx = twistDMP(6);
883  // debugOutputData.getWriteBuffer().modifiedTwist_ry = twistDMP(7);
884  // debugOutputData.getWriteBuffer().modifiedTwist_rz = twistDMP(8);
885 
886  // debugOutputData.getWriteBuffer().forcePID = forcePIDInRootForDebug;
887 
888  debugOutputData.commitWrite();
889  }
890 
891  void
892  NJointBimanualObjLevelController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
893  const Ice::Current&)
894  {
895  objectDMP->learnDMPFromFiles(fileNames);
896  }
897 
898  void
899  NJointBimanualObjLevelController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
900  {
901  LockGuardType guard(controllerMutex);
902  objectDMP->setGoalPoseVec(goals);
903  }
904 
905  void
906  NJointBimanualObjLevelController::runDMP(const Ice::DoubleSeq& goals,
907  Ice::Double timeDuration,
908  const Ice::Current&)
909  {
910  while (!controlInterfaceBuffer.updateReadBuffer())
911  {
912  usleep(1000);
913  }
914 
915  Eigen::Matrix4f leftPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentLeftPose;
916  Eigen::Matrix4f rightPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentRightPose;
917 
918  VirtualRobot::MathTools::Quaternion boxOri =
919  VirtualRobot::MathTools::eigen4f2quat(leftPose);
920  Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
921 
922  ARMARX_IMPORTANT << "runDMP: boxPosi: " << boxPosi;
923 
924  std::vector<double> boxInitialPose;
925  for (size_t i = 0; i < 3; ++i)
926  {
927  boxInitialPose.push_back(boxPosi(i) * 0.001); //Important: mm -> m
928  }
929  boxInitialPose.push_back(boxOri.w);
930  boxInitialPose.push_back(boxOri.x);
931  boxInitialPose.push_back(boxOri.y);
932  boxInitialPose.push_back(boxOri.z);
933 
934  objectDMP->prepareExecution(boxInitialPose, goals);
935  objectDMP->canVal = timeDuration;
936  objectDMP->config.motionTimeDuration = timeDuration;
937 
938 
939  finished = false;
940  dmpStarted = true;
941  }
942 
943  void
944  NJointBimanualObjLevelController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts,
945  const Ice::DoubleSeq& goals,
946  Ice::Double timeDuration,
947  const Ice::Current&)
948  {
949  while (!controlInterfaceBuffer.updateReadBuffer())
950  {
951  usleep(1000);
952  }
953  ARMARX_IMPORTANT << "obj level control: setup dmp ...";
954  objectDMP->prepareExecution(starts, goals);
955  objectDMP->canVal = timeDuration;
956  objectDMP->config.motionTimeDuration = timeDuration;
957 
958  finished = false;
959  dmpStarted = true;
960 
961  ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
962  }
963 
964  void
965  NJointBimanualObjLevelController::setViaPoints(Ice::Double u,
966  const Ice::DoubleSeq& viapoint,
967  const Ice::Current&)
968  {
969  // LockGuardType guard(controllerMutex);
970  ARMARX_INFO << "setting via points ";
971  objectDMP->setViaPose(u, viapoint);
972  }
973 
974  void
975  NJointBimanualObjLevelController::removeAllViaPoints(const Ice::Current&)
976  {
977  objectDMP->removeAllViaPoints();
978  }
979 
980  void
981  NJointBimanualObjLevelController::setKpImpedance(const Ice::FloatSeq& value,
982  const Ice::Current&)
983  {
984 
985  Eigen::VectorXf setpoint;
986  setpoint.setZero(value.size());
987  ARMARX_CHECK_EQUAL(value.size(), 12);
988 
989  for (size_t i = 0; i < value.size(); ++i)
990  {
991  setpoint(i) = value.at(i);
992  }
993 
994  LockGuardType guard{interfaceDataMutex};
995  interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
996  interface2rtBuffer.commitWrite();
997  }
998 
999  void
1000  NJointBimanualObjLevelController::setKdImpedance(const Ice::FloatSeq& value,
1001  const Ice::Current&)
1002  {
1003  Eigen::VectorXf setpoint;
1004  setpoint.setZero(value.size());
1005  ARMARX_CHECK_EQUAL(value.size(), 12);
1006 
1007  for (size_t i = 0; i < value.size(); ++i)
1008  {
1009  setpoint(i) = value.at(i);
1010  }
1011 
1012  LockGuardType guard{interfaceDataMutex};
1013  interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
1014  interface2rtBuffer.commitWrite();
1015  }
1016 
1017  void
1018  NJointBimanualObjLevelController::setKpAdmittance(const Ice::FloatSeq& value,
1019  const Ice::Current&)
1020  {
1021  Eigen::VectorXf setpoint;
1022  setpoint.setZero(value.size());
1023  ARMARX_CHECK_EQUAL(value.size(), 6);
1024 
1025  for (size_t i = 0; i < value.size(); ++i)
1026  {
1027  setpoint(i) = value.at(i);
1028  }
1029 
1030  LockGuardType guard{interfaceDataMutex};
1031  interface2rtBuffer.getWriteBuffer().KpAdmittance = setpoint;
1032  interface2rtBuffer.commitWrite();
1033  }
1034 
1035  void
1036  NJointBimanualObjLevelController::setKdAdmittance(const Ice::FloatSeq& value,
1037  const Ice::Current&)
1038  {
1039  Eigen::VectorXf setpoint;
1040  setpoint.setZero(value.size());
1041  ARMARX_CHECK_EQUAL(value.size(), 6);
1042 
1043  for (size_t i = 0; i < value.size(); ++i)
1044  {
1045  setpoint(i) = value.at(i);
1046  }
1047 
1048  LockGuardType guard{interfaceDataMutex};
1049  interface2rtBuffer.getWriteBuffer().KdAdmittance = setpoint;
1050  interface2rtBuffer.commitWrite();
1051  }
1052 
1053  std::vector<float>
1054  NJointBimanualObjLevelController::getCurrentObjVel(const Ice::Current&)
1055  {
1056  Eigen::Vector3f tvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
1057  std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
1058  return tvelvec;
1059  }
1060 
1061  std::vector<float>
1062  NJointBimanualObjLevelController::getCurrentObjForce(const Ice::Current&)
1063  {
1064  Eigen::Vector3f fvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjForce;
1065  std::vector<float> fvelvec = {fvel(0), fvel(1), fvel(2)};
1066  return fvelvec;
1067  }
1068 
1069  void
1070  NJointBimanualObjLevelController::setKmAdmittance(const Ice::FloatSeq& value,
1071  const Ice::Current&)
1072  {
1073  Eigen::VectorXf setpoint;
1074  setpoint.setZero(value.size());
1075  ARMARX_CHECK_EQUAL(value.size(), 6);
1076 
1077  for (size_t i = 0; i < value.size(); ++i)
1078  {
1079  setpoint(i) = value.at(i);
1080  }
1081 
1082  LockGuardType guard{interfaceDataMutex};
1083  interface2rtBuffer.getWriteBuffer().KmAdmittance = setpoint;
1084  interface2rtBuffer.commitWrite();
1085  }
1086 
1087  void
1088  NJointBimanualObjLevelController::onPublish(const SensorAndControl&,
1089  const DebugDrawerInterfacePrx&,
1090  const DebugObserverInterfacePrx& debugObs)
1091  {
1092 
1093  StringVariantBaseMap datafields;
1094  auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
1095  for (auto& pair : values)
1096  {
1097  datafields[pair.first] = new Variant(pair.second);
1098  }
1099 
1100  Eigen::VectorXf forceImpedance = debugOutputData.getUpToDateReadBuffer().forceImpedance;
1101  for (int i = 0; i < forceImpedance.rows(); ++i)
1102  {
1103  std::stringstream ss;
1104  ss << i;
1105  std::string data_name = "forceImpedance_" + ss.str();
1106  datafields[data_name] = new Variant(forceImpedance(i));
1107  }
1108 
1109  Eigen::VectorXf forcePID = debugOutputData.getUpToDateReadBuffer().forcePID;
1110  for (int i = 0; i < forcePID.rows(); ++i)
1111  {
1112  std::stringstream ss;
1113  ss << i;
1114  std::string data_name = "forcePID_" + ss.str();
1115  datafields[data_name] = new Variant(forcePID(i));
1116  }
1117 
1118 
1119  Eigen::VectorXf poseError = debugOutputData.getUpToDateReadBuffer().poseError;
1120  for (int i = 0; i < poseError.rows(); ++i)
1121  {
1122  std::stringstream ss;
1123  ss << i;
1124  std::string data_name = "poseError_" + ss.str();
1125  datafields[data_name] = new Variant(poseError(i));
1126  }
1127 
1128  Eigen::VectorXf wrenchesConstrained =
1129  debugOutputData.getUpToDateReadBuffer().wrenchesConstrained;
1130  for (int i = 0; i < wrenchesConstrained.rows(); ++i)
1131  {
1132  std::stringstream ss;
1133  ss << i;
1134  std::string data_name = "wrenchesConstrained_" + ss.str();
1135  datafields[data_name] = new Variant(wrenchesConstrained(i));
1136  }
1137 
1138  Eigen::VectorXf wrenchesMeasuredInRoot =
1139  debugOutputData.getUpToDateReadBuffer().wrenchesMeasuredInRoot;
1140  for (int i = 0; i < wrenchesMeasuredInRoot.rows(); ++i)
1141  {
1142  std::stringstream ss;
1143  ss << i;
1144  std::string data_name = "wrenchesMeasuredInRoot_" + ss.str();
1145  datafields[data_name] = new Variant(wrenchesMeasuredInRoot(i));
1146  }
1147 
1148 
1149  // Eigen::VectorXf wrenchDMP = debugOutputData.getUpToDateReadBuffer().wrenchDMP;
1150  // for (size_t i = 0; i < wrenchDMP.rows(); ++i)
1151  // {
1152  // std::stringstream ss;
1153  // ss << i;
1154  // std::string data_name = "wrenchDMP_" + ss.str();
1155  // datafields[data_name] = new Variant(wrenchDMP(i));
1156  // }
1157 
1158  // Eigen::VectorXf computedBoxWrench = debugOutputData.getUpToDateReadBuffer().computedBoxWrench;
1159  // for (size_t i = 0; i < computedBoxWrench.rows(); ++i)
1160  // {
1161  // std::stringstream ss;
1162  // ss << i;
1163  // std::string data_name = "computedBoxWrench_" + ss.str();
1164  // datafields[data_name] = new Variant(computedBoxWrench(i));
1165  // }
1166 
1167 
1168  datafields["virtualPose_x"] =
1169  new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_x);
1170  datafields["virtualPose_y"] =
1171  new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_y);
1172  datafields["virtualPose_z"] =
1173  new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_z);
1174 
1175  datafields["objPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_x);
1176  datafields["objPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_y);
1177  datafields["objPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_z);
1178 
1179  datafields["objForce_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_x);
1180  datafields["objForce_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_y);
1181  datafields["objForce_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_z);
1182  datafields["objTorque_x"] =
1183  new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_x);
1184  datafields["objTorque_y"] =
1185  new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_y);
1186  datafields["objTorque_z"] =
1187  new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_z);
1188 
1189  datafields["objVel_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_x);
1190  datafields["objVel_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_y);
1191  datafields["objVel_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_z);
1192  datafields["objVel_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_rx);
1193  datafields["objVel_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_ry);
1194  datafields["objVel_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_rz);
1195 
1196  datafields["deltaPose_x"] =
1197  new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_x);
1198  datafields["deltaPose_y"] =
1199  new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_y);
1200  datafields["deltaPose_z"] =
1201  new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_z);
1202  datafields["deltaPose_rx"] =
1203  new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_rx);
1204  datafields["deltaPose_ry"] =
1205  new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_ry);
1206  datafields["deltaPose_rz"] =
1207  new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_rz);
1208 
1209  datafields["modifiedPoseRight_x"] =
1210  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_x);
1211  datafields["modifiedPoseRight_y"] =
1212  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_y);
1213  datafields["modifiedPoseRight_z"] =
1214  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_z);
1215  datafields["currentPoseLeft_x"] =
1216  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
1217  datafields["currentPoseLeft_y"] =
1218  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
1219  datafields["currentPoseLeft_z"] =
1220  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
1221 
1222  datafields["leftQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_w);
1223  datafields["leftQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_x);
1224  datafields["leftQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_y);
1225  datafields["leftQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_z);
1226  datafields["rightQuat_w"] =
1227  new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_w);
1228  datafields["rightQuat_x"] =
1229  new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_x);
1230  datafields["rightQuat_y"] =
1231  new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_y);
1232  datafields["rightQuat_z"] =
1233  new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_z);
1234 
1235  datafields["modifiedPoseLeft_x"] =
1236  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_x);
1237  datafields["modifiedPoseLeft_y"] =
1238  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_y);
1239  datafields["modifiedPoseLeft_z"] =
1240  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_z);
1241 
1242  datafields["currentPoseRight_x"] =
1243  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
1244  datafields["currentPoseRight_y"] =
1245  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
1246  datafields["currentPoseRight_z"] =
1247  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
1248  datafields["dmpBoxPose_x"] =
1249  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
1250  datafields["dmpBoxPose_y"] =
1251  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
1252  datafields["dmpBoxPose_z"] =
1253  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
1254  datafields["dmpTwist_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_x);
1255  datafields["dmpTwist_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_y);
1256  datafields["dmpTwist_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_z);
1257 
1258  datafields["modifiedTwist_lx"] =
1259  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lx);
1260  datafields["modifiedTwist_ly"] =
1261  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ly);
1262  datafields["modifiedTwist_lz"] =
1263  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lz);
1264  datafields["modifiedTwist_rx"] =
1265  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rx);
1266  datafields["modifiedTwist_ry"] =
1267  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ry);
1268  datafields["modifiedTwist_rz"] =
1269  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rz);
1270 
1271  datafields["rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().rx);
1272  datafields["ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().ry);
1273  datafields["rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().rz);
1274 
1275 
1276  debugObs->setDebugChannel("BimanualForceController", datafields);
1277  }
1278 
1279  void
1280  NJointBimanualObjLevelController::onInitNJointController()
1281  {
1282 
1283 
1284  ARMARX_INFO << "init ...";
1285  runTask("NJointBimanualObjLevelController",
1286  [&]
1287  {
1288  CycleUtil c(1);
1289  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
1290  while (getState() == eManagedIceObjectStarted)
1291  {
1292  if (isControllerActive())
1293  {
1294  controllerRun();
1295  }
1296  c.waitForCycleDuration();
1297  }
1298  });
1299  }
1300 
1301  void
1302  NJointBimanualObjLevelController::onDisconnectNJointController()
1303  {
1304  ARMARX_INFO << "stopped ...";
1305  }
1306 } // namespace armarx::control::deprecated_njoint_mp_controller::bimanual
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelControlData >::reinitTripleBuffer
void reinitTripleBuffer(const NJointBimanualObjLevelControlData &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::tsvmp::PhaseStopParams::Kori
float Kori
Definition: TaskSpaceVMP.h:54
MathUtils.h
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::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::setKpImpedance
void setKpImpedance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:981
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelControlData >::rtGetControlStruct
const NJointBimanualObjLevelControlData & 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
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::setKdAdmittance
void setKdAdmittance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:1036
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::setKmAdmittance
void setKmAdmittance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:1070
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::onDisconnectNJointController
void onDisconnectNJointController()
Definition: NJointBimanualObjLevelController.cpp:1302
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::NJointBimanualObjLevelController::setKdImpedance
void setKdImpedance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:1000
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::skew
Eigen::Matrix3f skew(Eigen::Vector3f vec)
Definition: NJointBimanualObjLevelController.h:64
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelControlData::boxPose
Eigen::Matrix4f boxPose
Definition: NJointBimanualObjLevelController.h:32
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::NJointBimanualObjLevelController
NJointBimanualObjLevelController(const RobotUnitPtr &, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointBimanualObjLevelController.cpp:28
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::NJointBimanualObjLevelController::onInitNJointController
void onInitNJointController()
Definition: NJointBimanualObjLevelController.cpp:1280
SensorValueForceTorque.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelControlData::boxTwist
Eigen::VectorXf boxTwist
Definition: NJointBimanualObjLevelController.h:33
armarx::NJointBimanualObjLevelControllerInterface::getMPWeights
DoubleSeqSeq getMPWeights()
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig
Definition: TaskSpaceVMP.h:59
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::phaseStopParas
PhaseStopParams phaseStopParas
Definition: TaskSpaceVMP.h:67
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition: NJointBimanualObjLevelController.cpp:416
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelControlData >::getWriterControlStruct
NJointBimanualObjLevelControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition: TaskSpaceVMP.h:48
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelControlData
Definition: NJointBimanualObjLevelController.h:28
armarx::control::deprecated_njoint_mp_controller::bimanual::registrationControllerNJointBimanualObjLevelController
NJointControllerRegistration< NJointBimanualObjLevelController > registrationControllerNJointBimanualObjLevelController("NJointBimanualObjLevelController")
armarx::SensorValueForceTorque::force
Eigen::Vector3f force
Definition: SensorValueForceTorque.h:36
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
NJointBimanualObjLevelController.h
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::NJointBimanualObjLevelControllerInterface::getCurrentObjVel
Ice::FloatSeq getCurrentObjVel()
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::runDMPWithVirtualStart
void runDMPWithVirtualStart(const Ice::DoubleSeq &starts, const Ice::DoubleSeq &goals, Ice::Double timeDuration, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:944
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::learnDMPFromFiles
void learnDMPFromFiles(const Ice::StringSeq &fileNames, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:892
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:920
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:965
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dori
float Dori
Definition: TaskSpaceVMP.h:55
armarx::NJointBimanualObjLevelControllerInterface::getMPRotWeights
DoubleSeqSeq getMPRotWeights()
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:855
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition: NJointControllerBase.h:778
armarx::PIDController
Definition: PIDController.h:43
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::NJointBimanualObjLevelController::controllerRun
void controllerRun()
Definition: NJointBimanualObjLevelController.cpp:389
armarx::NJointBimanualObjLevelControllerInterface::getCurrentObjForce
Ice::FloatSeq getCurrentObjForce()
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition: NJointBimanualObjLevelController.cpp:1088
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:108
armarx::SensorValueForceTorque::torque
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION Eigen::Vector3f torque
Definition: SensorValueForceTorque.h:35
ArmarXObjectScheduler.h
ControlTarget1DoFActuator.h
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition: TaskSpaceVMP.h:64
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::slop
float slop
Definition: TaskSpaceVMP.h:51
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::getClassName
std::string getClassName(const Ice::Current &) const
Definition: NJointBimanualObjLevelController.cpp:383
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition: TaskSpaceVMP.h:52
CycleUtil.h
PIDController.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::setKpAdmittance
void setKpAdmittance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:1018
NJointController.h
armarx::NJointBimanualObjLevelControllerInterface::removeAllViaPoints
void removeAllViaPoints()
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::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition: TaskSpaceVMP.h:62
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition: TaskSpaceVMP.h:56
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::firstLoop
bool firstLoop
Definition: NJointBimanualObjLevelController.h:322
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::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::NJointBimanualObjLevelController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:899
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::runDMP
void runDMP(const Ice::DoubleSeq &goals, Ice::Double timeDuration, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:906
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::bimanual::NJointBimanualObjLevelController::setMPRotWeights
void setMPRotWeights(const DoubleSeqSeq &weights, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:340
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition: NJointBimanualObjLevelController.cpp:365
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition: TaskSpaceVMP.h:90
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelController::setMPWeights
void setMPWeights(const DoubleSeqSeq &weights, const Ice::Current &)
Definition: NJointBimanualObjLevelController.cpp:316
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