DSJointCarryController.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package DSController::ArmarXObjects::DSJointCarryController
17  * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "DSJointCarryController.h"
24 
25 #include <VirtualRobot/IK/DifferentialIK.h>
26 #include <VirtualRobot/RobotNodeSet.h>
27 #include <VirtualRobot/Tools/Gravity.h>
28 
29 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
30 #include <ArmarXCore/core/time/CycleUtil.h>
31 
32 namespace armarx::control::ds_controller
33 {
34 
35  NJointControllerRegistration<DSJointCarryController>
36  registrationControllerDSJointCarryController("DSJointCarryController");
37 
38  void
39  DSJointCarryController::onInitNJointController()
40  {
41  ARMARX_INFO << "init ...";
42  controllerStopRequested = false;
43  controllerRunFinished = false;
44  runTask("DSJointCarryControllerTask",
45  [&]
46  {
47  CycleUtil c(1);
48  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
49  while (getState() == eManagedIceObjectStarted && !controllerStopRequested)
50  {
51  ARMARX_INFO << deactivateSpam(1) << "control fct";
52  if (isControllerActive())
53  {
54  controllerRun();
55  }
56  c.waitForCycleDuration();
57  }
58  controllerRunFinished = true;
59  ARMARX_INFO << "Control Fct finished";
60  });
61  }
62 
63  void
64  DSJointCarryController::onDisconnectNJointController()
65  {
66  ARMARX_INFO << "disconnect";
67  controllerStopRequested = true;
68  while (!controllerRunFinished)
69  {
70  ARMARX_INFO << deactivateSpam(1) << "waiting for run function";
71  usleep(10000);
72  }
73  }
74 
75  DSJointCarryController::DSJointCarryController(const RobotUnitPtr& robotUnit,
76  const armarx::NJointControllerConfigPtr& config,
77  const VirtualRobot::RobotPtr&)
78  {
79  cfg = DSJointCarryControllerConfigPtr::dynamicCast(config);
80  useSynchronizedRtRobot();
81 
82  VirtualRobot::RobotNodeSetPtr left_ns = rtGetRobot()->getRobotNodeSet("LeftArm");
83  VirtualRobot::RobotNodeSetPtr right_ns = rtGetRobot()->getRobotNodeSet("RightArm");
84 
85  left_jointNames.clear();
86  right_jointNames.clear();
87 
88  ARMARX_CHECK_EXPRESSION(left_ns) << "LeftArm";
89  ARMARX_CHECK_EXPRESSION(right_ns) << "RightArm";
90 
91  // for left arm
92  for (size_t i = 0; i < left_ns->getSize(); ++i)
93  {
94  std::string jointName = left_ns->getNode(i)->getName();
95  left_jointNames.push_back(jointName);
96  ControlTargetBase* ct = useControlTarget(
97  jointName,
98  ControlModes::
99  Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
100  ARMARX_CHECK_EXPRESSION(ct);
101  const SensorValueBase* sv = useSensorValue(jointName);
102  ARMARX_CHECK_EXPRESSION(sv);
103  auto casted_ct = ct->asA<
104  ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
105  ARMARX_CHECK_EXPRESSION(casted_ct);
106  left_torque_targets.push_back(casted_ct);
107 
108  const SensorValue1DoFActuatorTorque* torqueSensor =
109  sv->asA<SensorValue1DoFActuatorTorque>();
110  const SensorValue1DoFActuatorVelocity* velocitySensor =
111  sv->asA<SensorValue1DoFActuatorVelocity>();
112  const SensorValue1DoFGravityTorque* gravityTorqueSensor =
113  sv->asA<SensorValue1DoFGravityTorque>();
114  const SensorValue1DoFActuatorPosition* positionSensor =
115  sv->asA<SensorValue1DoFActuatorPosition>();
116  if (!torqueSensor)
117  {
118  ARMARX_WARNING << "No Torque sensor available for " << jointName;
119  }
120  if (!gravityTorqueSensor)
121  {
122  ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
123  }
124 
125  left_torqueSensors.push_back(torqueSensor);
126  left_gravityTorqueSensors.push_back(gravityTorqueSensor);
127  left_velocitySensors.push_back(velocitySensor);
128  left_positionSensors.push_back(positionSensor);
129  };
130 
131  // for right arm
132  for (size_t i = 0; i < right_ns->getSize(); ++i)
133  {
134  std::string jointName = right_ns->getNode(i)->getName();
135  right_jointNames.push_back(jointName);
136  ControlTargetBase* ct = useControlTarget(
137  jointName,
138  ControlModes::
139  Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
140  ARMARX_CHECK_EXPRESSION(ct);
141  const SensorValueBase* sv = useSensorValue(jointName);
142  ARMARX_CHECK_EXPRESSION(sv);
143  auto casted_ct = ct->asA<
144  ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
145  ARMARX_CHECK_EXPRESSION(casted_ct);
146  right_torque_targets.push_back(casted_ct);
147 
148  const SensorValue1DoFActuatorTorque* torqueSensor =
149  sv->asA<SensorValue1DoFActuatorTorque>();
150  const SensorValue1DoFActuatorVelocity* velocitySensor =
151  sv->asA<SensorValue1DoFActuatorVelocity>();
152  const SensorValue1DoFGravityTorque* gravityTorqueSensor =
153  sv->asA<SensorValue1DoFGravityTorque>();
154  const SensorValue1DoFActuatorPosition* positionSensor =
155  sv->asA<SensorValue1DoFActuatorPosition>();
156  if (!torqueSensor)
157  {
158  ARMARX_WARNING << "No Torque sensor available for " << jointName;
159  }
160  if (!gravityTorqueSensor)
161  {
162  ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
163  }
164 
165  right_torqueSensors.push_back(torqueSensor);
166  right_gravityTorqueSensors.push_back(gravityTorqueSensor);
167  right_velocitySensors.push_back(velocitySensor);
168  right_positionSensors.push_back(positionSensor);
169  };
170 
171 
172  const SensorValueBase* svlf = useSensorValue("FT L");
173  leftForceTorque = svlf->asA<SensorValueForceTorque>();
174  const SensorValueBase* svrf = useSensorValue("FT R");
175  rightForceTorque = svrf->asA<SensorValueForceTorque>();
176 
177  ARMARX_INFO << "Initialized " << left_torque_targets.size() << " targets for the left arm";
178  ARMARX_INFO << "Initialized " << right_torque_targets.size()
179  << " targets for the right arm";
180 
181  left_arm_tcp = left_ns->getTCP();
182  right_arm_tcp = right_ns->getTCP();
183 
184  left_sensor_frame = left_ns->getRobot()->getRobotNode("ArmL_FT");
185  right_sensor_frame = right_ns->getRobot()->getRobotNode("ArmR_FT");
186 
187  left_ik.reset(new VirtualRobot::DifferentialIK(
188  left_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
189  right_ik.reset(new VirtualRobot::DifferentialIK(
190  right_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
191 
192 
193  // ?? not sure about this
194  DSJointCarryControllerSensorData initSensorData;
195  initSensorData.left_tcpPose = left_arm_tcp->getPoseInRootFrame();
196  initSensorData.right_tcpPose = right_arm_tcp->getPoseInRootFrame();
197  initSensorData.left_force.setZero();
198  initSensorData.right_force.setZero();
199  initSensorData.currentTime = 0;
200  controllerSensorData.reinitAllBuffers(initSensorData);
201 
202  std::vector<float> left_desiredQuaternionVec = cfg->left_desiredQuaternion;
203  ARMARX_CHECK_EQUAL(left_desiredQuaternionVec.size(), 4);
204 
205  std::vector<float> right_desiredQuaternionVec = cfg->right_desiredQuaternion;
206  ARMARX_CHECK_EQUAL(right_desiredQuaternionVec.size(), 4);
207 
208  left_desiredQuaternion.w() = left_desiredQuaternionVec.at(0);
209  left_desiredQuaternion.x() = left_desiredQuaternionVec.at(1);
210  left_desiredQuaternion.y() = left_desiredQuaternionVec.at(2);
211  left_desiredQuaternion.z() = left_desiredQuaternionVec.at(3);
212  right_desiredQuaternion.w() = right_desiredQuaternionVec.at(0);
213  right_desiredQuaternion.x() = right_desiredQuaternionVec.at(1);
214  right_desiredQuaternion.y() = right_desiredQuaternionVec.at(2);
215  right_desiredQuaternion.z() = right_desiredQuaternionVec.at(3);
216 
217 
218  // set initial joint velocities filter
219  left_jointVelocity_filtered.resize(left_torque_targets.size());
220  left_jointVelocity_filtered.setZero();
221  right_jointVelocity_filtered.resize(left_torque_targets.size());
222  right_jointVelocity_filtered.setZero();
223 
224  // do we need to duplicate this?
225  DSJointCarryControllerControlData initData;
226  for (size_t i = 0; i < 3; ++i)
227  {
228  initData.leftDesiredLinearVelocity(i) = 0;
229  initData.leftDesiredAngularError(i) = 0;
230  initData.rightDesiredLinearVelocity(i) = 0;
231  initData.rightDesiredAngularError(i) = 0;
232  }
233  reinitTripleBuffer(initData);
234 
235  Ctrl2InterfaceData initCtrl2InterfaceData;
236  initCtrl2InterfaceData.guardZVel = 0;
237  ctrl2InterfaceData.reinitAllBuffers(initCtrl2InterfaceData);
238 
239  Interface2CtrlData initInterface2CtrlData;
240  initInterface2CtrlData.guardToHandInMeter.setZero();
241  initInterface2CtrlData.guardToHandInMeter[1] = cfg->guardLength / 2;
242  initInterface2CtrlData.guardOriInRobotBase.setIdentity();
243  initInterface2CtrlData.desiredGuardOriInRobotBase.w() = cfg->defaultGuardOri[0];
244  initInterface2CtrlData.desiredGuardOriInRobotBase.x() = cfg->defaultGuardOri[1];
245  initInterface2CtrlData.desiredGuardOriInRobotBase.y() = cfg->defaultGuardOri[2];
246  initInterface2CtrlData.desiredGuardOriInRobotBase.z() = cfg->defaultGuardOri[3];
247  initInterface2CtrlData.guardRotationStiffness << cfg->defaultRotationStiffness[0],
248  cfg->defaultRotationStiffness[1], cfg->defaultRotationStiffness[2];
249  initInterface2CtrlData.guardObsAvoidVel.setZero();
250  interface2CtrlData.reinitAllBuffers(initInterface2CtrlData);
251 
252  // initial filter
253  left_desiredTorques_filtered.resize(left_torque_targets.size());
254  left_desiredTorques_filtered.setZero();
255  right_desiredTorques_filtered.resize(right_torque_targets.size());
256  right_desiredTorques_filtered.setZero();
257 
258 
259  left_currentTCPLinearVelocity_filtered.setZero();
260  right_currentTCPLinearVelocity_filtered.setZero();
261 
262  left_currentTCPAngularVelocity_filtered.setZero();
263  right_currentTCPAngularVelocity_filtered.setZero();
264 
265  left_tcpDesiredTorque_filtered.setZero();
266  right_tcpDesiredTorque_filtered.setZero();
267 
268 
269  smooth_startup = 0;
270 
271  filterTimeConstant = cfg->filterTimeConstant;
272  posiKp = cfg->posiKp;
273  v_max = cfg->v_max;
274  Damping = cfg->posiDamping;
275  torqueLimit = cfg->torqueLimit;
276  null_torqueLimit = cfg->NullTorqueLimit;
277  oriKp = cfg->oriKp;
278  oriDamping = cfg->oriDamping;
279 
280  // nullspace control
281  left_qnullspace.resize(cfg->leftarm_qnullspaceVec.size());
282  right_qnullspace.resize(cfg->rightarm_qnullspaceVec.size());
283  for (size_t i = 0; i < cfg->leftarm_qnullspaceVec.size(); ++i)
284  {
285  left_qnullspace(i) = cfg->leftarm_qnullspaceVec[i];
286  right_qnullspace(i) = cfg->rightarm_qnullspaceVec[i];
287  }
288  nullspaceKp = cfg->nullspaceKp;
289  nullspaceDamping = cfg->nullspaceDamping;
290 
291 
292  //set GMM parameters ...
293  if (cfg->gmmParamsFile == "")
294  {
295  ARMARX_ERROR << "gmm params file cannot be empty string ...";
296  }
297  gmmMotionGenerator.reset(new JointCarryGMMMotionGen(cfg->gmmParamsFile));
298 
299  ARMARX_INFO << "Initialization done";
300  }
301 
302  void
303  DSJointCarryController::controllerRun()
304  {
305  if (!controllerSensorData.updateReadBuffer())
306  {
307  return;
308  }
309 
310  // receive the measurements
311  Eigen::Matrix4f left_currentTCPPose = controllerSensorData.getReadBuffer().left_tcpPose;
312  Eigen::Matrix4f right_currentTCPPose = controllerSensorData.getReadBuffer().right_tcpPose;
313 
314  Eigen::Vector3f left_currentTCPPositionInMeter;
315  Eigen::Vector3f right_currentTCPPositionInMeter;
316  left_currentTCPPositionInMeter << left_currentTCPPose(0, 3), left_currentTCPPose(1, 3),
317  left_currentTCPPose(2, 3);
318  right_currentTCPPositionInMeter << right_currentTCPPose(0, 3), right_currentTCPPose(1, 3),
319  right_currentTCPPose(2, 3);
320  left_currentTCPPositionInMeter = 0.001 * left_currentTCPPositionInMeter;
321  right_currentTCPPositionInMeter = 0.001 * right_currentTCPPositionInMeter;
322 
323  interface2CtrlData.updateReadBuffer();
324  Eigen::Quaternionf guard_currentOrientation =
325  interface2CtrlData.getReadBuffer().guardOriInRobotBase;
326  Eigen::Quaternionf guard_desiredOrientation =
327  interface2CtrlData.getReadBuffer().desiredGuardOriInRobotBase;
328  Eigen::Vector3f guardToHandInMeter = interface2CtrlData.getReadBuffer().guardToHandInMeter;
329  Eigen::Vector3f guardRotationStiffness =
330  interface2CtrlData.getReadBuffer().guardRotationStiffness;
331  Eigen::Vector3f guardModulatedVel = interface2CtrlData.getReadBuffer().guardObsAvoidVel;
332 
333  Eigen::Vector3f left_to_right =
334  right_currentTCPPositionInMeter - left_currentTCPPositionInMeter;
335  left_to_right.normalize();
336  // calculate the desired position velocity of the guard
337  Eigen::Vector3f guard_currentPosition;
338  guard_currentPosition =
339  (left_currentTCPPositionInMeter + right_currentTCPPositionInMeter) / 2 +
340  guardToHandInMeter;
341  gmmMotionGenerator->updateDesiredVelocity(guard_currentPosition,
342  cfg->positionErrorTolerance);
343  Eigen::Vector3f guardDesiredLinearVelocity = gmmMotionGenerator->guard_desiredVelocity;
344 
345 
346  ctrl2InterfaceData.getWriteBuffer().guardZVel = guardDesiredLinearVelocity[2];
347  ctrl2InterfaceData.commitWrite();
348 
349  guardDesiredLinearVelocity[2] = guardDesiredLinearVelocity[2] + guardModulatedVel[2];
350 
351  // calculate the desired rotation velocity of the guard
352  if (guard_desiredOrientation.coeffs().dot(guard_currentOrientation.coeffs()) < 0.0)
353  {
354  guard_currentOrientation.coeffs() << -guard_currentOrientation.coeffs();
355  }
356  Eigen::Quaternionf errorOri = guard_currentOrientation * guard_desiredOrientation.inverse();
357  Eigen::AngleAxisf err_axang(errorOri);
358  Eigen::Vector3f guard_angular_error = err_axang.axis() * err_axang.angle();
359  Eigen::Vector3f guard_desired_angular_vel =
360  -1 * guardRotationStiffness.cwiseProduct(guard_angular_error);
361 
362  // calculate the hand desired linear velocity
363  Eigen::Vector3f guard_to_left = left_currentTCPPositionInMeter - guard_currentPosition;
364  Eigen::Vector3f leftOriGenLinearVelocity = guard_desired_angular_vel.cross(guard_to_left);
365  leftOriGenLinearVelocity -= leftOriGenLinearVelocity.dot(left_to_right) * left_to_right;
366  Eigen::Vector3f leftDesiredLinearVelocity =
367  leftOriGenLinearVelocity + guardDesiredLinearVelocity;
368 
369  if (leftDesiredLinearVelocity.norm() > cfg->handVelLimit)
370  {
371  leftDesiredLinearVelocity =
372  cfg->handVelLimit * leftDesiredLinearVelocity / leftDesiredLinearVelocity.norm();
373  }
374 
375  Eigen::Vector3f guard_to_right = right_currentTCPPositionInMeter - guard_currentPosition;
376  Eigen::Vector3f rightOriGenLinearVelocity = guard_desired_angular_vel.cross(guard_to_right);
377  rightOriGenLinearVelocity -= rightOriGenLinearVelocity.dot(left_to_right) * left_to_right;
378  Eigen::Vector3f rightDesiredLinearVelocity =
379  rightOriGenLinearVelocity + guardDesiredLinearVelocity;
380 
381  if (rightDesiredLinearVelocity.norm() > cfg->handVelLimit)
382  {
383  rightDesiredLinearVelocity =
384  cfg->handVelLimit * rightDesiredLinearVelocity / rightDesiredLinearVelocity.norm();
385  }
386 
387  // calculate the desired orientation of the hand
388  Eigen::Matrix3f left_currentRotMat = left_currentTCPPose.block<3, 3>(0, 0);
389  Eigen::Matrix3f right_currentRotMat = right_currentTCPPose.block<3, 3>(0, 0);
390  Eigen::Matrix3f left_desiredRotMat = left_desiredQuaternion.normalized().toRotationMatrix();
391  Eigen::Matrix3f right_desiredRotMat =
392  right_desiredQuaternion.normalized().toRotationMatrix();
393  Eigen::Matrix3f left_orientationError = left_currentRotMat * left_desiredRotMat.inverse();
394  Eigen::Matrix3f right_orientationError =
395  right_currentRotMat * right_desiredRotMat.inverse();
396  Eigen::Quaternionf left_diffQuaternion(left_orientationError);
397  Eigen::Quaternionf right_diffQuaternion(right_orientationError);
398  Eigen::AngleAxisf left_angleAxis(left_diffQuaternion);
399  Eigen::AngleAxisf right_angleAxis(right_diffQuaternion);
400  Eigen::Vector3f left_tcpDesiredAngularError =
401  left_angleAxis.angle() * left_angleAxis.axis();
402  Eigen::Vector3f right_tcpDesiredAngularError =
403  right_angleAxis.angle() * right_angleAxis.axis();
404 
405  // writing to the buffer
406  LockGuardType guard{controlDataMutex};
407  getWriterControlStruct().leftDesiredLinearVelocity = leftDesiredLinearVelocity;
408  getWriterControlStruct().leftDesiredAngularError = left_tcpDesiredAngularError;
409  getWriterControlStruct().rightDesiredLinearVelocity = rightDesiredLinearVelocity;
410  getWriterControlStruct().rightDesiredAngularError = right_tcpDesiredAngularError;
411  writeControlStruct();
412 
413  debugCtrlDataInfo.getWriteBuffer().leftDesiredLinearVelocity = leftDesiredLinearVelocity;
414  debugCtrlDataInfo.getWriteBuffer().rightDesiredLinearVelocity = rightDesiredLinearVelocity;
415  debugCtrlDataInfo.commitWrite();
416  }
417 
418  void
419  DSJointCarryController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
420  const IceUtil::Time& timeSinceLastIteration)
421  {
422  /* get sensor data */
423  double deltaT = timeSinceLastIteration.toSecondsDouble();
424 
425  Eigen::Matrix4f leftSensorFrame = left_sensor_frame->getPoseInRootFrame();
426  Eigen::Matrix4f rightSensorFrame = right_sensor_frame->getPoseInRootFrame();
427  Eigen::Vector3f left_forceInRoot =
428  leftSensorFrame.block<3, 3>(0, 0) * (leftForceTorque->force);
429  Eigen::Vector3f right_forceInRoot =
430  rightSensorFrame.block<3, 3>(0, 0) * (rightForceTorque->force);
431  left_forceInRoot(2) = left_forceInRoot(2); // - 4 + 8.5;
432  right_forceInRoot(2) = right_forceInRoot(2); // + 30 - 5.2;
433  Eigen::Matrix4f left_currentTCPPose = left_arm_tcp->getPoseInRootFrame();
434  Eigen::Matrix4f right_currentTCPPose = right_arm_tcp->getPoseInRootFrame();
435 
436 
437  Eigen::MatrixXf left_jacobi = left_ik->getJacobianMatrix(
438  left_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
439  Eigen::MatrixXf right_jacobi = right_ik->getJacobianMatrix(
440  right_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
441 
442  Eigen::VectorXf left_qpos;
443  Eigen::VectorXf left_qvel;
444 
445  Eigen::VectorXf right_qpos;
446  Eigen::VectorXf right_qvel;
447 
448  left_qpos.resize(left_positionSensors.size());
449  left_qvel.resize(left_velocitySensors.size());
450 
451  right_qpos.resize(right_positionSensors.size());
452  right_qvel.resize(right_velocitySensors.size());
453 
454  int jointDim = left_positionSensors.size();
455 
456  for (size_t i = 0; i < left_velocitySensors.size(); ++i)
457  {
458  left_qpos(i) = left_positionSensors[i]->position;
459  left_qvel(i) = left_velocitySensors[i]->velocity;
460 
461  right_qpos(i) = right_positionSensors[i]->position;
462  right_qvel(i) = right_velocitySensors[i]->velocity;
463  }
464 
465  Eigen::VectorXf left_tcptwist = left_jacobi * left_qvel;
466  Eigen::VectorXf right_tcptwist = right_jacobi * right_qvel;
467 
468  Eigen::Vector3f left_currentTCPLinearVelocity;
469  Eigen::Vector3f right_currentTCPLinearVelocity;
470  Eigen::Vector3f left_currentTCPAngularVelocity;
471  Eigen::Vector3f right_currentTCPAngularVelocity;
472  left_currentTCPLinearVelocity << 0.001 * left_tcptwist(0), 0.001 * left_tcptwist(1),
473  0.001 * left_tcptwist(2);
474  right_currentTCPLinearVelocity << 0.001 * right_tcptwist(0), 0.001 * right_tcptwist(1),
475  0.001 * right_tcptwist(2);
476  left_currentTCPAngularVelocity << left_tcptwist(3), left_tcptwist(4), left_tcptwist(5);
477  right_currentTCPAngularVelocity << right_tcptwist(3), right_tcptwist(4), right_tcptwist(5);
478  double filterFactor = deltaT / (filterTimeConstant + deltaT);
479  left_currentTCPLinearVelocity_filtered =
480  (1 - filterFactor) * left_currentTCPLinearVelocity_filtered +
481  filterFactor * left_currentTCPLinearVelocity;
482  right_currentTCPLinearVelocity_filtered =
483  (1 - filterFactor) * right_currentTCPLinearVelocity_filtered +
484  filterFactor * right_currentTCPLinearVelocity;
485  left_currentTCPAngularVelocity_filtered =
486  (1 - filterFactor) * left_currentTCPAngularVelocity_filtered +
487  left_currentTCPAngularVelocity;
488  right_currentTCPAngularVelocity_filtered =
489  (1 - filterFactor) * right_currentTCPAngularVelocity_filtered +
490  right_currentTCPAngularVelocity;
491  left_jointVelocity_filtered =
492  (1 - filterFactor) * left_jointVelocity_filtered + filterFactor * left_qvel;
493  right_jointVelocity_filtered =
494  (1 - filterFactor) * right_jointVelocity_filtered + filterFactor * right_qvel;
495 
496  controllerSensorData.getWriteBuffer().left_tcpPose = left_currentTCPPose;
497  controllerSensorData.getWriteBuffer().right_tcpPose = right_currentTCPPose;
498  controllerSensorData.getWriteBuffer().left_force = left_forceInRoot;
499  controllerSensorData.getWriteBuffer().right_force = right_forceInRoot;
500  controllerSensorData.getWriteBuffer().currentTime += deltaT;
501  controllerSensorData.commitWrite();
502 
503 
504  Eigen::Vector3f leftDesiredLinearVelocity = rtGetControlStruct().leftDesiredLinearVelocity;
505  Eigen::Vector3f rightDesiredLinearVelocity =
506  rtGetControlStruct().rightDesiredLinearVelocity;
507  Eigen::Vector3f left_tcpDesiredAngularError = rtGetControlStruct().leftDesiredAngularError;
508  Eigen::Vector3f right_tcpDesiredAngularError =
509  rtGetControlStruct().rightDesiredAngularError;
510 
511  // computing the task-specific forces
512  Eigen::Vector3f left_DS_force =
513  -(left_currentTCPLinearVelocity_filtered - leftDesiredLinearVelocity);
514  Eigen::Vector3f right_DS_force =
515  -(right_currentTCPLinearVelocity_filtered - rightDesiredLinearVelocity);
516  for (int i = 0; i < 3; ++i)
517  {
518  left_DS_force(i) = left_DS_force(i) * Damping[i];
519  right_DS_force(i) = right_DS_force(i) * Damping[i];
520  left_DS_force(i) = deadzone(left_DS_force(i), 0.1, 100);
521  right_DS_force(i) = deadzone(right_DS_force(i), 0.1, 100);
522  }
523 
524  Eigen::Vector3f left_tcpDesiredTorque =
525  -oriKp * left_tcpDesiredAngularError -
526  oriDamping * left_currentTCPAngularVelocity_filtered;
527  Eigen::Vector3f right_tcpDesiredTorque =
528  -oriKp * right_tcpDesiredAngularError -
529  oriDamping * right_currentTCPAngularVelocity_filtered;
530 
531  left_tcpDesiredTorque_filtered = (1 - filterFactor) * left_tcpDesiredTorque_filtered +
532  filterFactor * left_tcpDesiredTorque;
533  right_tcpDesiredTorque_filtered = (1 - filterFactor) * right_tcpDesiredTorque_filtered +
534  filterFactor * right_tcpDesiredTorque;
535 
536  Eigen::Vector6f left_tcpDesiredWrench;
537  Eigen::Vector6f right_tcpDesiredWrench;
538 
539  // times 0.001 because Jacobi matrix is in mm and radian.
540  left_tcpDesiredWrench << 0.001 * left_DS_force, left_tcpDesiredTorque_filtered;
541  right_tcpDesiredWrench << 0.001 * right_DS_force, right_tcpDesiredTorque_filtered;
542 
543  // left_tcpDesiredWrench(2) += 0.001 * cfg->guardGravity / 4;
544  // right_tcpDesiredWrench(2) += 0.001 * cfg->guardGravity / 4;
545 
546 
547  // calculate the null-spcae torque
548  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
549  float lambda = 0.2;
550  Eigen::MatrixXf left_jtpinv =
551  left_ik->computePseudoInverseJacobianMatrix(left_jacobi.transpose(), lambda);
552  Eigen::MatrixXf right_jtpinv =
553  right_ik->computePseudoInverseJacobianMatrix(right_jacobi.transpose(), lambda);
554  Eigen::VectorXf left_nullqerror = left_qpos - left_qnullspace;
555  Eigen::VectorXf right_nullqerror = right_qpos - right_qnullspace;
556 
557  for (int i = 0; i < left_nullqerror.size(); ++i)
558  {
559  if (fabs(left_nullqerror(i)) < 0.09)
560  {
561  left_nullqerror(i) = 0;
562  }
563 
564  if (fabs(right_nullqerror(i)) < 0.09)
565  {
566  right_nullqerror(i) = 0;
567  }
568  }
569 
570  Eigen::VectorXf left_nullspaceTorque =
571  -nullspaceKp * left_nullqerror - nullspaceDamping * left_jointVelocity_filtered;
572  Eigen::VectorXf right_nullspaceTorque =
573  -nullspaceKp * right_nullqerror - nullspaceDamping * right_jointVelocity_filtered;
574  Eigen::VectorXf left_projectedNullTorque =
575  (I - left_jacobi.transpose() * left_jtpinv) * left_nullspaceTorque;
576  Eigen::VectorXf right_projectedNullTorque =
577  (I - right_jacobi.transpose() * right_jtpinv) * right_nullspaceTorque;
578  float left_nulltorque_norm = left_projectedNullTorque.norm();
579  float right_nulltorque_norm = right_projectedNullTorque.norm();
580  if (left_nulltorque_norm > null_torqueLimit)
581  {
582  left_projectedNullTorque =
583  (null_torqueLimit / left_nulltorque_norm) * left_projectedNullTorque;
584  }
585  if (right_nulltorque_norm > null_torqueLimit)
586  {
587  right_projectedNullTorque =
588  (null_torqueLimit / right_nulltorque_norm) * right_projectedNullTorque;
589  }
590 
591  Eigen::VectorXf left_jointDesiredTorques =
592  left_jacobi.transpose() * left_tcpDesiredWrench + left_projectedNullTorque;
593  Eigen::VectorXf right_jointDesiredTorques =
594  right_jacobi.transpose() * right_tcpDesiredWrench + right_projectedNullTorque;
595  for (size_t i = 0; i < left_torque_targets.size(); ++i)
596  {
597  float desiredTorque = smooth_startup * left_jointDesiredTorques(i);
598  desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
599  left_desiredTorques_filtered(i) =
600  (1 - cfg->TorqueFilterConstant) * left_desiredTorques_filtered(i) +
601  cfg->TorqueFilterConstant * desiredTorque;
602 
603  std::string names = left_jointNames[i] + "_desiredTorques";
604  debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
605  names = names + "_filtered";
606  debugDataInfo.getWriteBuffer().desired_torques[names] = left_desiredTorques_filtered(i);
607 
608  if (fabs(left_desiredTorques_filtered(i)) > torqueLimit)
609  {
610  left_torque_targets.at(i)->torque = 0;
611  }
612  else
613  {
614  left_torque_targets.at(i)->torque =
615  left_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
616  }
617  }
618 
619  for (size_t i = 0; i < right_torque_targets.size(); ++i)
620  {
621  float desiredTorque = smooth_startup * right_jointDesiredTorques(i);
622  desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
623  right_desiredTorques_filtered(i) =
624  (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered(i) +
625  cfg->TorqueFilterConstant * desiredTorque;
626 
627  std::string names = right_jointNames[i] + "_desiredTorques";
628  debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
629  names = names + "_filtered";
630  debugDataInfo.getWriteBuffer().desired_torques[names] =
631  right_desiredTorques_filtered(i);
632 
633  if (fabs(right_desiredTorques_filtered(i)) > torqueLimit)
634  {
635  right_torque_targets.at(i)->torque = 0;
636  }
637  else
638  {
639  right_torque_targets.at(i)->torque =
640  right_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
641  }
642  }
643 
644  smooth_startup = smooth_startup + 0.001 * (1.1 - smooth_startup);
645  smooth_startup = (smooth_startup > 1) ? 1 : smooth_startup;
646  smooth_startup = (smooth_startup < 0) ? 0 : smooth_startup;
647 
648  debugDataInfo.commitWrite();
649  }
650 
651  void
652  DSJointCarryController::setGuardOrientation(const Ice::FloatSeq& guardOrientationInRobotBase,
653  const Ice::Current&)
654  {
655  Eigen::Quaternionf guardOri;
656  guardOri.w() = guardOrientationInRobotBase[0];
657  guardOri.x() = guardOrientationInRobotBase[1];
658  guardOri.y() = guardOrientationInRobotBase[2];
659  guardOri.z() = guardOrientationInRobotBase[3];
660 
661  LockGuardType guard{interface2CtrlDataMutex};
662  interface2CtrlData.getWriteBuffer().guardOriInRobotBase = guardOri;
663  interface2CtrlData.commitWrite();
664  }
665 
666  void
667  DSJointCarryController::setGuardInHandPosition(const Ice::FloatSeq& guardPositionToHandInMeter,
668  const Ice::Current&)
669  {
670  Eigen::Vector3f guardPosi;
671  guardPosi << guardPositionToHandInMeter[0], guardPositionToHandInMeter[1],
672  guardPositionToHandInMeter[2];
673 
674  LockGuardType guard{interface2CtrlDataMutex};
675  interface2CtrlData.getWriteBuffer().guardToHandInMeter = guardPosi;
676  interface2CtrlData.commitWrite();
677  }
678 
679  void
680  DSJointCarryController::setDesiredGuardOri(const Ice::FloatSeq& desiredOrientationInRobotBase,
681  const Ice::Current&)
682  {
683  Eigen::Quaternionf guardOri;
684  guardOri.w() = desiredOrientationInRobotBase[0];
685  guardOri.x() = desiredOrientationInRobotBase[1];
686  guardOri.y() = desiredOrientationInRobotBase[2];
687  guardOri.z() = desiredOrientationInRobotBase[3];
688 
689  LockGuardType guard{interface2CtrlDataMutex};
690  interface2CtrlData.getWriteBuffer().desiredGuardOriInRobotBase = guardOri;
691  interface2CtrlData.commitWrite();
692  }
693 
694  void
695  DSJointCarryController::setRotationStiffness(const Ice::FloatSeq& rotationStiffness,
696  const Ice::Current&)
697  {
698  Eigen::Vector3f rotStiffness;
699  rotStiffness << rotationStiffness[0], rotStiffness[1], rotStiffness[2];
700 
701  LockGuardType guard{interface2CtrlDataMutex};
702  interface2CtrlData.getWriteBuffer().guardRotationStiffness = rotStiffness;
703  interface2CtrlData.commitWrite();
704  }
705 
706  void
707  DSJointCarryController::setGuardObsAvoidVel(const Ice::FloatSeq& guardObsAvoidVel,
708  const Ice::Current&)
709  {
710  LockGuardType guard{interface2CtrlDataMutex};
711  interface2CtrlData.getWriteBuffer().guardObsAvoidVel(0) = guardObsAvoidVel[0];
712  interface2CtrlData.getWriteBuffer().guardObsAvoidVel(1) = guardObsAvoidVel[1];
713  interface2CtrlData.getWriteBuffer().guardObsAvoidVel(2) = guardObsAvoidVel[2];
714  interface2CtrlData.commitWrite();
715  }
716 
717  float
718  DSJointCarryController::getGMMVel(const Ice::Current&)
719  {
720  float gmmZVel = ctrl2InterfaceData.getUpToDateReadBuffer().guardZVel;
721  return gmmZVel;
722  }
723 
724  float
725  DSJointCarryController::deadzone(float input, float disturbance, float threshold)
726  {
727  if (input > 0)
728  {
729  input = input - disturbance;
730  input = (input < 0) ? 0 : input;
731  input = (input > threshold) ? threshold : input;
732  }
733  else if (input < 0)
734  {
735  input = input + disturbance;
736  input = (input > 0) ? 0 : input;
737  input = (input < -threshold) ? -threshold : input;
738  }
739 
740 
741  return input;
742  }
743 
744  Eigen::Quaternionf
745  DSJointCarryController::quatSlerp(double t,
746  const Eigen::Quaternionf& q0,
747  const Eigen::Quaternionf& q1)
748  {
749  double cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
750 
751 
752  Eigen::Quaternionf q1x = q1;
753  if (cosHalfTheta < 0)
754  {
755  q1x.w() = -q1.w();
756  q1x.x() = -q1.x();
757  q1x.y() = -q1.y();
758  q1x.z() = -q1.z();
759  }
760 
761 
762  if (fabs(cosHalfTheta) >= 1.0)
763  {
764  return q0;
765  }
766 
767  double halfTheta = acos(cosHalfTheta);
768  double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
769 
770 
771  Eigen::Quaternionf result;
772  if (fabs(sinHalfTheta) < 0.001)
773  {
774  result.w() = (1 - t) * q0.w() + t * q1x.w();
775  result.x() = (1 - t) * q0.x() + t * q1x.x();
776  result.y() = (1 - t) * q0.y() + t * q1x.y();
777  result.z() = (1 - t) * q0.z() + t * q1x.z();
778  }
779  else
780  {
781  double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
782  double ratioB = sin(t * halfTheta) / sinHalfTheta;
783 
784  result.w() = ratioA * q0.w() + ratioB * q1x.w();
785  result.x() = ratioA * q0.x() + ratioB * q1x.x();
786  result.y() = ratioA * q0.y() + ratioB * q1x.y();
787  result.z() = ratioA * q0.z() + ratioB * q1x.z();
788  }
789 
790  return result;
791  }
792 
793  void
794  DSJointCarryController::onPublish(const SensorAndControl&,
795  const DebugDrawerInterfacePrx&,
796  const DebugObserverInterfacePrx& debugObs)
797  {
798 
799  StringVariantBaseMap datafields;
800  auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
801  for (auto& pair : values)
802  {
803  datafields[pair.first] = new Variant(pair.second);
804  }
805 
806 
807  datafields["leftDesiredLinearVelocity_x"] =
808  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[0]);
809  datafields["leftDesiredLinearVelocity_y"] =
810  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[1]);
811  datafields["leftDesiredLinearVelocity_z"] =
812  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[2]);
813  datafields["rightDesiredLinearVelocity_x"] =
814  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[0]);
815  datafields["rightDesiredLinearVelocity_y"] =
816  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[1]);
817  datafields["rightDesiredLinearVelocity_z"] =
818  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[2]);
819  debugObs->setDebugChannel("DSJointCarry", datafields);
820  }
821 
822  void
823  DSJointCarryController::rtPreActivateController()
824  {
825  }
826 
827  void
828  DSJointCarryController::rtPostDeactivateController()
829  {
830  }
831 } // namespace armarx::control::ds_controller
armarx::NJointControllerWithTripleBuffer< DSJointCarryControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const DSJointCarryControllerControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::ControlTargetBase::asA
const T * asA() const
Definition: ControlTargetBase.h:76
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:223
armarx::control::ds_controller::DSJointCarryControllerControlData::rightDesiredLinearVelocity
Eigen::Vector3f rightDesiredLinearVelocity
Definition: DSJointCarryController.h:160
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::NJointControllerWithTripleBuffer< DSJointCarryControllerControlData >::rtGetControlStruct
const DSJointCarryControllerControlData & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
armarx::SensorValueBase
The SensorValueBase class.
Definition: SensorValueBase.h:40
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition: ControlTargetBase.h:47
armarx::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:163
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::NJointControllerBase::useControlTarget
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition: NJointController.cpp:410
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
armarx::ManagedIceObject::getState
int getState() const
Retrieve current state of the ManagedIceObject.
Definition: ManagedIceObject.cpp:769
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::control::ds_controller
Definition: DSJointCarryController.cpp:32
armarx::control::ds_controller::DSJointCarryController::setDesiredGuardOri
void setDesiredGuardOri(const Ice::FloatSeq &desiredOrientationInRobotBase, const Ice::Current &)
Definition: DSJointCarryController.cpp:680
armarx::control::ds_controller::DSJointCarryController::setGuardOrientation
void setGuardOrientation(const Ice::FloatSeq &guardOrientationInRobotBase, const Ice::Current &)
Definition: DSJointCarryController.cpp:652
armarx::NJointControllerWithTripleBuffer< DSJointCarryControllerControlData >::getWriterControlStruct
DSJointCarryControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::ds_controller::DSJointCarryController::onPublish
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition: DSJointCarryController.cpp:794
armarx::NJointControllerWithTripleBuffer< DSJointCarryControllerControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::control::ds_controller::DSJointCarryController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition: DSJointCarryController.cpp:823
armarx::control::ds_controller::DSJointCarryController::onInitNJointController
void onInitNJointController()
Definition: DSJointCarryController.cpp:39
armarx::control::ds_controller::DSJointCarryController::DSJointCarryController
DSJointCarryController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: DSJointCarryController.cpp:75
armarx::SensorValueForceTorque::force
Eigen::Vector3f force
Definition: SensorValueForceTorque.h:36
armarx::DSJointCarryControllerInterface::getGMMVel
float getGMMVel()
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:846
armarx::control::ds_controller::DSJointCarryController::controllerRun
void controllerRun()
Definition: DSJointCarryController.cpp:303
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::control::ds_controller::JointCarryGMMMotionGen
Definition: DSJointCarryController.h:57
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition: NJointControllerBase.h:778
armarx::NJointControllerBase::runTask
void runTask(const std::string &taskName, Task &&task)
Executes a given task in a separate thread from the Application ThreadPool.
Definition: NJointControllerBase.h:754
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:12
armarx::NJointControllerWithTripleBuffer< DSJointCarryControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:108
armarx::control::ds_controller::DSJointCarryControllerControlData::leftDesiredLinearVelocity
Eigen::Vector3f leftDesiredLinearVelocity
Definition: DSJointCarryController.h:158
ArmarXObjectScheduler.h
armarx::control::ds_controller::DSJointCarryControllerControlData::rightDesiredAngularError
Eigen::Vector3f rightDesiredAngularError
Definition: DSJointCarryController.h:161
armarx::control::ds_controller::DSJointCarryController::rtPostDeactivateController
void rtPostDeactivateController()
This function is called after the controller is deactivated.
Definition: DSJointCarryController.cpp:828
armarx::NJointControllerWithTripleBuffer< DSJointCarryControllerControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
CycleUtil.h
armarx::control::ds_controller::registrationControllerDSJointCarryController
NJointControllerRegistration< DSJointCarryController > registrationControllerDSJointCarryController("DSJointCarryController")
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:704
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::viz::data::ElementFlags::names
const simox::meta::IntEnumNames names
Definition: json_elements.cpp:13
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::control::ds_controller::DSJointCarryController::setGuardObsAvoidVel
void setGuardObsAvoidVel(const Ice::FloatSeq &guardVel, const Ice::Current &)
Definition: DSJointCarryController.cpp:707
armarx::Quaternion< float, 0 >
armarx::control::ds_controller::DSJointCarryController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition: DSJointCarryController.cpp:419
IceUtil::Handle< class RobotUnit >
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::control::ds_controller::DSJointCarryControllerControlData::leftDesiredAngularError
Eigen::Vector3f leftDesiredAngularError
Definition: DSJointCarryController.h:159
armarx::control::ds_controller::DSJointCarryController::setRotationStiffness
void setRotationStiffness(const Ice::FloatSeq &rotationStiffness, const Ice::Current &)
Definition: DSJointCarryController.cpp:695
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:99
Eigen::Matrix< float, 6, 1 >
armarx::control::ds_controller::DSJointCarryController::setGuardInHandPosition
void setGuardInHandPosition(const Ice::FloatSeq &guardPositionToHandInMeter, const Ice::Current &)
Definition: DSJointCarryController.cpp:667
armarx::control::ds_controller::DSJointCarryController::onDisconnectNJointController
void onDisconnectNJointController()
Definition: DSJointCarryController.cpp:64
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
armarx::TripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:116
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::SensorValueForceTorque
Definition: SensorValueForceTorque.h:32
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::TripleBuffer::reinitAllBuffers
std::enable_if< std::is_copy_constructible< U >::value >::type reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:171
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:776
armarx::control::ds_controller::DSJointCarryControllerControlData
Definition: DSJointCarryController.h:155
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
DSJointCarryController.h
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
armarx::TripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:143