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
32namespace 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
1108
1109 void
1110 DSRTBimanualController::setToDefaultTarget(const Ice::Current&)
1111 {
1112 isDefaultTarget = true;
1113 }
1114} // namespace armarx::control::ds_controller
c
constexpr T c
Definition UnscentedKalmanFilterTest.cpp:46
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition ControlTargetBase.h:48
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition CycleUtil.h:41
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
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition ManagedIceObject.cpp:776
armarx::ManagedIceObject::getState
int getState() const
Retrieve current state of the ManagedIceObject.
Definition ManagedIceObject.cpp:769
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition NJointControllerBase.h:778
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
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::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition NJointController.cpp:293
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition NJointControllerBase.h:846
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::SensorValueBase
The SensorValueBase class.
Definition SensorValueBase.h:41
armarx::SensorValueBase::asA
const T * asA() const
Definition SensorValueBase.h:82
armarx::Variant
The Variant class is described here: Variants.
Definition Variant.h:224
armarx::control::ds_controller::BimanualGMMMotionGen
Definition DSRTBimanualController.h:69
armarx::control::ds_controller::DSRTBimanualControllerControlData
Definition DSRTBimanualController.h:44
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::control::ds_controller::DSRTBimanualControllerControlData::left_tcpDesiredLinearVelocity
Eigen::Vector3f left_tcpDesiredLinearVelocity
Definition DSRTBimanualController.h:46
armarx::control::ds_controller::DSRTBimanualControllerControlData::right_tcpDesiredAngularError
Eigen::Vector3f right_tcpDesiredAngularError
Definition DSRTBimanualController.h:50
armarx::control::ds_controller::DSRTBimanualController::onDisconnectNJointController
void onDisconnectNJointController()
Definition DSRTBimanualController.cpp:63
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::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::DSRTBimanualController::rtPostDeactivateController
void rtPostDeactivateController()
This function is called after the controller is deactivated.
Definition DSRTBimanualController.cpp:1105
armarx::control::ds_controller::DSRTBimanualController::controllerRun
void controllerRun()
Definition DSRTBimanualController.cpp:352
armarx::control::ds_controller::DSRTBimanualController::onInitNJointController
void onInitNJointController()
Definition DSRTBimanualController.cpp:38
armarx::control::ds_controller::DSRTBimanualController::onPublish
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition DSRTBimanualController.cpp:956
armarx::control::ds_controller::DSRTBimanualController::DSRTBimanualController
DSRTBimanualController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition DSRTBimanualController.cpp:74
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_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_INFO
#define ARMARX_INFO
The normal logging level.
Definition Logging.h:181
ARMARX_ERROR
#define ARMARX_ERROR
The logging level for unexpected behaviour, that must be fixed.
Definition Logging.h:196
ARMARX_WARNING
#define ARMARX_WARNING
The logging level for unexpected behaviour, but not a serious problem.
Definition Logging.h:193
ARMARX_VERBOSE
#define ARMARX_VERBOSE
The logging level for verbose information.
Definition Logging.h:187
Eigen::Quaternionf
Quaternion< float, 0 > Quaternionf
Definition EigenForwardDeclarations.h:61
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition VectorXD.h:704
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition Bus.h:19
armarx::control::ds_controller
Definition DSJointCarryController.cpp:33
armarx::control::ds_controller::registrationControllerDSRTBimanualController
NJointControllerRegistration< DSRTBimanualController > registrationControllerDSRTBimanualController("DSRTBimanualController")
armarx::DebugObserverInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface > DebugObserverInterfacePrx
Definition JointController.h:44
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition ManagedIceObject.h:110
armarx::RobotUnitPtr
IceUtil::Handle< class RobotUnit > RobotUnitPtr
Definition FTSensor.h:34
armarx::DebugDrawerInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface > DebugDrawerInterfacePrx
Definition JointController.h:42
armarx::SensorAndControl
detail::ControlThreadOutputBufferEntry SensorAndControl
Definition NJointControllerBase.h:73