diff --git a/.gitignore b/.gitignore index 4a20580..59f3c6a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,5 @@ .vscode/ -*.setup_assistant +*/.setup_assistant log/ install/ build/ \ No newline at end of file diff --git a/README.md b/README.md index 6841262..53e71c8 100644 --- a/README.md +++ b/README.md @@ -135,7 +135,7 @@ It contains the same information described in the previous paragraph, but custom ## Use case -As described in the previous section, this repository contains two demos that shows the possibility to control a yarp-based robot within the MoveIt2 framework in the Cartesian space. First of all, make sure that `yarpserver` is running on your machine. Then, you can start launching a basic simulation: +As described in the previous section, this repository contains two demos that show the possibility to control a yarp-based robot within the MoveIt2 framework in the Cartesian space. First of all, make sure that `yarpserver` is running on your machine. Then, you can start launching a basic simulation: ```shell # Build the packages within your ros2 workspace @@ -179,7 +179,7 @@ source install/setup.bash export YARP_ROBOT_NAME="icub" # For the grasping demo -ros2 launch grasp_moveit grasp_demo.launch.py +ros2 launch robot_moveit grasp_demo.launch.py ``` If you want to see iCub performing a circle movement, instead of the last line, you can run: @@ -191,5 +191,6 @@ ros2 launch robot_moveit circle_demo.launch.py and follow the instructions on the third shell you opened. You should have something like this: - \ No newline at end of file +
+
\ No newline at end of file diff --git a/assets/icub_root_link.png b/assets/icub_root_link.png new file mode 100644 index 0000000..6a312bc Binary files /dev/null and b/assets/icub_root_link.png differ diff --git a/assets/manipulation1_tollerance.png b/assets/manipulation1_tollerance.png new file mode 100644 index 0000000..64d4cdc Binary files /dev/null and b/assets/manipulation1_tollerance.png differ diff --git a/assets/solve_type.png b/assets/solve_type.png new file mode 100644 index 0000000..d1b8016 Binary files /dev/null and b/assets/solve_type.png differ diff --git a/assets/workspace_estimation.png b/assets/workspace_estimation.png new file mode 100644 index 0000000..c6a04cf Binary files /dev/null and b/assets/workspace_estimation.png differ diff --git a/ergocub_moveit_config/.setup_assistant b/ergocub_moveit_config/.setup_assistant index a526662..4219e1c 100644 --- a/ergocub_moveit_config/.setup_assistant +++ b/ergocub_moveit_config/.setup_assistant @@ -7,4 +7,19 @@ moveit_setup_assistant_config: package_settings: author_name: Martina Gloria author_email: martina.gloria@iit.it - generated_timestamp: 1695734298 \ No newline at end of file + generated_timestamp: 1699889574 + control_xacro: + command: + - position + state: + - position + - velocity + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + - position + state: + - position + - velocity \ No newline at end of file diff --git a/ergocub_moveit_config/config/ergocub.ros2_control.xacro b/ergocub_moveit_config/config/ergocub.ros2_control.xacro index f9195c1..d172ce9 100644 --- a/ergocub_moveit_config/config/ergocub.ros2_control.xacro +++ b/ergocub_moveit_config/config/ergocub.ros2_control.xacro @@ -274,7 +274,7 @@ - + robot_controller/RobotController ros2_cb_node @@ -301,52 +301,52 @@ - + - ${initial_positions['l_shoulder_pitch']} + ${initial_positions['r_shoulder_pitch']} - + - ${initial_positions['l_shoulder_roll']} + ${initial_positions['r_shoulder_roll']} - + - ${initial_positions['l_shoulder_yaw']} + ${initial_positions['r_shoulder_yaw']} - + - ${initial_positions['l_elbow']} + ${initial_positions['r_elbow']} - + - ${initial_positions['l_wrist_yaw']} + ${initial_positions['r_wrist_yaw']} - + - ${initial_positions['l_wrist_roll']} + ${initial_positions['r_wrist_roll']} - + - ${initial_positions['l_wrist_pitch']} + ${initial_positions['r_wrist_pitch']} diff --git a/ergocub_moveit_config/config/ergocub.srdf b/ergocub_moveit_config/config/ergocub.srdf index fc4646e..bbffeba 100644 --- a/ergocub_moveit_config/config/ergocub.srdf +++ b/ergocub_moveit_config/config/ergocub.srdf @@ -59,47 +59,12 @@ - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - - - - - - - - + diff --git a/ergocub_moveit_config/config/initial_positions.yaml b/ergocub_moveit_config/config/initial_positions.yaml index ab01e9b..bc5af9d 100644 --- a/ergocub_moveit_config/config/initial_positions.yaml +++ b/ergocub_moveit_config/config/initial_positions.yaml @@ -7,22 +7,10 @@ initial_positions: l_hip_pitch: 0 l_hip_roll: 0 l_hip_yaw: 0 - l_index_add: 0 - l_index_dist: 0 - l_index_prox: 0 l_knee: 0 - l_middle_dist: 0 - l_middle_prox: 0 - l_pinkie_dist: 0 - l_pinkie_prox: 0 - l_ring_dist: 0 - l_ring_prox: 0 l_shoulder_pitch: 0 l_shoulder_roll: 0 l_shoulder_yaw: 0 - l_thumb_add: 0 - l_thumb_dist: 0 - l_thumb_prox: 0 l_wrist_pitch: 0 l_wrist_roll: 0 l_wrist_yaw: 0 @@ -35,25 +23,13 @@ initial_positions: r_hip_pitch: 0 r_hip_roll: 0 r_hip_yaw: 0 - r_index_add: 0 - r_index_dist: 0 - r_index_prox: 0 r_knee: 0 - r_middle_dist: 0 - r_middle_prox: 0 - r_pinkie_dist: 0 - r_pinkie_prox: 0 - r_ring_dist: 0 - r_ring_prox: 0 r_shoulder_pitch: 0 r_shoulder_roll: 0 r_shoulder_yaw: 0 - r_thumb_add: 0 - r_thumb_dist: 0 - r_thumb_prox: 0 r_wrist_pitch: 0 r_wrist_roll: 0 r_wrist_yaw: 0 torso_pitch: 0 torso_roll: 0 - torso_yaw: 0 + torso_yaw: 0 \ No newline at end of file diff --git a/ergocub_moveit_config/config/kinematics.yaml b/ergocub_moveit_config/config/kinematics.yaml index 9a6d0dc..4427033 100644 --- a/ergocub_moveit_config/config/kinematics.yaml +++ b/ergocub_moveit_config/config/kinematics.yaml @@ -6,15 +6,15 @@ left_arm: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin kinematics_solver_search_resolution: 0.0050000000000000001 kinematics_solver_timeout: 0.0050000000000000001 -left_arm_torso: +left_leg: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin kinematics_solver_search_resolution: 0.0050000000000000001 kinematics_solver_timeout: 0.0050000000000000001 -left_leg: +right_arm: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin kinematics_solver_search_resolution: 0.0050000000000000001 kinematics_solver_timeout: 0.0050000000000000001 -right_arm: +right_arm_torso: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin kinematics_solver_search_resolution: 0.0050000000000000001 kinematics_solver_timeout: 0.0050000000000000001 diff --git a/ergocub_moveit_config/config/moveit.rviz b/ergocub_moveit_config/config/moveit.rviz index 58f18d9..5b74535 100644 --- a/ergocub_moveit_config/config/moveit.rviz +++ b/ergocub_moveit_config/config/moveit.rviz @@ -2,56 +2,2512 @@ Panels: - Class: rviz_visual_tools/RvizVisualToolsGui Name: RvizVisualToolsGui - Class: rviz_common/Displays + Help Height: 70 Name: Displays Property Tree Widget: + Expanded: + - /TF1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 267 - Class: rviz_common/Help Name: Help Visualization Manager: + Class: "" Displays: - - Class: rviz_default_plugins/Grid + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: Value: true - Class: rviz_default_plugins/MarkerArray - Name: MarkerArray Enabled: true - Topic: /display_contacts + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /display_contacts + Value: true - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: false + chest: + Value: false + chest_ems_gyro_eb1: + Value: false + chest_ems_gyro_eb2: + Value: false + chest_ems_gyro_eb3: + Value: false + chest_ems_gyro_eb4: + Value: false + head: + Value: false + head_imu_0: + Value: false + l_ankle_2: + Value: false + l_arm_ft: + Value: false + l_elbow_1: + Value: false + l_foot: + Value: false + l_foot_dh_frame: + Value: false + l_foot_ft: + Value: false + l_foot_ft_gyro_3b13: + Value: false + l_forearm: + Value: false + l_forearm_dh_frame: + Value: false + l_forearm_skin_0: + Value: false + l_forearm_skin_1: + Value: false + l_forearm_skin_10: + Value: false + l_forearm_skin_11: + Value: false + l_forearm_skin_12: + Value: false + l_forearm_skin_13: + Value: false + l_forearm_skin_14: + Value: false + l_forearm_skin_15: + Value: false + l_forearm_skin_16: + Value: false + l_forearm_skin_17: + Value: false + l_forearm_skin_19: + Value: false + l_forearm_skin_2: + Value: false + l_forearm_skin_22: + Value: false + l_forearm_skin_24: + Value: false + l_forearm_skin_25: + Value: false + l_forearm_skin_28: + Value: false + l_forearm_skin_29: + Value: false + l_forearm_skin_3: + Value: false + l_forearm_skin_4: + Value: false + l_forearm_skin_5: + Value: false + l_forearm_skin_6: + Value: false + l_forearm_skin_7: + Value: false + l_forearm_skin_8: + Value: false + l_forearm_skin_9: + Value: false + l_hand: + Value: false + l_hand_dh_frame: + Value: false + l_hip_2: + Value: false + l_hip_3: + Value: false + l_leg_ft: + Value: false + l_lower_leg: + Value: false + l_lower_leg_ems_gyro_eb7: + Value: false + l_shoulder_1: + Value: false + l_shoulder_2: + Value: false + l_shoulder_3: + Value: false + l_sole: + Value: false + l_sole_skin_0: + Value: false + l_sole_skin_1: + Value: false + l_sole_skin_10: + Value: false + l_sole_skin_11: + Value: false + l_sole_skin_12: + Value: false + l_sole_skin_13: + Value: false + l_sole_skin_14: + Value: false + l_sole_skin_15: + Value: false + l_sole_skin_17: + Value: false + l_sole_skin_18: + Value: false + l_sole_skin_19: + Value: false + l_sole_skin_2: + Value: false + l_sole_skin_20: + Value: false + l_sole_skin_21: + Value: false + l_sole_skin_22: + Value: false + l_sole_skin_24: + Value: false + l_sole_skin_25: + Value: false + l_sole_skin_26: + Value: false + l_sole_skin_27: + Value: false + l_sole_skin_28: + Value: false + l_sole_skin_29: + Value: false + l_sole_skin_3: + Value: false + l_sole_skin_4: + Value: false + l_sole_skin_8: + Value: false + l_sole_skin_9: + Value: false + l_upper_arm: + Value: false + l_upper_leg: + Value: false + l_upper_leg_back_contact: + Value: false + l_upper_leg_dh_frame: + Value: false + l_upper_leg_ems_gyro_eb10: + Value: false + l_upper_leg_ems_gyro_eb6: + Value: false + l_upper_leg_ft_gyro_3b12: + Value: false + l_wrist_1: + Value: false + neck_1: + Value: false + neck_2: + Value: false + r_ankle_2: + Value: false + r_arm_ft: + Value: false + r_elbow_1: + Value: false + r_foot: + Value: false + r_foot_dh_frame: + Value: false + r_foot_ft: + Value: false + r_foot_ft_gyro_3b14: + Value: false + r_forearm: + Value: false + r_forearm_dh_frame: + Value: false + r_forearm_skin_0: + Value: false + r_forearm_skin_1: + Value: false + r_forearm_skin_10: + Value: false + r_forearm_skin_11: + Value: false + r_forearm_skin_12: + Value: false + r_forearm_skin_13: + Value: false + r_forearm_skin_14: + Value: false + r_forearm_skin_15: + Value: false + r_forearm_skin_16: + Value: false + r_forearm_skin_17: + Value: false + r_forearm_skin_19: + Value: false + r_forearm_skin_2: + Value: false + r_forearm_skin_22: + Value: false + r_forearm_skin_24: + Value: false + r_forearm_skin_25: + Value: false + r_forearm_skin_28: + Value: false + r_forearm_skin_29: + Value: false + r_forearm_skin_3: + Value: false + r_forearm_skin_4: + Value: false + r_forearm_skin_5: + Value: false + r_forearm_skin_6: + Value: false + r_forearm_skin_7: + Value: false + r_forearm_skin_8: + Value: false + r_forearm_skin_9: + Value: false + r_hand: + Value: false + r_hand_dh_frame: + Value: true + r_hip_2: + Value: false + r_hip_3: + Value: false + r_leg_ft: + Value: false + r_lower_leg: + Value: false + r_lower_leg_ems_gyro_eb9: + Value: false + r_lower_leg_skin_0: + Value: false + r_lower_leg_skin_10: + Value: false + r_lower_leg_skin_11: + Value: false + r_lower_leg_skin_14: + Value: false + r_lower_leg_skin_15: + Value: false + r_lower_leg_skin_16: + Value: false + r_lower_leg_skin_17: + Value: false + r_lower_leg_skin_19: + Value: false + r_lower_leg_skin_20: + Value: false + r_lower_leg_skin_21: + Value: false + r_lower_leg_skin_28: + Value: false + r_lower_leg_skin_29: + Value: false + r_lower_leg_skin_3: + Value: false + r_lower_leg_skin_31: + Value: false + r_lower_leg_skin_32: + Value: false + r_lower_leg_skin_35: + Value: false + r_lower_leg_skin_36: + Value: false + r_lower_leg_skin_37: + Value: false + r_lower_leg_skin_38: + Value: false + r_lower_leg_skin_4: + Value: false + r_lower_leg_skin_41: + Value: false + r_lower_leg_skin_42: + Value: false + r_lower_leg_skin_43: + Value: false + r_lower_leg_skin_46: + Value: false + r_lower_leg_skin_47: + Value: false + r_lower_leg_skin_49: + Value: false + r_lower_leg_skin_5: + Value: false + r_lower_leg_skin_50: + Value: false + r_lower_leg_skin_51: + Value: false + r_lower_leg_skin_52: + Value: false + r_lower_leg_skin_53: + Value: false + r_lower_leg_skin_54: + Value: false + r_lower_leg_skin_55: + Value: false + r_lower_leg_skin_56: + Value: false + r_lower_leg_skin_6: + Value: false + r_lower_leg_skin_60: + Value: false + r_lower_leg_skin_61: + Value: false + r_lower_leg_skin_9: + Value: false + r_shoulder_1: + Value: false + r_shoulder_2: + Value: false + r_shoulder_3: + Value: false + r_sole: + Value: false + r_sole_skin_0: + Value: false + r_sole_skin_1: + Value: false + r_sole_skin_10: + Value: false + r_sole_skin_11: + Value: false + r_sole_skin_12: + Value: false + r_sole_skin_13: + Value: false + r_sole_skin_14: + Value: false + r_sole_skin_15: + Value: false + r_sole_skin_17: + Value: false + r_sole_skin_18: + Value: false + r_sole_skin_19: + Value: false + r_sole_skin_2: + Value: false + r_sole_skin_20: + Value: false + r_sole_skin_21: + Value: false + r_sole_skin_22: + Value: false + r_sole_skin_24: + Value: false + r_sole_skin_25: + Value: false + r_sole_skin_26: + Value: false + r_sole_skin_27: + Value: false + r_sole_skin_28: + Value: false + r_sole_skin_29: + Value: false + r_sole_skin_3: + Value: false + r_sole_skin_4: + Value: false + r_sole_skin_8: + Value: false + r_sole_skin_9: + Value: false + r_upper_arm: + Value: false + r_upper_leg: + Value: false + r_upper_leg_back_contact: + Value: false + r_upper_leg_dh_frame: + Value: false + r_upper_leg_ems_gyro_eb11: + Value: false + r_upper_leg_ems_gyro_eb8: + Value: false + r_upper_leg_ft_gyro_3b11: + Value: false + r_wrist_1: + Value: false + root_link: + Value: true + root_link_ems_gyro_eb5: + Value: false + torso_1: + Value: false + torso_2: + Value: false + Marker Scale: 1 Name: TF - Enabled: false - - Class: moveit_rviz_plugin/MotionPlanning + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + root_link: + base_link: + {} + root_link_ems_gyro_eb5: + {} + torso_1: + torso_2: + chest: + chest_ems_gyro_eb1: + {} + chest_ems_gyro_eb2: + {} + chest_ems_gyro_eb3: + {} + chest_ems_gyro_eb4: + {} + l_shoulder_1: + l_shoulder_2: + l_shoulder_3: + l_arm_ft: + {} + l_upper_arm: + l_elbow_1: + l_forearm: + l_forearm_dh_frame: + {} + l_forearm_skin_0: + {} + l_forearm_skin_1: + {} + l_forearm_skin_10: + {} + l_forearm_skin_11: + {} + l_forearm_skin_12: + {} + l_forearm_skin_13: + {} + l_forearm_skin_14: + {} + l_forearm_skin_15: + {} + l_forearm_skin_16: + {} + l_forearm_skin_17: + {} + l_forearm_skin_19: + {} + l_forearm_skin_2: + {} + l_forearm_skin_22: + {} + l_forearm_skin_24: + {} + l_forearm_skin_25: + {} + l_forearm_skin_28: + {} + l_forearm_skin_29: + {} + l_forearm_skin_3: + {} + l_forearm_skin_4: + {} + l_forearm_skin_5: + {} + l_forearm_skin_6: + {} + l_forearm_skin_7: + {} + l_forearm_skin_8: + {} + l_forearm_skin_9: + {} + l_wrist_1: + l_hand: + l_hand_dh_frame: + {} + neck_1: + neck_2: + head: + head_imu_0: + {} + r_shoulder_1: + r_shoulder_2: + r_shoulder_3: + r_arm_ft: + {} + r_upper_arm: + r_elbow_1: + r_forearm: + r_forearm_dh_frame: + {} + r_forearm_skin_0: + {} + r_forearm_skin_1: + {} + r_forearm_skin_10: + {} + r_forearm_skin_11: + {} + r_forearm_skin_12: + {} + r_forearm_skin_13: + {} + r_forearm_skin_14: + {} + r_forearm_skin_15: + {} + r_forearm_skin_16: + {} + r_forearm_skin_17: + {} + r_forearm_skin_19: + {} + r_forearm_skin_2: + {} + r_forearm_skin_22: + {} + r_forearm_skin_24: + {} + r_forearm_skin_25: + {} + r_forearm_skin_28: + {} + r_forearm_skin_29: + {} + r_forearm_skin_3: + {} + r_forearm_skin_4: + {} + r_forearm_skin_5: + {} + r_forearm_skin_6: + {} + r_forearm_skin_7: + {} + r_forearm_skin_8: + {} + r_forearm_skin_9: + {} + r_wrist_1: + r_hand: + r_hand_dh_frame: + {} + Update Interval: 0 + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + chest: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + chest_ems_gyro_eb1: + Alpha: 1 + Show Axes: false + Show Trail: false + chest_ems_gyro_eb2: + Alpha: 1 + Show Axes: false + Show Trail: false + chest_ems_gyro_eb3: + Alpha: 1 + Show Axes: false + Show Trail: false + chest_ems_gyro_eb4: + Alpha: 1 + Show Axes: false + Show Trail: false + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_imu_0: + Alpha: 1 + Show Axes: false + Show Trail: false + l_ankle_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_ankle_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_arm_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + l_elbow_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_foot_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_foot_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + l_foot_ft_gyro_3b13: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_0: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_1: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_10: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_11: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_12: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_13: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_14: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_15: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_16: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_17: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_19: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_2: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_22: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_24: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_25: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_28: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_29: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_3: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_4: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_5: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_6: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_7: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_8: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_9: + Alpha: 1 + Show Axes: false + Show Trail: false + l_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hand_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_hip_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_leg_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + l_lower_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_lower_leg_ems_gyro_eb7: + Alpha: 1 + Show Axes: false + Show Trail: false + l_shoulder_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_0: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_1: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_10: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_11: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_12: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_13: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_14: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_15: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_17: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_18: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_19: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_2: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_20: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_21: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_22: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_24: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_25: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_26: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_27: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_28: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_29: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_3: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_4: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_8: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_9: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_leg_back_contact: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_leg_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_leg_ems_gyro_eb10: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_leg_ems_gyro_eb6: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_leg_ft_gyro_3b12: + Alpha: 1 + Show Axes: false + Show Trail: false + l_wrist_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + neck_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + neck_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_ankle_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_ankle_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_arm_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + r_elbow_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_foot_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_foot_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + r_foot_ft_gyro_3b14: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_0: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_1: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_10: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_11: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_12: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_13: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_14: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_15: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_16: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_17: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_19: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_2: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_22: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_24: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_25: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_28: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_29: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_3: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_4: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_5: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_6: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_7: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_8: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_9: + Alpha: 1 + Show Axes: false + Show Trail: false + r_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hand_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_hip_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_leg_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_lower_leg_ems_gyro_eb9: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_0: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_10: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_11: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_14: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_15: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_16: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_17: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_19: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_20: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_21: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_28: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_29: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_3: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_31: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_32: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_35: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_36: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_37: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_38: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_4: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_41: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_42: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_43: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_46: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_47: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_49: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_5: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_50: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_51: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_52: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_53: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_54: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_55: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_56: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_6: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_60: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_61: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_9: + Alpha: 1 + Show Axes: false + Show Trail: false + r_shoulder_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_0: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_1: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_10: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_11: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_12: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_13: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_14: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_15: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_17: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_18: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_19: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_2: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_20: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_21: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_22: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_24: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_25: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_26: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_27: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_28: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_29: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_3: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_4: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_8: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_9: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_leg_back_contact: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_leg_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_leg_ems_gyro_eb11: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_leg_ems_gyro_eb8: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_leg_ft_gyro_3b11: + Alpha: 1 + Show Axes: false + Show Trail: false + r_wrist_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + root_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + root_link_ems_gyro_eb5: + Alpha: 1 + Show Axes: false + Show Trail: false + torso_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Loop Animation: false - State Display Time: 0.05 s + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false Show Robot Visual: false + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: head + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 Planning Scene Topic: monitored_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + chest: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + chest_ems_gyro_eb1: + Alpha: 1 + Show Axes: false + Show Trail: false + chest_ems_gyro_eb2: + Alpha: 1 + Show Axes: false + Show Trail: false + chest_ems_gyro_eb3: + Alpha: 1 + Show Axes: false + Show Trail: false + chest_ems_gyro_eb4: + Alpha: 1 + Show Axes: false + Show Trail: false + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_imu_0: + Alpha: 1 + Show Axes: false + Show Trail: false + l_ankle_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_ankle_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_arm_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + l_elbow_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_foot_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_foot_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + l_foot_ft_gyro_3b13: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_0: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_1: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_10: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_11: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_12: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_13: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_14: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_15: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_16: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_17: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_19: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_2: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_22: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_24: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_25: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_28: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_29: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_3: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_4: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_5: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_6: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_7: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_8: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_skin_9: + Alpha: 1 + Show Axes: false + Show Trail: false + l_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hand_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_hip_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_leg_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + l_lower_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_lower_leg_ems_gyro_eb7: + Alpha: 1 + Show Axes: false + Show Trail: false + l_shoulder_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_0: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_1: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_10: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_11: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_12: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_13: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_14: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_15: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_17: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_18: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_19: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_2: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_20: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_21: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_22: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_24: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_25: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_26: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_27: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_28: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_29: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_3: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_4: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_8: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole_skin_9: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_leg_back_contact: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_leg_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_leg_ems_gyro_eb10: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_leg_ems_gyro_eb6: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_leg_ft_gyro_3b12: + Alpha: 1 + Show Axes: false + Show Trail: false + l_wrist_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + neck_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + neck_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_ankle_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_ankle_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_arm_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + r_elbow_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_foot_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_foot_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + r_foot_ft_gyro_3b14: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_0: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_1: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_10: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_11: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_12: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_13: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_14: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_15: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_16: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_17: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_19: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_2: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_22: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_24: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_25: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_28: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_29: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_3: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_4: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_5: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_6: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_7: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_8: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_skin_9: + Alpha: 1 + Show Axes: false + Show Trail: false + r_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hand_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_hip_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_leg_ft: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_lower_leg_ems_gyro_eb9: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_0: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_10: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_11: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_14: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_15: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_16: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_17: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_19: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_20: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_21: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_28: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_29: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_3: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_31: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_32: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_35: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_36: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_37: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_38: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_4: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_41: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_42: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_43: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_46: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_47: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_49: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_5: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_50: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_51: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_52: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_53: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_54: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_55: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_56: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_6: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_60: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_61: + Alpha: 1 + Show Axes: false + Show Trail: false + r_lower_leg_skin_9: + Alpha: 1 + Show Axes: false + Show Trail: false + r_shoulder_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_0: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_1: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_10: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_11: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_12: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_13: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_14: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_15: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_17: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_18: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_19: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_2: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_20: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_21: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_22: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_24: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_25: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_26: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_27: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_28: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_29: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_3: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_4: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_8: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole_skin_9: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_leg_back_contact: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_leg_dh_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_leg_ems_gyro_eb11: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_leg_ems_gyro_eb8: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_leg_ft_gyro_3b11: + Alpha: 1 + Show Axes: false + Show Trail: false + r_wrist_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + root_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + root_link_ems_gyro_eb5: + Alpha: 1 + Show Axes: false + Show Trail: false + torso_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true Global Options: + Background Color: 48; 48; 48 Fixed Frame: root_link + Frame Rate: 30 + Name: root Tools: - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.0 + Distance: 2 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false Focal Point: - X: -0.1 + X: -0.10000000149011612 Y: 0.25 - Z: 0.30 + Z: 0.30000001192092896 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Pitch: 0.5 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.12500008940696716 Target Frame: root_link - Yaw: -0.623 + Value: Orbit (rviz_default_plugins) + Yaw: 3.220193386077881 + Saved: ~ Window Geometry: - Height: 975 - QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Width: 1200 + Displays: + collapsed: false + Height: 1136 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000416fc0200000006fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d0000018e000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001d1000002230000017d00fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b50000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006901000003fa000000590000004100ffffff0000047e0000041600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RvizVisualToolsGui: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/ergocub_moveit_config/config/moveit_controllers.yaml b/ergocub_moveit_config/config/moveit_controllers.yaml index 5f48d1c..7571971 100644 --- a/ergocub_moveit_config/config/moveit_controllers.yaml +++ b/ergocub_moveit_config/config/moveit_controllers.yaml @@ -9,7 +9,7 @@ controller_names: - right_leg_controller - head_controller - torso_controller - - left_arm_torso_controller + - right_arm_torso_controller left_arm_controller: type: FollowJointTrajectory @@ -77,7 +77,7 @@ torso_controller: - torso_pitch - torso_roll - torso_yaw -left_arm_torso_controller: +right_arm_torso_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true @@ -85,10 +85,10 @@ left_arm_torso_controller: - torso_roll - torso_pitch - torso_yaw - - l_shoulder_pitch - - l_shoulder_roll - - l_shoulder_yaw - - l_elbow - - l_wrist_yaw - - l_wrist_roll - - l_wrist_pitch + - r_shoulder_pitch + - r_shoulder_roll + - r_shoulder_yaw + - r_elbow + - r_wrist_yaw + - r_wrist_roll + - r_wrist_pitch \ No newline at end of file diff --git a/ergocub_moveit_config/config/ros2_controllers.yaml b/ergocub_moveit_config/config/ros2_controllers.yaml index 535918b..e1f46d0 100644 --- a/ergocub_moveit_config/config/ros2_controllers.yaml +++ b/ergocub_moveit_config/config/ros2_controllers.yaml @@ -27,7 +27,7 @@ controller_manager: type: joint_trajectory_controller/JointTrajectoryController - left_arm_torso_controller: + right_arm_torso_controller: type: joint_trajectory_controller/JointTrajectoryController @@ -114,19 +114,19 @@ torso_controller: state_interfaces: - position - velocity -left_arm_torso_controller: +right_arm_torso_controller: ros__parameters: joints: - torso_roll - torso_pitch - torso_yaw - - l_shoulder_pitch - - l_shoulder_roll - - l_shoulder_yaw - - l_elbow - - l_wrist_yaw - - l_wrist_roll - - l_wrist_pitch + - r_shoulder_pitch + - r_shoulder_roll + - r_shoulder_yaw + - r_elbow + - r_wrist_yaw + - r_wrist_roll + - r_wrist_pitch command_interfaces: - position state_interfaces: diff --git a/grasp_moveit/src/grasp.cpp b/grasp_moveit/src/grasp.cpp index 8e1c94b..18db839 100644 --- a/grasp_moveit/src/grasp.cpp +++ b/grasp_moveit/src/grasp.cpp @@ -118,8 +118,17 @@ int main(int argc, char** argv) visual_tools.deleteAllMarkers(); visual_tools.trigger(); - move_group.setJointValueTarget(joint_values); - move_group.move(); + // move_group.setJointValueTarget(joint_values); + // move_group.move(); + + waypoints3.clear(); + rclcpp::sleep_for(std::chrono::milliseconds(1s)); + + waypoints3.push_back(start_pose); + moveit_msgs::msg::RobotTrajectory inv_trajectory; + double fraction = move_group.computeCartesianPath(waypoints3, eef_step, jump_threshold, inv_trajectory, avoid_collisions); + RCLCPP_INFO(LOGGER, "Cartesian path: %.2f%% achieved", fraction * 100.0); + move_group.execute(inv_trajectory); rclcpp::shutdown(); return 0; diff --git a/robot_moveit/launch/circle_demo.launch.py b/robot_moveit/launch/circle_demo.launch.py index e66222f..ab472be 100644 --- a/robot_moveit/launch/circle_demo.launch.py +++ b/robot_moveit/launch/circle_demo.launch.py @@ -3,10 +3,20 @@ from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder -robot_name = os.environ["YARP_ROBOT_NAME"] +def check_robot_name(): + try: + if "icub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "icub" + elif "ergocub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "ergocub" + return yarp_robot_name + except Exception as e: + print(f"Caught exception: env variable {e} is not set, please provide it.") def generate_launch_description(): + robot_name = check_robot_name() + moveit_config = MoveItConfigsBuilder(robot_name).to_moveit_configs() circle_demo = Node( diff --git a/robot_moveit/launch/grasp_demo.launch.py b/robot_moveit/launch/grasp_demo.launch.py index 3d7de25..cb062d3 100644 --- a/robot_moveit/launch/grasp_demo.launch.py +++ b/robot_moveit/launch/grasp_demo.launch.py @@ -3,10 +3,20 @@ from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder -robot_name = os.environ["YARP_ROBOT_NAME"] +def check_robot_name(): + try: + if "icub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "icub" + elif "ergocub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "ergocub" + return yarp_robot_name + except Exception as e: + print(f"Caught exception: env variable {e} is not set, please provide it.") def generate_launch_description(): + robot_name = check_robot_name() + moveit_config = MoveItConfigsBuilder(robot_name).to_moveit_configs() grasping_demo = Node( diff --git a/robot_moveit/launch/robot.launch.py b/robot_moveit/launch/robot.launch.py index e4e0a21..6f704e4 100644 --- a/robot_moveit/launch/robot.launch.py +++ b/robot_moveit/launch/robot.launch.py @@ -5,6 +5,16 @@ from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder +def check_robot_name(): + try: + if "icub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "icub" + elif "ergocub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "ergocub" + return yarp_robot_name + except Exception as e: + print(f"Caught exception: env variable {e} is not set, please provide it.") + def load_file(package_name, file_path): package_path = get_package_share_directory(package_name) absolute_file_path = os.path.join(package_path, file_path) @@ -22,11 +32,11 @@ def load_yaml(package_name, file_path): return yaml.safe_load(file) except EnvironmentError: return None - -robot_name = os.environ["YARP_ROBOT_NAME"] def generate_launch_description(): + robot_name = check_robot_name() + moveit_config = ( MoveItConfigsBuilder(robot_name) .robot_description(file_path="config/"+robot_name+".urdf.xacro") diff --git a/robot_moveit/launch/robot_controls.launch.py b/robot_moveit/launch/robot_controls.launch.py index 1366b74..c3db551 100644 --- a/robot_moveit/launch/robot_controls.launch.py +++ b/robot_moveit/launch/robot_controls.launch.py @@ -6,10 +6,20 @@ from launch.event_handlers import OnProcessExit from ament_index_python.packages import get_package_share_directory -robot_name = os.environ["YARP_ROBOT_NAME"] +def check_robot_name(): + try: + if "icub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "icub" + elif "ergocub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "ergocub" + return yarp_robot_name + except Exception as e: + print(f"Caught exception: env variable {e} is not set, please provide it.") def generate_launch_description(): + robot_name = check_robot_name() + moveit_config = MoveItConfigsBuilder(robot_name).to_moveit_configs() ros2_controllers_path = os.path.join( diff --git a/robot_moveit/launch/robot_sim.launch.py b/robot_moveit/launch/robot_sim.launch.py index 1c08569..4ac836e 100644 --- a/robot_moveit/launch/robot_sim.launch.py +++ b/robot_moveit/launch/robot_sim.launch.py @@ -7,7 +7,16 @@ from moveit_configs_utils import MoveItConfigsBuilder from launch.launch_description_sources import PythonLaunchDescriptionSource - +def check_robot_name(): + try: + if "icub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "icub" + elif "ergocub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "ergocub" + return yarp_robot_name + except Exception as e: + print(f"Caught exception: env variable {e} is not set, please provide it.") + def load_file(package_name, file_path): package_path = get_package_share_directory(package_name) absolute_file_path = os.path.join(package_path, file_path) @@ -26,9 +35,10 @@ def load_yaml(package_name, file_path): except EnvironmentError: return None -robot_name = os.environ["YARP_ROBOT_NAME"] - def generate_launch_description(): + + robot_name = check_robot_name() + gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']) ) diff --git a/test_controller/CMakeLists.txt b/test_controller/CMakeLists.txt new file mode 100644 index 0000000..8e4b489 --- /dev/null +++ b/test_controller/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.8) +project(test_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(gazebo_msgs REQUIRED) +find_package(moveit_visual_tools REQUIRED) +find_package(controller_manager REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(yarp_control_msgs REQUIRED) +find_package(YARP REQUIRED) + +add_executable(${PROJECT_NAME} src/test_controller.cpp) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + ${PROJECT_NAME} + "moveit_ros_planning_interface" + "rclcpp" + "rclpy" + "moveit_visual_tools" + "controller_manager" + "yarp_control_msgs" + "rclcpp_lifecycle" + "hardware_interface" +) + +target_sources(test_controller PRIVATE src/test_controller.cpp) +target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/test_controller/launch/test_controller.launch.py b/test_controller/launch/test_controller.launch.py new file mode 100644 index 0000000..b2a2c09 --- /dev/null +++ b/test_controller/launch/test_controller.launch.py @@ -0,0 +1,37 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + +def check_robot_name(): + try: + if "icub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "icub" + elif "ergocub" in os.environ["YARP_ROBOT_NAME"].casefold(): + yarp_robot_name = "ergocub" + return yarp_robot_name + except Exception as e: + print(f"Caught exception: env variable {e} is not set, please provide it.") + +def generate_launch_description(): + + robot_name = check_robot_name() + + moveit_config = MoveItConfigsBuilder(robot_name).to_moveit_configs() + + test_controller_node = Node( + name="test_controller", + package="test_controller", + executable="test_controller", + output="screen", + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + {"use_sim_time": True} + ], + ) + + return LaunchDescription([ + test_controller_node + ]) \ No newline at end of file diff --git a/test_controller/package.xml b/test_controller/package.xml new file mode 100644 index 0000000..2327b7a --- /dev/null +++ b/test_controller/package.xml @@ -0,0 +1,27 @@ + + + + test_controller + 0.0.0 + TODO: Package description + martinagloria + TODO: License declaration + + ament_cmake + + moveit_ros_planning_interface + rclcpp + rclpy + moveit_visual_tools + gazebo_msgs + controller_manager + gazebo_ros2_control + yarp_control_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/test_controller/report.md b/test_controller/report.md new file mode 100644 index 0000000..21219ce --- /dev/null +++ b/test_controller/report.md @@ -0,0 +1,73 @@ +# Test on the controller performance + +The following report describes the design of the test and the procedures adopted to sample the iCub workspace using ros2_control and MoveIt2 tools. +- [Test on the controller performance](#test-on-the-controller-performance) + - [1. Identify the workspace](#1-identify-the-workspace) + - [2. Choose the orientation](#2-choose-the-orientation) + - [3. Tuning TRAC-IK parameters](#3-tuning-trac-ik-parameters) + - [3.1 Change `epsilon` values with fixed `solve_type`](#31-change-epsilon-values-with-fixed-solve_type) + - [3.2 Change `solve_type` values with chosen `epsilon`](#32-change-solve_type-values-with-chosen-epsilon) + - [4. Results](#4-results) + +⚠ **Important note** + +These tests were carried out in a simulated environment using `iCubGazeboV2_5 feet fixed` model. Moreover, the parts involved are `torso + right_arm`, with `right_hand` as the end-effector to be controlled in the Cartesian space. + +### 1. Identify the workspace +The very first thing we took into account was the portion of space to be analyzed. In this sense, starting from what was done in the past (see [here](https://github.com/robotology/icub-workspace-estimation) for reference), we decided to focus on the poses in front of the robot, usually involved in reaching tasks. + +Considering the iCub `root_link` reference frame: + +![icub_root_link](../assets/icub_root_link.png) + +we defined the workspace limits as follow: + +- x direction: `[-0.3; -0.1] m`; +- y direction: `[-0.1; 0.3] m`; +- z direction: `[-0.2; 0.2] m`; + +The spatial step chosen for this analysis was of `0.05 m` for each direction. + +### 2. Choose the orientation +Once we decided the portion of space to be sampled, we focused on the orientation. To narrow down the analysis, we decided to select only two orientations per position: + +- `palm down` +- `palm oriented leftward` + +### 3. Tuning TRAC-IK parameters +[`TRACK-IK`](http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/trac_ik/trac_ik_tutorial.html) is the inverse kinematics solver chosen for the `ros2_control` implemented in this repository. + +The available parameters for this solver are: + +- `epsilon`: Cartesian error distance used to determine a valid solution. It is `1e-5` by default; +- `position_only_ik`: it's `false` by default. If it's set to true, the solver will ignore the orientation with respect to the position; +- `solve_type`: it can be `Speed`, `Distance`, `Manipulation1` or `Manipulation2` (see [documentation](https://bitbucket.org/traclabs/trac_ik/src/rolling-devel/trac_ik_lib/) for details). + +Since we were interested in both position and orientation, `position_only_ik` remained set to `false` for every test. + +In order to understand which are the best parameters for our application, the workspace sampling test was repeated each time with a different combination of them. Moreover, for each repetition, the ideal poses and the current (real) ones were acquired to compute the error between the two. + +#### 3.1 Change `epsilon` values with fixed `solve_type` +The first analysis examined the impact of the `epsilon` parameter. In this sense, we decided to stay on a fixed `solve_type` (`Manipulation1` in this specific case), and change the Cartesian error distance in increments of 10. + +![manip1_tollerance](../assets/manipulation1_tollerance.png) + +The first thing that stood out was that with a very low value (i.e. 1e-6), it resulted in large errors from both position and orientation points of view. Concerning the other three cases, instead, no significant differences were observed; for this purpose, we decided to go with the middle value, which is `epsilon = 1e-4`. + +#### 3.2 Change `solve_type` values with chosen `epsilon` +Once the first parameter was tuned, we moved to the `solve_type`. We repeated the same test described above for the four available types and here it is was we obtained: + +![solve-type-differences](../assets/solve_type.png) + +Even in this case, there was one case between the proposed that was significantly different from the others: the results used `Speed` were the worst if compared with the other plots, so we excluded it. `Distance`, `Manipulation1` and `Manipulation2`, instead, were very similar each others but we took the former since, as described in the documentation, it is not dependent on the singular values of the Jacobian. + +### 4. Results +To summarize, the tuning of the TRAC-IK parameters resulted in: + +- `epsilon = 1e-4`; +- `position_only_ik = false`; +- `solve_type = Distance` + +Once we chose the best of those parameters, we repeated the test one last time for both the orientations taken into account (see [Section 2](#2-choose-the-orientation)), and we plotted the results in terms of position and orientation error between the set pose and the obtained one. + +![final_plots](../assets/workspace_estimation.png) \ No newline at end of file diff --git a/test_controller/src/test_controller.cpp b/test_controller/src/test_controller.cpp new file mode 100644 index 0000000..cfc6400 --- /dev/null +++ b/test_controller/src/test_controller.cpp @@ -0,0 +1,199 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_controller"); + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_node = rclcpp::Node::make_shared("move_group_interface", node_options); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_node); + std::thread([&executor]() { executor.spin(); }).detach(); + + static const std::string PLANNING_GROUP = "right_arm_torso"; + moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP); + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + moveit::core::RobotState start_state(*move_group.getCurrentState()); + + const moveit::core::JointModelGroup* joint_model_group = + move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); + std::vector joint_values; + const std::vector& joint_names = joint_model_group->getVariableNames(); + start_state.copyJointGroupPositions(joint_model_group, joint_values); + for (std::size_t i = 0; i < joint_names.size(); ++i) + { + RCLCPP_INFO(LOGGER, "Joint %s: %f", joint_names[i].c_str(), joint_values[i]); + } + + RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str()); + RCLCPP_INFO(LOGGER, "Pose reference frame: %s", move_group.getPoseReferenceFrame().c_str()); + RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str()); + + namespace rvt = rviz_visual_tools; + moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "root_link", "display_contacts", + move_group.getRobotModel()); + visual_tools.deleteAllMarkers(); + visual_tools.loadRemoteControl(); + visual_tools.trigger(); + + // define cartesian trajectories + const double jump_threshold = 0.0; + const double eef_step = 0.1; + bool avoid_collisions = false; + geometry_msgs::msg::Pose start_pose = move_group.getCurrentPose().pose; + + int count = 0; + int rep = 0; + + std::ofstream data("poses_ws_1e-4_distance_dw.txt"); + std::ofstream current_data("current_data_1e-4_distance_dw.txt"); + + std::vector> data_poses; + std::vector> current_poses; + + std::vector ws_x_limits(2, 0.0); + std::vector ws_y_limits(2, 0.0); + std::vector ws_z_limits(2, 0.0); + + ws_x_limits[0] = -0.3; + ws_x_limits[1] = -0.1; + ws_y_limits[0] = -0.1; + ws_y_limits[1] = 0.3; + ws_z_limits[0] = -0.2; + ws_z_limits[1] = 0.2; + + double dist = 0.05; + std::vector set_pose(9, 0.0); + std::vector real_pose(7, 0.0); + + for (auto i = ws_x_limits[0]; i <= ws_x_limits[1]; i = i + dist) + { + for (auto j = ws_y_limits[0]; j <= ws_y_limits[1]; j = j + dist) + { + for (auto k = ws_z_limits[0]; k <= ws_z_limits[1]; k = k + dist) + { + rep++; + + set_pose[0] = i; + set_pose[1] = j; + set_pose[2] = k; + + geometry_msgs::msg::Pose rand_position; + std::vector waypoints; + + rand_position.position.x = i; + rand_position.position.y = j; + rand_position.position.z = k; + + tf2::Quaternion orientation_hand; //downward + orientation_hand.setRPY(0, -M_PI, 0); + rand_position.orientation = tf2::toMsg(orientation_hand); + + // tf2::Quaternion grasp_orientation({-0.152, 0.702, -0.696}, 2.804); //leftward + // rand_position.orientation = tf2::toMsg(grasp_orientation); + + set_pose[3] = rand_position.orientation.x; + set_pose[4] = rand_position.orientation.y; + set_pose[5] = rand_position.orientation.z; + set_pose[6] = rand_position.orientation.w; + + waypoints.push_back(rand_position); + + moveit_msgs::msg::RobotTrajectory rand_trajectory; + double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, rand_trajectory, avoid_collisions); + RCLCPP_INFO(LOGGER, "Cartesian path: %.2f%% achieved", fraction * 100.0); + visual_tools.publishTrajectoryLine(rand_trajectory, joint_model_group); + visual_tools.trigger(); + move_group.execute(rand_trajectory); + + if (fraction == 1.0) + { + count++; + set_pose[7] = 1; //success + } + + else + set_pose[7] = 0; //failure + + set_pose[8] = fraction * 100.0; + + data_poses.push_back(set_pose); + + geometry_msgs::msg::PoseStamped current_pose = move_group.getCurrentPose(); + std::cout << "Current end effector pose " << ": x=" << current_pose.pose.position.x << ", y=" << current_pose.pose.position.y << ", z=" << current_pose.pose.position.z << std::endl; + std::cout << "Rep number: " << rep << std::endl; + + real_pose[0] = current_pose.pose.position.x; + real_pose[1] = current_pose.pose.position.y; + real_pose[2] = current_pose.pose.position.z; + real_pose[3] = current_pose.pose.orientation.x; + real_pose[4] = current_pose.pose.orientation.y; + real_pose[5] = current_pose.pose.orientation.z; + real_pose[6] = current_pose.pose.orientation.w; + + current_poses.push_back(real_pose); + + waypoints.clear(); + rclcpp::sleep_for(std::chrono::milliseconds(1s)); + waypoints.push_back(start_pose); + + // plan the inverse trajectory in cartesian space + moveit_msgs::msg::RobotTrajectory inv_trajectory; + fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, inv_trajectory, avoid_collisions); + RCLCPP_INFO(LOGGER, "Cartesian path: %.2f%% achieved", fraction * 100.0); + move_group.execute(inv_trajectory); + + rclcpp::sleep_for(std::chrono::milliseconds(1s)); + } + } + } + + RCLCPP_INFO(LOGGER, "How many time the 100%% is achieved? %d", count); + + for (const auto&d : data_poses) { + data << d[0] << " " + << d[1] << " " + << d[2] << " " + << d[3] << " " + << d[4] << " " + << d[5] << " " + << d[6] << " " + << d[7] << " " + << d[8] << std::endl; + } + + for (const auto&d : current_poses) { + current_data << d[0] << " " + << d[1] << " " + << d[2] << " " + << d[3] << " " + << d[4] << " " + << d[5] << " " + << d[6] << std::endl; + } + + data.close(); + current_data.close(); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/test_controller/utils/trac_ik_params_tuning.m b/test_controller/utils/trac_ik_params_tuning.m new file mode 100644 index 0000000..02bb687 --- /dev/null +++ b/test_controller/utils/trac_ik_params_tuning.m @@ -0,0 +1,466 @@ +%% HAND LEFTWARD +clear +clc +tl = tiledlayout(2,4); +title(tl, "Manipulation1 with right hand leftward") +colorMap1 = [ones(8,1), zeros(8,2)]; +colorMap2 = [linspace(0,1,50)',linspace(1,0,50)',zeros(50,1)]; +colorMap = [colorMap2; colorMap1]; + +%% 1e^-3 pos +set_pose_lw = readtable("poses_ws_1e-3_manipulation1.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-3_manipulation1.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; + +position_error_lw = sqrt((set_pose_lw.pos_x - current_pose_lw.pos_x).^2 + ... + (set_pose_lw.pos_y - current_pose_lw.pos_y).^2 + ... + (set_pose_lw.pos_z - current_pose_lw.pos_z).^2); + +position_error_lw(position_error_lw > 0.04) = 0.04; + +nexttile +s1 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], position_error_lw, 'filled'); +s1.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Tollerance 1e^{-3} position") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Position error [m]") + +%% 1e^-4 pos +set_pose_lw = readtable("poses_ws_1e-4_manipulation1.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_manipulation1.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; + +position_error_lw = sqrt((set_pose_lw.pos_x - current_pose_lw.pos_x).^2 + ... + (set_pose_lw.pos_y - current_pose_lw.pos_y).^2 + ... + (set_pose_lw.pos_z - current_pose_lw.pos_z).^2); + +position_error_lw(position_error_lw > 0.04) = 0.04; + +nexttile +s2 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], position_error_lw, 'filled'); +s2.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Tollerance 1e^{-4} position") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Position error [m]") + +%% 1e^-5 pos +set_pose_lw = readtable("poses_ws_lw.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_lw.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; + +position_error_lw = sqrt((set_pose_lw.pos_x - current_pose_lw.pos_x).^2 + ... + (set_pose_lw.pos_y - current_pose_lw.pos_y).^2 + ... + (set_pose_lw.pos_z - current_pose_lw.pos_z).^2); + +position_error_lw(position_error_lw > 0.04) = 0.04; + +nexttile +s3 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], position_error_lw, 'filled'); +s3.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Tollerance 1e^{-5} position") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Position error [m]") + +%% 1e^-6 pos +set_pose_lw = readtable("poses_ws_1e-6_manipulation1.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-6_manipulation1.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; + +position_error_lw = sqrt((set_pose_lw.pos_x - current_pose_lw.pos_x).^2 + ... + (set_pose_lw.pos_y - current_pose_lw.pos_y).^2 + ... + (set_pose_lw.pos_z - current_pose_lw.pos_z).^2); + +position_error_lw(position_error_lw > 0.04) = 0.04; + +nexttile +s4 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], position_error_lw, 'filled'); +s4.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Tollerance 1e^{-6} position") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Position error [m]") + +%% 1e-3 orientation +set_pose_lw = readtable("poses_ws_1e-3_manipulation1.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-3_manipulation1.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; +set_quaternion_lw = [set_pose_lw.or_w, set_pose_lw.or_x, set_pose_lw.or_y, set_pose_lw.or_z]; +quat_id_lw = quaternion(set_quaternion_lw); +current_quaterion_lw = [current_pose_lw.or_w, current_pose_lw.or_x, current_pose_lw.or_y, current_pose_lw.or_z]; +quat_real_lw = quaternion(current_quaterion_lw); + +%(w,x,y,z) convention +angular_distance_lw = rad2deg(dist(quat_id_lw, quat_real_lw)); + +%plot +nexttile +s5 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], angular_distance_lw, 'filled'); +s5.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Tollerance 1e^{-3} orientation") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Angular error [deg]") + +%% 1e-4 orientation +set_pose_lw = readtable("poses_ws_1e-4_manipulation1.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_manipulation1.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; +set_quaternion_lw = [set_pose_lw.or_w, set_pose_lw.or_x, set_pose_lw.or_y, set_pose_lw.or_z]; +quat_id_lw = quaternion(set_quaternion_lw); +current_quaterion_lw = [current_pose_lw.or_w, current_pose_lw.or_x, current_pose_lw.or_y, current_pose_lw.or_z]; +quat_real_lw = quaternion(current_quaterion_lw); + +%(w,x,y,z) convention +angular_distance_lw = rad2deg(dist(quat_id_lw, quat_real_lw)); + +%plot +nexttile +s5 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], angular_distance_lw, 'filled'); +s5.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Tollerance 1e^{-4} orientation") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Angular error [deg]") + +%% 1e-5 orientation +set_pose_lw = readtable("poses_ws_lw.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_lw.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; +set_quaternion_lw = [set_pose_lw.or_w, set_pose_lw.or_x, set_pose_lw.or_y, set_pose_lw.or_z]; +quat_id_lw = quaternion(set_quaternion_lw); +current_quaterion_lw = [current_pose_lw.or_w, current_pose_lw.or_x, current_pose_lw.or_y, current_pose_lw.or_z]; +quat_real_lw = quaternion(current_quaterion_lw); + +%(w,x,y,z) convention +angular_distance_lw = rad2deg(dist(quat_id_lw, quat_real_lw)); + +%plot +nexttile +s5 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], angular_distance_lw, 'filled'); +s5.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Tollerance 1e^{-5} orientation") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Angular error [deg]") + +%% 1e-6 orientation +set_pose_lw = readtable("poses_ws_1e-6_manipulation1.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-6_manipulation1.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; +set_quaternion_lw = [set_pose_lw.or_w, set_pose_lw.or_x, set_pose_lw.or_y, set_pose_lw.or_z]; +quat_id_lw = quaternion(set_quaternion_lw); +current_quaterion_lw = [current_pose_lw.or_w, current_pose_lw.or_x, current_pose_lw.or_y, current_pose_lw.or_z]; +quat_real_lw = quaternion(current_quaterion_lw); + +%(w,x,y,z) convention +angular_distance_lw = rad2deg(dist(quat_id_lw, quat_real_lw)); +angular_distance_lw(angular_distance_lw > 26) = 26; +%plot +nexttile +s5 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], angular_distance_lw, 'filled'); +s5.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Tollerance 1e^{-6} orientation") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Angular error [deg]") + +%% Change solve type +figure(2) +tl1 = tiledlayout(2,4); +title(tl1, "Solve type with tollerance 1e^{-4} with right hand leftward") + +%% Distance pos +set_pose_lw = readtable("poses_ws_1e-4_distance.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_distance.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; + +position_error_lw = sqrt((set_pose_lw.pos_x - current_pose_lw.pos_x).^2 + ... + (set_pose_lw.pos_y - current_pose_lw.pos_y).^2 + ... + (set_pose_lw.pos_z - current_pose_lw.pos_z).^2); + +position_error_lw(position_error_lw > 0.04) = 0.04; + +nexttile +s2 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], position_error_lw, 'filled'); +s2.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Distance position") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Position error [m]") + +%% Manipulation1 pos +set_pose_lw = readtable("poses_ws_1e-4_manipulation1.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_manipulation1.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; + +position_error_lw = sqrt((set_pose_lw.pos_x - current_pose_lw.pos_x).^2 + ... + (set_pose_lw.pos_y - current_pose_lw.pos_y).^2 + ... + (set_pose_lw.pos_z - current_pose_lw.pos_z).^2); + +position_error_lw(position_error_lw > 0.04) = 0.04; + +nexttile +s2 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], position_error_lw, 'filled'); +s2.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Manipulation1 position") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Position error [m]") + +%% Manipulation2 pos +set_pose_lw = readtable("poses_ws_1e-4_manipulation2.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_manipulation2.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; + +position_error_lw = sqrt((set_pose_lw.pos_x - current_pose_lw.pos_x).^2 + ... + (set_pose_lw.pos_y - current_pose_lw.pos_y).^2 + ... + (set_pose_lw.pos_z - current_pose_lw.pos_z).^2); + +position_error_lw(position_error_lw > 0.04) = 0.04; + +nexttile +s2 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], position_error_lw, 'filled'); +s2.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Manipulation2 position") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Position error [m]") + +%% Speed pos +set_pose_lw = readtable("poses_ws_1e-4_speed.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_speed.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; + +position_error_lw = sqrt((set_pose_lw.pos_x - current_pose_lw.pos_x).^2 + ... + (set_pose_lw.pos_y - current_pose_lw.pos_y).^2 + ... + (set_pose_lw.pos_z - current_pose_lw.pos_z).^2); + +position_error_lw(position_error_lw > 0.04) = 0.04; + +nexttile +s2 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], position_error_lw, 'filled'); +s2.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Speed position") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +% colormap(cool); +cb2 = colorbar; +xlabel(cb2, "Position error [m]") + +%% Distance or +set_pose_lw = readtable("poses_ws_1e-4_distance.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_distance.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; +set_quaternion_lw = [set_pose_lw.or_w, set_pose_lw.or_x, set_pose_lw.or_y, set_pose_lw.or_z]; +quat_id_lw = quaternion(set_quaternion_lw); +current_quaterion_lw = [current_pose_lw.or_w, current_pose_lw.or_x, current_pose_lw.or_y, current_pose_lw.or_z]; +quat_real_lw = quaternion(current_quaterion_lw); + +%(w,x,y,z) convention +angular_distance_lw = rad2deg(dist(quat_id_lw, quat_real_lw)); +angular_distance_lw(angular_distance_lw > 20) = 20; + +%plot +nexttile +s5 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], angular_distance_lw, 'filled'); +s5.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Distance orientation") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Angular error [deg]") + +%% Manip1 or +set_pose_lw = readtable("poses_ws_1e-4_manipulation1.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_manipulation1.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; +set_quaternion_lw = [set_pose_lw.or_w, set_pose_lw.or_x, set_pose_lw.or_y, set_pose_lw.or_z]; +quat_id_lw = quaternion(set_quaternion_lw); +current_quaterion_lw = [current_pose_lw.or_w, current_pose_lw.or_x, current_pose_lw.or_y, current_pose_lw.or_z]; +quat_real_lw = quaternion(current_quaterion_lw); + +%(w,x,y,z) convention +angular_distance_lw = rad2deg(dist(quat_id_lw, quat_real_lw)); +angular_distance_lw(angular_distance_lw > 20) = 20; + +%plot +nexttile +s5 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], angular_distance_lw, 'filled'); +s5.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Manipulation1 orientation") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Angular error [deg]") + +%% Manip2 or +set_pose_lw = readtable("poses_ws_1e-4_manipulation2.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_manipulation2.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; +set_quaternion_lw = [set_pose_lw.or_w, set_pose_lw.or_x, set_pose_lw.or_y, set_pose_lw.or_z]; +quat_id_lw = quaternion(set_quaternion_lw); +current_quaterion_lw = [current_pose_lw.or_w, current_pose_lw.or_x, current_pose_lw.or_y, current_pose_lw.or_z]; +quat_real_lw = quaternion(current_quaterion_lw); + +%(w,x,y,z) convention +angular_distance_lw = rad2deg(dist(quat_id_lw, quat_real_lw)); +angular_distance_lw(angular_distance_lw > 20) = 20; + +%plot +nexttile +s5 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], angular_distance_lw, 'filled'); +s5.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Manipulation2 orientation") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Angular error [deg]") + +%% Speed or +set_pose_lw = readtable("poses_ws_1e-4_speed.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_speed.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; +set_quaternion_lw = [set_pose_lw.or_w, set_pose_lw.or_x, set_pose_lw.or_y, set_pose_lw.or_z]; +quat_id_lw = quaternion(set_quaternion_lw); +current_quaterion_lw = [current_pose_lw.or_w, current_pose_lw.or_x, current_pose_lw.or_y, current_pose_lw.or_z]; +quat_real_lw = quaternion(current_quaterion_lw); + +%(w,x,y,z) convention +angular_distance_lw = rad2deg(dist(quat_id_lw, quat_real_lw)); +angular_distance_lw(angular_distance_lw > 20) = 20; + +%plot +nexttile +s5 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], angular_distance_lw, 'filled'); +s5.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Speed orientation") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Angular error [deg]") diff --git a/test_controller/utils/workspace_estimation.m b/test_controller/utils/workspace_estimation.m new file mode 100644 index 0000000..a59157d --- /dev/null +++ b/test_controller/utils/workspace_estimation.m @@ -0,0 +1,122 @@ +%% Hand downward +clear +clc + +tl = tiledlayout(2,2); +title(tl, "Workspace estimation with right hand as end-effector") +colorMap1 = [ones(8,1), zeros(8,2)]; +colorMap2 = [linspace(0,1,50)',linspace(1,0,50)',zeros(50,1)]; +colorMap = [colorMap2; colorMap1]; +%% Hand lw pos +set_pose_lw = readtable("poses_ws_1e-4_distance.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_distance.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; + +position_error_lw = sqrt((set_pose_lw.pos_x - current_pose_lw.pos_x).^2 + ... + (set_pose_lw.pos_y - current_pose_lw.pos_y).^2 + ... + (set_pose_lw.pos_z - current_pose_lw.pos_z).^2); + +position_error_lw(position_error_lw > 0.04) = 0.04; + +nexttile +s2 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], position_error_lw, 'filled'); +s2.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Position estimation with hand leftward oriented") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Position error [m]") + +%% Hand dw pos +set_pose_lw = readtable("poses_ws_1e-4_distance_dw.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_distance_dw.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; + +position_error_lw = sqrt((set_pose_lw.pos_x - current_pose_lw.pos_x).^2 + ... + (set_pose_lw.pos_y - current_pose_lw.pos_y).^2 + ... + (set_pose_lw.pos_z - current_pose_lw.pos_z).^2); + +position_error_lw(position_error_lw > 0.04) = 0.04; + +nexttile +s2 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], position_error_lw, 'filled'); +s2.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Position estimation with hand downward oriented") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Position error [m]") + +%% Hand lw orientation +set_pose_lw = readtable("poses_ws_1e-4_distance.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_distance.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; +set_quaternion_lw = [set_pose_lw.or_w, set_pose_lw.or_x, set_pose_lw.or_y, set_pose_lw.or_z]; +quat_id_lw = quaternion(set_quaternion_lw); +current_quaterion_lw = [current_pose_lw.or_w, current_pose_lw.or_x, current_pose_lw.or_y, current_pose_lw.or_z]; +quat_real_lw = quaternion(current_quaterion_lw); + +%(w,x,y,z) convention +angular_distance_lw = rad2deg(dist(quat_id_lw, quat_real_lw)); +angular_distance_lw(angular_distance_lw > 20) = 20; + +%plot +nexttile +s5 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], angular_distance_lw, 'filled'); +s5.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Orientation estimation with hand leftward oriented") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Angular error [deg]") + +%% Hand dw orientation +set_pose_lw = readtable("poses_ws_1e-4_distance_dw.txt"); +set_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w', 'success', 'percentage'}; +current_pose_lw = readtable("current_data_1e-4_distance_dw.txt"); +current_pose_lw.Properties.VariableNames = {'pos_x', 'pos_y', 'pos_z', 'or_x', 'or_y', 'or_z', 'or_w'}; +set_quaternion_lw = [set_pose_lw.or_w, set_pose_lw.or_x, set_pose_lw.or_y, set_pose_lw.or_z]; +quat_id_lw = quaternion(set_quaternion_lw); +current_quaterion_lw = [current_pose_lw.or_w, current_pose_lw.or_x, current_pose_lw.or_y, current_pose_lw.or_z]; +quat_real_lw = quaternion(current_quaterion_lw); + +%(w,x,y,z) convention +angular_distance_lw = rad2deg(dist(quat_id_lw, quat_real_lw)); +angular_distance_lw(angular_distance_lw > 20) = 20; + +%plot +nexttile +s5 = scatter3(set_pose_lw.pos_x, set_pose_lw.pos_y, set_pose_lw.pos_z, [], angular_distance_lw, 'filled'); +s5.SizeData = 50; +xlabel("x [m]"); +ylabel("y [m]"); +zlabel("z [m]"); +title("Orientation estimation with hand downward oriented") +xlim([-0.3, -0.1]) +ylim([-0.3, 0.4]) +zlim([-0.3, 0.3]) +daspect([5 5 5]) +colormap(colorMap); +cb2 = colorbar; +xlabel(cb2, "Angular error [deg]") \ No newline at end of file