NJointBimanualCartesianAdmittanceController.cpp
Go to the documentation of this file.
1 #include "NJointBimanualCartesianAdmittanceController.h"
2 
3 #include <SimoxUtility/math/isfinite.h>
4 #include <SimoxUtility/math/pose/is_homogeneous_transform.h>
5 #include <SimoxUtility/math/pose/pose.h>
6 
7 #include <ArmarXCore/core/time/CycleUtil.h>
8 
9 // Simox
10 #include <VirtualRobot/IK/DifferentialIK.h>
11 #include <VirtualRobot/MathTools.h>
12 #include <VirtualRobot/Nodes/RobotNode.h>
13 #include <VirtualRobot/Nodes/Sensor.h>
14 #include <VirtualRobot/Robot.h>
15 #include <VirtualRobot/RobotNodeSet.h>
16 
17 // armarx
18 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
19 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
20 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
21 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
22 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
23 #include <RobotAPI/libraries/core/PIDController.h>
24 #include <RobotAPI/libraries/core/math/MathUtils.h>
25 
26 // control
27 #include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
28 #include <armarx/control/deprecated_njoint_mp_controller/bimanual/CartesianAdmittanceControllerInterface.h>
29 
30 namespace armarx::control::deprecated_njoint_mp_controller::bimanual
31 {
32  NJointControllerRegistration<NJointBimanualCartesianAdmittanceController>
33  registrationControllerNJointBimanualCartesianAdmittanceController(
34  "NJointBimanualCartesianAdmittanceController");
35 
36  NJointBimanualCartesianAdmittanceController::NJointBimanualCartesianAdmittanceController(
37  const RobotUnitPtr& robUnit,
38  const armarx::NJointControllerConfigPtr& config,
39  const VirtualRobot::RobotPtr& robot)
40  {
41  ARMARX_INFO << "Preparing ... bimanual ";
42  useSynchronizedRtRobot();
43  auto cfgPtr = NJointBimanualCartesianAdmittanceControllerConfigPtr::dynamicCast(config);
44  ARMARX_CHECK_NOT_NULL(cfgPtr);
45  //init rtData
46  {
47  auto initRtData =
48  [&](auto& data, const auto& rnsName, const auto& ftName, const auto& ftRefFrame)
49  {
50  data.rns = rtGetRobot()->getRobotNodeSet(rnsName);
51  ARMARX_CHECK_NOT_NULL(data.rns) << "No robot node set " << rnsName;
52  data.tcp = data.rns->getTCP();
53  ARMARX_CHECK_NOT_NULL(data.tcp) << "No TCP in robot node set " << rnsName;
54  data.frameFTSensor = rtGetRobot()->getSensor(ftName)->getRobotNode();
55  ARMARX_CHECK_NOT_NULL(data.frameFTSensor) << "No ref frame for ft sensor in robot ";
56 
57  for (size_t i = 0; i < data.rns->getSize(); ++i)
58  {
59  std::string jointName = data.rns->getNode(i)->getName();
60  data.jointNames.push_back(jointName);
61  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
62  const SensorValueBase* sv = useSensorValue(jointName);
63  ARMARX_CHECK_NOT_NULL(ct) << "No control target available for " << jointName;
64  ARMARX_CHECK_NOT_NULL(sv) << "No sensor value available for " << jointName;
65  data.targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
66  const SensorValue1DoFActuatorVelocity* velocitySensor =
67  sv->asA<SensorValue1DoFActuatorVelocity>();
68  const SensorValue1DoFActuatorPosition* positionSensor =
69  sv->asA<SensorValue1DoFActuatorPosition>();
70  ARMARX_CHECK_NOT_NULL(velocitySensor)
71  << "No velocitySensor available for " << jointName;
72  ARMARX_CHECK_NOT_NULL(positionSensor)
73  << "No positionSensor available for " << jointName;
74  data.velocitySensors.push_back(velocitySensor);
75  data.positionSensors.push_back(positionSensor);
76  }
77  const auto ftDev = robUnit->getSensorDevice(ftName);
78  ARMARX_CHECK_NOT_NULL(ftDev) << "No sensor device available for " << ftName;
79  const SensorValueBase* svlf = ftDev->getSensorValue();
80  ARMARX_CHECK_NOT_NULL(svlf) << "No sensor value available for " << ftName;
81  data.forceTorque = svlf->asA<SensorValueForceTorque>();
82  ARMARX_CHECK_NOT_NULL(data.forceTorque)
83  << "Sensor value for " << ftName << " is not of type SensorValueForceTorque";
84  data.IK.reset(
85  new VirtualRobot::DifferentialIK(data.rns,
86  data.rns->getRobot()->getRootNode(),
87  VirtualRobot::JacobiProvider::eSVDDamped));
88  };
89 
90  initRtData(rt.left,
91  cfgPtr->kinematicChainLeft,
92  cfgPtr->ftSensorLeft,
93  cfgPtr->ftSensorLeftFrame);
94  initRtData(rt.right,
95  cfgPtr->kinematicChainRight,
96  cfgPtr->ftSensorRight,
97  cfgPtr->ftSensorRightFrame);
98  }
99 
100  //init cfg + check it
101  {
102  setConfig(cfgPtr);
103  cfgBuf.reinitAllBuffers(cfgBuf.getWriteBuffer());
104  }
105 
106 
107  // {
108  // rt2ControlData initSensorData;
109  // initSensorData.deltaT = 0;
110  // initSensorData.currentTime = 0;
111  // initSensorData.currentPose = boxInitialPose;
112  // initSensorData.currentTwist.setZero();
113  // rt2ControlBuffer.reinitAllBuffers(initSensorData);
114  // }
115 
116 
117  // {
118  // ControlInterfaceData initInterfaceData;
119  // initInterfaceData.currentLeftPose = rt.left.tcp->getPoseInRootFrame();
120  // initInterfaceData.currentRightPose = rt.right.tcp->getPoseInRootFrame();
121  // controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
122  // }
123 
124  //////////////////////////////TODO
125  // leftInitialPose = rt.left.tcp->getPoseInRootFrame();
126  // rightInitialPose = rt.right.rns->getPoseInRootFrame();
127  // leftInitialPose.block<3, 1>(0, 3) = 0.001 * leftInitialPose.block<3, 1>(0, 3);
128  // rightInitialPose.block<3, 1>(0, 3) = 0.001 * rightInitialPose.block<3, 1>(0, 3);
129 
130  // // leftInitialPose = boxInitialPose;
131  // // leftInitialPose(0, 3) -= cfg->boxWidth * 0.5;
132  // // rightInitialPose = boxInitialPose;
133  // // rightInitialPose(0, 3) += cfg->boxWidth * 0.5;
134  //////////////////////////////TODO
135  // forcePIDControllers.resize(12);
136  // for (size_t i = 0; i < 6; i++)
137  // {
138  // forcePIDControllers.at(i).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
139  // forcePIDControllers.at(i + 6).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
140  // forcePIDControllers.at(i)->reset();
141  // forcePIDControllers.at(i + 6)->reset();
142  // }
143  //////////////////////////////TODO
144  // filter
145  // filterCoeff = cfg->filterCoeff;
146  // ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
147  // filteredOldValue.setZero(12);
148 
149  // static compensation
150  rt.left.sensorFrame2TcpFrame.setZero();
151  rt.right.sensorFrame2TcpFrame.setZero();
152  // NJointBimanualObjLevelControlData initData;
153  // initData.boxPose = boxInitialPose;
154  // initData.boxTwist.setZero(6);
155  // reinitTripleBuffer(initData);
156 
157  // ARMARX_INFO << "left initial pose: \n" << leftInitialPose << "\n right initial pose: \n" << rightInitialPose;
158 
159  // ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
160  ARMARX_IMPORTANT << "finished construction!";
161 
162  // targetWrench.setZero(cfg->targetWrench.size());
163  // for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
164  // {
165  // targetWrench(i) = cfg->targetWrench[i];
166  // }
167  }
168 
169  void
170  NJointBimanualCartesianAdmittanceController::rtPreActivateController()
171  {
172  // NJointBimanualObjLevelControlData initData;
173  // initData.boxPose = boxInitPose;
174  // initData.boxTwist.resize(6);
175  // reinitTripleBuffer(initData);
176 
177  rt.virtualAcc.setZero();
178  rt.virtualVel.setZero();
179  rt.virtualPose.setZero();
180  rt.filteredOldValue.setZero();
181  // rt.ftOffset.setZero();
182  rt.firstLoop = true;
183  rt.ftcalibrationTimer = 0;
184 
185 
186  const Eigen::Matrix4f leftPose =
187  simox::math::scaled_position(rt.left.tcp->getPoseInRootFrame(), 0.001);
188  const Eigen::Matrix4f rightPose =
189  simox::math::scaled_position(rt.right.tcp->getPoseInRootFrame(), 0.001);
190 
191  rt.virtualPose.block<3, 1>(0, 3) =
192  0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
193  rt.virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
194 
195  const Eigen::Matrix4f leftSensorFrame =
196  simox::math::scaled_position(rt.left.frameFTSensor->getPoseInRootFrame(), 0.001);
197  const Eigen::Matrix4f rightSensorFrame =
198  simox::math::scaled_position(rt.right.frameFTSensor->getPoseInRootFrame(), 0.001);
199 
200  rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) =
201  leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
202  rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) =
203  rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
204 
205  // ARMARX_INFO << "modified left pose:\n " << leftPose;
206  // ARMARX_INFO << "modified right pose:\n " << rightPose;
207  }
208 
209  std::string
210  NJointBimanualCartesianAdmittanceController::getClassName(const Ice::Current&) const
211  {
212  return "NJointBimanualCartesianAdmittanceController";
213  }
214 
215  // void NJointBimanualCartesianAdmittanceController::controllerRun()
216  // {
217  // if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
218  // {
219  // return;
220  // }
221 
222  // double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
223  // Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
224  // Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
225  // //ARMARX_IMPORTANT << "canVal: " << objectDMP->canVal;
226 
227  // if (objectDMP->canVal < 1e-8)
228  // {
229  // finished = true;
230  // dmpStarted = false;
231  // }
232 
233  // objectDMP->flow(deltaT, currentPose, currentTwist);
234 
235  // LockGuardType guard {controlDataMutex};
236  // getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
237  // getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
238  // writeControlStruct();
239  // }
240 
241 
242  void
243  NJointBimanualCartesianAdmittanceController::rtRun(const IceUtil::Time&,
244  const IceUtil::Time& timeSinceLastIteration)
245  {
246 
247  const Eigen::Matrix4f currentLeftPose =
248  simox::math::scaled_position(rt.left.tcp->getPoseInRootFrame(), 0.001);
249  const Eigen::Matrix4f currentRightPose =
250  simox::math::scaled_position(rt.right.tcp->getPoseInRootFrame(), 0.001);
251  const Eigen::Matrix4f currentBoxPose = [&]
252  {
253  Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
254  pose.block<3, 1>(0, 3) =
255  0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
256  pose.block<3, 3>(0, 0) = currentLeftPose.block<3, 3>(0, 0);
257  return pose;
258  }();
259  // {
260  // controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
261  // controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
262  // controlInterfaceBuffer.commitWrite();
263  // }
264  const double deltaT = timeSinceLastIteration.toSecondsDouble();
265 
266  ARMARX_ON_SCOPE_EXIT
267  {
268  rt.firstLoop = false;
269  };
270 
271  rt.ftcalibrationTimer += deltaT;
272 
273  // -------------------------------------------- config ---------------------------------------------
274 
275  if (cfgBuf.updateReadBuffer())
276  {
277  auto& cfg = cfgBuf._getNonConstReadBuffer();
278  const Eigen::Vector3f tmpL =
279  rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * cfg.CoMVecLeft;
280  const Eigen::Vector3f tmpR =
281  rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * cfg.CoMVecRight;
282  cfg.CoMVecLeft = tmpL;
283  cfg.CoMVecRight = tmpR;
284  }
285  if (rt.firstLoop)
286  {
287  auto& trg = targBuf._getNonConstReadBuffer();
288  trg.pose = currentBoxPose;
289  trg.vel.setZero();
290  }
291  auto& dbgOut = debugOutputData.getWriteBuffer();
292  const auto& targ = targBuf.getWriteBuffer();
293  const auto& cfg = cfgBuf.getReadBuffer();
294 
295  if (rt.ftcalibrationTimer < cfg.ftCalibrationTime)
296  {
297  // rt.ftOffset.block<3, 1>(0, 0) = 0.5 * rt.ftOffset.block<3, 1>(0, 0) + 0.5 * rt.right.forceTorque->force;
298  // rt.ftOffset.block<3, 1>(3, 0) = 0.5 * rt.ftOffset.block<3, 1>(3, 0) + 0.5 * rt.right.forceTorque->torque;
299  // rt.ftOffset.block<3, 1>(6, 0) = 0.5 * rt.ftOffset.block<3, 1>(6, 0) + 0.5 * rt.left.forceTorque->force;
300  // rt.ftOffset.block<3, 1>(9, 0) = 0.5 * rt.ftOffset.block<3, 1>(9, 0) + 0.5 * rt.left.forceTorque->torque;
301  // cfg.KmAdmittance.setZero();
302  }
303 
304  const Eigen::Vector6f KmAdmittance = (rt.ftcalibrationTimer < cfg.ftCalibrationTime)
305  ? Eigen::Vector6f::Zero()
306  : cfg.KmAdmittance;
307 
308  // -------------------------------------------- target wrench ---------------------------------------------
309  const Eigen::Vector12f deltaPoseForWrenchControl =
310  cfg.targetWrench.array() / cfg.KpImpedance.array();
311 
312  // ------------------------------------------- current tcp pose -------------------------------------------
313 
314 
315  // --------------------------------------------- grasp matrix ---------------------------------------------
316  const auto skew = [](auto& vec)
317  {
318  Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
319  mat(1, 2) = -vec(0);
320  mat(0, 2) = vec(1);
321  mat(0, 1) = -vec(2);
322  mat(2, 1) = vec(0);
323  mat(2, 0) = -vec(1);
324  mat(1, 0) = vec(2);
325  return mat;
326  };
327  const Eigen::Vector3f objCom2TCPLeft{-cfg.boxWidth * 0.5f, 0.f, 0.f};
328  const Eigen::Vector3f objCom2TCPRight{+cfg.boxWidth * 0.5f, 0.f, 0.f};
329 
330  Eigen::Matrix_6_12_f graspMatrix;
331  graspMatrix.setZero();
332  graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
333  graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
334 
335  const Eigen::Vector3f rLeft = rt.virtualPose.block<3, 3>(0, 0) * objCom2TCPLeft;
336  const Eigen::Vector3f rRight = rt.virtualPose.block<3, 3>(0, 0) * objCom2TCPRight;
337 
338  graspMatrix.block<3, 3>(3, 0) = skew(rLeft);
339  graspMatrix.block<3, 3>(3, 6) = skew(rRight);
340 
341  // // projection of grasp matrix
342  // Eigen::MatrixXf pinvG = rt.left.IK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
343  // Eigen::MatrixXf G_range = pinvG * graspMatrix;
344  // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
345  float lambda = 1;
346  const Eigen::MatrixXf pinvGraspMatrixT =
347  rt.left.IK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
348 
349  // ---------------------------------------------- object pose ----------------------------------------------
350  Eigen::Matrix4f boxCurrentPose = currentRightPose;
351  boxCurrentPose.block<3, 1>(0, 3) =
352  0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
353  Eigen::Vector6f boxCurrentTwist = Eigen::Vector6f::Zero();
354 
355  // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
356  const Eigen::MatrixXf I =
357  Eigen::MatrixXf::Identity(rt.left.targets.size(), rt.left.targets.size());
358  // Jacobian matrices
359  Eigen::MatrixXf jacobiL = rt.left.IK->getJacobianMatrix(
360  rt.left.tcp, VirtualRobot::IKSolver::CartesianSelection::All);
361  Eigen::MatrixXf jacobiR = rt.right.IK->getJacobianMatrix(
362  rt.right.tcp, VirtualRobot::IKSolver::CartesianSelection::All);
363  jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
364  jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
365 
366  // qpos, qvel
367  Eigen::VectorXf leftqpos(rt.left.positionSensors.size());
368  Eigen::VectorXf leftqvel(rt.left.velocitySensors.size());
369  for (size_t i = 0; i < rt.left.velocitySensors.size(); ++i)
370  {
371  leftqpos(i) = rt.left.positionSensors[i]->position;
372  leftqvel(i) = rt.left.velocitySensors[i]->velocity;
373  }
374 
375  Eigen::VectorXf rightqpos(rt.right.positionSensors.size());
376  Eigen::VectorXf rightqvel(rt.right.velocitySensors.size());
377  for (size_t i = 0; i < rt.right.velocitySensors.size(); ++i)
378  {
379  rightqpos(i) = rt.right.positionSensors[i]->position;
380  rightqvel(i) = rt.right.velocitySensors[i]->velocity;
381  }
382 
383  // -------------------------------------- compute TCP and object velocity -------------------------------------
384  const Eigen::Vector6f currentLeftTwist = jacobiL * leftqvel;
385  const Eigen::Vector6f currentRightTwist = jacobiR * rightqvel;
386 
387  Eigen::Vector12f currentTwist;
388  currentTwist << currentLeftTwist, currentRightTwist;
389  boxCurrentTwist = pinvGraspMatrixT * currentTwist;
390 
391  // rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
392  // rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
393  // rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
394  // rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
395  // rt2ControlBuffer.commitWrite();
396 
397  // --------------------------------------------- get ft sensor ---------------------------------------------
398  // static compensation
399  const Eigen::Vector3f gravity{0.0, 0.0, -9.8};
400  const Eigen::Vector3f localGravityLeft =
401  currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
402  const Eigen::Vector3f localForceVecLeft = cfg.massLeft * localGravityLeft;
403  const Eigen::Vector3f localTorqueVecLeft = cfg.CoMVecLeft.cross(localForceVecLeft);
404 
405  const Eigen::Vector3f localGravityRight =
406  currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
407  const Eigen::Vector3f localForceVecRight = cfg.massRight * localGravityRight;
408  const Eigen::Vector3f localTorqueVecRight = cfg.CoMVecRight.cross(localForceVecRight);
409 
410  // mapping of measured wrenches
411  Eigen::Vector12f wrenchesMeasured;
412  wrenchesMeasured << rt.right.forceTorque->force - cfg.forceOffsetLeft,
413  rt.right.forceTorque->torque - cfg.torqueOffsetLeft,
414  rt.left.forceTorque->force - cfg.forceOffsetRight,
415  rt.left.forceTorque->torque - cfg.torqueOffsetRight;
416  for (size_t i = 0; i < 12; i++)
417  {
418  wrenchesMeasured(i) = (1 - cfg.filterCoeff) * wrenchesMeasured(i) +
419  cfg.filterCoeff * rt.filteredOldValue(i);
420  }
421  rt.filteredOldValue = wrenchesMeasured;
422  wrenchesMeasured.block<3, 1>(0, 0) =
423  rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) -
424  localForceVecLeft;
425  wrenchesMeasured.block<3, 1>(3, 0) =
426  rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) -
427  localTorqueVecLeft;
428  wrenchesMeasured.block<3, 1>(6, 0) =
429  rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) -
430  localForceVecRight;
431  wrenchesMeasured.block<3, 1>(9, 0) =
432  rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) -
433  localTorqueVecRight;
434  // if (wrenchesMeasured.norm() < cfg->forceThreshold)
435  // {
436  // wrenchesMeasured.setZero();
437  // }
438 
439  Eigen::Vector12f wrenchesMeasuredInRoot;
440  wrenchesMeasuredInRoot.block<3, 1>(0, 0) =
441  currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
442  wrenchesMeasuredInRoot.block<3, 1>(3, 0) =
443  currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
444  wrenchesMeasuredInRoot.block<3, 1>(6, 0) =
445  currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
446  wrenchesMeasuredInRoot.block<3, 1>(9, 0) =
447  currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
448 
449  // map to object
450  Eigen::Vector6f objFTValue = graspMatrix * wrenchesMeasuredInRoot;
451  for (size_t i = 0; i < 6; i++)
452  {
453  if (fabs(objFTValue(i)) < cfg.forceThreshold(i))
454  {
455  objFTValue(i) = 0;
456  }
457  else
458  {
459  objFTValue(i) -= cfg.forceThreshold(i) * objFTValue(i) / fabs(objFTValue(i));
460  }
461  }
462 
463  // --------------------------------------------- get MP target ---------------------------------------------
464  const Eigen::Matrix4f boxPose = targ.pose;
465  const Eigen::Vector6f boxTwist = targ.vel;
466  // --------------------------------------------- obj admittance control ---------------------------------------------
467  // admittance
468  Eigen::Vector6f objPoseError;
469  objPoseError.head<3>() = rt.virtualPose.block<3, 1>(0, 3) - boxPose.block<3, 1>(0, 3);
470  const Eigen::Matrix3f objDiffMat =
471  rt.virtualPose.block<3, 3>(0, 0) * boxPose.block<3, 3>(0, 0).transpose();
472  objPoseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
473 
474 
475  Eigen::Vector6f objAcc = Eigen::Vector6f::Zero();
476  Eigen::Vector6f objVel = Eigen::Vector6f::Zero();
477  for (size_t i = 0; i < 6; i++)
478  {
479  objAcc(i) = KmAdmittance(i) * objFTValue(i) - cfg.KpAdmittance(i) * objPoseError(i) -
480  cfg.KdAdmittance(i) * rt.virtualVel(i);
481  }
482  objVel = rt.virtualVel + 0.5 * deltaT * (objAcc + rt.virtualAcc);
483  const Eigen::Vector6f deltaObjPose = 0.5 * deltaT * (objVel + rt.virtualVel);
484  rt.virtualAcc = objAcc;
485  rt.virtualVel = objVel;
486  rt.virtualPose.block<3, 1>(0, 3) += deltaObjPose.head<3>();
487  rt.virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(
488  deltaObjPose(3), deltaObjPose(4), deltaObjPose(5)) *
489  rt.virtualPose.block<3, 3>(0, 0);
490 
491  // --------------------------------------------- convert to tcp pose ---------------------------------------------
492  Eigen::Matrix4f tcpTargetPoseLeft = rt.virtualPose;
493  Eigen::Matrix4f tcpTargetPoseRight = rt.virtualPose;
494  tcpTargetPoseLeft.block<3, 1>(0, 3) +=
495  rt.virtualPose.block<3, 3>(0, 0) *
496  (objCom2TCPLeft - deltaPoseForWrenchControl.block<3, 1>(0, 0));
497  tcpTargetPoseRight.block<3, 1>(0, 3) +=
498  rt.virtualPose.block<3, 3>(0, 0) *
499  (objCom2TCPRight - deltaPoseForWrenchControl.block<3, 1>(6, 0));
500 
501  // --------------------------------------------- Impedance control ---------------------------------------------
502  Eigen::Vector12f poseError;
503  Eigen::Matrix3f diffMat =
504  tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
505  poseError.block<3, 1>(0, 0) =
506  tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
507  poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
508 
509  diffMat =
510  tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
511  poseError.block<3, 1>(6, 0) =
512  tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
513  poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
514 
515  Eigen::Vector12f forceImpedance;
516  for (size_t i = 0; i < 12; i++)
517  {
518  forceImpedance(i) =
519  cfg.KpImpedance(i) * poseError(i) - cfg.KdImpedance(i) * currentTwist(i);
520  // forceImpedance(i + 6) = KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
521  }
522 
523  // --------------------------------------------- Nullspace control ---------------------------------------------
524  const Eigen::VectorXf leftNullspaceTorque =
525  cfg.knull * (cfg.desiredJointValuesLeft - leftqpos) - cfg.dnull * leftqvel;
526  const Eigen::VectorXf rightNullspaceTorque =
527  cfg.knull * (cfg.desiredJointValuesRight - rightqpos) - cfg.dnull * rightqvel;
528 
529  // --------------------------------------------- Set Torque Control Command ---------------------------------------------
530  // float lambda = 1;
531 
532  // torque limit
533  const auto setTargets =
534  [&](auto& rtarm, const auto& jacobi, const auto& nullspaceTorque, int forceImpOffset)
535  {
536  const Eigen::MatrixXf jtpinv =
537  rtarm.IK->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
538  const Eigen::VectorXf desiredJointTorques =
539  jacobi.transpose() * forceImpedance.block<6, 1>(forceImpOffset, 0) +
540  (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
541  for (size_t i = 0; i < rtarm.targets.size(); ++i)
542  {
543  float desiredTorque = desiredJointTorques(i);
544  if (isnan(desiredTorque))
545  {
546  desiredTorque = 0;
547  }
548  desiredTorque = (desiredTorque > cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
549  desiredTorque =
550  (desiredTorque < -cfg.torqueLimit) ? -cfg.torqueLimit : desiredTorque;
551  dbgOut.desired_torques[rtarm.jointNames[i]] = desiredJointTorques(i);
552  rtarm.targets.at(i)->torque = desiredTorque;
553  }
554  };
555  setTargets(rt.left, jacobiL, leftNullspaceTorque, 0);
556  setTargets(rt.right, jacobiR, rightNullspaceTorque, 6);
557  {
558  const Eigen::MatrixXf jtpinvL =
559  rt.left.IK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
560  const Eigen::VectorXf leftJointDesiredTorques =
561  jacobiL.transpose() * forceImpedance.head<6>() +
562  (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
563  for (size_t i = 0; i < rt.left.targets.size(); ++i)
564  {
565  float desiredTorque = leftJointDesiredTorques(i);
566  if (isnan(desiredTorque))
567  {
568  desiredTorque = 0;
569  }
570  desiredTorque = (desiredTorque > cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
571  desiredTorque =
572  (desiredTorque < -cfg.torqueLimit) ? -cfg.torqueLimit : desiredTorque;
573  dbgOut.desired_torques[rt.left.jointNames[i]] = leftJointDesiredTorques(i);
574  rt.left.targets.at(i)->torque = desiredTorque;
575  }
576  }
577 
578  {
579  const Eigen::MatrixXf jtpinvR =
580  rt.right.IK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
581  const Eigen::VectorXf rightJointDesiredTorques =
582  jacobiR.transpose() * forceImpedance.tail<6>() +
583  (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
584  for (size_t i = 0; i < rt.right.targets.size(); ++i)
585  {
586  float desiredTorque = rightJointDesiredTorques(i);
587  if (isnan(desiredTorque))
588  {
589  desiredTorque = 0;
590  }
591  desiredTorque = (desiredTorque > cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
592  desiredTorque =
593  (desiredTorque < -cfg.torqueLimit) ? -cfg.torqueLimit : desiredTorque;
594  dbgOut.desired_torques[rt.right.jointNames[i]] = rightJointDesiredTorques(i);
595  rt.right.targets.at(i)->torque = desiredTorque;
596  }
597  }
598 
599  // --------------------------------------------- debug output ---------------------------------------------
600  dbgOut.currentBoxPose = currentBoxPose;
601  dbgOut.forceImpedance = forceImpedance;
602  dbgOut.poseError = poseError;
603  // dbgOut.wrenchesConstrained = wrenchesConstrained;
604  dbgOut.wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
605  // dbgOut.wrenchDMP = wrenchDMP;
606  // dbgOut.computedBoxWrench = computedBoxWrench;
607 
608  dbgOut.virtualPose_x = rt.virtualPose(0, 3);
609  dbgOut.virtualPose_y = rt.virtualPose(1, 3);
610  dbgOut.virtualPose_z = rt.virtualPose(2, 3);
611 
612  dbgOut.objPose_x = boxCurrentPose(0, 3);
613  dbgOut.objPose_y = boxCurrentPose(1, 3);
614  dbgOut.objPose_z = boxCurrentPose(2, 3);
615 
616  dbgOut.objForce_x = objFTValue(0);
617  dbgOut.objForce_y = objFTValue(1);
618  dbgOut.objForce_z = objFTValue(2);
619  dbgOut.objTorque_x = objFTValue(3);
620  dbgOut.objTorque_y = objFTValue(4);
621  dbgOut.objTorque_z = objFTValue(5);
622 
623  dbgOut.objVel_x = objVel(0);
624  dbgOut.objVel_y = objVel(1);
625  dbgOut.objVel_z = objVel(2);
626  dbgOut.objVel_rx = objVel(3);
627  dbgOut.objVel_ry = objVel(4);
628  dbgOut.objVel_rz = objVel(5);
629 
630  dbgOut.deltaPose_x = deltaObjPose(0);
631  dbgOut.deltaPose_y = deltaObjPose(1);
632  dbgOut.deltaPose_z = deltaObjPose(2);
633  dbgOut.deltaPose_rx = deltaObjPose(3);
634  dbgOut.deltaPose_ry = deltaObjPose(4);
635  dbgOut.deltaPose_rz = deltaObjPose(5);
636 
637  dbgOut.modifiedPoseRight_x = tcpTargetPoseRight(0, 3);
638  dbgOut.modifiedPoseRight_y = tcpTargetPoseRight(1, 3);
639  dbgOut.modifiedPoseRight_z = tcpTargetPoseRight(2, 3);
640 
641  dbgOut.currentPoseLeft_x = currentLeftPose(0, 3);
642  dbgOut.currentPoseLeft_y = currentLeftPose(1, 3);
643  dbgOut.currentPoseLeft_z = currentLeftPose(2, 3);
644 
645 
646  dbgOut.modifiedPoseLeft_x = tcpTargetPoseLeft(0, 3);
647  dbgOut.modifiedPoseLeft_y = tcpTargetPoseLeft(1, 3);
648  dbgOut.modifiedPoseLeft_z = tcpTargetPoseLeft(2, 3);
649 
650  dbgOut.currentPoseRight_x = currentRightPose(0, 3);
651  dbgOut.currentPoseRight_y = currentRightPose(1, 3);
652  dbgOut.currentPoseRight_z = currentRightPose(2, 3);
653 
654 
655  dbgOut.dmpBoxPose_x = boxPose(0, 3);
656  dbgOut.dmpBoxPose_y = boxPose(1, 3);
657  dbgOut.dmpBoxPose_z = boxPose(2, 3);
658 
659  dbgOut.dmpTwist_x = boxTwist(0);
660  dbgOut.dmpTwist_y = boxTwist(1);
661  dbgOut.dmpTwist_z = boxTwist(2);
662  dbgOut.rx = rRight(0);
663  dbgOut.ry = rRight(1);
664  dbgOut.rz = rRight(2);
665 
666  // dbgOut.modifiedTwist_lx = twistDMP(0);
667  // dbgOut.modifiedTwist_ly = twistDMP(1);
668  // dbgOut.modifiedTwist_lz = twistDMP(2);
669  // dbgOut.modifiedTwist_rx = twistDMP(6);
670  // dbgOut.modifiedTwist_ry = twistDMP(7);
671  // dbgOut.modifiedTwist_rz = twistDMP(8);
672 
673  // dbgOut.forcePID = forcePIDInRootForDebug;
674 
675  debugOutputData.commitWrite();
676  }
677 
678  void
679  NJointBimanualCartesianAdmittanceController::onPublish(
680  const SensorAndControl&,
681  const DebugDrawerInterfacePrx&,
682  const DebugObserverInterfacePrx& debugObs)
683  {
684  std::lock_guard guard{debugOutputDataReadMutex};
685  const auto& buf = debugOutputData.getUpToDateReadBuffer();
686  StringVariantBaseMap datafields;
687 
688  for (const auto& [name, val] : buf.desired_torques)
689  {
690  datafields[name] = new Variant(val);
691  }
692 
693  const auto& reportElements = [&](const auto& vec, const std::string& pre)
694  {
695  for (int i = 0; i < vec.rows(); ++i)
696  {
697  datafields[pre + std::to_string(i)] = new Variant(vec(i));
698  }
699  };
700  reportElements(buf.forceImpedance, "forceImpedance_");
701  reportElements(buf.forcePID, "forcePID_");
702  reportElements(buf.poseError, "poseError_");
703  reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
704  reportElements(buf.wrenchesMeasuredInRoot, "wrenchesMeasuredInRoot_");
705  reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
706  reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
707 
708  datafields["virtualPose_x"] = new Variant(buf.virtualPose_x);
709  datafields["virtualPose_y"] = new Variant(buf.virtualPose_y);
710  datafields["virtualPose_z"] = new Variant(buf.virtualPose_z);
711 
712  datafields["objPose_x"] = new Variant(buf.objPose_x);
713  datafields["objPose_y"] = new Variant(buf.objPose_y);
714  datafields["objPose_z"] = new Variant(buf.objPose_z);
715 
716  datafields["objForce_x"] = new Variant(buf.objForce_x);
717  datafields["objForce_y"] = new Variant(buf.objForce_y);
718  datafields["objForce_z"] = new Variant(buf.objForce_z);
719  datafields["objTorque_x"] = new Variant(buf.objTorque_x);
720  datafields["objTorque_y"] = new Variant(buf.objTorque_y);
721  datafields["objTorque_z"] = new Variant(buf.objTorque_z);
722 
723  datafields["objVel_x"] = new Variant(buf.objVel_x);
724  datafields["objVel_y"] = new Variant(buf.objVel_y);
725  datafields["objVel_z"] = new Variant(buf.objVel_z);
726  datafields["objVel_rx"] = new Variant(buf.objVel_rx);
727  datafields["objVel_ry"] = new Variant(buf.objVel_ry);
728  datafields["objVel_rz"] = new Variant(buf.objVel_rz);
729 
730  datafields["deltaPose_x"] = new Variant(buf.deltaPose_x);
731  datafields["deltaPose_y"] = new Variant(buf.deltaPose_y);
732  datafields["deltaPose_z"] = new Variant(buf.deltaPose_z);
733  datafields["deltaPose_rx"] = new Variant(buf.deltaPose_rx);
734  datafields["deltaPose_ry"] = new Variant(buf.deltaPose_ry);
735  datafields["deltaPose_rz"] = new Variant(buf.deltaPose_rz);
736 
737  datafields["modifiedPoseRight_x"] = new Variant(buf.modifiedPoseRight_x);
738  datafields["modifiedPoseRight_y"] = new Variant(buf.modifiedPoseRight_y);
739  datafields["modifiedPoseRight_z"] = new Variant(buf.modifiedPoseRight_z);
740  datafields["currentPoseLeft_x"] = new Variant(buf.currentPoseLeft_x);
741  datafields["currentPoseLeft_y"] = new Variant(buf.currentPoseLeft_y);
742  datafields["currentPoseLeft_z"] = new Variant(buf.currentPoseLeft_z);
743 
744 
745  datafields["modifiedPoseLeft_x"] = new Variant(buf.modifiedPoseLeft_x);
746  datafields["modifiedPoseLeft_y"] = new Variant(buf.modifiedPoseLeft_y);
747  datafields["modifiedPoseLeft_z"] = new Variant(buf.modifiedPoseLeft_z);
748 
749  datafields["currentPoseRight_x"] = new Variant(buf.currentPoseRight_x);
750  datafields["currentPoseRight_y"] = new Variant(buf.currentPoseRight_y);
751  datafields["currentPoseRight_z"] = new Variant(buf.currentPoseRight_z);
752  datafields["dmpBoxPose_x"] = new Variant(buf.dmpBoxPose_x);
753  datafields["dmpBoxPose_y"] = new Variant(buf.dmpBoxPose_y);
754  datafields["dmpBoxPose_z"] = new Variant(buf.dmpBoxPose_z);
755  datafields["dmpTwist_x"] = new Variant(buf.dmpTwist_x);
756  datafields["dmpTwist_y"] = new Variant(buf.dmpTwist_y);
757  datafields["dmpTwist_z"] = new Variant(buf.dmpTwist_z);
758 
759  datafields["modifiedTwist_lx"] = new Variant(buf.modifiedTwist_lx);
760  datafields["modifiedTwist_ly"] = new Variant(buf.modifiedTwist_ly);
761  datafields["modifiedTwist_lz"] = new Variant(buf.modifiedTwist_lz);
762  datafields["modifiedTwist_rx"] = new Variant(buf.modifiedTwist_rx);
763  datafields["modifiedTwist_ry"] = new Variant(buf.modifiedTwist_ry);
764  datafields["modifiedTwist_rz"] = new Variant(buf.modifiedTwist_rz);
765 
766  datafields["rx"] = new Variant(buf.rx);
767  datafields["ry"] = new Variant(buf.ry);
768  datafields["rz"] = new Variant(buf.rz);
769 
770 
771  debugObs->setDebugChannel("BimanualForceController", datafields);
772  }
773 
774  Eigen::Matrix4f
775  NJointBimanualCartesianAdmittanceController::getBoxPose(const Ice::Current&) const
776  {
777  std::lock_guard guard{debugOutputDataReadMutex};
778  return debugOutputData.getUpToDateReadBuffer().currentBoxPose;
779  }
780 
781  void
782  NJointBimanualCartesianAdmittanceController::setBoxPose(const Eigen::Matrix4f& pose,
783  const Ice::Current&)
784  {
785  ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
786  std::lock_guard guard{targBufWriteMutex};
787  targBuf.getWriteBuffer().pose = pose;
788  targBuf.commitWrite();
789  }
790 
791  void
792  NJointBimanualCartesianAdmittanceController::setBoxWidth(float w, const Ice::Current&)
793  {
794  ARMARX_CHECK_GREATER_EQUAL(w, 0);
795  std::lock_guard{cfgBufWriteMutex};
796  cfgBuf.getWriteBuffer().boxWidth = w;
797  cfgBuf.commitWrite();
798  }
799 
800  void
801  NJointBimanualCartesianAdmittanceController::setBoxVelocity(const Eigen::Vector3f& velXYZ,
802  const Eigen::Vector3f& velRPY,
803  const Ice::Current&)
804  {
805  ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velXYZ));
806  ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velRPY));
807  std::lock_guard guard{targBufWriteMutex};
808  targBuf.getWriteBuffer().vel.head<3>() = velXYZ;
809  targBuf.getWriteBuffer().vel.tail<3>() = velRPY;
810  targBuf.commitWrite();
811  }
812 
813  void
814  NJointBimanualCartesianAdmittanceController::setBoxPoseAndVelocity(
815  const Eigen::Matrix4f& pose,
816  const Eigen::Vector3f& velXYZ,
817  const Eigen::Vector3f& velRPY,
818  const Ice::Current&)
819  {
820  ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
821  ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velXYZ));
822  ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velRPY));
823  std::lock_guard guard{targBufWriteMutex};
824  targBuf.getWriteBuffer().pose = pose;
825  targBuf.getWriteBuffer().vel.head<3>() = velXYZ;
826  targBuf.getWriteBuffer().vel.tail<3>() = velRPY;
827  targBuf.commitWrite();
828  }
829 
830  void
831  NJointBimanualCartesianAdmittanceController::moveBoxPose(const Eigen::Matrix4f& pose,
832  const Ice::Current&)
833  {
834  ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
835  std::lock_guard guard{targBufWriteMutex};
836  const Eigen::Matrix4f tmp = pose * targBuf.getWriteBuffer().pose;
837  targBuf.getWriteBuffer().pose = tmp;
838  targBuf.commitWrite();
839  }
840 
841  void
842  NJointBimanualCartesianAdmittanceController::moveBoxPosition(const Eigen::Vector3f& pos,
843  const Ice::Current&)
844  {
845  ARMARX_CHECK_EXPRESSION(simox::math::isfinite(pos));
846  std::lock_guard guard{targBufWriteMutex};
847  targBuf.getWriteBuffer().pose.topRightCorner<3, 1>() += pos;
848  targBuf.commitWrite();
849  }
850 
851  void
852  NJointBimanualCartesianAdmittanceController::setConfig(
853  const NJointBimanualCartesianAdmittanceControllerConfigPtr& ptr,
854  const Ice::Current&)
855  {
856  ARMARX_CHECK_NOT_NULL(ptr);
857  ARMARX_CHECK_EQUAL(ptr->nullspace.desiredJointValuesLeft.size(), rt.left.targets.size());
858  ARMARX_CHECK_EQUAL(ptr->nullspace.desiredJointValuesRight.size(), rt.right.targets.size());
859  std::lock_guard{cfgBufWriteMutex};
860  updateConfig(*ptr);
861  cfgBuf.commitWrite();
862  }
863 
864  void
865  NJointBimanualCartesianAdmittanceController::setDesiredJointValuesLeft(
866  const Ice::FloatSeq& vals,
867  const Ice::Current&)
868  {
869  std::lock_guard{cfgBufWriteMutex};
870  updateDesiredJointValuesLeft(vals);
871  cfgBuf.commitWrite();
872  }
873 
874  void
875  NJointBimanualCartesianAdmittanceController::setDesiredJointValuesRight(
876  const Ice::FloatSeq& vals,
877  const Ice::Current&)
878  {
879  std::lock_guard{cfgBufWriteMutex};
880  updateDesiredJointValuesRight(vals);
881  cfgBuf.commitWrite();
882  }
883 
884  void
885  NJointBimanualCartesianAdmittanceController::setNullspaceConfig(
886  const detail::NJBmanCartAdmCtrl::Nullspace& nullspace,
887  const Ice::Current&)
888  {
889  std::lock_guard{cfgBufWriteMutex};
890  updateNullspaceConfig(nullspace);
891  cfgBuf.commitWrite();
892  }
893 
894  void
895  NJointBimanualCartesianAdmittanceController::setAdmittanceConfig(
896  const detail::NJBmanCartAdmCtrl::Admittance& admittanceObject,
897  const Ice::Current&)
898  {
899  std::lock_guard{cfgBufWriteMutex};
900  updateAdmittanceConfig(admittanceObject);
901  cfgBuf.commitWrite();
902  }
903 
904  void
905  NJointBimanualCartesianAdmittanceController::setForceConfig(
906  const detail::NJBmanCartAdmCtrl::Force& left,
907  const detail::NJBmanCartAdmCtrl::Force& right,
908  const Ice::Current&)
909  {
910  std::lock_guard{cfgBufWriteMutex};
911  updateForceConfig(left, right);
912  cfgBuf.commitWrite();
913  }
914 
915  void
916  NJointBimanualCartesianAdmittanceController::setImpedanceConfig(
917  const detail::NJBmanCartAdmCtrl::Impedance& left,
918  const detail::NJBmanCartAdmCtrl::Impedance& right,
919  const Ice::Current&)
920  {
921  std::lock_guard{cfgBufWriteMutex};
922  updateImpedanceConfig(left, right);
923  cfgBuf.commitWrite();
924  }
925 
926  void
927  NJointBimanualCartesianAdmittanceController::updateConfig(
928  const NJointBimanualCartesianAdmittanceControllerConfig& cfg)
929  {
930  std::lock_guard{cfgBufWriteMutex};
931  auto& buf = cfgBuf.getWriteBuffer();
932  updateNullspaceConfig(cfg.nullspace);
933  updateAdmittanceConfig(cfg.admittanceObject);
934  updateForceConfig(cfg.forceLeft, cfg.forceRight);
935  updateImpedanceConfig(cfg.impedanceLeft, cfg.impedanceRight);
936  buf.torqueLimit = cfg.torqueLimit;
937  buf.filterCoeff = cfg.filterCoeff;
938  buf.ftCalibrationTime = cfg.ftCalibrationTime;
939  buf.boxWidth = cfg.box.width;
940  }
941 
942  void
943  NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesLeft(
944  const Ice::FloatSeq& vals)
945  {
946  std::lock_guard{cfgBufWriteMutex};
947  auto& buf = cfgBuf.getWriteBuffer();
948  ARMARX_CHECK_EQUAL(vals.size(), rt.left.targets.size());
949  buf.desiredJointValuesLeft = Eigen::Map<const Eigen::VectorXf>(vals.data(), vals.size());
950  }
951 
952  void
953  NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesRight(
954  const Ice::FloatSeq& vals)
955  {
956  std::lock_guard{cfgBufWriteMutex};
957  auto& buf = cfgBuf.getWriteBuffer();
958  ARMARX_CHECK_EQUAL(vals.size(), rt.right.targets.size());
959  buf.desiredJointValuesRight = Eigen::Map<const Eigen::VectorXf>(vals.data(), vals.size());
960  }
961 
962  void
963  NJointBimanualCartesianAdmittanceController::updateNullspaceConfig(
964  const detail::NJBmanCartAdmCtrl::Nullspace& nullspace)
965  {
966  std::lock_guard{cfgBufWriteMutex};
967  auto& cfg = cfgBuf.getWriteBuffer();
968  updateDesiredJointValuesLeft(nullspace.desiredJointValuesLeft);
969  updateDesiredJointValuesRight(nullspace.desiredJointValuesRight);
970  cfg.knull = nullspace.k;
971  cfg.dnull = nullspace.d;
972  }
973 
974  void
975  NJointBimanualCartesianAdmittanceController::updateAdmittanceConfig(
976  const detail::NJBmanCartAdmCtrl::Admittance admittanceObject)
977  {
978  std::lock_guard{cfgBufWriteMutex};
979  auto& cfg = cfgBuf.getWriteBuffer();
980  cfg.KmAdmittance.block<3, 1>(0, 0) = admittanceObject.KmXYZ;
981  cfg.KmAdmittance.block<3, 1>(3, 0) = admittanceObject.KmRPY;
982 
983  cfg.KpAdmittance.block<3, 1>(0, 0) = admittanceObject.KpXYZ;
984  cfg.KpAdmittance.block<3, 1>(3, 0) = admittanceObject.KpRPY;
985 
986  cfg.KdAdmittance.block<3, 1>(0, 0) = admittanceObject.KdXYZ;
987  cfg.KdAdmittance.block<3, 1>(3, 0) = admittanceObject.KdRPY;
988  }
989 
990  void
991  NJointBimanualCartesianAdmittanceController::updateForceConfig(
992  const detail::NJBmanCartAdmCtrl::Force& forceLeft,
993  const detail::NJBmanCartAdmCtrl::Force& forceRight)
994  {
995  std::lock_guard{cfgBufWriteMutex};
996  auto& cfg = cfgBuf.getWriteBuffer();
997  //left
998  cfg.massLeft = forceLeft.mass;
999  cfg.CoMVecLeft = forceLeft.com;
1000  cfg.forceOffsetLeft = forceLeft.offsetForce;
1001  cfg.torqueOffsetLeft = forceLeft.offsetTorque;
1002  cfg.targetWrench.block<3, 1>(0, 0) = forceLeft.wrenchXYZ;
1003  cfg.targetWrench.block<3, 1>(3, 0) = forceLeft.wrenchRPY;
1004  cfg.forceThreshold.block<3, 1>(0, 0) = forceLeft.forceThreshold;
1005  //right
1006  cfg.massRight = forceRight.mass;
1007  cfg.CoMVecRight = forceRight.com;
1008  cfg.forceOffsetRight = forceRight.offsetForce;
1009  cfg.torqueOffsetRight = forceRight.offsetTorque;
1010  cfg.targetWrench.block<3, 1>(6, 0) = forceRight.wrenchXYZ;
1011  cfg.targetWrench.block<3, 1>(9, 0) = forceRight.wrenchRPY;
1012  cfg.forceThreshold.block<3, 1>(3, 0) = forceRight.forceThreshold;
1013  }
1014 
1015  void
1016  NJointBimanualCartesianAdmittanceController::updateImpedanceConfig(
1017  const detail::NJBmanCartAdmCtrl::Impedance& impedanceLeft,
1018  const detail::NJBmanCartAdmCtrl::Impedance& impedanceRight)
1019  {
1020  std::lock_guard{cfgBufWriteMutex};
1021  auto& cfg = cfgBuf.getWriteBuffer();
1022  cfg.KpImpedance.block<3, 1>(0, 0) = impedanceLeft.KpXYZ;
1023  cfg.KpImpedance.block<3, 1>(3, 0) = impedanceLeft.KpRPY;
1024  cfg.KpImpedance.block<3, 1>(6, 0) = impedanceRight.KpXYZ;
1025  cfg.KpImpedance.block<3, 1>(9, 0) = impedanceRight.KpRPY;
1026 
1027  cfg.KdImpedance.block<3, 1>(0, 0) = impedanceLeft.KdXYZ;
1028  cfg.KdImpedance.block<3, 1>(3, 0) = impedanceLeft.KdRPY;
1029  cfg.KdImpedance.block<3, 1>(6, 0) = impedanceRight.KdXYZ;
1030  cfg.KdImpedance.block<3, 1>(9, 0) = impedanceRight.KdRPY;
1031  }
1032 } // namespace armarx::control::deprecated_njoint_mp_controller::bimanual
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: NJointBimanualCartesianAdmittanceController.cpp:210
armarx::detail::NJBmanCartAdmCtrl::Admittance::KmRPY
Eigen::Vector3f KmRPY
Definition: CartesianAdmittanceControllerInterface.ice:67
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::detail::NJBmanCartAdmCtrl::Nullspace
Definition: CartesianAdmittanceControllerInterface.ice:44
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition: NJointBimanualCartesianAdmittanceController.cpp:170
armarx::detail::NJBmanCartAdmCtrl::Impedance::KpRPY
Eigen::Vector3f KpRPY
Definition: CartesianAdmittanceControllerInterface.ice:55
armarx::detail::NJBmanCartAdmCtrl::Admittance
Definition: CartesianAdmittanceControllerInterface.ice:60
MathUtils.h
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:92
armarx::NJointControllerRegistration
Definition: NJointControllerRegistry.h:74
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setImpedanceConfig
void setImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance &left, const detail::NJBmanCartAdmCtrl::Impedance &right, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:916
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::NJointBimanualCartesianAdmittanceControllerConfig::ftCalibrationTime
double ftCalibrationTime
Definition: CartesianAdmittanceControllerInterface.ice:98
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition: ControlTargetBase.h:47
RobotUnit.h
armarx::NJointBimanualCartesianAdmittanceControllerConfig
Definition: CartesianAdmittanceControllerInterface.ice:86
armarx::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:163
armarx::detail::NJBmanCartAdmCtrl::Force::offsetTorque
Eigen::Vector3f offsetTorque
Definition: CartesianAdmittanceControllerInterface.ice:79
armarx::WriteBufferedTripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:383
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::detail::NJBmanCartAdmCtrl::Admittance::KdXYZ
Eigen::Vector3f KdXYZ
Definition: CartesianAdmittanceControllerInterface.ice:64
armarx::NJointBimanualCartesianAdmittanceControllerConfig::nullspace
detail::NJBmanCartAdmCtrl::Nullspace nullspace
Definition: CartesianAdmittanceControllerInterface.ice:89
armarx::detail::NJBmanCartAdmCtrl::Force::mass
float mass
Definition: CartesianAdmittanceControllerInterface.ice:76
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_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::NJointBimanualCartesianAdmittanceControllerInterface::getBoxPose
Eigen::Matrix4f getBoxPose()
armarx::WriteBufferedTripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:377
SensorValueForceTorque.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesLeft
void updateDesiredJointValuesLeft(const Ice::FloatSeq &cfg)
Definition: NJointBimanualCartesianAdmittanceController.cpp:943
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateConfig
void updateConfig(const NJointBimanualCartesianAdmittanceControllerConfig &cfg)
Definition: NJointBimanualCartesianAdmittanceController.cpp:927
armarx::detail::NJBmanCartAdmCtrl::Force
Definition: CartesianAdmittanceControllerInterface.ice:70
ARMARX_ON_SCOPE_EXIT
#define ARMARX_ON_SCOPE_EXIT
Executes given code when the enclosing scope is left.
Definition: OnScopeExit.h:120
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setBoxPose
void setBoxPose(const Eigen::Matrix4f &pose, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:782
armarx::NJointBimanualCartesianAdmittanceControllerConfig::filterCoeff
float filterCoeff
Definition: CartesianAdmittanceControllerInterface.ice:96
armarx::NJointBimanualCartesianAdmittanceControllerConfig::admittanceObject
detail::NJBmanCartAdmCtrl::Admittance admittanceObject
Definition: CartesianAdmittanceControllerInterface.ice:92
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::moveBoxPose
void moveBoxPose(const Eigen::Matrix4f &pose, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:831
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setDesiredJointValuesRight
void setDesiredJointValuesRight(const Ice::FloatSeq &vals, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:875
armarx::detail::NJBmanCartAdmCtrl::Force::wrenchRPY
Eigen::Vector3f wrenchRPY
Definition: CartesianAdmittanceControllerInterface.ice:73
armarx::detail::NJBmanCartAdmCtrl::Force::com
Eigen::Vector3f com
Definition: CartesianAdmittanceControllerInterface.ice:77
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setForceConfig
void setForceConfig(const detail::NJBmanCartAdmCtrl::Force &left, const detail::NJBmanCartAdmCtrl::Force &right, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:905
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:846
armarx::detail::NJBmanCartAdmCtrl::Force::wrenchXYZ
Eigen::Vector3f wrenchXYZ
Definition: CartesianAdmittanceControllerInterface.ice:72
std::isfinite
bool isfinite(const std::vector< T, Ts... > &v)
Definition: algorithm.h:366
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
armarx::NJointBimanualCartesianAdmittanceControllerConfig::box
detail::NJBmanCartAdmCtrl::Box box
Definition: CartesianAdmittanceControllerInterface.ice:88
armarx::detail::NJBmanCartAdmCtrl::Impedance::KdRPY
Eigen::Vector3f KdRPY
Definition: CartesianAdmittanceControllerInterface.ice:57
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::detail::NJBmanCartAdmCtrl::Nullspace::desiredJointValuesLeft
Ice::FloatSeq desiredJointValuesLeft
Definition: CartesianAdmittanceControllerInterface.ice:46
armarx::detail::NJBmanCartAdmCtrl::Box::width
float width
Definition: CartesianAdmittanceControllerInterface.ice:41
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateAdmittanceConfig
void updateAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance admittanceObject)
Definition: NJointBimanualCartesianAdmittanceController.cpp:975
armarx::detail::NJBmanCartAdmCtrl::Nullspace::k
float k
Definition: CartesianAdmittanceControllerInterface.ice:48
armarx::WriteBufferedTripleBuffer::_getNonConstReadBuffer
T & _getNonConstReadBuffer()
Definition: TripleBuffer.h:353
armarx::detail::NJBmanCartAdmCtrl::Impedance::KpXYZ
Eigen::Vector3f KpXYZ
Definition: CartesianAdmittanceControllerInterface.ice:54
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setNullspaceConfig
void setNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace &nullspace, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:885
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateForceConfig
void updateForceConfig(const detail::NJBmanCartAdmCtrl::Force &left, const detail::NJBmanCartAdmCtrl::Force &right)
Definition: NJointBimanualCartesianAdmittanceController.cpp:991
armarx::detail::NJBmanCartAdmCtrl::Admittance::KpXYZ
Eigen::Vector3f KpXYZ
Definition: CartesianAdmittanceControllerInterface.ice:62
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::NJointBimanualCartesianAdmittanceController
NJointBimanualCartesianAdmittanceController(const RobotUnitPtr &, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointBimanualCartesianAdmittanceController.cpp:36
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: NJointBimanualCartesianAdmittanceController.cpp:243
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::moveBoxPosition
void moveBoxPosition(const Eigen::Vector3f &pos, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:842
armarx::detail::NJBmanCartAdmCtrl::Force::forceThreshold
Eigen::Vector3f forceThreshold
Definition: CartesianAdmittanceControllerInterface.ice:81
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateImpedanceConfig
void updateImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance &left, const detail::NJBmanCartAdmCtrl::Impedance &right)
Definition: NJointBimanualCartesianAdmittanceController.cpp:1016
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setBoxWidth
void setBoxWidth(float w, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:792
ControlTarget1DoFActuator.h
armarx::WriteBufferedTripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:325
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition: NJointBimanualCartesianAdmittanceController.cpp:679
armarx::detail::NJBmanCartAdmCtrl::Impedance
Definition: CartesianAdmittanceControllerInterface.ice:52
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition: ExpressionException.h:123
armarx::NJointBimanualCartesianAdmittanceControllerConfig::impedanceRight
detail::NJBmanCartAdmCtrl::Impedance impedanceRight
Definition: CartesianAdmittanceControllerInterface.ice:91
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
TaskSpaceVMP.h
armarx::detail::NJBmanCartAdmCtrl::Force::offsetForce
Eigen::Vector3f offsetForce
Definition: CartesianAdmittanceControllerInterface.ice:78
armarx::NJointBimanualCartesianAdmittanceControllerConfig::forceLeft
detail::NJBmanCartAdmCtrl::Force forceLeft
Definition: CartesianAdmittanceControllerInterface.ice:93
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesRight
void updateDesiredJointValuesRight(const Ice::FloatSeq &cfg)
Definition: NJointBimanualCartesianAdmittanceController.cpp:953
CycleUtil.h
PIDController.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setConfig
void setConfig(const NJointBimanualCartesianAdmittanceControllerConfigPtr &ptr, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:852
NJointController.h
armarx::WriteBufferedTripleBuffer::reinitAllBuffers
void reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:390
armarx::detail::NJBmanCartAdmCtrl::Admittance::KmXYZ
Eigen::Vector3f KmXYZ
Definition: CartesianAdmittanceControllerInterface.ice:66
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
IceUtil::Handle< class RobotUnit >
NJointBimanualCartesianAdmittanceController.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setBoxVelocity
void setBoxVelocity(const Eigen::Vector3f &velXYZ, const Eigen::Vector3f &velRPY, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:801
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setBoxPoseAndVelocity
void setBoxPoseAndVelocity(const Eigen::Matrix4f &pose, const Eigen::Vector3f &velXYZ, const Eigen::Vector3f &velRPY, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:814
armarx::control::deprecated_njoint_mp_controller::bimanual::registrationControllerNJointBimanualCartesianAdmittanceController
NJointControllerRegistration< NJointBimanualCartesianAdmittanceController > registrationControllerNJointBimanualCartesianAdmittanceController("NJointBimanualCartesianAdmittanceController")
armarx::NJointBimanualCartesianAdmittanceControllerConfig::forceRight
detail::NJBmanCartAdmCtrl::Force forceRight
Definition: CartesianAdmittanceControllerInterface.ice:94
Eigen::Matrix< float, 6, 1 >
armarx::WriteBufferedTripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:339
armarx::NJointBimanualCartesianAdmittanceControllerConfig::torqueLimit
float torqueLimit
Definition: CartesianAdmittanceControllerInterface.ice:97
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::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setDesiredJointValuesLeft
void setDesiredJointValuesLeft(const Ice::FloatSeq &vals, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:865
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:772
armarx::detail::NJBmanCartAdmCtrl::Admittance::KdRPY
Eigen::Vector3f KdRPY
Definition: CartesianAdmittanceControllerInterface.ice:65
armarx::NJointBimanualCartesianAdmittanceControllerConfig::impedanceLeft
detail::NJBmanCartAdmCtrl::Impedance impedanceLeft
Definition: CartesianAdmittanceControllerInterface.ice:90
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateNullspaceConfig
void updateNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace &nullspace)
Definition: NJointBimanualCartesianAdmittanceController.cpp:963
armarx::detail::NJBmanCartAdmCtrl::Admittance::KpRPY
Eigen::Vector3f KpRPY
Definition: CartesianAdmittanceControllerInterface.ice:63
armarx::detail::NJBmanCartAdmCtrl::Impedance::KdXYZ
Eigen::Vector3f KdXYZ
Definition: CartesianAdmittanceControllerInterface.ice:56
SensorValue1DoFActuator.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setAdmittanceConfig
void setAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance &admittanceObject, const Ice::Current &=Ice::emptyCurrent) override
Definition: NJointBimanualCartesianAdmittanceController.cpp:895
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::detail::NJBmanCartAdmCtrl::Nullspace::desiredJointValuesRight
Ice::FloatSeq desiredJointValuesRight
Definition: CartesianAdmittanceControllerInterface.ice:47
armarx::detail::NJBmanCartAdmCtrl::Nullspace::d
float d
Definition: CartesianAdmittanceControllerInterface.ice:49