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