NJointBimanualForceController.cpp
Go to the documentation of this file.
1 #include "NJointBimanualForceController.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 // RobotAPI
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<NJointBimanualForceController>
26  registrationControllerNJointBimanualForceController("NJointBimanualForceController");
27 
28  NJointBimanualForceController::NJointBimanualForceController(
29  const RobotUnitPtr& robUnit,
30  const armarx::NJointControllerConfigPtr& config,
31  const VirtualRobot::RobotPtr&)
32  {
33  ARMARX_INFO << "Preparing ... bimanual ";
34  useSynchronizedRtRobot();
35  cfg = NJointBimanualForceControllerConfigPtr::dynamicCast(config);
36  // ARMARX_CHECK_EXPRESSION(prov);
37  // RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
38  // ARMARX_CHECK_EXPRESSION(robotUnit);
39  leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
40 
41  for (size_t i = 0; i < leftRNS->getSize(); ++i)
42  {
43  std::string jointName = leftRNS->getNode(i)->getName();
44  leftJointNames.push_back(jointName);
45  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
46  const SensorValueBase* sv = useSensorValue(jointName);
47  leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
48  const SensorValue1DoFActuatorVelocity* velocitySensor =
49  sv->asA<SensorValue1DoFActuatorVelocity>();
50  const SensorValue1DoFActuatorPosition* positionSensor =
51  sv->asA<SensorValue1DoFActuatorPosition>();
52 
53  if (!velocitySensor)
54  {
55  ARMARX_WARNING << "No velocitySensor available for " << jointName;
56  }
57  if (!positionSensor)
58  {
59  ARMARX_WARNING << "No positionSensor available for " << jointName;
60  }
61 
62 
63  leftVelocitySensors.push_back(velocitySensor);
64  leftPositionSensors.push_back(positionSensor);
65  };
66 
67  rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
68 
69  for (size_t i = 0; i < rightRNS->getSize(); ++i)
70  {
71  std::string jointName = rightRNS->getNode(i)->getName();
72  rightJointNames.push_back(jointName);
73  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
74  const SensorValueBase* sv = useSensorValue(jointName);
75  rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
76 
77  const SensorValue1DoFActuatorVelocity* velocitySensor =
78  sv->asA<SensorValue1DoFActuatorVelocity>();
79  const SensorValue1DoFActuatorPosition* positionSensor =
80  sv->asA<SensorValue1DoFActuatorPosition>();
81 
82  if (!velocitySensor)
83  {
84  ARMARX_WARNING << "No velocitySensor available for " << jointName;
85  }
86  if (!positionSensor)
87  {
88  ARMARX_WARNING << "No positionSensor available for " << jointName;
89  }
90 
91  rightVelocitySensors.push_back(velocitySensor);
92  rightPositionSensors.push_back(positionSensor);
93  };
94 
95 
96  // const SensorValueBase* svlf = prov->getSensorValue("FT L");
97  const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
98  leftForceTorque = svlf->asA<SensorValueForceTorque>();
99  // const SensorValueBase* svrf = prov->getSensorValue("FT R");
100  const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
101  rightForceTorque = svrf->asA<SensorValueForceTorque>();
102 
103  leftIK.reset(new VirtualRobot::DifferentialIK(
104  leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
105  rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS,
106  rightRNS->getRobot()->getRootNode(),
107  VirtualRobot::JacobiProvider::eSVDDamped));
108 
109 
110  tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
111  taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
112  taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
113  taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
114  taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
115  taskSpaceDMPConfig.DMPAmplitude = 1.0;
116  taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
117  taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
118  taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
119  taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
120  taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
121  taskSpaceDMPConfig.phaseStopParas.Dori = 0;
122  taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
123  taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
124  taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
125 
126 
127  objectDMP.reset(new tsvmp::TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
128  ARMARX_IMPORTANT << "dmp finieshed";
129 
130  tcpLeft = leftRNS->getTCP();
131  tcpRight = rightRNS->getTCP();
132 
133  KpImpedance.resize(cfg->KpImpedance.size());
134  ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
135 
136  for (int i = 0; i < KpImpedance.size(); ++i)
137  {
138  KpImpedance(i) = cfg->KpImpedance.at(i);
139  }
140 
141  KdImpedance.resize(cfg->KdImpedance.size());
142  ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 6);
143 
144  for (int i = 0; i < KdImpedance.size(); ++i)
145  {
146  KdImpedance(i) = cfg->KdImpedance.at(i);
147  }
148 
149  KpAdmittance.resize(cfg->KpAdmittance.size());
150  ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
151 
152  for (int i = 0; i < KpAdmittance.size(); ++i)
153  {
154  KpAdmittance(i) = cfg->KpAdmittance.at(i);
155  }
156 
157  KdAdmittance.resize(cfg->KdAdmittance.size());
158  ARMARX_CHECK_EQUAL(cfg->KdAdmittance.size(), 6);
159 
160  for (int i = 0; i < KdAdmittance.size(); ++i)
161  {
162  KdAdmittance(i) = cfg->KdAdmittance.at(i);
163  }
164 
165  KmAdmittance.resize(cfg->KmAdmittance.size());
166  ARMARX_CHECK_EQUAL(cfg->KmAdmittance.size(), 6);
167 
168  for (int i = 0; i < KmAdmittance.size(); ++i)
169  {
170  KmAdmittance(i) = cfg->KmAdmittance.at(i);
171  }
172 
173  leftDesiredJointValues.resize(leftTargets.size());
174  ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
175 
176  for (size_t i = 0; i < leftTargets.size(); ++i)
177  {
178  leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
179  }
180 
181  rightDesiredJointValues.resize(rightTargets.size());
182  ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
183 
184  for (size_t i = 0; i < rightTargets.size(); ++i)
185  {
186  rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
187  }
188 
189  KmPID.resize(cfg->KmPID.size());
190  ARMARX_CHECK_EQUAL(cfg->KmPID.size(), 6);
191 
192  for (int i = 0; i < KmPID.size(); ++i)
193  {
194  KmPID(i) = cfg->KmPID.at(i);
195  }
196 
197 
198  modifiedAcc.setZero(12);
199  modifiedTwist.setZero(12);
200  ARMARX_INFO << "got controller params";
201 
202 
203  boxInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->boxInitialPose[4],
204  cfg->boxInitialPose[5],
205  cfg->boxInitialPose[6],
206  cfg->boxInitialPose[3]);
207  for (int i = 0; i < 3; ++i)
208  {
209  boxInitialPose(i, 3) = cfg->boxInitialPose[i];
210  }
211 
212  NJointBimanualForceControllerSensorData initSensorData;
213  initSensorData.deltaT = 0;
214  initSensorData.currentTime = 0;
215  initSensorData.currentPose = boxInitialPose;
216  initSensorData.currentTwist.setZero();
217  controllerSensorData.reinitAllBuffers(initSensorData);
218 
219 
220  NJointBimanualForceControllerInterfaceData initInterfaceData;
221  initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
222  initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
223  interfaceData.reinitAllBuffers(initInterfaceData);
224 
225  leftInitialPose = boxInitialPose;
226  leftInitialPose(0, 3) -= cfg->boxWidth;
227  rightInitialPose = boxInitialPose;
228  rightInitialPose(0, 3) += cfg->boxWidth;
229 
230  forcePIDControllers.resize(12);
231  for (size_t i = 0; i < 6; i++)
232  {
233  forcePIDControllers.at(i).reset(new PIDController(
234  cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
235  forcePIDControllers.at(i + 6).reset(new PIDController(
236  cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
237  forcePIDControllers.at(i)->reset();
238  forcePIDControllers.at(i + 6)->reset();
239  }
240 
241  // filter
242  filterCoeff = cfg->filterCoeff;
243  ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
244  filteredOldValue.setZero(12);
245 
246  // static compensation
247  massLeft = cfg->massLeft;
248  CoMVecLeft << cfg->CoMVecLeft[0], cfg->CoMVecLeft[1], cfg->CoMVecLeft[2];
249  forceOffsetLeft << cfg->forceOffsetLeft[0], cfg->forceOffsetLeft[1],
250  cfg->forceOffsetLeft[2];
251  torqueOffsetLeft << cfg->torqueOffsetLeft[0], cfg->torqueOffsetLeft[1],
252  cfg->torqueOffsetLeft[2];
253 
254  massRight = cfg->massRight;
255  CoMVecRight << cfg->CoMVecRight[0], cfg->CoMVecRight[1], cfg->CoMVecRight[2];
256  forceOffsetRight << cfg->forceOffsetRight[0], cfg->forceOffsetRight[1],
257  cfg->forceOffsetRight[2];
258  torqueOffsetRight << cfg->torqueOffsetRight[0], cfg->torqueOffsetRight[1],
259  cfg->torqueOffsetRight[2];
260 
261  sensorFrame2TcpFrameLeft.setZero();
262  sensorFrame2TcpFrameRight.setZero();
263 
264  NJointBimanualForceControlData initData;
265  initData.boxPose = boxInitialPose;
266  initData.boxTwist.setZero(6);
267  reinitTripleBuffer(initData);
268 
269  firstLoop = true;
270  ARMARX_INFO << "left initial pose: \n"
271  << leftInitialPose << "\n right initial pose: \n"
272  << rightInitialPose;
273 
274  ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
275  ARMARX_IMPORTANT << "finished construction!";
276 
277  dmpStarted = false;
278 
279  targetWrench.setZero(cfg->targetWrench.size());
280  for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
281  {
282  targetWrench(i) = cfg->targetWrench[i];
283  }
284  }
285 
286  std::string
287  NJointBimanualForceController::getClassName(const Ice::Current&) const
288  {
289  return "NJointBimanualForceController";
290  }
291 
292  // void NJointBimanualForceController::rtPreActivateController()
293  // {
294  // // modifiedLeftPose = tcpLeft->getPoseInRootFrame();
295  // // modifiedRightPose = tcpRight->getPoseInRootFrame();
296  // // Eigen::Matrix4f leftSensorFrame = leftRNS->getRobot()->getRobotNode("ArmL_FT")->getPoseInRootFrame();
297  // // Eigen::Matrix4f rightSensorFrame = rightRNS->getRobot()->getRobotNode("ArmR_FT")->getPoseInRootFrame();
298  // // sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) = modifiedLeftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
299  // // sensorFrame2TcpFrameRight.block<3, 3>(0, 0) = modifiedRightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
300  // // CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
301  // // CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
302  // }
303 
304 
305  void
306  NJointBimanualForceController::controllerRun()
307  {
308  if (!controllerSensorData.updateReadBuffer() || !dmpStarted)
309  {
310  return;
311  }
312 
313  double deltaT = controllerSensorData.getReadBuffer().deltaT;
314  Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
315  Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
316 
317  if (objectDMP->canVal < 1e-8)
318  {
319  finished = true;
320  }
321 
322  objectDMP->flow(deltaT, currentPose, currentTwist);
323 
324  LockGuardType guard{controlDataMutex};
325  getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
326  getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
327  writeControlStruct();
328  }
329 
330  void
331  NJointBimanualForceController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
332  const IceUtil::Time& timeSinceLastIteration)
333  {
334  if (firstLoop)
335  {
336  modifiedLeftPose = tcpLeft->getPoseInRootFrame();
337  modifiedRightPose = tcpRight->getPoseInRootFrame();
338  modifiedLeftPose.block<3, 1>(0, 3) = modifiedLeftPose.block<3, 1>(0, 3) * 0.001;
339  modifiedRightPose.block<3, 1>(0, 3) = modifiedRightPose.block<3, 1>(0, 3) * 0.001;
340 
341  Eigen::Matrix4f leftSensorFrame =
342  rtGetRobot()->getSensor("FT L")->getRobotNode()->getPoseInRootFrame();
343  Eigen::Matrix4f rightSensorFrame =
344  rtGetRobot()->getSensor("FT R")->getRobotNode()->getPoseInRootFrame();
345  leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
346  rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
347 
348  sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) =
349  modifiedLeftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
350  sensorFrame2TcpFrameRight.block<3, 3>(0, 0) =
351  modifiedRightPose.block<3, 3>(0, 0).transpose() *
352  rightSensorFrame.block<3, 3>(0, 0);
353  CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
354  CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
355  firstLoop = false;
356  ARMARX_INFO << "modified left pose:\n " << modifiedLeftPose;
357  ARMARX_INFO << "modified right pose:\n " << modifiedRightPose;
358  }
359  double deltaT = timeSinceLastIteration.toSecondsDouble();
360 
361 
362  // grasp matrix
363  Eigen::Vector3f rToBoxCoM;
364  rToBoxCoM << cfg->boxWidth, 0, 0;
365  Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
366  Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
367 
368  interfaceData.getWriteBuffer().currentLeftPose = currentLeftPose;
369  interfaceData.getWriteBuffer().currentRightPose = currentRightPose;
370  interfaceData.commitWrite();
371 
372  Eigen::VectorXf currentTargetWrench = targetWrench;
373  if (fabs(currentLeftPose(0, 3) - currentRightPose(0, 3)) < 0.8 * 2 * cfg->boxWidth)
374  {
375  currentTargetWrench.setZero();
376  }
377  currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
378  currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
379  Eigen::MatrixXf graspMatrix;
380  graspMatrix.setZero(6, 12);
381  graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
382  graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
383  Eigen::Vector3f r = -currentLeftPose.block<3, 3>(0, 0) * rToBoxCoM;
384  graspMatrix(4, 2) = -r(0);
385  graspMatrix(3, 2) = r(1);
386  graspMatrix(3, 1) = -r(2);
387  graspMatrix(4, 0) = r(2);
388  graspMatrix(5, 0) = -r(1);
389  graspMatrix(5, 1) = r(0);
390  r = currentRightPose.block<3, 3>(0, 0) * rToBoxCoM;
391  graspMatrix(4, 8) = -r(0);
392  graspMatrix(3, 8) = r(1);
393  graspMatrix(3, 7) = -r(2);
394  graspMatrix(4, 6) = r(2);
395  graspMatrix(5, 6) = -r(1);
396  graspMatrix(5, 7) = r(0);
397  // projection of grasp matrix
398  Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
399  Eigen::MatrixXf G_range = pinvG * graspMatrix;
400  Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
401 
402  // box pose
403  Eigen::Matrix4f boxCurrentPose = currentLeftPose;
404  boxCurrentPose.block<3, 1>(0, 3) =
405  0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
406  Eigen::VectorXf boxCurrentTwist;
407  boxCurrentTwist.setZero(6);
408 
409  // cartesian vel controller
410  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
411 
412  Eigen::MatrixXf jacobiL =
413  leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
414 
415  Eigen::VectorXf leftqpos;
416  Eigen::VectorXf leftqvel;
417  leftqpos.resize(leftPositionSensors.size());
418  leftqvel.resize(leftVelocitySensors.size());
419  for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
420  {
421  leftqpos(i) = leftPositionSensors[i]->position;
422  leftqvel(i) = leftVelocitySensors[i]->velocity;
423  }
424 
425  Eigen::MatrixXf jacobiR =
426  rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
427 
428  // jacobiL used in L304
429  jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
430  jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
431 
432  Eigen::VectorXf rightqpos;
433  Eigen::VectorXf rightqvel;
434  rightqpos.resize(rightPositionSensors.size());
435  rightqvel.resize(rightVelocitySensors.size());
436 
437  for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
438  {
439  rightqpos(i) = rightPositionSensors[i]->position;
440  rightqvel(i) = rightVelocitySensors[i]->velocity;
441  }
442 
443  // what is the unit of jacobiL, 0.001?
444  Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
445  Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
446  Eigen::VectorXf currentTwist(12);
447  currentTwist << currentLeftTwist, currentRightTwist;
448  Eigen::MatrixXf pinvGraspMatrixT =
449  leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), 0);
450  boxCurrentTwist = pinvGraspMatrixT * currentTwist;
451 
452 
453  controllerSensorData.getWriteBuffer().currentPose = boxCurrentPose;
454  controllerSensorData.getWriteBuffer().currentTwist = boxCurrentTwist;
455  controllerSensorData.getWriteBuffer().deltaT = deltaT;
456  controllerSensorData.getWriteBuffer().currentTime += deltaT;
457  controllerSensorData.commitWrite();
458 
459 
460  Eigen::Matrix4f boxPose = rtGetControlStruct().boxPose;
461  Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
462 
463  Eigen::VectorXf leftJointControlWrench;
464  Eigen::VectorXf rightJointControlWrench;
465 
466 
467  //Todo: calculate desired wrench from required box pose
468  // Eigen::VectorXf boxPoseError(6);
469  // Eigen::Matrix3f diffMat = boxPose.block<3, 3>(0, 0) * boxCurrentPose.block<3, 3>(0, 0).transpose();
470  // boxPoseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
471  // boxPoseError.head<3>() = boxPose.block<3, 1>(0, 3) - boxCurrentPose.block<3, 1>(0, 3);
472  // Eigen::VectorXf computedBoxWrench(6);
473  // computedBoxWrench = KpAdmittance.cwiseProduct(boxPoseError);// + KdAdmittance.cwiseProduct(boxTwist - boxCurrentTwist);
474  // Eigen::VectorXf wrenchDMP = graspMatrix.transpose() * computedBoxWrench;
475  // wrenchDMP.setZero();
476  Eigen::VectorXf twistDMP = graspMatrix.transpose() * boxTwist;
477  Eigen::VectorXf deltaInitialPose = deltaT * twistDMP;
478  leftInitialPose.block<3, 1>(0, 3) =
479  leftInitialPose.block<3, 1>(0, 3) + deltaInitialPose.block<3, 1>(0, 0);
480  rightInitialPose.block<3, 1>(0, 3) =
481  rightInitialPose.block<3, 1>(0, 3) + deltaInitialPose.block<3, 1>(6, 0);
482  leftInitialPose.block<3, 3>(0, 0) =
483  VirtualRobot::MathTools::rpy2eigen3f(
484  deltaInitialPose(3), deltaInitialPose(4), deltaInitialPose(5)) *
485  leftInitialPose.block<3, 3>(0, 0);
486  rightInitialPose.block<3, 3>(0, 0) =
487  VirtualRobot::MathTools::rpy2eigen3f(
488  deltaInitialPose(9), deltaInitialPose(10), deltaInitialPose(11)) *
489  rightInitialPose.block<3, 3>(0, 0);
490 
491 
492  // static compensation
493  Eigen::Vector3f gravity;
494  gravity << 0.0, 0.0, -9.8;
495  Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
496  Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
497  Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
498 
499  Eigen::Vector3f localGravityRight =
500  currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
501  Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
502  Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
503 
504  // mapping of measured wrenches
505  Eigen::VectorXf wrenchesMeasured(12);
506  wrenchesMeasured << leftForceTorque->force - forceOffsetLeft,
507  leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight,
508  rightForceTorque->torque - torqueOffsetRight;
509  for (size_t i = 0; i < 12; i++)
510  {
511  wrenchesMeasured(i) =
512  (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
513  }
514  filteredOldValue = wrenchesMeasured;
515  wrenchesMeasured.block<3, 1>(0, 0) =
516  sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) -
517  localForceVecLeft;
518  wrenchesMeasured.block<3, 1>(3, 0) =
519  sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) -
520  localTorqueVecLeft;
521  wrenchesMeasured.block<3, 1>(6, 0) =
522  sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) -
523  localForceVecRight;
524  wrenchesMeasured.block<3, 1>(9, 0) =
525  sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) -
526  localTorqueVecRight;
527  if (wrenchesMeasured.norm() < cfg->forceThreshold)
528  {
529  wrenchesMeasured.setZero();
530  }
531 
532  // PID force controller
533  // Eigen::VectorXf wrenchesConstrainedInLocal(12);
534  // wrenchesConstrainedInLocal.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(0, 0);
535  // wrenchesConstrainedInLocal.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(3, 0);
536  // wrenchesConstrainedInLocal.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(6, 0);
537  // wrenchesConstrainedInLocal.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(9, 0);
538  Eigen::VectorXf forcePID(12);
539  // Eigen::VectorXf forcePIDControlValue(12);
540  // for (size_t i = 0; i < 12; i++)
541  // {
542  // forcePIDControllers[i]->update(deltaT, wrenchesConstrainedInLocal(i), cfg->targetWrench[i]);
543  // forcePIDControllers[i]->update(deltaT, wrenchesMeasured(i), cfg->targetWrench[i]);
544  // forcePIDControlValue(i) = forcePIDControllers[i]->getControlValue();
545  // forcePID(i) = - forcePIDControllers[i]->getControlValue();
546 
547  // }
548  for (size_t i = 0; i < 6; i++)
549  {
550  forcePID(i) = cfg->forceP[i] * (currentTargetWrench(i) - wrenchesMeasured(i));
551  forcePID(i + 6) =
552  cfg->forceP[i] * (currentTargetWrench(i + 6) - wrenchesMeasured(i + 6));
553  }
554  Eigen::VectorXf forcePIDInRoot(12);
555  forcePIDInRoot.block<3, 1>(0, 0) =
556  currentLeftPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(0, 0);
557  forcePIDInRoot.block<3, 1>(3, 0) =
558  currentLeftPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(3, 0);
559  forcePIDInRoot.block<3, 1>(6, 0) =
560  currentRightPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(6, 0);
561  forcePIDInRoot.block<3, 1>(9, 0) =
562  currentRightPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(9, 0);
563 
564  // forcePIDInRoot = PG * forcePIDInRoot;
565  Eigen::VectorXf forcePIDInRootForDebug = forcePIDInRoot;
566 
567  Eigen::VectorXf wrenchesMeasuredInRoot(12);
568  wrenchesMeasuredInRoot.block<3, 1>(0, 0) =
569  currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
570  wrenchesMeasuredInRoot.block<3, 1>(3, 0) =
571  currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
572  wrenchesMeasuredInRoot.block<3, 1>(6, 0) =
573  currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
574  wrenchesMeasuredInRoot.block<3, 1>(9, 0) =
575  currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
576  Eigen::VectorXf wrenchesConstrained = PG * wrenchesMeasuredInRoot;
577  // wrenchesConstrained.setZero();
578 
579 
580  // admittance
581  Eigen::VectorXf poseError(12);
582  poseError.block<3, 1>(0, 0) =
583  leftInitialPose.block<3, 1>(0, 3) - modifiedLeftPose.block<3, 1>(0, 3);
584  Eigen::Matrix3f diffMat =
585  leftInitialPose.block<3, 3>(0, 0) * modifiedLeftPose.block<3, 3>(0, 0).transpose();
586  poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
587  poseError.block<3, 1>(6, 0) =
588  rightInitialPose.block<3, 1>(0, 3) - modifiedRightPose.block<3, 1>(0, 3);
589  diffMat =
590  rightInitialPose.block<3, 3>(0, 0) * modifiedRightPose.block<3, 3>(0, 0).transpose();
591  poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
592 
593  Eigen::VectorXf acc;
594  Eigen::VectorXf twist;
595  twist.setZero(12);
596  acc.setZero(12);
597  for (size_t i = 0; i < 6; i++)
598  {
599  // acc(i) = KpAdmittance(i) * poseError(i) - KdAdmittance(i) * modifiedTwist(i) + wrenchDMP(i) - KmAdmittance(i) * wrenchesConstrained(i);
600  // acc(i + 6) = KpAdmittance(i) * poseError(i + 6) - KdAdmittance(i) * modifiedTwist(i + 6) + wrenchDMP(i + 6) - KmAdmittance(i) * wrenchesConstrained(i + 6);
601  acc(i) = KpAdmittance(i) * poseError(i) - KdAdmittance(i) * modifiedTwist(i) -
602  KmAdmittance(i) * wrenchesConstrained(i) - KmPID(i) * forcePIDInRoot(i);
603  acc(i + 6) =
604  KpAdmittance(i) * poseError(i + 6) - KdAdmittance(i) * modifiedTwist(i + 6) -
605  KmAdmittance(i) * wrenchesConstrained(i + 6) - KmPID(i) * forcePIDInRoot(i + 6);
606  }
607  twist = modifiedTwist + 0.5 * deltaT * (acc + modifiedAcc);
608  Eigen::VectorXf deltaPose = 0.5 * deltaT * (twist + modifiedTwist);
609  modifiedAcc = acc;
610  modifiedTwist = twist;
611 
612  modifiedLeftPose.block<3, 1>(0, 3) =
613  modifiedLeftPose.block<3, 1>(0, 3) + deltaPose.block<3, 1>(0, 0);
614  modifiedRightPose.block<3, 1>(0, 3) =
615  modifiedRightPose.block<3, 1>(0, 3) + deltaPose.block<3, 1>(6, 0);
616  modifiedLeftPose.block<3, 3>(0, 0) =
617  VirtualRobot::MathTools::rpy2eigen3f(deltaPose(3), deltaPose(4), deltaPose(5)) *
618  modifiedLeftPose.block<3, 3>(0, 0);
619  modifiedRightPose.block<3, 3>(0, 0) =
620  VirtualRobot::MathTools::rpy2eigen3f(deltaPose(9), deltaPose(10), deltaPose(11)) *
621  modifiedRightPose.block<3, 3>(0, 0);
622 
623  // for (size_t i = 0; i < 6; i++)
624  // {
625  // poseError(i) = (wrenchDMP(i) + wrenchesConstrained(i)) / KpAdmittance(i);
626  // poseError(i + 6) = (wrenchDMP(i + 6) + wrenchesConstrained(i + 6)) / KpAdmittance(i);
627  // }
628  // modifiedLeftPose.block<3, 1>(0, 3) = leftInitialPose.block<3, 1>(0, 3) + poseError.block<3, 1>(0, 0);
629  // modifiedRightPose.block<3, 1>(0, 3) = rightInitialPose.block<3, 1>(0, 3) + poseError.block<3, 1>(6, 0);
630  // modifiedLeftPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(poseError(3), poseError(4), poseError(5)) * leftInitialPose.block<3, 3>(0, 0);
631  // modifiedRightPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(poseError(9), poseError(10), poseError(11)) * rightInitialPose.block<3, 3>(0, 0) * ;
632 
633  // impedance control
634  diffMat =
635  modifiedLeftPose.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
636  poseError.block<3, 1>(0, 0) =
637  modifiedLeftPose.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
638  poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
639 
640  diffMat =
641  modifiedRightPose.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
642  poseError.block<3, 1>(6, 0) =
643  modifiedRightPose.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
644  poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
645 
646  Eigen::VectorXf forceImpedance(12);
647  for (size_t i = 0; i < 6; i++)
648  {
649  forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
650  forceImpedance(i + 6) =
651  KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
652  }
653 
654 
655  // nullspace
656  Eigen::VectorXf leftNullspaceTorque =
657  cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
658  Eigen::VectorXf rightNullspaceTorque =
659  cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
660 
661  float lambda = 1;
662 
663  // forcePIDInRoot.setZero();
664  forcePIDInRoot.setZero();
665  leftJointControlWrench = forceImpedance.head<6>() + forcePIDInRoot.head<6>();
666  rightJointControlWrench = forceImpedance.tail<6>() + forcePIDInRoot.tail<6>();
667  Eigen::MatrixXf jtpinvL =
668  leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
669  Eigen::MatrixXf jtpinvR =
670  rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
671  Eigen::VectorXf leftJointDesiredTorques =
672  jacobiL.transpose() * leftJointControlWrench +
673  (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
674  Eigen::VectorXf rightJointDesiredTorques =
675  jacobiR.transpose() * rightJointControlWrench +
676  (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
677 
678 
679  // torque limit
680  for (size_t i = 0; i < leftTargets.size(); ++i)
681  {
682  float desiredTorque = leftJointDesiredTorques(i);
683 
684  if (isnan(desiredTorque))
685  {
686  desiredTorque = 0;
687  }
688 
689  desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
690  desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
691 
692  debugOutputData.getWriteBuffer().desired_torques[leftJointNames[i]] =
693  leftJointDesiredTorques(i);
694 
695  leftTargets.at(i)->torque = desiredTorque;
696  }
697 
698 
699  for (size_t i = 0; i < rightTargets.size(); ++i)
700  {
701  float desiredTorque = rightJointDesiredTorques(i);
702 
703  if (isnan(desiredTorque))
704  {
705  desiredTorque = 0;
706  }
707 
708  desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
709  desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
710 
711  debugOutputData.getWriteBuffer().desired_torques[rightJointNames[i]] =
712  rightJointDesiredTorques(i);
713 
714  rightTargets.at(i)->torque = desiredTorque;
715  }
716  // debugOutputData.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
717 
718 
719  debugOutputData.getWriteBuffer().forceImpedance = forceImpedance;
720  debugOutputData.getWriteBuffer().poseError = poseError;
721  debugOutputData.getWriteBuffer().wrenchesConstrained = wrenchesConstrained;
722  debugOutputData.getWriteBuffer().wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
723  // debugOutputData.getWriteBuffer().wrenchDMP = wrenchDMP;
724  // debugOutputData.getWriteBuffer().computedBoxWrench = computedBoxWrench;
725 
726  debugOutputData.getWriteBuffer().modifiedPoseRight_x = modifiedRightPose(0, 3);
727  debugOutputData.getWriteBuffer().modifiedPoseRight_y = modifiedRightPose(1, 3);
728  debugOutputData.getWriteBuffer().modifiedPoseRight_z = modifiedRightPose(2, 3);
729 
730  debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
731  debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
732  debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
733 
734 
735  debugOutputData.getWriteBuffer().modifiedPoseLeft_x = modifiedLeftPose(0, 3);
736  debugOutputData.getWriteBuffer().modifiedPoseLeft_y = modifiedLeftPose(1, 3);
737  debugOutputData.getWriteBuffer().modifiedPoseLeft_z = modifiedLeftPose(2, 3);
738 
739  debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
740  debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
741  debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
742 
743 
744  debugOutputData.getWriteBuffer().dmpBoxPose_x = boxPose(0, 3);
745  debugOutputData.getWriteBuffer().dmpBoxPose_y = boxPose(1, 3);
746  debugOutputData.getWriteBuffer().dmpBoxPose_z = boxPose(2, 3);
747 
748  debugOutputData.getWriteBuffer().dmpTwist_x = boxTwist(0);
749  debugOutputData.getWriteBuffer().dmpTwist_y = boxTwist(1);
750  debugOutputData.getWriteBuffer().dmpTwist_z = boxTwist(2);
751  debugOutputData.getWriteBuffer().rx = r(0);
752  debugOutputData.getWriteBuffer().ry = r(1);
753  debugOutputData.getWriteBuffer().rz = r(2);
754 
755  debugOutputData.getWriteBuffer().modifiedTwist_lx = twistDMP(0);
756  debugOutputData.getWriteBuffer().modifiedTwist_ly = twistDMP(1);
757  debugOutputData.getWriteBuffer().modifiedTwist_lz = twistDMP(2);
758  debugOutputData.getWriteBuffer().modifiedTwist_rx = twistDMP(6);
759  debugOutputData.getWriteBuffer().modifiedTwist_ry = twistDMP(7);
760  debugOutputData.getWriteBuffer().modifiedTwist_rz = twistDMP(8);
761 
762  debugOutputData.getWriteBuffer().forcePID = forcePIDInRootForDebug;
763 
764  debugOutputData.commitWrite();
765  }
766 
767  void
768  NJointBimanualForceController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
769  const Ice::Current&)
770  {
771  objectDMP->learnDMPFromFiles(fileNames);
772  }
773 
774  void
775  NJointBimanualForceController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
776  {
777  LockGuardType guard(controllerMutex);
778  objectDMP->setGoalPoseVec(goals);
779  }
780 
781  void
782  NJointBimanualForceController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
783  {
784  while (!interfaceData.updateReadBuffer())
785  {
786  usleep(1000);
787  }
788 
789  Eigen::Matrix4f leftPose = interfaceData.getUpToDateReadBuffer().currentLeftPose;
790  Eigen::Matrix4f rightPose = interfaceData.getUpToDateReadBuffer().currentRightPose;
791 
792  VirtualRobot::MathTools::Quaternion boxOri =
793  VirtualRobot::MathTools::eigen4f2quat(leftPose);
794  Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
795 
796  std::vector<double> boxInitialPose;
797  for (size_t i = 0; i < 3; ++i)
798  {
799  boxInitialPose.push_back(boxPosi(i) * 0.001); //Important: mm -> m
800  }
801  boxInitialPose.push_back(boxOri.w);
802  boxInitialPose.push_back(boxOri.x);
803  boxInitialPose.push_back(boxOri.y);
804  boxInitialPose.push_back(boxOri.z);
805 
806  virtualtimer = cfg->timeDuration;
807  objectDMP->prepareExecution(boxInitialPose, goals);
808 
809  finished = false;
810  dmpStarted = true;
811  }
812 
813  void
814  NJointBimanualForceController::setViaPoints(Ice::Double u,
815  const Ice::DoubleSeq& viapoint,
816  const Ice::Current&)
817  {
818  // LockGuardType guard(controllerMutex);
819  ARMARX_INFO << "setting via points ";
820  objectDMP->setViaPose(u, viapoint);
821  }
822 
823  void
824  NJointBimanualForceController::onPublish(const SensorAndControl&,
825  const DebugDrawerInterfacePrx&,
826  const DebugObserverInterfacePrx& debugObs)
827  {
828 
829  StringVariantBaseMap datafields;
830  auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
831  for (auto& pair : values)
832  {
833  datafields[pair.first] = new Variant(pair.second);
834  }
835 
836  Eigen::VectorXf forceImpedance = debugOutputData.getUpToDateReadBuffer().forceImpedance;
837  for (int i = 0; i < forceImpedance.rows(); ++i)
838  {
839  std::stringstream ss;
840  ss << i;
841  std::string data_name = "forceImpedance_" + ss.str();
842  datafields[data_name] = new Variant(forceImpedance(i));
843  }
844 
845  Eigen::VectorXf forcePID = debugOutputData.getUpToDateReadBuffer().forcePID;
846  for (int i = 0; i < forcePID.rows(); ++i)
847  {
848  std::stringstream ss;
849  ss << i;
850  std::string data_name = "forcePID_" + ss.str();
851  datafields[data_name] = new Variant(forcePID(i));
852  }
853 
854 
855  Eigen::VectorXf poseError = debugOutputData.getUpToDateReadBuffer().poseError;
856  for (int i = 0; i < poseError.rows(); ++i)
857  {
858  std::stringstream ss;
859  ss << i;
860  std::string data_name = "poseError_" + ss.str();
861  datafields[data_name] = new Variant(poseError(i));
862  }
863 
864  Eigen::VectorXf wrenchesConstrained =
865  debugOutputData.getUpToDateReadBuffer().wrenchesConstrained;
866  for (int i = 0; i < wrenchesConstrained.rows(); ++i)
867  {
868  std::stringstream ss;
869  ss << i;
870  std::string data_name = "wrenchesConstrained_" + ss.str();
871  datafields[data_name] = new Variant(wrenchesConstrained(i));
872  }
873 
874  Eigen::VectorXf wrenchesMeasuredInRoot =
875  debugOutputData.getUpToDateReadBuffer().wrenchesMeasuredInRoot;
876  for (int i = 0; i < wrenchesMeasuredInRoot.rows(); ++i)
877  {
878  std::stringstream ss;
879  ss << i;
880  std::string data_name = "wrenchesMeasuredInRoot_" + ss.str();
881  datafields[data_name] = new Variant(wrenchesMeasuredInRoot(i));
882  }
883 
884 
885  // Eigen::VectorXf wrenchDMP = debugOutputData.getUpToDateReadBuffer().wrenchDMP;
886  // for (size_t i = 0; i < wrenchDMP.rows(); ++i)
887  // {
888  // std::stringstream ss;
889  // ss << i;
890  // std::string data_name = "wrenchDMP_" + ss.str();
891  // datafields[data_name] = new Variant(wrenchDMP(i));
892  // }
893 
894  // Eigen::VectorXf computedBoxWrench = debugOutputData.getUpToDateReadBuffer().computedBoxWrench;
895  // for (size_t i = 0; i < computedBoxWrench.rows(); ++i)
896  // {
897  // std::stringstream ss;
898  // ss << i;
899  // std::string data_name = "computedBoxWrench_" + ss.str();
900  // datafields[data_name] = new Variant(computedBoxWrench(i));
901  // }
902 
903 
904  datafields["modifiedPoseRight_x"] =
905  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_x);
906  datafields["modifiedPoseRight_y"] =
907  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_y);
908  datafields["modifiedPoseRight_z"] =
909  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_z);
910  datafields["currentPoseLeft_x"] =
911  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
912  datafields["currentPoseLeft_y"] =
913  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
914  datafields["currentPoseLeft_z"] =
915  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
916 
917 
918  datafields["modifiedPoseLeft_x"] =
919  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_x);
920  datafields["modifiedPoseLeft_y"] =
921  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_y);
922  datafields["modifiedPoseLeft_z"] =
923  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_z);
924 
925  datafields["currentPoseRight_x"] =
926  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
927  datafields["currentPoseRight_y"] =
928  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
929  datafields["currentPoseRight_z"] =
930  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
931  datafields["dmpBoxPose_x"] =
932  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
933  datafields["dmpBoxPose_y"] =
934  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
935  datafields["dmpBoxPose_z"] =
936  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
937  datafields["dmpTwist_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_x);
938  datafields["dmpTwist_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_y);
939  datafields["dmpTwist_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_z);
940 
941  datafields["modifiedTwist_lx"] =
942  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lx);
943  datafields["modifiedTwist_ly"] =
944  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ly);
945  datafields["modifiedTwist_lz"] =
946  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lz);
947  datafields["modifiedTwist_rx"] =
948  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rx);
949  datafields["modifiedTwist_ry"] =
950  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ry);
951  datafields["modifiedTwist_rz"] =
952  new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rz);
953 
954  datafields["rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().rx);
955  datafields["ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().ry);
956  datafields["rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().rz);
957 
958 
959  debugObs->setDebugChannel("BimanualForceController", datafields);
960  }
961 
962  void
963  NJointBimanualForceController::onInitNJointController()
964  {
965  ARMARX_INFO << "init ...";
966  runTask("NJointBimanualForceController",
967  [&]
968  {
969  CycleUtil c(1);
970  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
971  while (getState() == eManagedIceObjectStarted)
972  {
973  if (isControllerActive())
974  {
975  controllerRun();
976  }
977  c.waitForCycleDuration();
978  }
979  });
980  }
981 
982  void
983  NJointBimanualForceController::onDisconnectNJointController()
984  {
985  ARMARX_INFO << "stopped ...";
986  }
987 } // namespace armarx::control::deprecated_njoint_mp_controller::bimanual
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::NJointBimanualForceController
NJointBimanualForceController(const RobotUnitPtr &, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointBimanualForceController.cpp:28
armarx::NJointControllerWithTripleBuffer< NJointBimanualForceControlData >::reinitTripleBuffer
void reinitTripleBuffer(const NJointBimanualForceControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceControlData::boxTwist
Eigen::VectorXf boxTwist
Definition: NJointBimanualForceController.h:37
armarx::ControlTargetBase::asA
const T * asA() const
Definition: ControlTargetBase.h:76
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:223
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceControlData::boxPose
Eigen::Matrix4f boxPose
Definition: NJointBimanualForceController.h:36
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kori
float Kori
Definition: TaskSpaceVMP.h:54
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::controllerRun
void controllerRun()
Definition: NJointBimanualForceController.cpp:306
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::NJointControllerWithTripleBuffer< NJointBimanualForceControlData >::rtGetControlStruct
const NJointBimanualForceControlData & 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::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceControlData
Definition: NJointBimanualForceController.h:32
armarx::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:163
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::NJointControllerBase::useControlTarget
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition: NJointController.cpp:410
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::onDisconnectNJointController
void onDisconnectNJointController()
Definition: NJointBimanualForceController.cpp:983
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::learnDMPFromFiles
void learnDMPFromFiles(const Ice::StringSeq &fileNames, const Ice::Current &)
Definition: NJointBimanualForceController.cpp:768
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
SensorValueForceTorque.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
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::NJointControllerWithTripleBuffer< NJointBimanualForceControlData >::getWriterControlStruct
NJointBimanualForceControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition: NJointBimanualForceController.cpp:824
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::firstLoop
bool firstLoop
Definition: NJointBimanualForceController.h:245
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition: TaskSpaceVMP.h:48
armarx::NJointControllerWithTripleBuffer< NJointBimanualForceControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::SensorValueForceTorque::force
Eigen::Vector3f force
Definition: SensorValueForceTorque.h:36
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::motionTimeDuration
float motionTimeDuration
Definition: TaskSpaceVMP.h:66
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:846
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::backDist
float backDist
Definition: TaskSpaceVMP.h:49
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::maxValue
float maxValue
Definition: TaskSpaceVMP.h:50
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::getClassName
std::string getClassName(const Ice::Current &) const
Definition: NJointBimanualForceController.cpp:287
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:920
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dori
float Dori
Definition: TaskSpaceVMP.h:55
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
NJointBimanualForceController.h
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::NJointControllerWithTripleBuffer< NJointBimanualForceControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:108
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &)
Definition: NJointBimanualForceController.cpp:814
armarx::SensorValueForceTorque::torque
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION Eigen::Vector3f torque
Definition: SensorValueForceTorque.h:35
ArmarXObjectScheduler.h
ControlTarget1DoFActuator.h
armarx::NJointControllerWithTripleBuffer< NJointBimanualForceControlData >::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::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition: TaskSpaceVMP.h:52
CycleUtil.h
PIDController.h
NJointController.h
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::bimanual::NJointBimanualForceController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition: NJointBimanualForceController.cpp:775
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition: TaskSpaceVMP.h:56
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::onInitNJointController
void onInitNJointController()
Definition: NJointBimanualForceController.cpp:963
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::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition: NJointBimanualForceController.cpp:331
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::NJointBimanualForceController::runDMP
void runDMP(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition: NJointBimanualForceController.cpp:782
armarx::TripleBuffer::reinitAllBuffers
std::enable_if< std::is_copy_constructible< U >::value >::type reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:171
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition: TaskSpaceVMP.h:90
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:776
armarx::control::deprecated_njoint_mp_controller::bimanual::registrationControllerNJointBimanualForceController
NJointControllerRegistration< NJointBimanualForceController > registrationControllerNJointBimanualForceController("NJointBimanualForceController")
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