DSRTBimanualController.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::DSRTBimanualController
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 "DSRTBimanualController.h"
24 
25 #include <VirtualRobot/RobotNodeSet.h>
26 #include <VirtualRobot/Tools/Gravity.h>
27 #include <VirtualRobot/IK/DifferentialIK.h>
28 
29 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
30 #include <ArmarXCore/core/time/CycleUtil.h>
31 
32 namespace armarx::control::ds_controller
33 {
34  NJointControllerRegistration<DSRTBimanualController>
35  registrationControllerDSRTBimanualController("DSRTBimanualController");
36 
37  void
38  DSRTBimanualController::onInitNJointController()
39  {
40  ARMARX_INFO << "init ...";
41  controllerStopRequested = false;
42  controllerRunFinished = false;
43  runTask("DSRTBimanualControllerTask",
44  [&]
45  {
46  CycleUtil c(1);
47  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
48  while (getState() == eManagedIceObjectStarted && !controllerStopRequested)
49  {
50  ARMARX_VERBOSE << deactivateSpam(1) << "control fct";
51  if (isControllerActive())
52  {
53  controllerRun();
54  }
55  c.waitForCycleDuration();
56  }
57  controllerRunFinished = true;
58  ARMARX_INFO << "Control Fct finished";
59  });
60  }
61 
62  void
63  DSRTBimanualController::onDisconnectNJointController()
64  {
65  ARMARX_INFO << "disconnect";
66  controllerStopRequested = true;
67  while (!controllerRunFinished)
68  {
69  ARMARX_INFO << deactivateSpam(1) << "waiting for run function";
70  usleep(10000);
71  }
72  }
73 
74  DSRTBimanualController::DSRTBimanualController(const RobotUnitPtr& robotUnit,
75  const armarx::NJointControllerConfigPtr& config,
76  const VirtualRobot::RobotPtr&)
77  {
78  cfg = DSRTBimanualControllerConfigPtr::dynamicCast(config);
79  useSynchronizedRtRobot();
80 
81  VirtualRobot::RobotNodeSetPtr left_ns = rtGetRobot()->getRobotNodeSet("LeftArm");
82  VirtualRobot::RobotNodeSetPtr right_ns = rtGetRobot()->getRobotNodeSet("RightArm");
83 
84  left_jointNames.clear();
85  right_jointNames.clear();
86 
87  ARMARX_CHECK_EXPRESSION(left_ns) << "LeftArm";
88  ARMARX_CHECK_EXPRESSION(right_ns) << "RightArm";
89 
90  // for left arm
91  for (size_t i = 0; i < left_ns->getSize(); ++i)
92  {
93  std::string jointName = left_ns->getNode(i)->getName();
94  left_jointNames.push_back(jointName);
95  ControlTargetBase* ct = useControlTarget(
96  jointName,
97  ControlModes::
98  Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
99  ARMARX_CHECK_EXPRESSION(ct);
100  const SensorValueBase* sv = useSensorValue(jointName);
101  ARMARX_CHECK_EXPRESSION(sv);
102  auto casted_ct = ct->asA<
103  ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
104  ARMARX_CHECK_EXPRESSION(casted_ct);
105  left_torque_targets.push_back(casted_ct);
106 
107  const SensorValue1DoFActuatorTorque* torqueSensor =
108  sv->asA<SensorValue1DoFActuatorTorque>();
109  const SensorValue1DoFActuatorVelocity* velocitySensor =
110  sv->asA<SensorValue1DoFActuatorVelocity>();
111  const SensorValue1DoFGravityTorque* gravityTorqueSensor =
112  sv->asA<SensorValue1DoFGravityTorque>();
113  const SensorValue1DoFActuatorPosition* positionSensor =
114  sv->asA<SensorValue1DoFActuatorPosition>();
115  if (!torqueSensor)
116  {
117  ARMARX_WARNING << "No Torque sensor available for " << jointName;
118  }
119  if (!gravityTorqueSensor)
120  {
121  ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
122  }
123 
124  left_torqueSensors.push_back(torqueSensor);
125  left_gravityTorqueSensors.push_back(gravityTorqueSensor);
126  left_velocitySensors.push_back(velocitySensor);
127  left_positionSensors.push_back(positionSensor);
128  };
129 
130  // for right arm
131  for (size_t i = 0; i < right_ns->getSize(); ++i)
132  {
133  std::string jointName = right_ns->getNode(i)->getName();
134  right_jointNames.push_back(jointName);
135  ControlTargetBase* ct = useControlTarget(
136  jointName,
137  ControlModes::
138  Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
139  ARMARX_CHECK_EXPRESSION(ct);
140  const SensorValueBase* sv = useSensorValue(jointName);
141  ARMARX_CHECK_EXPRESSION(sv);
142  auto casted_ct = ct->asA<
143  ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
144  ARMARX_CHECK_EXPRESSION(casted_ct);
145  right_torque_targets.push_back(casted_ct);
146 
147  const SensorValue1DoFActuatorTorque* torqueSensor =
148  sv->asA<SensorValue1DoFActuatorTorque>();
149  const SensorValue1DoFActuatorVelocity* velocitySensor =
150  sv->asA<SensorValue1DoFActuatorVelocity>();
151  const SensorValue1DoFGravityTorque* gravityTorqueSensor =
152  sv->asA<SensorValue1DoFGravityTorque>();
153  const SensorValue1DoFActuatorPosition* positionSensor =
154  sv->asA<SensorValue1DoFActuatorPosition>();
155  if (!torqueSensor)
156  {
157  ARMARX_WARNING << "No Torque sensor available for " << jointName;
158  }
159  if (!gravityTorqueSensor)
160  {
161  ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
162  }
163 
164  right_torqueSensors.push_back(torqueSensor);
165  right_gravityTorqueSensors.push_back(gravityTorqueSensor);
166  right_velocitySensors.push_back(velocitySensor);
167  right_positionSensors.push_back(positionSensor);
168  };
169 
170 
171  const SensorValueBase* svlf = useSensorValue("FT L");
172  leftForceTorque = svlf->asA<SensorValueForceTorque>();
173  const SensorValueBase* svrf = useSensorValue("FT R");
174  rightForceTorque = svrf->asA<SensorValueForceTorque>();
175 
176  ARMARX_INFO << "Initialized " << left_torque_targets.size() << " targets for the left arm";
177  ARMARX_INFO << "Initialized " << right_torque_targets.size()
178  << " targets for the right arm";
179 
180  left_arm_tcp = left_ns->getTCP();
181  right_arm_tcp = right_ns->getTCP();
182 
183  left_sensor_frame = left_ns->getRobot()->getRobotNode("ArmL_FT");
184  right_sensor_frame = right_ns->getRobot()->getRobotNode("ArmR_FT");
185 
186  left_ik.reset(new VirtualRobot::DifferentialIK(
187  left_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
188  right_ik.reset(new VirtualRobot::DifferentialIK(
189  right_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
190 
191 
192  // ?? not sure about this
193  DSRTBimanualControllerSensorData initSensorData;
194 
195  initSensorData.left_tcpPose = left_arm_tcp->getPoseInRootFrame();
196  initSensorData.currentTime = 0;
197  controllerSensorData.reinitAllBuffers(initSensorData);
198 
199  initSensorData.right_tcpPose = right_arm_tcp->getPoseInRootFrame();
200  initSensorData.currentTime = 0;
201  controllerSensorData.reinitAllBuffers(initSensorData);
202 
203 
204  left_oldPosition = left_arm_tcp->getPositionInRootFrame();
205  left_oldOrientation = left_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
206 
207  right_oldPosition = right_arm_tcp->getPositionInRootFrame();
208  right_oldOrientation = right_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
209 
210 
211  std::vector<float> left_desiredQuaternionVec = cfg->left_desiredQuaternion;
212  ARMARX_CHECK_EQUAL(left_desiredQuaternionVec.size(), 4);
213 
214  std::vector<float> right_desiredQuaternionVec = cfg->right_desiredQuaternion;
215  ARMARX_CHECK_EQUAL(right_desiredQuaternionVec.size(), 4);
216 
217  left_desiredQuaternion.w() = left_desiredQuaternionVec.at(0);
218  left_desiredQuaternion.x() = left_desiredQuaternionVec.at(1);
219  left_desiredQuaternion.y() = left_desiredQuaternionVec.at(2);
220  left_desiredQuaternion.z() = left_desiredQuaternionVec.at(3);
221 
222  right_desiredQuaternion.w() = right_desiredQuaternionVec.at(0);
223  right_desiredQuaternion.x() = right_desiredQuaternionVec.at(1);
224  right_desiredQuaternion.y() = right_desiredQuaternionVec.at(2);
225  right_desiredQuaternion.z() = right_desiredQuaternionVec.at(3);
226 
227 
228  // set initial joint velocities filter
229 
230  left_jointVelocity_filtered.resize(left_torque_targets.size());
231  left_jointVelocity_filtered.setZero();
232  right_jointVelocity_filtered.resize(left_torque_targets.size());
233  right_jointVelocity_filtered.setZero();
234 
235  // do we need to duplicate this?
236  DSRTBimanualControllerControlData initData;
237  for (size_t i = 0; i < 3; ++i)
238  {
239  initData.left_tcpDesiredLinearVelocity(i) = 0;
240  initData.right_tcpDesiredLinearVelocity(i) = 0;
241  }
242 
243  for (size_t i = 0; i < 3; ++i)
244  {
245  initData.left_tcpDesiredAngularError(i) = 0;
246  initData.right_tcpDesiredAngularError(i) = 0;
247  }
248  reinitTripleBuffer(initData);
249 
250 
251  // initial filter
252  left_desiredTorques_filtered.resize(left_torque_targets.size());
253  left_desiredTorques_filtered.setZero();
254  right_desiredTorques_filtered.resize(right_torque_targets.size());
255  right_desiredTorques_filtered.setZero();
256 
257 
258  left_currentTCPLinearVelocity_filtered.setZero();
259  right_currentTCPLinearVelocity_filtered.setZero();
260 
261  left_currentTCPAngularVelocity_filtered.setZero();
262  right_currentTCPAngularVelocity_filtered.setZero();
263 
264  left_tcpDesiredTorque_filtered.setZero();
265  right_tcpDesiredTorque_filtered.setZero();
266 
267 
268  smooth_startup = 0;
269 
270 
271  filterTimeConstant = cfg->filterTimeConstant;
272  posiKp = cfg->posiKp;
273  v_max = cfg->v_max;
274  Damping = cfg->posiDamping;
275  Coupling_stiffness = cfg->couplingStiffness;
276  Coupling_force_limit = cfg->couplingForceLimit;
277  forward_gain = cfg->forwardGain;
278  torqueLimit = cfg->torqueLimit;
279  null_torqueLimit = cfg->NullTorqueLimit;
280  oriKp = cfg->oriKp;
281  oriDamping = cfg->oriDamping;
282  contactForce = cfg->contactForce;
283 
284  left_oriUp.w() = cfg->left_oriUp[0];
285  left_oriUp.x() = cfg->left_oriUp[1];
286  left_oriUp.y() = cfg->left_oriUp[2];
287  left_oriUp.z() = cfg->left_oriUp[3];
288 
289  left_oriDown.w() = cfg->left_oriDown[0];
290  left_oriDown.x() = cfg->left_oriDown[1];
291  left_oriDown.y() = cfg->left_oriDown[2];
292  left_oriDown.z() = cfg->left_oriDown[3];
293 
294  right_oriUp.w() = cfg->right_oriUp[0];
295  right_oriUp.x() = cfg->right_oriUp[1];
296  right_oriUp.y() = cfg->right_oriUp[2];
297  right_oriUp.z() = cfg->right_oriUp[3];
298 
299  right_oriDown.w() = cfg->right_oriDown[0];
300  right_oriDown.x() = cfg->right_oriDown[1];
301  right_oriDown.y() = cfg->right_oriDown[2];
302  right_oriDown.z() = cfg->right_oriDown[3];
303 
304 
305  guardTargetZUp = cfg->guardTargetZUp;
306  guardTargetZDown = cfg->guardTargetZDown;
307  guardDesiredZ = guardTargetZUp;
308  guard_mounting_correction_z = 0;
309 
310  guardXYHighFrequency = 0;
311  left_force_old.setZero();
312  right_force_old.setZero();
313 
314  left_contactForceZ_correction = 0;
315  right_contactForceZ_correction = 0;
316 
317 
318  std::vector<float> leftarm_qnullspaceVec = cfg->leftarm_qnullspaceVec;
319  std::vector<float> rightarm_qnullspaceVec = cfg->rightarm_qnullspaceVec;
320 
321  left_qnullspace.resize(leftarm_qnullspaceVec.size());
322  right_qnullspace.resize(rightarm_qnullspaceVec.size());
323 
324  for (size_t i = 0; i < leftarm_qnullspaceVec.size(); ++i)
325  {
326  left_qnullspace(i) = leftarm_qnullspaceVec[i];
327  right_qnullspace(i) = rightarm_qnullspaceVec[i];
328  }
329 
330  nullspaceKp = cfg->nullspaceKp;
331  nullspaceDamping = cfg->nullspaceDamping;
332 
333 
334  //set GMM parameters ...
335  if (cfg->gmmParamsFile == "")
336  {
337  ARMARX_ERROR << "gmm params file cannot be empty string ...";
338  }
339  gmmMotionGenerator.reset(new BimanualGMMMotionGen(cfg->gmmParamsFile));
340  positionErrorTolerance = cfg->positionErrorTolerance;
341  forceFilterCoeff = cfg->forceFilterCoeff;
342  for (size_t i = 0; i < 3; ++i)
343  {
344  leftForceOffset[i] = cfg->forceLeftOffset.at(i);
345  rightForceOffset[i] = cfg->forceRightOffset.at(i);
346  }
347  isDefaultTarget = false;
348  ARMARX_INFO << "Initialization done";
349  }
350 
351  void
352  DSRTBimanualController::controllerRun()
353  {
354  if (!controllerSensorData.updateReadBuffer())
355  {
356  return;
357  }
358 
359 
360  // receive the measurements
361  Eigen::Matrix4f left_currentTCPPose = controllerSensorData.getReadBuffer().left_tcpPose;
362  Eigen::Matrix4f right_currentTCPPose = controllerSensorData.getReadBuffer().right_tcpPose;
363 
364  Eigen::Vector3f left_force = controllerSensorData.getReadBuffer().left_force;
365  Eigen::Vector3f right_force = controllerSensorData.getReadBuffer().right_force;
366  Eigen::Vector3f both_arm_force_ave =
367  -0.5 * (left_force + right_force); // negative sign for the direction
368 
369 
370  // float left_force_z = left_force(2);
371  // float right_force_z = right_force(2);
372 
373  Eigen::Vector3f left_currentTCPPositionInMeter;
374  Eigen::Vector3f right_currentTCPPositionInMeter;
375 
376  left_currentTCPPositionInMeter << left_currentTCPPose(0, 3), left_currentTCPPose(1, 3),
377  left_currentTCPPose(2, 3);
378  right_currentTCPPositionInMeter << right_currentTCPPose(0, 3), right_currentTCPPose(1, 3),
379  right_currentTCPPose(2, 3);
380 
381  left_currentTCPPositionInMeter = 0.001 * left_currentTCPPositionInMeter;
382  right_currentTCPPositionInMeter = 0.001 * right_currentTCPPositionInMeter;
383 
384 
385  // updating the desired height for the guard based on the interaction forces
386  float both_arm_height_z_ave =
387  0.5 * (left_currentTCPPositionInMeter(2) + right_currentTCPPositionInMeter(2));
388 
389 
390  float adaptive_ratio = 1;
391  float force_error_z = 0;
392  float guard_mounting_correction_x = 0;
393  float guard_mounting_correction_y = 0;
394 
395 
396  if (cfg->guardDesiredDirection)
397  {
398  adaptive_ratio = 1;
399  force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->liftingForce;
400 
401  // meausures the high-frequency components of the interaction forces
402  float diff_norm = (left_force - left_force_old).squaredNorm() +
403  (right_force - right_force_old).squaredNorm();
404  // diff_norm = deadzone(diff_norm,0.1,2);
405  guardXYHighFrequency =
406  cfg->highPassFilterFactor * guardXYHighFrequency + 0.1 * diff_norm;
407 
408  guardXYHighFrequency = (guardXYHighFrequency > 10) ? 10 : guardXYHighFrequency;
409 
410  left_force_old = left_force;
411  right_force_old = right_force;
412 
413  if (fabs(both_arm_height_z_ave - guardTargetZUp) < cfg->mountingDistanceTolerance)
414  {
415  guard_mounting_correction_z =
416  (1 - cfg->mountingCorrectionFilterFactor) * guard_mounting_correction_z +
417  cfg->mountingCorrectionFilterFactor * (-0.1 * (guardXYHighFrequency - 8));
418  guard_mounting_correction_z = deadzone(guard_mounting_correction_z, 0, 0.1);
419 
420 
421  guard_mounting_correction_x =
422  guard_mounting_correction_x -
423  cfg->forceErrorZGain * deadzone(both_arm_force_ave(0), 1, 3);
424  guard_mounting_correction_y =
425  guard_mounting_correction_y -
426  cfg->forceErrorZGain * deadzone(both_arm_force_ave(1), 1, 3);
427 
428  guard_mounting_correction_x = deadzone(guard_mounting_correction_x, 0, 0.1);
429  guard_mounting_correction_y = deadzone(guard_mounting_correction_y, 0, 0.1);
430  }
431  }
432  else
433  {
434  adaptive_ratio = (0.5 * (both_arm_height_z_ave - guardTargetZDown) /
435  (guardTargetZUp - guardTargetZDown) +
436  0.5);
437  force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->loweringForce;
438  }
439 
440 
441  force_error_z =
442  deadzone(force_error_z, cfg->forceErrorZDisturbance, cfg->forceErrorZThreshold);
443  guardDesiredZ = guardDesiredZ - cfg->forceErrorZGain * (force_error_z);
444 
445  guardDesiredZ = (guardDesiredZ > guardTargetZUp) ? guardTargetZUp : guardDesiredZ;
446  guardDesiredZ = (guardDesiredZ < guardTargetZDown) ? guardTargetZDown : guardDesiredZ;
447 
448  if (isDefaultTarget)
449  {
450  guardDesiredZ = guardTargetZDown;
451  }
452 
453  if (!cfg->guardDesiredDirection && guardDesiredZ < 1.5)
454  {
455  guard_mounting_correction_y = -0.1;
456  }
457 
458  // update the desired velocity
459  gmmMotionGenerator->updateDesiredVelocity(left_currentTCPPositionInMeter,
460  right_currentTCPPositionInMeter,
461  positionErrorTolerance,
462  guardDesiredZ,
463  guard_mounting_correction_x,
464  guard_mounting_correction_y,
465  guard_mounting_correction_z);
466 
467  // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->left_DS_DesiredVelocity;
468  // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->right_DS_DesiredVelocity;
469 
470 
471  Eigen::Vector3f left_tcpDesiredAngularError;
472  Eigen::Vector3f right_tcpDesiredAngularError;
473 
474  left_tcpDesiredAngularError << 0, 0, 0;
475  right_tcpDesiredAngularError << 0, 0, 0;
476 
477 
478  Eigen::Matrix3f left_currentRotMat = left_currentTCPPose.block<3, 3>(0, 0);
479  Eigen::Matrix3f right_currentRotMat = right_currentTCPPose.block<3, 3>(0, 0);
480 
481 
482  float lqratio = (left_currentTCPPositionInMeter(2) - guardTargetZDown) /
483  (guardTargetZUp - guardTargetZDown);
484  float rqratio = (right_currentTCPPositionInMeter(2) - guardTargetZDown) /
485  (guardTargetZUp - guardTargetZDown);
486 
487  lqratio = (lqratio > 1) ? 1 : lqratio;
488  lqratio = (lqratio < 0) ? 0 : lqratio;
489  rqratio = (rqratio > 1) ? 1 : rqratio;
490  rqratio = (rqratio < 0) ? 0 : rqratio;
491 
492 
493  Eigen::Quaternionf left_desiredQuaternion = quatSlerp(lqratio, left_oriDown, left_oriUp);
494  Eigen::Quaternionf right_desiredQuaternion = quatSlerp(rqratio, right_oriDown, right_oriUp);
495 
496 
497  Eigen::Matrix3f left_desiredRotMat = left_desiredQuaternion.normalized().toRotationMatrix();
498  Eigen::Matrix3f right_desiredRotMat =
499  right_desiredQuaternion.normalized().toRotationMatrix();
500 
501  Eigen::Matrix3f left_orientationError = left_currentRotMat * left_desiredRotMat.inverse();
502  Eigen::Matrix3f right_orientationError =
503  right_currentRotMat * right_desiredRotMat.inverse();
504 
505  Eigen::Quaternionf left_diffQuaternion(left_orientationError);
506  Eigen::Quaternionf right_diffQuaternion(right_orientationError);
507 
508  Eigen::AngleAxisf left_angleAxis(left_diffQuaternion);
509  Eigen::AngleAxisf right_angleAxis(right_diffQuaternion);
510 
511  left_tcpDesiredAngularError = left_angleAxis.angle() * left_angleAxis.axis();
512  right_tcpDesiredAngularError = right_angleAxis.angle() * right_angleAxis.axis();
513 
514 
515  // writing to the buffer
516  getWriterControlStruct().left_tcpDesiredLinearVelocity =
517  gmmMotionGenerator->left_DS_DesiredVelocity;
518  getWriterControlStruct().right_tcpDesiredLinearVelocity =
519  gmmMotionGenerator->right_DS_DesiredVelocity;
520  getWriterControlStruct().left_right_distanceInMeter =
521  gmmMotionGenerator->left_right_position_errorInMeter;
522 
523  getWriterControlStruct().left_tcpDesiredAngularError = left_tcpDesiredAngularError;
524  getWriterControlStruct().right_tcpDesiredAngularError = right_tcpDesiredAngularError;
525 
526  writeControlStruct();
527  debugCtrlDataInfo.getWriteBuffer().desredZ = guardDesiredZ;
528  debugCtrlDataInfo.getWriteBuffer().force_error_z = force_error_z;
529  debugCtrlDataInfo.getWriteBuffer().guardXYHighFrequency = guardXYHighFrequency;
530  debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_x =
531  guard_mounting_correction_x;
532  debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_y =
533  guard_mounting_correction_y;
534  debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_z =
535  guard_mounting_correction_z;
536  debugCtrlDataInfo.commitWrite();
537  }
538 
539  void
540  DSRTBimanualController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
541  const IceUtil::Time& timeSinceLastIteration)
542  {
543  deltaT = timeSinceLastIteration.toSecondsDouble();
544 
545  /// measure the sesor data, update robot status for the other threads
546  left_forceInRoot = left_sensor_frame->getPoseInRootFrame().block<3, 3>(0, 0) *
547  (leftForceTorque->force - leftForceOffset);
548  right_forceInRoot = right_sensor_frame->getPoseInRootFrame().block<3, 3>(0, 0) *
549  (rightForceTorque->force - rightForceOffset);
550  left_forceInRoot(2) = left_forceInRoot(2) + cfg->leftForceOffsetZ; // - 4 + 8.5;
551  right_forceInRoot(2) = right_forceInRoot(2) + cfg->rightForceOffsetZ; // + 30 - 5.2;
552 
553 
554  left_currentTCPPose = left_arm_tcp->getPoseInRootFrame();
555  right_currentTCPPose = right_arm_tcp->getPoseInRootFrame();
556 
557  left_jacobi = left_ik->getJacobianMatrix(left_arm_tcp,
558  VirtualRobot::IKSolver::CartesianSelection::All);
559  right_jacobi = right_ik->getJacobianMatrix(right_arm_tcp,
560  VirtualRobot::IKSolver::CartesianSelection::All);
561 
562  left_qpos.resize(left_positionSensors.size());
563  left_qvel.resize(left_velocitySensors.size());
564 
565  right_qpos.resize(right_positionSensors.size());
566  right_qvel.resize(right_velocitySensors.size());
567 
568  for (size_t i = 0; i < left_velocitySensors.size(); ++i)
569  {
570  left_qpos(i) = left_positionSensors[i]->position;
571  left_qvel(i) = left_velocitySensors[i]->velocity;
572 
573  right_qpos(i) = right_positionSensors[i]->position;
574  right_qvel(i) = right_velocitySensors[i]->velocity;
575  }
576 
577  left_tcptwist = left_jacobi * left_qvel;
578  right_tcptwist = right_jacobi * right_qvel;
579 
580  left_tcptwist.head<3>() *= 0.001;
581  right_tcptwist.head<3>() *= 0.001;
582 
583 
584  double filterFactor = deltaT / (filterTimeConstant + deltaT);
585  left_currentTCPLinearVelocity_filtered =
586  (1 - filterFactor) * left_currentTCPLinearVelocity_filtered +
587  filterFactor * left_tcptwist.head<3>();
588  right_currentTCPLinearVelocity_filtered =
589  (1 - filterFactor) * right_currentTCPLinearVelocity_filtered +
590  filterFactor * right_tcptwist.head<3>();
591  left_currentTCPAngularVelocity_filtered =
592  (1 - filterFactor) * left_currentTCPAngularVelocity_filtered + left_tcptwist.tail<3>();
593  right_currentTCPAngularVelocity_filtered =
594  (1 - filterFactor) * right_currentTCPAngularVelocity_filtered + right_tcptwist.tail<3>();
595  left_jointVelocity_filtered =
596  (1 - filterFactor) * left_jointVelocity_filtered + filterFactor * left_qvel;
597  right_jointVelocity_filtered =
598  (1 - filterFactor) * right_jointVelocity_filtered + filterFactor * right_qvel;
599 
600  /// writing into the bufffer for other threads
601  controllerSensorData.getWriteBuffer().left_tcpPose = left_currentTCPPose;
602  controllerSensorData.getWriteBuffer().right_tcpPose = right_currentTCPPose;
603  controllerSensorData.getWriteBuffer().left_force = left_forceInRoot;
604  controllerSensorData.getWriteBuffer().right_force = right_forceInRoot;
605  controllerSensorData.getWriteBuffer().currentTime += deltaT;
606  controllerSensorData.commitWrite();
607 
608  // inverse dynamics:
609 
610  // reading the tcp orientation
611 
612  /// computing the task-specific forces
613  left_DS_force = -(1.2 * left_currentTCPLinearVelocity_filtered -
614  forward_gain * rtGetControlStruct().left_tcpDesiredLinearVelocity);
615  right_DS_force = -(1.2 * right_currentTCPLinearVelocity_filtered -
616  forward_gain * rtGetControlStruct().right_tcpDesiredLinearVelocity);
617  for (int i = 0; i < 3; ++i)
618  {
619  left_DS_force(i) = left_DS_force(i) * Damping[i];
620  right_DS_force(i) = right_DS_force(i) * Damping[i];
621 
622  left_DS_force(i) = deadzone(left_DS_force(i), 0.1, 100);
623  right_DS_force(i) = deadzone(right_DS_force(i), 0.1, 100);
624  }
625 
626  /// computing coupling forces
627  coupling_force = -Coupling_stiffness * rtGetControlStruct().left_right_distanceInMeter;
628  float coupling_force_norm = coupling_force.norm();
629 
630  if (coupling_force_norm > Coupling_force_limit)
631  {
632  coupling_force = (Coupling_force_limit / coupling_force_norm) * coupling_force;
633  }
634 
635  coupling_force(0) = deadzone(coupling_force(0), 0.1, 100);
636  coupling_force(1) = deadzone(coupling_force(1), 0.1, 100);
637  coupling_force(2) = deadzone(coupling_force(2), 0.1, 100);
638 
639 
640  double v_both = left_currentTCPLinearVelocity_filtered.norm() +
641  right_currentTCPLinearVelocity_filtered.norm();
642  float force_contact_switch = 0;
643  float left_height = left_currentTCPPose(2, 3) * 0.001;
644  float right_height = right_currentTCPPose(2, 3) * 0.001;
645 
646  float disTolerance = cfg->contactDistanceTolerance;
647  bool isUp = fabs(left_height - guardTargetZUp) < disTolerance &&
648  fabs(right_height - guardTargetZUp) < disTolerance;
649  if (v_both < disTolerance && isUp)
650  {
651  force_contact_switch = 1;
652  }
653  else if (v_both < 0.05 && isUp)
654  {
655  force_contact_switch = (0.05 - v_both) / (0.05 - disTolerance);
656  }
657  else if (v_both >= 0.05)
658  {
659  force_contact_switch = 0;
660  }
661 
662  /// computing for contact forces
663  float left_force_error = force_contact_switch * (-left_forceInRoot(2) - contactForce);
664  float right_force_error = force_contact_switch * (-right_forceInRoot(2) - contactForce);
665 
666  // float left_force_error_limited = left_force_error;
667  // float right_force_error_limited = right_force_error;
668 
669  // left_force_error_limited = (left_force_error_limited > 2) ? 2 : left_force_error_limited;
670  // left_force_error_limited = (left_force_error_limited < -2) ? -2 : left_force_error_limited;
671 
672  // right_force_error_limited = (right_force_error_limited > 2) ? 2 : right_force_error_limited;
673  // right_force_error_limited = (right_force_error_limited < -2) ? -2 : right_force_error_limited;
674 
675 
676  left_force_error = deadzone(left_force_error, 0.5, 2);
677  right_force_error = deadzone(right_force_error, 0.5, 2);
678 
679  left_contactForceZ_correction =
680  left_contactForceZ_correction - forceFilterCoeff * left_force_error;
681  right_contactForceZ_correction =
682  right_contactForceZ_correction - forceFilterCoeff * right_force_error;
683 
684  left_contactForceZ_correction =
685  (left_contactForceZ_correction > 30) ? 30 : left_contactForceZ_correction;
686  right_contactForceZ_correction =
687  (right_contactForceZ_correction > 30) ? 30 : right_contactForceZ_correction;
688 
689  left_contactForceZ_correction =
690  (left_contactForceZ_correction < -30) ? -contactForce : left_contactForceZ_correction;
691  right_contactForceZ_correction =
692  (right_contactForceZ_correction < -30) ? -contactForce : right_contactForceZ_correction;
693 
694  /// compose the task space desired force and torque
695  left_tcpDesiredForce = left_DS_force + coupling_force;
696  right_tcpDesiredForce = right_DS_force - coupling_force;
697 
698  left_tcpDesiredForce(2) +=
699  force_contact_switch * (contactForce + left_contactForceZ_correction);
700  right_tcpDesiredForce(2) +=
701  force_contact_switch * (contactForce + right_contactForceZ_correction);
702 
703 
704  left_tcpDesiredTorque = -oriKp * rtGetControlStruct().left_tcpDesiredAngularError -
705  oriDamping * left_currentTCPAngularVelocity_filtered;
706  right_tcpDesiredTorque = -oriKp * rtGetControlStruct().right_tcpDesiredAngularError -
707  oriDamping * right_currentTCPAngularVelocity_filtered;
708 
709  left_tcpDesiredTorque_filtered = (1 - filterFactor) * left_tcpDesiredTorque_filtered +
710  filterFactor * left_tcpDesiredTorque;
711  right_tcpDesiredTorque_filtered = (1 - filterFactor) * right_tcpDesiredTorque_filtered +
712  filterFactor * right_tcpDesiredTorque;
713 
714  left_tcpDesiredWrench.head<3>() = 0.001 * left_tcpDesiredForce;
715  left_tcpDesiredWrench.tail<3>() = left_tcpDesiredTorque_filtered;
716  right_tcpDesiredWrench.head<3>() = 0.001 * right_tcpDesiredForce;
717  right_tcpDesiredWrench.tail<3>() = right_tcpDesiredTorque_filtered;
718 
719  /// calculate the null-spcae torque
720  float lambda = 0.2;
721  left_jtpinv = left_ik->computePseudoInverseJacobianMatrix(left_jacobi.transpose(), lambda);
722  right_jtpinv =
723  right_ik->computePseudoInverseJacobianMatrix(right_jacobi.transpose(), lambda);
724 
725  left_nullqerror = left_qpos - left_qnullspace;
726  right_nullqerror = right_qpos - right_qnullspace;
727 
728  for (int i = 0; i < left_nullqerror.size(); ++i)
729  {
730  if (fabs(left_nullqerror(i)) < 0.09)
731  {
732  left_nullqerror(i) = 0;
733  }
734 
735  if (fabs(right_nullqerror(i)) < 0.09)
736  {
737  right_nullqerror(i) = 0;
738  }
739  }
740 
741  left_nullspaceTorque =
742  -nullspaceKp * left_nullqerror - nullspaceDamping * left_jointVelocity_filtered;
743  right_nullspaceTorque =
744  -nullspaceKp * right_nullqerror - nullspaceDamping * right_jointVelocity_filtered;
745 
746  left_nullspaceTorque = (I - left_jacobi.transpose() * left_jtpinv) * left_nullspaceTorque;
747  right_nullspaceTorque =
748  (I - right_jacobi.transpose() * right_jtpinv) * right_nullspaceTorque;
749 
750  float left_nulltorque_norm = left_nullspaceTorque.norm();
751  float right_nulltorque_norm = right_nullspaceTorque.norm();
752 
753  if (left_nulltorque_norm > null_torqueLimit)
754  {
755  left_nullspaceTorque = (null_torqueLimit / left_nulltorque_norm) * left_nullspaceTorque;
756  }
757 
758  if (right_nulltorque_norm > null_torqueLimit)
759  {
760  right_nullspaceTorque =
761  (null_torqueLimit / right_nulltorque_norm) * right_nullspaceTorque;
762  }
763 
764  left_jointDesiredTorques =
765  left_jacobi.transpose() * left_tcpDesiredWrench + left_nullspaceTorque;
766  right_jointDesiredTorques =
767  right_jacobi.transpose() * right_tcpDesiredWrench + right_nullspaceTorque;
768 
769  right_desiredTorques_filtered =
770  (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered +
771  cfg->TorqueFilterConstant * right_jointDesiredTorques;
772 
773  /// filtering the desried joint torques, clip to torque limit and then set target to actuator.
774  for (size_t i = 0; i < left_torque_targets.size(); ++i)
775  {
776  float desiredTorque = smooth_startup * left_jointDesiredTorques(i);
777  desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
778  left_desiredTorques_filtered(i) =
779  (1 - cfg->TorqueFilterConstant) * left_desiredTorques_filtered(i) +
780  cfg->TorqueFilterConstant * desiredTorque;
781 
782  std::string names = left_jointNames[i] + "_desiredTorques";
783  debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
784  names = names + "_filtered";
785  debugDataInfo.getWriteBuffer().desired_torques[names] = left_desiredTorques_filtered(i);
786 
787  if (fabs(left_desiredTorques_filtered(i)) > torqueLimit)
788  {
789  left_torque_targets.at(i)->torque = 0;
790  }
791  else
792  {
793  left_torque_targets.at(i)->torque =
794  left_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
795  }
796  if (!left_torque_targets.at(i)->isValid())
797  {
798  ARMARX_INFO << deactivateSpam(1)
799  << "Torque controller target is invalid - setting to zero! set value: "
800  << left_torque_targets.at(i)->torque;
801  left_torque_targets.at(i)->torque = 0.0f;
802  }
803  }
804 
805  for (size_t i = 0; i < right_torque_targets.size(); ++i)
806  {
807  float desiredTorque = smooth_startup * right_jointDesiredTorques(i);
808  desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
809  right_desiredTorques_filtered(i) =
810  (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered(i) +
811  cfg->TorqueFilterConstant * desiredTorque;
812 
813  std::string names = right_jointNames[i] + "_desiredTorques";
814  debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
815  names = names + "_filtered";
816  debugDataInfo.getWriteBuffer().desired_torques[names] =
817  right_desiredTorques_filtered(i);
818 
819  if (fabs(right_desiredTorques_filtered(i)) > torqueLimit)
820  {
821  right_torque_targets.at(i)->torque = 0;
822  }
823  else
824  {
825  right_torque_targets.at(i)->torque =
826  right_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
827  }
828  if (!right_torque_targets.at(i)->isValid())
829  {
830  ARMARX_INFO << deactivateSpam(1)
831  << "Torque controller target is invalid - setting to zero! set value: "
832  << right_torque_targets.at(i)->torque;
833  right_torque_targets.at(i)->torque = 0.0f;
834  }
835  }
836 
837  smooth_startup = smooth_startup + 0.001 * (1.1 - smooth_startup);
838  smooth_startup = (smooth_startup > 1) ? 1 : smooth_startup;
839  smooth_startup = (smooth_startup < 0) ? 0 : smooth_startup;
840 
841 
842  debugDataInfo.getWriteBuffer().left_realForce_x = left_forceInRoot(0);
843  debugDataInfo.getWriteBuffer().left_realForce_y = left_forceInRoot(1);
844  debugDataInfo.getWriteBuffer().left_realForce_z = left_forceInRoot(2);
845  debugDataInfo.getWriteBuffer().right_realForce_x = right_forceInRoot(0);
846  debugDataInfo.getWriteBuffer().right_realForce_y = right_forceInRoot(1);
847  debugDataInfo.getWriteBuffer().right_realForce_z = right_forceInRoot(2);
848 
849  debugDataInfo.getWriteBuffer().left_force_error = left_force_error;
850  debugDataInfo.getWriteBuffer().right_force_error = right_force_error;
851 
852 
853  debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_x = left_tcpDesiredTorque_filtered(0);
854  debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_y = left_tcpDesiredTorque_filtered(1);
855  debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_z = left_tcpDesiredTorque_filtered(2);
856  debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_x =
857  right_tcpDesiredTorque_filtered(0);
858  debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_y =
859  right_tcpDesiredTorque_filtered(1);
860  debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_z =
861  right_tcpDesiredTorque_filtered(2);
862  // debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
863  // debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
864  // debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
865 
866 
867  // debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
868  // debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
869  // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
870 
871  // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
872  // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
873  // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
874 
875  // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
876  // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
877  // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
878 
879  // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
880  // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
881  // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
882 
883  debugDataInfo.commitWrite();
884  }
885 
886  float
887  DSRTBimanualController::deadzone(float input, float disturbance, float threshold)
888  {
889  if (input > 0)
890  {
891  input = input - disturbance;
892  input = (input < 0) ? 0 : input;
893  input = (input > threshold) ? threshold : input;
894  }
895  else if (input < 0)
896  {
897  input = input + disturbance;
898  input = (input > 0) ? 0 : input;
899  input = (input < -threshold) ? -threshold : input;
900  }
901 
902 
903  return input;
904  }
905 
906  Eigen::Quaternionf
907  DSRTBimanualController::quatSlerp(double t,
908  const Eigen::Quaternionf& q0,
909  const Eigen::Quaternionf& q1)
910  {
911  double cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
912 
913 
914  Eigen::Quaternionf q1x = q1;
915  if (cosHalfTheta < 0)
916  {
917  q1x.w() = -q1.w();
918  q1x.x() = -q1.x();
919  q1x.y() = -q1.y();
920  q1x.z() = -q1.z();
921  }
922 
923 
924  if (fabs(cosHalfTheta) >= 1.0)
925  {
926  return q0;
927  }
928 
929  double halfTheta = acos(cosHalfTheta);
930  double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
931 
932 
933  Eigen::Quaternionf result;
934  if (fabs(sinHalfTheta) < 0.001)
935  {
936  result.w() = (1 - t) * q0.w() + t * q1x.w();
937  result.x() = (1 - t) * q0.x() + t * q1x.x();
938  result.y() = (1 - t) * q0.y() + t * q1x.y();
939  result.z() = (1 - t) * q0.z() + t * q1x.z();
940  }
941  else
942  {
943  double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
944  double ratioB = sin(t * halfTheta) / sinHalfTheta;
945 
946  result.w() = ratioA * q0.w() + ratioB * q1x.w();
947  result.x() = ratioA * q0.x() + ratioB * q1x.x();
948  result.y() = ratioA * q0.y() + ratioB * q1x.y();
949  result.z() = ratioA * q0.z() + ratioB * q1x.z();
950  }
951 
952  return result;
953  }
954 
955  void
956  DSRTBimanualController::onPublish(const SensorAndControl&,
957  const DebugDrawerInterfacePrx&,
958  const DebugObserverInterfacePrx& debugObs)
959  {
960 
961  StringVariantBaseMap datafields;
962  auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
963  for (auto& pair : values)
964  {
965  datafields[pair.first] = new Variant(pair.second);
966  }
967 
968  // std::string nameJacobi = "jacobiori";
969  // std::string nameJacobipos = "jacobipos";
970 
971  // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
972  // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
973 
974  // for (size_t i = 0; i < jacobiVec.size(); ++i)
975  // {
976  // std::stringstream ss;
977  // ss << i;
978  // std::string name = nameJacobi + ss.str();
979  // datafields[name] = new Variant(jacobiVec[i]);
980  // std::string namepos = nameJacobipos + ss.str();
981  // datafields[namepos] = new Variant(jacobiPos[i]);
982 
983  // }
984 
985 
986  datafields["left_tcpDesiredTorque_x"] =
987  new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_x);
988  datafields["left_tcpDesiredTorque_y"] =
989  new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_y);
990  datafields["left_tcpDesiredTorque_z"] =
991  new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_z);
992  datafields["right_tcpDesiredTorque_x"] =
993  new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_x);
994  datafields["right_tcpDesiredTorque_y"] =
995  new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_y);
996  datafields["right_tcpDesiredTorque_z"] =
997  new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_z);
998 
999  datafields["left_realForce_x"] =
1000  new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_x);
1001  datafields["left_realForce_y"] =
1002  new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_y);
1003  datafields["left_realForce_z"] =
1004  new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_z);
1005  datafields["right_realForce_x"] =
1006  new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_x);
1007  datafields["right_realForce_y"] =
1008  new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_y);
1009  datafields["right_realForce_z"] =
1010  new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_z);
1011 
1012  datafields["left_force_error"] =
1013  new Variant(debugDataInfo.getUpToDateReadBuffer().left_force_error);
1014  datafields["right_force_error"] =
1015  new Variant(debugDataInfo.getUpToDateReadBuffer().right_force_error);
1016 
1017 
1018  datafields["Desired_Guard_Z"] =
1019  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().desredZ);
1020  datafields["Force_Error_Z"] =
1021  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().force_error_z);
1022  datafields["guardXYHighFrequency"] =
1023  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guardXYHighFrequency);
1024  datafields["guard_mounting_correction_x"] =
1025  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_x);
1026  datafields["guard_mounting_correction_y"] =
1027  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_y);
1028  datafields["guard_mounting_correction_z"] =
1029  new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_z);
1030 
1031 
1032  // datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
1033  // datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
1034  // datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
1035 
1036  // datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
1037  // datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
1038  // datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
1039 
1040  // datafields["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
1041  // datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
1042  // datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
1043 
1044  // datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
1045  // datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
1046  // datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
1047 
1048 
1049  // datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
1050  // datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
1051  // datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
1052 
1053 
1054  debugObs->setDebugChannel("DSBimanualControllerOutput", datafields);
1055  }
1056 
1057  void
1058  DSRTBimanualController::rtPreActivateController()
1059  {
1060  VirtualRobot::RobotNodeSetPtr rnsLeft = rtGetRobot()->getRobotNodeSet("LeftArm");
1061  left_currentTCPPose = rnsLeft->getTCP()->getPoseInRootFrame();
1062  VirtualRobot::RobotNodeSetPtr rnsRight = rtGetRobot()->getRobotNodeSet("RightArm");
1063  right_currentTCPPose = rnsRight->getTCP()->getPoseInRootFrame();
1064 
1065  left_qpos = rnsLeft->getJointValuesEigen();
1066  right_qpos = rnsRight->getJointValuesEigen();
1067  nJointLeft = left_qpos.size();
1068  nJointRight = right_qpos.size();
1069  left_qvel.setZero(nJointLeft);
1070  right_qvel.setZero(nJointRight);
1071 
1072  left_jacobi = Eigen::MatrixXf::Zero(6, nJointLeft);
1073  right_jacobi = Eigen::MatrixXf::Zero(6, nJointRight);
1074 
1075  left_tcptwist.setZero();
1076  right_tcptwist.setZero();
1077 
1078  left_forceInRoot.setZero();
1079  right_forceInRoot.setZero();
1080 
1081  left_DS_force.setZero();
1082  right_DS_force.setZero();
1083  coupling_force.setZero();
1084 
1085  left_tcpDesiredForce.setZero();
1086  right_tcpDesiredForce.setZero();
1087  left_tcpDesiredTorque.setZero();
1088  right_tcpDesiredTorque.setZero();
1089 
1090  left_tcpDesiredWrench.setZero();
1091  right_tcpDesiredWrench.setZero();
1092 
1093  I = Eigen::MatrixXf::Identity(nJointLeft, nJointLeft);
1094 
1095  left_jointDesiredTorques.setZero(nJointLeft);
1096  right_jointDesiredTorques.setZero(nJointRight);
1097  left_nullqerror.setZero(nJointLeft);
1098  right_nullqerror.setZero(nJointRight);
1099 
1100  left_jtpinv = Eigen::MatrixXf::Zero(6, nJointLeft);
1101  right_jtpinv = Eigen::MatrixXf::Zero(6, nJointRight);
1102  }
1103 
1104  void
1105  DSRTBimanualController::rtPostDeactivateController()
1106  {
1107  }
1108 
1109  void
1110  DSRTBimanualController::setToDefaultTarget(const Ice::Current&)
1111  {
1112  isDefaultTarget = true;
1113  }
1114 } // namespace armarx::control::ds_controller
armarx::NJointControllerWithTripleBuffer< DSRTBimanualControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const DSRTBimanualControllerControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::control::ds_controller::DSRTBimanualController::DSRTBimanualController
DSRTBimanualController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: DSRTBimanualController.cpp:74
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_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
armarx::control::ds_controller::DSRTBimanualController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition: DSRTBimanualController.cpp:540
armarx::control::ds_controller::DSRTBimanualControllerControlData::right_tcpDesiredLinearVelocity
Eigen::Vector3f right_tcpDesiredLinearVelocity
Definition: DSRTBimanualController.h:49
armarx::control::ds_controller::DSRTBimanualControllerControlData::left_tcpDesiredAngularError
Eigen::Vector3f left_tcpDesiredAngularError
Definition: DSRTBimanualController.h:47
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:92
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::NJointControllerWithTripleBuffer< DSRTBimanualControllerControlData >::rtGetControlStruct
const DSRTBimanualControllerControlData & 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
DSRTBimanualController.h
armarx::control::ds_controller::DSRTBimanualControllerControlData::left_tcpDesiredLinearVelocity
Eigen::Vector3f left_tcpDesiredLinearVelocity
Definition: DSRTBimanualController.h:46
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
armarx::control::ds_controller::DSRTBimanualController::controllerRun
void controllerRun()
Definition: DSRTBimanualController.cpp:352
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::control::ds_controller
Definition: DSJointCarryController.cpp:32
armarx::control::ds_controller::DSRTBimanualController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition: DSRTBimanualController.cpp:1058
armarx::control::ds_controller::DSRTBimanualControllerControlData::right_tcpDesiredAngularError
Eigen::Vector3f right_tcpDesiredAngularError
Definition: DSRTBimanualController.h:50
armarx::NJointControllerWithTripleBuffer< DSRTBimanualControllerControlData >::getWriterControlStruct
DSRTBimanualControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::ds_controller::DSRTBimanualController::rtPostDeactivateController
void rtPostDeactivateController()
This function is called after the controller is deactivated.
Definition: DSRTBimanualController.cpp:1105
armarx::SensorValueForceTorque::force
Eigen::Vector3f force
Definition: SensorValueForceTorque.h:36
armarx::control::ds_controller::BimanualGMMMotionGen
Definition: DSRTBimanualController.h:68
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:846
armarx::control::ds_controller::registrationControllerDSRTBimanualController
NJointControllerRegistration< DSRTBimanualController > registrationControllerDSRTBimanualController("DSRTBimanualController")
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::control::ds_controller::DSRTBimanualController::onPublish
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition: DSRTBimanualController.cpp:956
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< DSRTBimanualControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:108
ArmarXObjectScheduler.h
armarx::control::ds_controller::DSRTBimanualController::onInitNJointController
void onInitNJointController()
Definition: DSRTBimanualController.cpp:38
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::control::ds_controller::DSRTBimanualControllerControlData
Definition: DSRTBimanualController.h:43
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
for
for(;yybottom<=yytop;yybottom++)
Definition: Grammar.cpp:705
CycleUtil.h
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::Quaternion< float, 0 >
IceUtil::Handle< class RobotUnit >
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
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
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::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::DSBimanualControllerInterface::setToDefaultTarget
void setToDefaultTarget()
armarx::control::ds_controller::DSRTBimanualController::onDisconnectNJointController
void onDisconnectNJointController()
Definition: DSRTBimanualController.cpp:63
armarx::TripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:143