23 #ifndef _ARMARX_LIB_DSController_DSRTBimanualController_H
24 #define _ARMARX_LIB_DSController_DSRTBimanualController_H
26 #include <VirtualRobot/VirtualRobot.h>
36 #include <armarx/control/ds_controller/DSControllerInterface.h>
62 std::vector<double>
Mu_;
100 std::ifstream infile{fileName};
101 std::string objDefs = {std::istreambuf_iterator<char>(infile),
102 std::istreambuf_iterator<char>()};
110 json->fromString(objDefs);
144 scaling = json->getDouble(
"Scaling");
145 v_max = json->getDouble(
"MaxVelocity");
174 std::cout <<
"line 162." << std::endl;
177 for (
int i = 0; i < 3; ++i)
183 std::cout <<
"Finished GMM." << std::endl;
188 const Eigen::Vector3f& rightarm_PositionInMeter,
189 float positionErrorToleranceInMeter,
196 position_error.Resize(3);
199 desired_vel.Resize(3);
205 leftarm_Target_corrected(0) += correction_x;
206 rightarm_Target_corrected(0) += correction_x;
208 leftarm_Target_corrected(1) += correction_y;
209 rightarm_Target_corrected(1) += correction_y;
211 leftarm_Target_corrected(2) = desiredZ + correction_z;
212 rightarm_Target_corrected(2) = desiredZ + correction_z;
216 Eigen::Vector3f leftarm_PositionError =
217 leftarm_PositionInMeter - leftarm_Target_corrected;
218 if (leftarm_PositionError.norm() < positionErrorToleranceInMeter)
220 leftarm_PositionError.setZero();
223 for (
int i = 0; i < 3; ++i)
225 position_error(i) = leftarm_PositionError(i);
228 desired_vel =
leftarm_gmm->getVelocity(position_error);
230 Eigen::Vector3f leftarm_desired_vel;
231 leftarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
233 leftarm_desired_vel =
scaling * leftarm_desired_vel;
235 float lenVec = leftarm_desired_vel.norm();
237 if (std::isnan(lenVec))
239 leftarm_desired_vel.setZero();
253 leftarm_desired_vel = (
v_max / lenVec) * leftarm_desired_vel;
260 Eigen::Vector3f rightarm_PositionError =
261 rightarm_PositionInMeter - rightarm_Target_corrected;
262 if (rightarm_PositionError.norm() < positionErrorToleranceInMeter)
264 rightarm_PositionError.setZero();
267 for (
int i = 0; i < 3; ++i)
269 position_error(i) = rightarm_PositionError(i);
272 desired_vel =
rightarm_gmm->getVelocity(position_error);
274 Eigen::Vector3f rightarm_desired_vel;
275 rightarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
277 rightarm_desired_vel =
scaling * rightarm_desired_vel;
279 lenVec = rightarm_desired_vel.norm();
280 if (std::isnan(lenVec))
282 rightarm_desired_vel.setZero();
287 rightarm_desired_vel = (
v_max / lenVec) * rightarm_desired_vel;
328 const NJointControllerConfigPtr& config,
334 return "DSRTBimanualController";
344 float deadzone(
float currentValue,
float targetValue,
float threshold);
351 struct DSRTBimanualControllerSensorData
359 Eigen::Vector3f left_force;
360 Eigen::Vector3f right_force;
368 struct DSCtrlDebugInfo
372 float guardXYHighFrequency;
373 float guard_mounting_correction_x;
374 float guard_mounting_correction_y;
375 float guard_mounting_correction_z;
382 StringFloatDictionary desired_torques;
383 float desiredForce_x;
384 float desiredForce_y;
385 float desiredForce_z;
386 float tcpDesiredTorque_x;
387 float tcpDesiredTorque_y;
388 float tcpDesiredTorque_z;
390 float tcpDesiredAngularError_x;
391 float tcpDesiredAngularError_y;
392 float tcpDesiredAngularError_z;
394 float currentTCPAngularVelocity_x;
395 float currentTCPAngularVelocity_y;
396 float currentTCPAngularVelocity_z;
398 float currentTCPLinearVelocity_x;
399 float currentTCPLinearVelocity_y;
400 float currentTCPLinearVelocity_z;
403 float left_realForce_x;
404 float left_realForce_y;
405 float left_realForce_z;
406 float right_realForce_x;
407 float right_realForce_y;
408 float right_realForce_z;
409 float left_force_error;
410 float right_force_error;
412 float left_tcpDesiredTorque_x;
413 float left_tcpDesiredTorque_y;
414 float left_tcpDesiredTorque_z;
415 float right_tcpDesiredTorque_x;
416 float right_tcpDesiredTorque_y;
417 float right_tcpDesiredTorque_z;
420 TripleBuffer<DSRTDebugInfo> debugDataInfo;
423 std::vector<const SensorValue1DoFActuatorTorque*> left_torqueSensors;
424 std::vector<const SensorValue1DoFGravityTorque*> left_gravityTorqueSensors;
425 std::vector<const SensorValue1DoFActuatorVelocity*> left_velocitySensors;
426 std::vector<const SensorValue1DoFActuatorPosition*> left_positionSensors;
428 std::vector<const SensorValue1DoFActuatorTorque*> right_torqueSensors;
429 std::vector<const SensorValue1DoFGravityTorque*> right_gravityTorqueSensors;
430 std::vector<const SensorValue1DoFActuatorVelocity*> right_velocitySensors;
431 std::vector<const SensorValue1DoFActuatorPosition*> right_positionSensors;
433 const SensorValueForceTorque* leftForceTorque;
434 const SensorValueForceTorque* rightForceTorque;
436 std::vector<ControlTarget1DoFActuatorTorque*> left_torque_targets;
437 std::vector<ControlTarget1DoFActuatorTorque*> right_torque_targets;
440 VirtualRobot::RobotNodePtr left_arm_tcp;
441 VirtualRobot::RobotNodePtr right_arm_tcp;
443 VirtualRobot::RobotNodePtr left_sensor_frame;
444 VirtualRobot::RobotNodePtr right_sensor_frame;
446 VirtualRobot::DifferentialIKPtr left_ik;
447 Eigen::MatrixXf left_jacobip;
448 Eigen::MatrixXf left_jacobio;
450 VirtualRobot::DifferentialIKPtr right_ik;
451 Eigen::MatrixXf right_jacobip;
452 Eigen::MatrixXf right_jacobio;
455 Eigen::Vector3f left_oldPosition;
459 Eigen::Vector3f right_oldPosition;
462 Eigen::Vector3f left_currentTCPLinearVelocity_filtered;
463 Eigen::Vector3f left_currentTCPAngularVelocity_filtered;
465 Eigen::VectorXf left_jointVelocity_filtered;
466 Eigen::VectorXf right_jointVelocity_filtered;
468 Eigen::VectorXf left_desiredTorques_filtered;
469 Eigen::VectorXf right_desiredTorques_filtered;
472 Eigen::Vector3f left_tcpDesiredTorque_filtered;
473 Eigen::Vector3f right_tcpDesiredTorque_filtered;
475 Eigen::Vector3f leftForceOffset;
476 Eigen::Vector3f rightForceOffset;
478 float left_contactForceZ_correction;
479 float right_contactForceZ_correction;
481 float smooth_startup;
483 DSRTBimanualControllerConfigPtr cfg;
485 Eigen::Vector3f right_currentTCPLinearVelocity_filtered;
486 Eigen::Vector3f right_currentTCPAngularVelocity_filtered;
488 float filterTimeConstant;
490 std::vector<std::string> left_jointNames;
491 std::vector<std::string> right_jointNames;
495 std::vector<float> Damping;
497 float null_torqueLimit;
499 float Coupling_stiffness;
500 float Coupling_force_limit;
508 float nullspaceDamping;
512 float guardTargetZUp;
513 float guardTargetZDown;
515 float guard_mounting_correction_z;
517 float guardXYHighFrequency;
518 Eigen::Vector3f left_force_old;
519 Eigen::Vector3f right_force_old;
521 Eigen::VectorXf left_qnullspace;
522 Eigen::VectorXf right_qnullspace;
532 float forceFilterCoeff;
534 float positionErrorTolerance;
535 bool controllerStopRequested =
false;
536 bool controllerRunFinished =
false;
538 bool isDefaultTarget =
true;
547 unsigned int nJointLeft = 0;
548 unsigned int nJointRight = 0;
550 Eigen::MatrixXf left_jacobi;
551 Eigen::MatrixXf right_jacobi;
553 Eigen::VectorXf left_qpos;
554 Eigen::VectorXf left_qvel;
556 Eigen::VectorXf right_qpos;
557 Eigen::VectorXf right_qvel;
559 Eigen::Vector3f left_forceInRoot;
560 Eigen::Vector3f right_forceInRoot;
564 Eigen::Vector3f left_DS_force;
565 Eigen::Vector3f right_DS_force;
566 Eigen::Vector3f coupling_force;
568 Eigen::Vector3f left_tcpDesiredForce;
569 Eigen::Vector3f right_tcpDesiredForce;
570 Eigen::Vector3f left_tcpDesiredTorque;
571 Eigen::Vector3f right_tcpDesiredTorque;
573 Eigen::VectorXf left_nullspaceTorque;
574 Eigen::VectorXf right_nullspaceTorque;
581 Eigen::VectorXf left_jointDesiredTorques;
582 Eigen::VectorXf right_jointDesiredTorques;
584 Eigen::MatrixXf left_jtpinv;
585 Eigen::MatrixXf right_jtpinv;
587 Eigen::VectorXf left_nullqerror;
588 Eigen::VectorXf right_nullqerror;