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