From bc9e4af8177bf633edfbf83748e8bfc25c9ea216 Mon Sep 17 00:00:00 2001 From: dcam0050 Date: Sat, 19 May 2018 15:34:13 +0000 Subject: [PATCH 1/6] Updates to pepper_description to fix Odometry issue. Introduction of pid gazebo for mimic joints to stop pepper from falling over --- .../pepperGazebo.xacro | 749 ++++++++++++++++++ .../pepperTransmission.xacro | 156 ++++ .../config/pepper_gazebo_controller.yaml | 140 ++++ .../launch/pepper_gazebo_plugin_Y20.launch | 11 +- 4 files changed, 1052 insertions(+), 4 deletions(-) create mode 100644 pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro create mode 100644 pepper_description/urdf/pepper1.0_generated_urdf/pepperTransmission.xacro create mode 100644 pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml diff --git a/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro b/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro new file mode 100644 index 0000000..69169dd --- /dev/null +++ b/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro @@ -0,0 +1,749 @@ + + + + + + + ${side}Finger11_link + + + ${side}Finger12_link + + + ${side}Finger13_link + + + ${side}Finger21_link + + + ${side}Finger22_link + + + ${side}Finger23_link + + + ${side}Finger31_link + + + ${side}Finger32_link + + + ${side}Finger33_link + + + ${side}Finger41_link + + + ${side}Finger42_link + + + ${side}Finger43_link + + + ${side}Thumb1_link + + + ${side}Thumb2_link + + + ${side_small}_gripper + + + + + + + ${side}Hand + ${side}Finger${num}1 + -${PI/4} + ${PI/4} + + + ${side}Hand + ${side}Finger${num}2 + -${PI/4} + ${PI/4} + + + ${side}Hand + ${side}Finger${num}3 + -${PI/4} + ${PI/4} + + + + + + ${side}Hand + ${side}Thumb1 + -${PI/4} + ${PI/4} + + + ${side}Hand + ${side}Thumb2 + -${PI/4} + ${PI/4} + + + + + + + + + gazebo_ros_control/DefaultRobotHWSim + + + + + + + + + + + + + + + + + + + + + + + false + + + false + + + + 0.5 + 0.5 + false + false + + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + true + + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + true + + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + + 1 + 0 + + + + 0.003 + false + + + 0.5 + 0.5 + + + + 0.003 + false + + + 0 + 1 + + + + 0.003 + false + + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + + 0.5 + 0.5 + false + true + + + 0.5 + 0.5 + false + true + + + + + + 5.0 + + 1.0 + + 640 + 480 + R8G8B8 + + + 0.3 + 500 + + + gaussian + 0.0 + 0.007 + + + + /pepper_robot + true + 5.0 + camera/front + image_raw + camera_info + CameraTop_optical_frame + 0.07 + -0.0545211 + 0.06919734 + -0.0241095 + -0.0112245 + 0.0 + + + + + + + 5.0 + + 1.0 + + 640 + 480 + R8G8B8 + + + 0.05 + 500 + + + gaussian + 0.0 + 0.007 + + + + /pepper_robot + true + 5.0 + camera/bottom + image_raw + camera_info + CameraBottom_optical_frame + 0.07 + -0.0648764 + 0.06125202 + 0.00382815 + -0.00551104 + 0.0 + + + + + + + true + 20.0 + + ${58.0*3.14159265359/180.0} + + B8G8R8 + 320 + 240 + + + 0.4 + 8.0 + + + + true + 20.0 + pepper_robot/camera + ir/image_raw + depth/camera_info + depth/image_raw + depth/points + CameraDepth_optical_frame + 0.4 + 8.0 + + + + + + + + + + + + 0 0 0 0 0 0 + 20 + false + + + + 5 + 3 + -0.52 + 0.52 + + + 5 + 3 + -0.52 + 0.52 + + + + 0.0 + 5.0 + 3 + + + + /pepper_robot + 0.05 + true + 20 + sonar_back + SonarBack_frame + 0.0 + 5.0 + 1.05 + ultrasound + + + + + + + 0 0 0 0 0 0 + 20 + false + + + + 5 + 3 + -0.52 + 0.52 + + + 5 + 3 + -0.52 + 0.52 + + + + 0.0 + 5.0 + 3 + + + + /pepper_robot + 0.05 + true + 20 + sonar_front + SonarFront_frame + 0.0 + 5.0 + 1.05 + ultrasound + + + + + + + + true + false + 100 + / + WheelFL + WheelFR + 0.170 + 0.140 + 36.9 + cmd_vel + odom_diffdrive + odom + world + 1 + na + 0 + 5 + base_link + false + false + + + + diff --git a/pepper_description/urdf/pepper1.0_generated_urdf/pepperTransmission.xacro b/pepper_description/urdf/pepper1.0_generated_urdf/pepperTransmission.xacro new file mode 100644 index 0000000..7f571ef --- /dev/null +++ b/pepper_description/urdf/pepper1.0_generated_urdf/pepperTransmission.xacro @@ -0,0 +1,156 @@ + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + + PositionJointInterface + ${speed_red_type1A} + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + + PositionJointInterface + ${speed_red_type2A} + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + PositionJointInterface + ${speed_red_type1A} + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + PositionJointInterface + ${speed_red_type2A} + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + PositionJointInterface + ${speed_red_type1A} + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + PositionJointInterface + ${speed_red_type2A} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + PositionJointInterface + ${speed_red_type2A} + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + PositionJointInterface + ${speed_red_type3B} + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + + PositionJointInterface + ${speed_red_type4A} + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + + PositionJointInterface + ${speed_red_type5A} + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + + PositionJointInterface + ${speed_red_type5A} + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + + EffortJointInterface + ${speed_red_type5B} + + + + + + + diff --git a/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml b/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml new file mode 100644 index 0000000..0089a15 --- /dev/null +++ b/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml @@ -0,0 +1,140 @@ +# Note: You MUST load these PID parameters for all joints that are using the +# PositionJointInterface, otherwise the arm + gripper will act like a giant +# parachute, counteracting gravity, and causing some of the wheels to lose +# contact with the ground, so the robot won't be able to properly navigate. See +# https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612 +gazebo_ros_control: + pid_gains: + HeadYaw: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + HeadPitch: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LeftShoulderPitch: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RightShoulderPitch: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LeftShoulderRoll: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RightShoulderRoll: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LeftElbowYaw: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RightElbowYaw: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LeftElbowRoll: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RightElbowRoll: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LeftWristYaw: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RightWristYaw: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LeftHand: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RightHand: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + HipRoll: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + HipPitch: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + KneePitch: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + WheelLeft: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + WheelRight: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true \ No newline at end of file diff --git a/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch b/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch index 85a749d..6164df1 100644 --- a/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch +++ b/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch @@ -3,6 +3,9 @@ + + + @@ -10,13 +13,13 @@ - + + ---> + From 097e6e6922f9b98e5a5103b48dde9abced1db4ce Mon Sep 17 00:00:00 2001 From: dcam0050 Date: Sat, 19 May 2018 16:16:07 +0000 Subject: [PATCH 2/6] moved xacro to pepper_robot repo. Modified default pepper launch world --- .../pepperGazebo.xacro | 749 ------------------ .../pepperTransmission.xacro | 156 ---- .../launch/pepper_gazebo_plugin_Y20.launch | 2 +- 3 files changed, 1 insertion(+), 906 deletions(-) delete mode 100644 pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro delete mode 100644 pepper_description/urdf/pepper1.0_generated_urdf/pepperTransmission.xacro diff --git a/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro b/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro deleted file mode 100644 index 69169dd..0000000 --- a/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro +++ /dev/null @@ -1,749 +0,0 @@ - - - - - - - ${side}Finger11_link - - - ${side}Finger12_link - - - ${side}Finger13_link - - - ${side}Finger21_link - - - ${side}Finger22_link - - - ${side}Finger23_link - - - ${side}Finger31_link - - - ${side}Finger32_link - - - ${side}Finger33_link - - - ${side}Finger41_link - - - ${side}Finger42_link - - - ${side}Finger43_link - - - ${side}Thumb1_link - - - ${side}Thumb2_link - - - ${side_small}_gripper - - - - - - - ${side}Hand - ${side}Finger${num}1 - -${PI/4} - ${PI/4} - - - ${side}Hand - ${side}Finger${num}2 - -${PI/4} - ${PI/4} - - - ${side}Hand - ${side}Finger${num}3 - -${PI/4} - ${PI/4} - - - - - - ${side}Hand - ${side}Thumb1 - -${PI/4} - ${PI/4} - - - ${side}Hand - ${side}Thumb2 - -${PI/4} - ${PI/4} - - - - - - - - - gazebo_ros_control/DefaultRobotHWSim - - - - - - - - - - - - - - - - - - - - - - - false - - - false - - - - 0.5 - 0.5 - false - false - - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - true - - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - true - - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - 0.5 - 0.5 - false - false - - - - 1 - 0 - - - - 0.003 - false - - - 0.5 - 0.5 - - - - 0.003 - false - - - 0 - 1 - - - - 0.003 - false - - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - - 0.5 - 0.5 - false - true - - - 0.5 - 0.5 - false - true - - - - - - 5.0 - - 1.0 - - 640 - 480 - R8G8B8 - - - 0.3 - 500 - - - gaussian - 0.0 - 0.007 - - - - /pepper_robot - true - 5.0 - camera/front - image_raw - camera_info - CameraTop_optical_frame - 0.07 - -0.0545211 - 0.06919734 - -0.0241095 - -0.0112245 - 0.0 - - - - - - - 5.0 - - 1.0 - - 640 - 480 - R8G8B8 - - - 0.05 - 500 - - - gaussian - 0.0 - 0.007 - - - - /pepper_robot - true - 5.0 - camera/bottom - image_raw - camera_info - CameraBottom_optical_frame - 0.07 - -0.0648764 - 0.06125202 - 0.00382815 - -0.00551104 - 0.0 - - - - - - - true - 20.0 - - ${58.0*3.14159265359/180.0} - - B8G8R8 - 320 - 240 - - - 0.4 - 8.0 - - - - true - 20.0 - pepper_robot/camera - ir/image_raw - depth/camera_info - depth/image_raw - depth/points - CameraDepth_optical_frame - 0.4 - 8.0 - - - - - - - - - - - - 0 0 0 0 0 0 - 20 - false - - - - 5 - 3 - -0.52 - 0.52 - - - 5 - 3 - -0.52 - 0.52 - - - - 0.0 - 5.0 - 3 - - - - /pepper_robot - 0.05 - true - 20 - sonar_back - SonarBack_frame - 0.0 - 5.0 - 1.05 - ultrasound - - - - - - - 0 0 0 0 0 0 - 20 - false - - - - 5 - 3 - -0.52 - 0.52 - - - 5 - 3 - -0.52 - 0.52 - - - - 0.0 - 5.0 - 3 - - - - /pepper_robot - 0.05 - true - 20 - sonar_front - SonarFront_frame - 0.0 - 5.0 - 1.05 - ultrasound - - - - - - - - true - false - 100 - / - WheelFL - WheelFR - 0.170 - 0.140 - 36.9 - cmd_vel - odom_diffdrive - odom - world - 1 - na - 0 - 5 - base_link - false - false - - - - diff --git a/pepper_description/urdf/pepper1.0_generated_urdf/pepperTransmission.xacro b/pepper_description/urdf/pepper1.0_generated_urdf/pepperTransmission.xacro deleted file mode 100644 index 7f571ef..0000000 --- a/pepper_description/urdf/pepper1.0_generated_urdf/pepperTransmission.xacro +++ /dev/null @@ -1,156 +0,0 @@ - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - - PositionJointInterface - ${speed_red_type1A} - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - - PositionJointInterface - ${speed_red_type2A} - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - PositionJointInterface - ${speed_red_type1A} - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - PositionJointInterface - ${speed_red_type2A} - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - PositionJointInterface - ${speed_red_type1A} - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - PositionJointInterface - ${speed_red_type2A} - - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - PositionJointInterface - ${speed_red_type2A} - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - PositionJointInterface - ${speed_red_type3B} - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - - PositionJointInterface - ${speed_red_type4A} - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - - PositionJointInterface - ${speed_red_type5A} - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - - PositionJointInterface - ${speed_red_type5A} - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - - EffortJointInterface - ${speed_red_type5B} - - - - - - - diff --git a/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch b/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch index 6164df1..ad4392b 100644 --- a/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch +++ b/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch @@ -8,7 +8,7 @@ - + From 6c36c9ef9900a456cf95c4f3ec990b4e43909e37 Mon Sep 17 00:00:00 2001 From: dcam0050 Date: Sat, 19 May 2018 18:02:33 +0000 Subject: [PATCH 3/6] base configuration --- .../config/pepper_gazebo_controller.yaml | 119 ------------------ .../launch/pepper_gazebo.launch | 27 ++++ .../launch/pepper_gazebo_plugin_Y20.launch | 8 +- pepper_gazebo_plugin/worlds/empty.world | 36 ++++++ 4 files changed, 67 insertions(+), 123 deletions(-) create mode 100644 pepper_gazebo_plugin/launch/pepper_gazebo.launch create mode 100644 pepper_gazebo_plugin/worlds/empty.world diff --git a/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml b/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml index 0089a15..464e813 100644 --- a/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml +++ b/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml @@ -13,125 +13,6 @@ gazebo_ros_control: antiwindup: false publish_state: true HeadPitch: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - LeftShoulderPitch: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - RightShoulderPitch: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - LeftShoulderRoll: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - RightShoulderRoll: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - LeftElbowYaw: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - RightElbowYaw: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - LeftElbowRoll: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - RightElbowRoll: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - LeftWristYaw: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - RightWristYaw: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - LeftHand: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - RightHand: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - HipRoll: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - HipPitch: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - KneePitch: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - WheelLeft: - p: 20.0 - i: 0.1 - d: 0.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - WheelRight: p: 20.0 i: 0.1 d: 0.0 diff --git a/pepper_gazebo_plugin/launch/pepper_gazebo.launch b/pepper_gazebo_plugin/launch/pepper_gazebo.launch new file mode 100644 index 0000000..eed4eef --- /dev/null +++ b/pepper_gazebo_plugin/launch/pepper_gazebo.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch b/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch index ad4392b..b933daf 100644 --- a/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch +++ b/pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch @@ -13,15 +13,15 @@ - - --> + - + + args="-param robot_description -urdf -x -0.5 -y 1 -z 0.83 -model pepper_MP" /> diff --git a/pepper_gazebo_plugin/worlds/empty.world b/pepper_gazebo_plugin/worlds/empty.world new file mode 100644 index 0000000..b645238 --- /dev/null +++ b/pepper_gazebo_plugin/worlds/empty.world @@ -0,0 +1,36 @@ + + + + + model://ground_plane + + + model://sun + + + + + + + From 760e0de02e407ff35d9f2839fe7c62ede06c992c Mon Sep 17 00:00:00 2001 From: dcam0050 Date: Sat, 19 May 2018 19:27:42 +0000 Subject: [PATCH 4/6] Pepper falls naturally under gravity --- .../config/pepper_gazebo_controller.yaml | 343 ++++++++++++++++++ 1 file changed, 343 insertions(+) diff --git a/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml b/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml index 464e813..5c3f974 100644 --- a/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml +++ b/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml @@ -13,6 +13,349 @@ gazebo_ros_control: antiwindup: false publish_state: true HeadPitch: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + WheelFL: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + WheelFR: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + WheelB: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + HandR: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + HandL: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LWristYaw: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RWristYaw: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + base_link_fixedjoint: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + base_footprint_joint: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + HipRoll: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + HipPitch: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger11: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger12: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger13: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger11: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger12: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger13: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger21: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger22: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger23: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger21: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger22: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger23: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger31: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger32: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger33: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger31: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger32: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger33: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger41: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger42: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LFinger43: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger41: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger42: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RFinger43: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LThumb1: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RThumb1: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LThumb2: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RThumb2: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LElbowRoll: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RElbowRoll: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LElbowYaw: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RElbowYaw: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LShoulderRoll: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RShoulderRoll: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + LShoulderPitch: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RShoulderPitch: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + RShoulder: + p: 20.0 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + antiwindup: false + publish_state: true + KneePitch: p: 20.0 i: 0.1 d: 0.0 From 81c333246399ab75dd23886753eacf1523d9e65b Mon Sep 17 00:00:00 2001 From: dcam0050 Date: Sat, 19 May 2018 20:31:14 +0000 Subject: [PATCH 5/6] Changed PID parameters for stable upright posture but have not tested control --- .../config/pepper_gazebo_controller.yaml | 306 +++++++++--------- 1 file changed, 153 insertions(+), 153 deletions(-) diff --git a/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml b/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml index 5c3f974..0db6d7f 100644 --- a/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml +++ b/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml @@ -6,359 +6,359 @@ gazebo_ros_control: pid_gains: HeadYaw: - p: 20.0 - i: 0.1 - d: 0.0 + p: 50 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true HeadPitch: - p: 20.0 - i: 0.1 - d: 0.0 + p: 50 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true WheelFL: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true WheelFR: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true WheelB: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true HandR: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true HandL: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LWristYaw: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RWristYaw: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true base_link_fixedjoint: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true base_footprint_joint: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true HipRoll: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true HipPitch: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger11: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger12: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger13: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger11: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger12: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger13: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger21: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger22: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger23: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger21: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger22: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger23: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger31: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger32: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger33: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger31: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger32: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger33: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger41: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger42: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger43: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger41: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger42: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger43: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LThumb1: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RThumb1: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LThumb2: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RThumb2: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LElbowRoll: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RElbowRoll: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LElbowYaw: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RElbowYaw: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LShoulderRoll: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RShoulderRoll: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LShoulderPitch: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RShoulderPitch: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true RShoulder: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true KneePitch: - p: 20.0 - i: 0.1 - d: 0.0 + p: 100 + i: 0.0 + d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true \ No newline at end of file From f49445644523c722c859308c1dcce10d15779780 Mon Sep 17 00:00:00 2001 From: dcam0050 Date: Sat, 19 May 2018 23:35:42 +0000 Subject: [PATCH 6/6] Better tuning of pid values for pepper and tested functioning control via moveit --- .../config/pepper_gazebo_controller.yaml | 219 +++++++++--------- 1 file changed, 106 insertions(+), 113 deletions(-) diff --git a/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml b/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml index 0db6d7f..27e1174 100644 --- a/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml +++ b/pepper_gazebo_plugin/config/pepper_gazebo_controller.yaml @@ -6,358 +6,351 @@ gazebo_ros_control: pid_gains: HeadYaw: - p: 50 - i: 0.0 - d: 5.0 + p: 100 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true HeadPitch: - p: 50 - i: 0.0 - d: 5.0 + p: 100 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true WheelFL: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true WheelFR: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true WheelB: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true HandR: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true HandL: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LWristYaw: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RWristYaw: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true base_link_fixedjoint: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true base_footprint_joint: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true HipRoll: - p: 100 - i: 0.0 + p: 1000 + i: 0.1 d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true HipPitch: - p: 100 - i: 0.0 + p: 1000 + i: 0.1 d: 5.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger11: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger12: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger13: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger11: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger12: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger13: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger21: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger22: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger23: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger21: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger22: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger23: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger31: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger32: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger33: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger31: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger32: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger33: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger41: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger42: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LFinger43: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger41: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger42: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RFinger43: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LThumb1: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RThumb1: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LThumb2: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RThumb2: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LElbowRoll: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RElbowRoll: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LElbowYaw: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RElbowYaw: p: 100 - i: 0.0 - d: 5.0 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LShoulderRoll: - p: 100 - i: 0.0 - d: 5.0 + p: 500 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RShoulderRoll: - p: 100 - i: 0.0 - d: 5.0 + p: 500 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true LShoulderPitch: - p: 100 - i: 0.0 - d: 5.0 + p: 500 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true RShoulderPitch: - p: 100 - i: 0.0 - d: 5.0 - i_clamp: 0.2 - antiwindup: false - publish_state: true - RShoulder: - p: 100 - i: 0.0 - d: 5.0 + p: 500 + i: 0.1 + d: 1.0 i_clamp: 0.2 antiwindup: false publish_state: true KneePitch: - p: 100 - i: 0.0 + p: 1000 + i: 0.1 d: 5.0 i_clamp: 0.2 antiwindup: false