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