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