From d4fc220baabda5dbc5f53b603ab37ebe0b3c52eb Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Mon, 30 Dec 2024 16:39:12 +0900 Subject: [PATCH] Update implementation. --- etc/HumanRetargetingController.in.yaml | 93 ++++-- .../ArmRetargetingManager.h | 39 +-- .../RetargetingManagerSet.h | 12 + rviz/display.rviz | 286 +++++++++--------- scripts/PublishPoseInteractiveMarker.py | 10 +- src/ArmRetargetingManager.cpp | 106 ++++--- src/HumanRetargetingController.cpp | 8 +- src/RetargetingManagerSet.cpp | 174 +++++++---- 8 files changed, 419 insertions(+), 309 deletions(-) diff --git a/etc/HumanRetargetingController.in.yaml b/etc/HumanRetargetingController.in.yaml index 6202f97..7411e9e 100644 --- a/etc/HumanRetargetingController.in.yaml +++ b/etc/HumanRetargetingController.in.yaml @@ -94,6 +94,28 @@ FootTaskList: stiffness: 1000.0 weight: 1000.0 +RetargetingTaskList: + LeftElbow: + type: transform + frame: LeftElbowFrame + weight: 500.0 + dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0] + LeftWrist: + type: transform + frame: LeftWristFrame + weight: 500.0 + dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0] + RightElbow: + type: transform + frame: RightElbowFrame + weight: 500.0 + dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0] + RightWrist: + type: transform + frame: RightWristFrame + weight: 500.0 + dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0] + FootManager: name: FootManager footstepDuration: 0.8 # [sec] @@ -199,6 +221,8 @@ CentroidalManager: RetargetingManagerSet: name: RetargetingManagerSet humanWaistPoseTopicName: /hrc/poses/waist + humanWaistPoseFromOrigin: + translation: [1.0, 0.0, 0.8] pointMarkerSize: 0.15 phaseMarkerPoseOffset: translation: [0.0, 0.0, 1.0] @@ -206,17 +230,17 @@ RetargetingManagerSet: Left: humanElbowPoseTopicName: /hrc/poses/left_elbow humanWristPoseTopicName: /hrc/poses/left_wrist - elbowBodyPart:: LeftElbow - wristBodyPart: LeftWrist + elbowTaskName: LeftElbow + wristTaskName: LeftWrist stiffness: 10.0 Right: humanElbowPoseTopicName: /hrc/poses/right_elbow humanWristPoseTopicName: /hrc/poses/right_wrist - elbowBodyPart:: RightElbow - wristBodyPart: RightWrist + elbowTaskName: RightElbow + wristTaskName: RightWrist stiffness: 10.0 -# OverwriteConfigKeys: [NoSensors] +OverwriteConfigKeys: [NoSensors] OverwriteConfigList: NoSensors: @@ -288,38 +312,22 @@ robots: frame: WAIST_R_S RetargetingTaskList: - - bodyPart: LeftElbow - type: transform - frame: LeftElbowFrame - weight: 500.0 - dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0] + LeftElbow: activeJoints: [ "Root", "WAIST_Y", # "WAIST_P", "WAIST_R", "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y"] - - bodyPart: LeftWrist - type: transform - frame: LeftWristFrame - weight: 500.0 - dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0] + LeftWrist: activeJoints: [ "Root", "WAIST_Y", # "WAIST_P", "WAIST_R", "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_P"] - - bodyPart: RightElbow - type: transform - frame: RightElbowFrame - weight: 500.0 - dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0] + RightElbow: activeJoints: [ "Root", "WAIST_Y", # "WAIST_P", "WAIST_R", "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y"] - - bodyPart: RightWrist - type: transform - frame: RightWristFrame - weight: 500.0 - dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0] + RightWrist: activeJoints: [ "Root", "WAIST_Y", # "WAIST_P", "WAIST_R", @@ -329,7 +337,8 @@ robots: robotBaseLinkName: WAIST_R_S syncJoints: [ "WAIST_Y", # "WAIST_P", "WAIST_R", - "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_P"] + "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_P", + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_P"] ArmRetargetingManagerList: Left: robotCalibPostures: @@ -357,7 +366,21 @@ robots: L_ELBOW_Y: 0.0 L_WRIST_R: 0.0 L_WRIST_Y: 0.0 - # calibResultConfig: {} + calibResultConfig: + humanTransFromBaseToShoulder: + translation: [0.0, 0.3, 0.3] + rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + robotTransFromBaseToShoulder: + translation: [0.03, 0.24, 0.28] + rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + elbowRotTransFromHumanToRobot: + translation: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + wristRotTransFromHumanToRobot: + translation: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + elbowScale: 0.8 + wristScale: 0.8 Right: robotCalibPostures: X: @@ -384,7 +407,21 @@ robots: R_ELBOW_Y: 0.0 R_WRIST_R: 0.0 R_WRIST_Y: 0.0 - # calibResultConfig: {} + calibResultConfig: + humanTransFromBaseToShoulder: + translation: [0.0, -0.3, 0.3] + rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + robotTransFromBaseToShoulder: + translation: [0.03, -0.24, 0.28] + rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + elbowRotTransFromHumanToRobot: + translation: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + wristRotTransFromHumanToRobot: + translation: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + elbowScale: 0.8 + wristScale: 0.8 frames: - name: LeftElbowFrame diff --git a/include/HumanRetargetingController/ArmRetargetingManager.h b/include/HumanRetargetingController/ArmRetargetingManager.h index 5e42893..68a63b3 100644 --- a/include/HumanRetargetingController/ArmRetargetingManager.h +++ b/include/HumanRetargetingController/ArmRetargetingManager.h @@ -41,11 +41,11 @@ class ArmRetargetingManager //! Topic name of human wrist pose std::string humanWristPoseTopicName; - //! Body part of elbow retargeting task - std::string elbowBodyPart; + //! Name of elbow retargeting task + std::string elbowTaskName; - //! Body part of wrist retargeting task - std::string wristBodyPart; + //! Name of wrist retargeting task + std::string wristTaskName; //! Retargeting task stiffness Eigen::Vector6d stiffness = Eigen::Vector6d::Constant(10); @@ -103,8 +103,8 @@ class ArmRetargetingManager */ void load(const mc_rtc::Configuration & mcRtcConfig); - /** \brief Dump in YAML format. */ - void dump() const; + /** \brief Get YAML format string. */ + std::string dump() const; }; /** \brief Calibration source. @@ -180,6 +180,12 @@ class ArmRetargetingManager /** \brief Accessor to the ROS pose manager for human waist pose. */ const std::shared_ptr & humanWaistPoseManager() const; + /** \brief Accessor to the retargeting task of elbow. */ + const std::shared_ptr & elbowTask() const; + + /** \brief Accessor to the retargeting task of wrist. */ + const std::shared_ptr & wristTask() const; + /** \brief Update target of retargeting task. */ void updateTaskTarget(const std::shared_ptr & task, const sva::PTransformd & pose); @@ -189,9 +195,6 @@ class ArmRetargetingManager /** \brief Set robot data as calibration source. */ void setRobotCalibSource(const std::string & axis); - /** \brief Clear robot for calibration. */ - void clearCalibRobot(); - /** \brief Update calibration. */ void updateCalib(); @@ -215,6 +218,15 @@ class ArmRetargetingManager //! ROS pose manager for human wrist pose std::shared_ptr humanWristPoseManager_; + //! Measured pose of human shoulder (expressed relative to human origin) + std::optional humanShoulderPose_ = std::nullopt; + + //! Measured pose of human elbow (expressed relative to human origin) + std::optional humanElbowPose_ = std::nullopt; + + //! Measured pose of human wrist (expressed relative to human origin) + std::optional humanWristPose_ = std::nullopt; + //! Target pose of robot shoulder std::optional robotShoulderPose_ = std::nullopt; @@ -224,12 +236,6 @@ class ArmRetargetingManager //! Target pose of robot wrist std::optional robotWristPose_ = std::nullopt; - //! Retargeting task of elbow - std::shared_ptr elbowTask_; - - //! Retargeting task of wrist - std::shared_ptr wristTask_; - //! Function to interpolate task stiffness std::shared_ptr> stiffnessRatioFunc_; @@ -241,8 +247,5 @@ class ArmRetargetingManager //! Calibration result CalibResult calibResult_; - - //! Robot for calibration - std::shared_ptr calibRobots_; }; } // namespace HRC diff --git a/include/HumanRetargetingController/RetargetingManagerSet.h b/include/HumanRetargetingController/RetargetingManagerSet.h index e4ebc47..cb71644 100644 --- a/include/HumanRetargetingController/RetargetingManagerSet.h +++ b/include/HumanRetargetingController/RetargetingManagerSet.h @@ -35,6 +35,9 @@ class RetargetingManagerSet : public std::unordered_map syncJoints; + //! Human waist pose from origin + sva::PTransformd humanWaistPoseFromOrigin = sva::PTransformd::Identity(); + //! Point marker size double pointMarkerSize = 0.15; @@ -118,6 +121,12 @@ class RetargetingManagerSet : public std::unordered_map calibRobots_; + //! ROS node handle std::shared_ptr nh_; diff --git a/rviz/display.rviz b/rviz/display.rviz index d8c7645..dd49e73 100644 --- a/rviz/display.rviz +++ b/rviz/display.rviz @@ -8,7 +8,7 @@ Panels: - /Surface1/Namespaces1 - /Vive1 Splitter Ratio: 0.4228571355342865 - Tree Height: 441 + Tree Height: 461 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -69,374 +69,310 @@ Visualization Manager: Enabled: true Links: All Links Enabled: true - BODY: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - CHEST_P_LINK: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - CHEST_Y_LINK: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Expand Joint Details: false Expand Link Details: false Expand Tree: false - HEAD_P_LINK: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - HEAD_Y_LINK: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LC-AI-base_LINK: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LC-AI-hinge_LINK: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LC-AO-base_LINK: + L_ANKLE_P_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC-AO-hinge_LINK: + L_ANKLE_R_S: Alpha: 1 Show Axes: false Show Trail: false - Value: true - LC-CI-base_LINK: + L_ELBOW_P_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC-CI-hinge_LINK: + L_ELBOW_Y_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC-CO-base_LINK: + L_HIP_P_S: Alpha: 1 Show Axes: false Show Trail: false - Value: true - LC-CO-hinge_LINK: + L_HIP_R_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC-E-base_LINK: + L_HIP_Y_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC-K-base_LINK: + L_KNEE_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC-KD-base_LINK: + L_LINDEX_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC_AI_LINK: + L_LLITTLE_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC_AO_LINK: + L_LTHUMB_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC_CI_LINK: + L_SHOULDER_P_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC_CO_LINK: + L_SHOULDER_R_S: Alpha: 1 Show Axes: false Show Trail: false - Value: true - LC_E_LINK: + L_SHOULDER_Y_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC_KD_LINK: + L_UINDEX_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - LC_K_LINK: + L_ULITTLE_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_ANKLE_P_LINK: + L_UTHUMB_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_ANKLE_R_LINK: + L_WRIST_R_S: Alpha: 1 Show Axes: false Show Trail: false - Value: true - L_CROTCH_P_LINK: + L_WRIST_Y_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_CROTCH_R_LINK: + Link Tree Style: Links in Alphabetic Order + NECK_P_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_CROTCH_Y_LINK: + NECK_R_S: Alpha: 1 Show Axes: false Show Trail: false - Value: true - L_ELBOW_P_LINK: + NECK_Y_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_ELBOW_Y_LINK: + PELVIS_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_HAND_LINK: + R_ANKLE_P_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_KNEE_P_LINK: + R_ANKLE_R_S: Alpha: 1 Show Axes: false Show Trail: false - Value: true - L_SHOULDER_P_LINK: + R_ELBOW_P_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_SHOULDER_R_LINK: + R_ELBOW_Y_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_SHOULDER_Y_LINK: + R_HIP_P_S: Alpha: 1 Show Axes: false Show Trail: false - Value: true - L_WRIST_P_LINK: + R_HIP_R_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_WRIST_Y_LINK: + R_HIP_Y_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - Link Tree Style: Links in Alphabetic Order - RC-AI-base_LINK: + R_KNEE_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC-AI-hinge_LINK: + R_LINDEX_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC-AO-base_LINK: + R_LLITTLE_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC-AO-hinge_LINK: + R_LTHUMB_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC-CI-base_LINK: + R_SHOULDER_P_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC-CI-hinge_LINK: + R_SHOULDER_R_S: Alpha: 1 Show Axes: false Show Trail: false - Value: true - RC-CO-base_LINK: + R_SHOULDER_Y_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC-CO-hinge_LINK: + R_UINDEX_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC-E-base_LINK: + R_ULITTLE_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC-K-base_LINK: + R_UTHUMB_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC-KD-base_LINK: + R_WRIST_R_S: Alpha: 1 Show Axes: false Show Trail: false - Value: true - RC_AI_LINK: + R_WRIST_Y_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC_AO_LINK: + WAIST_P_S: Alpha: 1 Show Axes: false Show Trail: false - Value: true - RC_CI_LINK: + WAIST_R_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC_CO_LINK: + WAIST_Y_S: Alpha: 1 Show Axes: false Show Trail: false Value: true - RC_E_LINK: + base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - RC_KD_LINK: + dcamera: Alpha: 1 Show Axes: false Show Trail: false - Value: true - RC_K_LINK: + gsensor: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_ANKLE_P_LINK: + gyrometer: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_ANKLE_R_LINK: + jvrc1_back_tactile_sensor: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_CROTCH_P_LINK: + jvrc1_left_foot_tactile_sensor: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_CROTCH_R_LINK: + jvrc1_left_knee_tactile_sensor: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_CROTCH_Y_LINK: + jvrc1_left_thigh_tactile_sensor: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_ELBOW_P_LINK: + jvrc1_right_elbow_tactile_sensor: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_ELBOW_Y_LINK: + jvrc1_right_foot_tactile_sensor: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_HAND_LINK: + jvrc1_right_thigh_tactile_sensor: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_KNEE_P_LINK: + l_ankle: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_SHOULDER_P_LINK: + l_wrist: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_SHOULDER_R_LINK: + lcamera: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_SHOULDER_Y_LINK: + lfsensor: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_WRIST_P_LINK: + lhsensor: Alpha: 1 Show Axes: false Show Trail: false - Value: true - R_WRIST_Y_LINK: + r_ankle: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l-one-finger-link: + r_wrist: Alpha: 1 Show Axes: false Show Trail: false - Value: true - r-one-finger-link: + ranger: Alpha: 1 Show Axes: false Show Trail: false - Value: true - rhp7_left_thigh_tactile_sensor: + rcamera: Alpha: 1 Show Axes: false Show Trail: false - rhp7_right_elbow_tactile_sensor: + rfsensor: Alpha: 1 Show Axes: false Show Trail: false - rhp7_right_thigh_tactile_sensor: + rhsensor: Alpha: 1 Show Axes: false Show Trail: false @@ -462,6 +398,22 @@ Visualization Manager: Update Interval: 0 Value: false Visual Enabled: true + - Alpha: 0.5 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: CalibRobotModel + Robot Description: /calib/robot_description + TF Prefix: calib + Update Interval: 0 + Value: false + Visual Enabled: true - Alpha: 1 Class: rviz/RobotModel Collision Enabled: false @@ -492,8 +444,7 @@ Visualization Manager: Marker Topic: /mc_rtc_rviz Name: mc_rtc GUI markers Namespaces: - Collisions/rhp7/rhp7/Arrows/CHEST_P_LINK::L_SHOULDER_Y_LINK_arrow_arrow: true - Collisions/rhp7/rhp7/Arrows/CHEST_P_LINK::R_SHOULDER_Y_LINK_arrow_arrow: true + Collisions/jvrc1/jvrc1/Arrows/WAIST_R_S::R_SHOULDER_Y_S_arrow_arrow: true HRC/CentroidalManager/ForceMarker/Left_Force0_arrow_arrow: true HRC/CentroidalManager/ForceMarker/Left_Force1_arrow_arrow: true HRC/CentroidalManager/ForceMarker/Left_Force2_arrow_arrow: true @@ -512,8 +463,15 @@ Visualization Manager: HRC/CentroidalManager/ForceMarker/Right_FricPyramid2_edges: true HRC/CentroidalManager/ForceMarker/Right_FricPyramid3_edges: true HRC/CentroidalManager/ForceMarker/Right_SurfaceRegion: true - HRC/RetargetingManagerSet/Marker/BaseToLeftShoulderArrow_arrow_arrow: true - HRC/RetargetingManagerSet/Marker/BaseToRightShoulderArrow_arrow_arrow: true + HRC/RetargetingManagerSet/Marker/HumanLeftElbowToWristArrow_arrow_arrow: true + HRC/RetargetingManagerSet/Marker/HumanLeftShoulderToElbowArrow_arrow_arrow: true + HRC/RetargetingManagerSet/Marker/HumanRightElbowToWristArrow_arrow_arrow: true + HRC/RetargetingManagerSet/Marker/HumanRightShoulderToElbowArrow_arrow_arrow: true + HRC/RetargetingManagerSet/Marker/Phase: true + HRC/RetargetingManagerSet/Marker/RobotLeftElbowToWristArrow_arrow_arrow: true + HRC/RetargetingManagerSet/Marker/RobotLeftShoulderToElbowArrow_arrow_arrow: true + HRC/RetargetingManagerSet/Marker/RobotRightElbowToWristArrow_arrow_arrow: true + HRC/RetargetingManagerSet/Marker/RobotRightShoulderToElbowArrow_arrow_arrow: true Queue Size: 100 Value: true - Class: rviz/TF @@ -590,12 +548,44 @@ Visualization Manager: Enabled: true Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 - Name: Vive/LeftHandPose + Name: Vive/LeftWristPose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /hrc/poses/left_wrist + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 0.15000000596046448 + Axes Radius: 0.029999999329447746 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Vive/RightElbowPose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /hrc/poses/right_elbow + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 0.15000000596046448 + Axes Radius: 0.029999999329447746 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Vive/RightWristPose Queue Size: 10 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Shape: Axes - Topic: /hrc/poses/left_hand + Topic: /hrc/poses/right_wrist Unreliable: false Value: true Enabled: false @@ -628,7 +618,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.9631181955337524 + Distance: 3.7299885749816895 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -636,17 +626,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.5775099396705627 - Y: 0.1039738580584526 - Z: 1.0262229442596436 + X: 1.570784330368042 + Y: -0.10924970358610153 + Z: 0.7618588209152222 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.40152257680892944 + Pitch: 1.016961932182312 Target Frame: - Yaw: 0.5476487278938293 + Yaw: 6.2372846603393555 Saved: ~ Window Geometry: Displays: @@ -654,7 +644,7 @@ Window Geometry: Height: 1672 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000003e50000037afc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000004bb000001df00000185000000b1fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000012e000004e9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003a000004e90000013500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000005f800000264fc0100000002fc00000000000005f8000001c400fffffffa000000010200000002fb00000018006d0063005f007200740063002000700061006e0065006c0100000000ffffffff000000d800fffffffb000000100044006900730070006c006100790073010000006f000002640000018400fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000058fc0100000002fb0000000800540069006d00650000000000000007800000065900fffffffb0000000800540069006d00650100000000000004500000000000000000000005f80000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003e50000037afc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000004bb000001df00000185000000b1fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000012e000004e9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003a000004e90000013500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000bf000000278fc0100000002fc0000000000000bf0000001c400fffffffa000000000200000002fb00000018006d0063005f007200740063002000700061006e0065006c0100000000ffffffff000000d800fffffffb000000100044006900730070006c006100790073010000006f000002640000018400fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000058fc0100000002fb0000000800540069006d00650000000000000007800000065900fffffffb0000000800540069006d0065010000000000000450000000000000000000000bf00000036600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -663,8 +653,8 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1528 - X: 1672 + Width: 3056 + X: 144 Y: 54 mc_rtc panel: collapsed: false diff --git a/scripts/PublishPoseInteractiveMarker.py b/scripts/PublishPoseInteractiveMarker.py index a8b022e..b1aecf5 100755 --- a/scripts/PublishPoseInteractiveMarker.py +++ b/scripts/PublishPoseInteractiveMarker.py @@ -12,11 +12,11 @@ class PublishPoseInteractiveMarker(object): def __init__(self): body_part_init_pos_map = { - "waist": [0.0, 0.0, 0.86], - "left_elbow": [0.0, 0.4, 1.0], - "left_wrist": [0.2, 0.4, 0.95], - "right_elbow": [0.0, -0.4, 1.0], - "right_wrist": [0.2, -0.4, 0.95], + "waist": [2.0, 0.0, 0.86], + "left_elbow": [2.2, 0.4, 1.0], + "left_wrist": [2.6, 0.4, 0.95], + "right_elbow": [2.2, -0.4, 1.0], + "right_wrist": [2.6, -0.4, 0.95], } self.im_server = InteractiveMarkerServer("im_server") diff --git a/src/ArmRetargetingManager.cpp b/src/ArmRetargetingManager.cpp index 3ef14bd..ace2b0e 100644 --- a/src/ArmRetargetingManager.cpp +++ b/src/ArmRetargetingManager.cpp @@ -2,7 +2,6 @@ #include #include #include -#include #include #include @@ -18,8 +17,8 @@ void ArmRetargetingManager::Configuration::load(const mc_rtc::Configuration & mc humanElbowPoseTopicName = static_cast(mcRtcConfig("humanElbowPoseTopicName")); humanWristPoseTopicName = static_cast(mcRtcConfig("humanWristPoseTopicName")); - elbowBodyPart = static_cast(mcRtcConfig("elbowBodyPart")); - wristBodyPart = static_cast(mcRtcConfig("wristBodyPart")); + elbowTaskName = static_cast(mcRtcConfig("elbowTaskName")); + wristTaskName = static_cast(mcRtcConfig("wristTaskName")); if(mcRtcConfig.has("stiffness")) { @@ -71,11 +70,11 @@ void ArmRetargetingManager::CalibResult::load(const mc_rtc::Configuration & mcRt wristScale = mcRtcConfig("wristScale"); } -void ArmRetargetingManager::CalibResult::dump() const +std::string ArmRetargetingManager::CalibResult::dump() const { mc_rtc::Configuration mcRtcConfig; - auto calibConfig = mcRtcConfig.add("calibResult"); + auto calibConfig = mcRtcConfig.add("calibResultConfig"); calibConfig.add("humanTransFromBaseToShoulder", humanTransFromBaseToShoulder); calibConfig.add("robotTransFromBaseToShoulder", robotTransFromBaseToShoulder); @@ -86,7 +85,7 @@ void ArmRetargetingManager::CalibResult::dump() const calibConfig.add("elbowScale", elbowScale); calibConfig.add("wristScale", wristScale); - std::cout << mcRtcConfig.dump(true, true) << std::endl; + return mcRtcConfig.dump(true, true); } ArmRetargetingManager::ArmRetargetingManager(HumanRetargetingController * ctlPtr, @@ -101,13 +100,14 @@ void ArmRetargetingManager::reset() humanElbowPoseManager_ = std::make_shared(ctlPtr_, config_.humanElbowPoseTopicName); humanWristPoseManager_ = std::make_shared(ctlPtr_, config_.humanWristPoseTopicName); + humanShoulderPose_ = std::nullopt; + humanElbowPose_ = std::nullopt; + humanWristPose_ = std::nullopt; + robotShoulderPose_ = std::nullopt; robotElbowPose_ = std::nullopt; robotWristPose_ = std::nullopt; - elbowTask_ = ctl().retargetingTasks_.at(config_.elbowBodyPart); - wristTask_ = ctl().retargetingTasks_.at(config_.wristBodyPart); - stiffnessRatioFunc_ = nullptr; humanCalibSource_.clear(); @@ -117,8 +117,6 @@ void ArmRetargetingManager::reset() { calibResult_.load(config_.calibResultConfig); } - - calibRobots_.reset(); } void ArmRetargetingManager::update() @@ -126,10 +124,15 @@ void ArmRetargetingManager::update() // Calculate robot pose if(calibResult_.isInitialized) { + humanShoulderPose_ = std::nullopt; + humanElbowPose_ = std::nullopt; + humanWristPose_ = std::nullopt; + robotShoulderPose_ = calibResult_.robotTransFromBaseToShoulder * ctl().robot().frame(ctl().retargetingManagerSet_->config().robotBaseLinkName).position(); robotElbowPose_ = std::nullopt; robotWristPose_ = std::nullopt; + if(humanWaistPoseManager()->isValid()) { auto scalePose = [&](const sva::PTransformd & origPose, double scale) -> sva::PTransformd { @@ -138,19 +141,26 @@ void ArmRetargetingManager::update() return newPose; }; - sva::PTransformd humanShoulderPose = calibResult_.humanTransFromBaseToShoulder * humanWaistPoseManager()->pose(); + const auto & humanWaistPoseFromOrigin = ctl().retargetingManagerSet_->config_.humanWaistPoseFromOrigin; + humanShoulderPose_ = calibResult_.humanTransFromBaseToShoulder * humanWaistPoseFromOrigin; if(humanElbowPoseManager_->isValid()) { - robotElbowPose_ = calibResult_.elbowRotTransFromHumanToRobot - * scalePose(humanElbowPoseManager_->pose() * humanShoulderPose.inv(), calibResult_.elbowScale) - * robotShoulderPose_.value(); + humanElbowPose_ = + humanElbowPoseManager_->pose() * humanWaistPoseManager()->pose().inv() * humanWaistPoseFromOrigin; + robotElbowPose_ = + calibResult_.elbowRotTransFromHumanToRobot + * scalePose(humanElbowPose_.value() * humanShoulderPose_.value().inv(), calibResult_.elbowScale) + * robotShoulderPose_.value(); } if(humanWristPoseManager_->isValid()) { - robotWristPose_ = calibResult_.wristRotTransFromHumanToRobot - * scalePose(humanWristPoseManager_->pose() * humanShoulderPose.inv(), calibResult_.wristScale) - * robotShoulderPose_.value(); + humanWristPose_ = + humanWristPoseManager_->pose() * humanWaistPoseManager()->pose().inv() * humanWaistPoseFromOrigin; + robotWristPose_ = + calibResult_.wristRotTransFromHumanToRobot + * scalePose(humanWristPose_.value() * humanShoulderPose_.value().inv(), calibResult_.wristScale) + * robotShoulderPose_.value(); } } } @@ -158,8 +168,8 @@ void ArmRetargetingManager::update() // Update task target if(ctl().retargetingManagerSet_->isEnabled_) { - updateTaskTarget(elbowTask_, robotElbowPose_.value()); - updateTaskTarget(wristTask_, robotWristPose_.value()); + updateTaskTarget(elbowTask(), robotElbowPose_.value()); + updateTaskTarget(wristTask(), robotWristPose_.value()); } // Interpolate task stiffness @@ -168,14 +178,14 @@ void ArmRetargetingManager::update() if(ctl().t() < stiffnessRatioFunc_->endTime()) { double stiffnessRatio = (*stiffnessRatioFunc_)(ctl().t()); - for(const auto & task : {elbowTask_, wristTask_}) + for(const auto & task : {elbowTask(), wristTask()}) { task->stiffness(stiffnessRatio * config_.stiffness); } } else { - for(const auto & task : {elbowTask_, wristTask_}) + for(const auto & task : {elbowTask(), wristTask()}) { task->stiffness(Eigen::VectorXd(config_.stiffness)); } @@ -186,7 +196,7 @@ void ArmRetargetingManager::update() void ArmRetargetingManager::stop() { - for(const auto & task : {elbowTask_, wristTask_}) + for(const auto & task : {elbowTask(), wristTask()}) { ctl().solver().removeTask(task); } @@ -212,7 +222,6 @@ void ArmRetargetingManager::addToGUI(mc_rtc::gui::StateBuilder & gui) mc_rtc::gui::Button("setRobot-X", [this]() { setRobotCalibSource("X"); }), mc_rtc::gui::Button("setRobot-Y", [this]() { setRobotCalibSource("Y"); }), mc_rtc::gui::Button("setRobot-Z", [this]() { setRobotCalibSource("Z"); })); - gui.addElement(calibCategory, mc_rtc::gui::Button("clearRobot", [this]() { clearCalibRobot(); })); } void ArmRetargetingManager::removeFromGUI(mc_rtc::gui::StateBuilder & gui) @@ -241,7 +250,7 @@ void ArmRetargetingManager::removeFromLogger(mc_rtc::Logger & logger) void ArmRetargetingManager::enable() { - for(const auto & task : {elbowTask_, wristTask_}) + for(const auto & task : {elbowTask(), wristTask()}) { task->reset(); ctl().solver().addTask(task); @@ -255,7 +264,7 @@ void ArmRetargetingManager::enable() void ArmRetargetingManager::disable() { - for(const auto & task : {elbowTask_, wristTask_}) + for(const auto & task : {elbowTask(), wristTask()}) { ctl().solver().removeTask(task); } @@ -268,6 +277,16 @@ const std::shared_ptr & ArmRetargetingManager::humanWaistPoseMan return ctl().retargetingManagerSet_->humanWaistPoseManager_; } +const std::shared_ptr & ArmRetargetingManager::elbowTask() const +{ + return ctl().retargetingTasks_.at(config_.elbowTaskName); +} + +const std::shared_ptr & ArmRetargetingManager::wristTask() const +{ + return ctl().retargetingTasks_.at(config_.wristTaskName); +} + void ArmRetargetingManager::updateTaskTarget(const std::shared_ptr & task, const sva::PTransformd & pose) { @@ -303,13 +322,9 @@ void ArmRetargetingManager::setHumanCalibSource(const std::string & axis) void ArmRetargetingManager::setRobotCalibSource(const std::string & axis) { - if(!calibRobots_) - { - calibRobots_ = mc_rbdyn::loadRobot(ctl().robot().module()); - mc_rtc::ROSBridge::update_robot_publisher("calib", ctl().dt(), calibRobots_->robot()); - } + ctl().retargetingManagerSet_->makeCalibRobot(); - auto & calibRobot = calibRobots_->robot(); + auto & calibRobot = ctl().retargetingManagerSet_->calibRobots_->robot(); for(const auto & [jointName, jointPos] : config_.robotCalibPostures.at(axis)) { calibRobot.q()[calibRobot.jointIndexByName(jointName)][0] = jointPos; @@ -317,26 +332,15 @@ void ArmRetargetingManager::setRobotCalibSource(const std::string & axis) calibRobot.forwardKinematics(); const auto & basePose = calibRobot.frame(ctl().retargetingManagerSet_->config().robotBaseLinkName).position(); - const auto & elbowPose = calibRobot.frame(elbowTask_->frame().name()).position(); - const auto & wristPose = calibRobot.frame(wristTask_->frame().name()).position(); + const auto & elbowPose = calibRobot.frame(elbowTask()->frame().name()).position(); + const auto & wristPose = calibRobot.frame(wristTask()->frame().name()).position(); robotCalibSource_.emplace(axis, std::array{elbowPose * basePose.inv(), wristPose * basePose.inv()}); } -void ArmRetargetingManager::clearCalibRobot() -{ - if(!calibRobots_) - { - return; - } - - mc_rtc::ROSBridge::stop_robot_publisher("calib"); - calibRobots_.reset(); -} - void ArmRetargetingManager::updateCalib() { - clearCalibRobot(); + ctl().retargetingManagerSet_->clearCalibRobot(); if(!(humanCalibSource_.count("X") && humanCalibSource_.count("Y") && humanCalibSource_.count("Z") && robotCalibSource_.count("X") && robotCalibSource_.count("Y") && robotCalibSource_.count("Z"))) @@ -366,8 +370,8 @@ void ArmRetargetingManager::updateCalib() calibResult_.elbowScale = robotElbowWristLengths[0] / humanElbowWristLengths[0]; calibResult_.wristScale = robotElbowWristLengths[1] / humanElbowWristLengths[1]; - mc_rtc::log::success("[ArmRetargetingManager({})] Calibration result:", std::to_string(config_.armSide)); - calibResult_.dump(); + mc_rtc::log::success("[ArmRetargetingManager({})] Calibration result:\n{}", std::to_string(config_.armSide), + calibResult_.dump()); } sva::PTransformd ArmRetargetingManager::calcShoulderPoseForCalib(const CalibSource & calibSource) const @@ -380,6 +384,12 @@ sva::PTransformd ArmRetargetingManager::calcShoulderPoseForCalib(const CalibSour { posMat.col(i) = calibSource.at(axes[i])[0].translation(); dirMat.col(i) = (calibSource.at(axes[i])[1].translation() - calibSource.at(axes[i])[0].translation()).normalized(); + + if(config_.armSide == ArmSide::Right && axes[i] == "Y") + { + posMat.col(i).y() *= -1.0; + dirMat.col(i).y() *= -1.0; + } } { diff --git a/src/HumanRetargetingController.cpp b/src/HumanRetargetingController.cpp index 60d9275..6c35ad3 100644 --- a/src/HumanRetargetingController.cpp +++ b/src/HumanRetargetingController.cpp @@ -19,12 +19,12 @@ HumanRetargetingController::HumanRetargetingController(mc_rbdyn::RobotModulePtr // Setup tasks if(config().has("RetargetingTaskList")) { - for(const auto & retargetingTaskConfig : config()("RetargetingTaskList")) + for(const auto & [taskName, retargetingTaskConfig] : + static_cast>(config()("RetargetingTaskList"))) { - std::string bodyPartName = retargetingTaskConfig("bodyPart"); retargetingTasks_.emplace( - bodyPartName, mc_tasks::MetaTaskLoader::load(solver(), retargetingTaskConfig)); - retargetingTasks_.at(bodyPartName)->name("RetargetingTask_" + bodyPartName); + taskName, mc_tasks::MetaTaskLoader::load(solver(), retargetingTaskConfig)); + retargetingTasks_.at(taskName)->name("RetargetingTask_" + taskName); } } else diff --git a/src/RetargetingManagerSet.cpp b/src/RetargetingManagerSet.cpp index 995f258..2a6cc56 100644 --- a/src/RetargetingManagerSet.cpp +++ b/src/RetargetingManagerSet.cpp @@ -4,6 +4,8 @@ #include #include #include +#include +#include #include #include @@ -24,6 +26,8 @@ void RetargetingManagerSet::Configuration::load(const mc_rtc::Configuration & mc mcRtcConfig("syncJoints", syncJoints); + mcRtcConfig("humanWaistPoseFromOrigin", humanWaistPoseFromOrigin); + mcRtcConfig("pointMarkerSize", pointMarkerSize); mcRtcConfig("phaseMarkerPoseOffset", phaseMarkerPoseOffset); } @@ -60,6 +64,8 @@ void RetargetingManagerSet::reset() isReady_ = false; isEnabled_ = false; + calibRobots_.reset(); + for(const auto & armManagerKV : *this) { armManagerKV.second->reset(); @@ -98,6 +104,9 @@ void RetargetingManagerSet::addToGUI(mc_rtc::gui::StateBuilder & gui) mc_rtc::gui::Label("isReady", [this]() { return isReady_ ? "Yes" : "No"; }), mc_rtc::gui::Label("isEnabled", [this]() { return isEnabled_ ? "Yes" : "No"; })); + gui.addElement({ctl().name(), config_.name, "Calib"}, + mc_rtc::gui::Button("clearRobot", [this]() { clearCalibRobot(); })); + for(const auto & armManagerKV : *this) { armManagerKV.second->addToGUI(gui); @@ -225,8 +234,7 @@ void RetargetingManagerSet::updateReadiness() if(!poseManager->isValid()) { isReady_ = false; - invalidReasonStr += - "[" + std::to_string(armManagerKV.first) + poseName + "] " + humanWaistPoseManager_->invalidReasonStr_; + invalidReasonStr += "[" + std::to_string(armManagerKV.first) + poseName + "] " + poseManager->invalidReasonStr_; } } } @@ -294,77 +302,127 @@ void RetargetingManagerSet::updateGUI() } // Add pose markers - auto getRobotPose = [](const std::shared_ptr & _armManager, - const std::string & _poseName) -> std::optional { - if(_poseName == "Shoulder") - { - return _armManager->robotShoulderPose_; - } - else if(_poseName == "Elbow") - { - return _armManager->robotElbowPose_; - } - else if(_poseName == "Wrist") - { - return _armManager->robotWristPose_; - } - else - { - mc_rtc::log::error_and_throw("[RetargetingManagerSet] Invalid pose name: {}", _poseName); - } - }; - mc_rtc::gui::Color pointColor = mc_rtc::gui::Color(0.0, 1.0, 0.0, 0.5); - mc_rtc::gui::Color arrowColor = mc_rtc::gui::Color(0.2, 0.4, 0.2, 0.6); + { + auto getPointColor = [](const std::string & _entityName) { + return _entityName == "Human" ? mc_rtc::gui::Color(1.0, 0.0, 0.0, 0.5) : mc_rtc::gui::Color(0.0, 1.0, 0.0, 0.5); + }; + auto getArrowColor = [](const std::string & _entityName) { + return _entityName == "Human" ? mc_rtc::gui::Color(0.4, 0.2, 0.2, 0.6) : mc_rtc::gui::Color(0.2, 0.4, 0.2, 0.6); + }; + auto getPose = [](const std::shared_ptr & _armManager, const std::string & _entityName, + const std::string & _poseName) -> std::optional { + if(_poseName == "Shoulder") + { + return _entityName == "Human" ? _armManager->humanShoulderPose_ : _armManager->robotShoulderPose_; + } + else if(_poseName == "Elbow") + { + return _entityName == "Human" ? _armManager->humanElbowPose_ : _armManager->robotElbowPose_; + } + else if(_poseName == "Wrist") + { + return _entityName == "Human" ? _armManager->humanWristPose_ : _armManager->robotWristPose_; + } + else + { + mc_rtc::log::error_and_throw("[RetargetingManagerSet] Invalid pose name: {}", _poseName); + } + }; - ctl().gui()->removeCategory({ctl().name(), config_.name, "Marker"}); + ctl().gui()->removeCategory({ctl().name(), config_.name, "Marker"}); - for(const auto & [armSide, armManager] : *this) - { - for(const auto & poseName : {"Shoulder", "Elbow", "Wrist"}) + for(const auto & [armSide, armManager] : *this) { - if(getRobotPose(armManager, poseName).has_value()) + for(const auto & poseName : {"Shoulder", "Elbow", "Wrist"}) { - ctl().gui()->addElement( - {ctl().name(), config_.name, "Marker"}, - mc_rtc::gui::Point3D(std::to_string(armSide) + poseName, - mc_rtc::gui::PointConfig(pointColor, config_.pointMarkerSize), - [=]() { return getRobotPose(armManager, poseName).value().translation(); })); + for(const auto & entityName : {"Human", "Robot"}) + { + if(getPose(armManager, entityName, poseName).has_value()) + { + ctl().gui()->addElement( + {ctl().name(), config_.name, "Marker"}, + mc_rtc::gui::Point3D(entityName + std::to_string(armSide) + poseName + "Point", + mc_rtc::gui::PointConfig(getPointColor(entityName), config_.pointMarkerSize), + [=]() { return getPose(armManager, entityName, poseName).value().translation(); }), + mc_rtc::gui::Transform(entityName + std::to_string(armSide) + poseName + "Transform", + [=]() { return getPose(armManager, entityName, poseName).value(); })); + } + } } - } - for(const auto & [poseName1, poseName2] : - std::vector>{{"Shoulder", "Elbow"}, {"Elbow", "Wrist"}}) - { - if(getRobotPose(armManager, poseName1).has_value() && getRobotPose(armManager, poseName2).has_value()) + for(const auto & [poseName1, poseName2] : + std::vector>{{"Shoulder", "Elbow"}, {"Elbow", "Wrist"}}) { - ctl().gui()->addElement({ctl().name(), config_.name, "Marker"}, - mc_rtc::gui::Arrow( - std::to_string(armSide) + poseName1 + "To" + poseName2, arrowColor, - [=]() { return getRobotPose(armManager, poseName1).value().translation(); }, - [=]() { return getRobotPose(armManager, poseName2).value().translation(); })); + for(const auto & entityName : {"Human", "Robot"}) + { + if(getPose(armManager, entityName, poseName1).has_value() + && getPose(armManager, entityName, poseName2).has_value()) + { + ctl().gui()->addElement( + {ctl().name(), config_.name, "Marker"}, + mc_rtc::gui::Arrow( + entityName + std::to_string(armSide) + poseName1 + "To" + poseName2 + "Arrow", + getArrowColor(entityName), + [=]() { return getPose(armManager, entityName, poseName1).value().translation(); }, + [=]() { return getPose(armManager, entityName, poseName2).value().translation(); })); + } + } } } } // Add phase marker - auto getPhaseColor = [this]() -> mc_rtc::gui::Color { - return isEnabled_ ? mc_rtc::gui::Color(1.0, 0.0, 1.0, 0.5) : mc_rtc::gui::Color(0.0, 1.0, 1.0, 0.5); - }; + { + auto getPose = [this]() -> sva::PTransformd { + return sva::PTransformd(config_.phaseMarkerPoseOffset.rotation(), + ctl().robot().posW().translation() + config_.phaseMarkerPoseOffset.translation()); + }; + auto getColor = [this]() -> mc_rtc::gui::Color { + return isEnabled_ ? mc_rtc::gui::Color(1.0, 0.0, 1.0, 0.5) : mc_rtc::gui::Color(0.0, 1.0, 1.0, 0.5); + }; + + if(isReady_) + { + ctl().gui()->addElement( + {ctl().name(), config_.name, "Marker"}, + mc_rtc::gui::Ellipsoid( + "Phase", {0.2, 0.2, 0.15}, [=]() { return getPose(); }, [=]() { return getColor(); })); + } + else + { + ctl().gui()->addElement({ctl().name(), config_.name, "Marker"}, + mc_rtc::gui::Cylinder( + "Phase", {0.1, 0.01}, [=]() { return getPose(); }, [=]() { return getColor(); })); + } + } + + // Visualize robot for calibration + if(calibRobots_) + { + mc_rtc::ROSBridge::update_robot_publisher("calib", ctl().dt(), calibRobots_->robot()); + } +} - if(isReady_) +void RetargetingManagerSet::makeCalibRobot() +{ + if(calibRobots_) { - ctl().gui()->addElement({ctl().name(), config_.name, "Marker"}, - mc_rtc::gui::Ellipsoid( - "Phase", {0.2, 0.2, 0.15}, - [this]() { return config_.phaseMarkerPoseOffset * ctl().robot().posW(); }, - [=]() { return getPhaseColor(); })); + return; } - else + + calibRobots_ = mc_rbdyn::Robots::make(); + ctl().robots().copy(*calibRobots_); + auto frames = ctl().config()("frames", std::vector{}); + calibRobots_->robot().makeFrames(frames); +} + +void RetargetingManagerSet::clearCalibRobot() +{ + if(!calibRobots_) { - ctl().gui()->addElement({ctl().name(), config_.name, "Marker"}, - mc_rtc::gui::Cylinder( - "Phase", {0.1, 0.01}, - [this]() { return config_.phaseMarkerPoseOffset * ctl().robot().posW(); }, - [=]() { return getPhaseColor(); })); + return; } + + mc_rtc::ROSBridge::stop_robot_publisher("calib"); + calibRobots_.reset(); }