From 3f8ad3924a7985b279a7594c87eeb1298e780a7c Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Thu, 16 Apr 2020 09:48:06 +0900 Subject: [PATCH] enable samplerobot gazebo simulation --- .../config/SampleRobot.yaml | 61 ++++----- .../config/SampleRobot_indigo.yaml | 127 ++++++++++++++++++ ...o_samplerobot_no_controllers_indigo.launch | 33 +++++ .../launch/samplerobot_hrpsys_bringup.launch | 3 +- .../SampleRobot/SampleRobot.urdf.xacro | 1 + .../models/samplerobot.yaml | 10 ++ 6 files changed, 204 insertions(+), 31 deletions(-) create mode 100644 hrpsys_gazebo_tutorials/config/SampleRobot_indigo.yaml create mode 100644 hrpsys_gazebo_tutorials/launch/gazebo_samplerobot_no_controllers_indigo.launch diff --git a/hrpsys_gazebo_tutorials/config/SampleRobot.yaml b/hrpsys_gazebo_tutorials/config/SampleRobot.yaml index b6c605fc..c799748e 100644 --- a/hrpsys_gazebo_tutorials/config/SampleRobot.yaml +++ b/hrpsys_gazebo_tutorials/config/SampleRobot.yaml @@ -1,6 +1,6 @@ hrpsys_gazebo_configuration: ### velocity feedback for joint control, use parameter gains/joint_name/p_v - use_velocity_feedback: true + use_velocity_feedback: false use_joint_effort: true # iob_rate: 200 ### loose synchronization default true @@ -75,35 +75,35 @@ hrpsys_gazebo_configuration: # 28 - CHEST ## joint gain settings gains: - LLEG_HIP_R: {p: 12000.0, d: 4.0, i: 0.0, vp: 6.0, i_clamp: 0.0, p_v: 250.0} - LLEG_HIP_P: {p: 24000.0, d: 6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} - LLEG_HIP_Y: {p: 4000.0, d: 4.0, i: 0.0, vp: 1.0, i_clamp: 0.0, p_v: 250.0} - LLEG_KNEE: {p: 36000.0, d: 6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} - LLEG_ANKLE_P: {p: 18000.0, d: 3.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} - LLEG_ANKLE_R: {p: 6000.0, d: 2.0, i: 0.0, vp: 4.0, i_clamp: 0.0, p_v: 250.0} - RLEG_HIP_R: {p: 12000.0, d: 4.0, i: 0.0, vp: 6.0, i_clamp: 0.0, p_v: 250.0} - RLEG_HIP_P: {p: 24000.0, d: 6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} - RLEG_HIP_Y: {p: 4000.0, d: 4.0, i: 0.0, vp: 1.0, i_clamp: 0.0, p_v: 250.0} - RLEG_KNEE: {p: 36000.0, d: 6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} - RLEG_ANKLE_P: {p: 18000.0, d: 3.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} - RLEG_ANKLE_R: {p: 6000.0, d: 2.0, i: 0.0, vp: 4.0, i_clamp: 0.0, p_v: 250.0} - WAIST_P: {p: 8000.0, d: 4.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} - WAIST_R: {p: 8000.0, d: 4.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} - CHEST: {p: 6000.0, d: 2.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} - LARM_SHOULDER_P: {p: 1200.0, d: 1.0, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} - LARM_SHOULDER_R: {p: 500.0, d: 0.5, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 120.0} - LARM_SHOULDER_Y: {p: 200.0, d: 0.3, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} - LARM_ELBOW: {p: 1000.0, d: 1.4, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} - LARM_WRIST_Y: {p: 200.0, d: 0.1, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} - LARM_WRIST_P: {p: 300.0, d: 0.2, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} - LARM_WRIST_R: {p: 20.0, d: 0.1, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} - RARM_SHOULDER_P: {p: 1200.0, d: 1.0, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} - RARM_SHOULDER_R: {p: 500.0, d: 0.5, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 120.0} - RARM_SHOULDER_Y: {p: 200.0, d: 0.3, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} - RARM_ELBOW: {p: 1000.0, d: 1.4, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} - RARM_WRIST_Y: {p: 200.0, d: 0.1, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} - RARM_WRIST_P: {p: 300.0, d: 0.2, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} - RARM_WRIST_R: {p: 20.0, d: 0.1, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + LLEG_HIP_R: {p: 6000.0, d: 0.1, i: 0.0, vp: 6.0, i_clamp: 0.0, p_v: 250.0} + LLEG_HIP_P: {p: 6000.0, d: 0.15, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + LLEG_HIP_Y: {p: 2000.0, d: 0.1, i: 0.0, vp: 1.0, i_clamp: 0.0, p_v: 250.0} + LLEG_KNEE: {p: 9000.0, d: 0.15, i: 0.0, vp: 1.0, i_clamp: 0.0, p_v: 250.0} + LLEG_ANKLE_P: {p: 4500.0, d: 0.075, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + LLEG_ANKLE_R: {p: 3000.0, d: 0.05, i: 0.0, vp: 4.0, i_clamp: 0.0, p_v: 250.0} + RLEG_HIP_R: {p: 6000.0, d: 0.1, i: 0.0, vp: 6.0, i_clamp: 0.0, p_v: 250.0} + RLEG_HIP_P: {p: 6000.0, d: 0.05, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + RLEG_HIP_Y: {p: 2000.0, d: 0.1, i: 0.0, vp: 1.0, i_clamp: 0.0, p_v: 250.0} + RLEG_KNEE: {p: 9000.0, d: 0.15, i: 0.0, vp: 1.0, i_clamp: 0.0, p_v: 250.0} + RLEG_ANKLE_P: {p: 4500.0, d: 0.075, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + RLEG_ANKLE_R: {p: 3000.0, d: 0.05, i: 0.0, vp: 4.0, i_clamp: 0.0, p_v: 250.0} + WAIST_P: {p: 8000.0, d: 0.1, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + WAIST_R: {p: 8000.0, d: 0.1, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + CHEST: {p: 6000.0, d: 0.05, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + LARM_SHOULDER_P: {p: 1200.0, d: 0.1, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} + LARM_SHOULDER_R: {p: 500.0, d: 0.05, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 120.0} + LARM_SHOULDER_Y: {p: 200.0, d: 0.03, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + LARM_ELBOW: {p: 1000.0, d: 0.14, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} + LARM_WRIST_Y: {p: 200.0, d: 0.01, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + LARM_WRIST_P: {p: 300.0, d: 0.02, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + LARM_WRIST_R: {p: 20.0, d: 0.01, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + RARM_SHOULDER_P: {p: 1200.0, d: 0.1, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} + RARM_SHOULDER_R: {p: 500.0, d: 0.05, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 120.0} + RARM_SHOULDER_Y: {p: 200.0, d: 0.03, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + RARM_ELBOW: {p: 1000.0, d: 0.14, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} + RARM_WRIST_Y: {p: 200.0, d: 0.01, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + RARM_WRIST_P: {p: 300.0, d: 0.02, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + RARM_WRIST_R: {p: 20.0, d: 0.01, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} ## force sensor settings ## list of force sensorname force_torque_sensors: @@ -121,6 +121,7 @@ hrpsys_gazebo_configuration: ## IMU sensor settings ## configuration of IMU sensor ## key of imu_sensors_config should be a member of imu_sensors +## pose of IMU sensor should be written in URDF imu_sensors: - imu_sensor0 imu_sensors_config: diff --git a/hrpsys_gazebo_tutorials/config/SampleRobot_indigo.yaml b/hrpsys_gazebo_tutorials/config/SampleRobot_indigo.yaml new file mode 100644 index 00000000..b6c605fc --- /dev/null +++ b/hrpsys_gazebo_tutorials/config/SampleRobot_indigo.yaml @@ -0,0 +1,127 @@ +hrpsys_gazebo_configuration: +### velocity feedback for joint control, use parameter gains/joint_name/p_v + use_velocity_feedback: true + use_joint_effort: true +# iob_rate: 200 +### loose synchronization default true +# use_loose_synchronized: false +### synchronized hrpsys and gazebo +# use_synchronized_command: false +# iob_substeps: 5 +### name of robot (using for namespace) + robotname: SampleRobot +### joint_id (order) conversion from gazebo to hrpsys, joint_id_list[gazebo_id] := hrpsys_id + joint_id_list: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28] +### joints : list used in gazebo, sizeof(joint_id_list) == sizeof(joints) + joints: + - RLEG_HIP_R + - RLEG_HIP_P + - RLEG_HIP_Y + - RLEG_KNEE + - RLEG_ANKLE_P + - RLEG_ANKLE_R + - RARM_SHOULDER_P + - RARM_SHOULDER_R + - RARM_SHOULDER_Y + - RARM_ELBOW + - RARM_WRIST_Y + - RARM_WRIST_P + - RARM_WRIST_R + - LLEG_HIP_R + - LLEG_HIP_P + - LLEG_HIP_Y + - LLEG_KNEE + - LLEG_ANKLE_P + - LLEG_ANKLE_R + - LARM_SHOULDER_P + - LARM_SHOULDER_R + - LARM_SHOULDER_Y + - LARM_ELBOW + - LARM_WRIST_Y + - LARM_WRIST_P + - LARM_WRIST_R + - WAIST_P + - WAIST_R + - CHEST +## comment for joint index +# 0 - RLEG_HIP_R +# 1 - RLEG_HIP_P +# 2 - RLEG_HIP_Y +# 3 - RLEG_KNEE +# 4 - RLEG_ANKLE_P +# 5 - RLEG_ANKLE_R +# 6 - RARM_SHOULDER_P +# 7 - RARM_SHOULDER_R +# 8 - RARM_SHOULDER_Y +# 9 - RARM_ELBOW +# 10 - RARM_WRIST_Y +# 11 - RARM_WRIST_P +# 12 - RARM_WRIST_R +# 13 - LLEG_HIP_R +# 14 - LLEG_HIP_P +# 15 - LLEG_HIP_Y +# 16 - LLEG_KNEE +# 17 - LLEG_ANKLE_P +# 18 - LLEG_ANKLE_R +# 19 - LARM_SHOULDER_P +# 20 - LARM_SHOULDER_R +# 21 - LARM_SHOULDER_Y +# 22 - LARM_ELBOW +# 23 - LARM_WRIST_Y +# 24 - LARM_WRIST_P +# 25 - LARM_WRIST_R +# 26 - WAIST_P +# 27 - WAIST_R +# 28 - CHEST +## joint gain settings + gains: + LLEG_HIP_R: {p: 12000.0, d: 4.0, i: 0.0, vp: 6.0, i_clamp: 0.0, p_v: 250.0} + LLEG_HIP_P: {p: 24000.0, d: 6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + LLEG_HIP_Y: {p: 4000.0, d: 4.0, i: 0.0, vp: 1.0, i_clamp: 0.0, p_v: 250.0} + LLEG_KNEE: {p: 36000.0, d: 6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + LLEG_ANKLE_P: {p: 18000.0, d: 3.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + LLEG_ANKLE_R: {p: 6000.0, d: 2.0, i: 0.0, vp: 4.0, i_clamp: 0.0, p_v: 250.0} + RLEG_HIP_R: {p: 12000.0, d: 4.0, i: 0.0, vp: 6.0, i_clamp: 0.0, p_v: 250.0} + RLEG_HIP_P: {p: 24000.0, d: 6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + RLEG_HIP_Y: {p: 4000.0, d: 4.0, i: 0.0, vp: 1.0, i_clamp: 0.0, p_v: 250.0} + RLEG_KNEE: {p: 36000.0, d: 6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + RLEG_ANKLE_P: {p: 18000.0, d: 3.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + RLEG_ANKLE_R: {p: 6000.0, d: 2.0, i: 0.0, vp: 4.0, i_clamp: 0.0, p_v: 250.0} + WAIST_P: {p: 8000.0, d: 4.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + WAIST_R: {p: 8000.0, d: 4.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + CHEST: {p: 6000.0, d: 2.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0} + LARM_SHOULDER_P: {p: 1200.0, d: 1.0, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} + LARM_SHOULDER_R: {p: 500.0, d: 0.5, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 120.0} + LARM_SHOULDER_Y: {p: 200.0, d: 0.3, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + LARM_ELBOW: {p: 1000.0, d: 1.4, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} + LARM_WRIST_Y: {p: 200.0, d: 0.1, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + LARM_WRIST_P: {p: 300.0, d: 0.2, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + LARM_WRIST_R: {p: 20.0, d: 0.1, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + RARM_SHOULDER_P: {p: 1200.0, d: 1.0, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} + RARM_SHOULDER_R: {p: 500.0, d: 0.5, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 120.0} + RARM_SHOULDER_Y: {p: 200.0, d: 0.3, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + RARM_ELBOW: {p: 1000.0, d: 1.4, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 160.0} + RARM_WRIST_Y: {p: 200.0, d: 0.1, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + RARM_WRIST_P: {p: 300.0, d: 0.2, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} + RARM_WRIST_R: {p: 20.0, d: 0.1, i: 0.0, vp: 0.0, i_clamp: 0.0, p_v: 100.0} +## force sensor settings +## list of force sensorname + force_torque_sensors: + - lfsensor + - rfsensor + - lhsensor + - rhsensor +## configuration of force sensor +## key of force_torque_sensors_config should be a member of force_torque_sensors + force_torque_sensors_config: + lfsensor: {joint_name: 'LLEG_ANKLE_R', frame_id: 'LLEG_LINK6', translation: [0, 0, 0], rotation: [1, 0, 0, 0]} + rfsensor: {joint_name: 'RLEG_ANKLE_R', frame_id: 'RLEG_LINK6', translation: [0, 0, 0], rotation: [1, 0, 0, 0]} + lhsensor: {joint_name: 'LARM_WRIST_R', frame_id: 'LARM_LINK7'} + rhsensor: {joint_name: 'RARM_WRIST_R', frame_id: 'RARM_LINK7'} +## IMU sensor settings +## configuration of IMU sensor +## key of imu_sensors_config should be a member of imu_sensors + imu_sensors: + - imu_sensor0 + imu_sensors_config: + imu_sensor0: {ros_name: 'sample_imu_sensor', link_name: 'WAIST_LINK0', frame_id: 'WAIST_LINK0'} diff --git a/hrpsys_gazebo_tutorials/launch/gazebo_samplerobot_no_controllers_indigo.launch b/hrpsys_gazebo_tutorials/launch/gazebo_samplerobot_no_controllers_indigo.launch new file mode 100644 index 00000000..3fe8392c --- /dev/null +++ b/hrpsys_gazebo_tutorials/launch/gazebo_samplerobot_no_controllers_indigo.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_gazebo_tutorials/launch/samplerobot_hrpsys_bringup.launch b/hrpsys_gazebo_tutorials/launch/samplerobot_hrpsys_bringup.launch index 3b6d566e..849915f7 100644 --- a/hrpsys_gazebo_tutorials/launch/samplerobot_hrpsys_bringup.launch +++ b/hrpsys_gazebo_tutorials/launch/samplerobot_hrpsys_bringup.launch @@ -22,7 +22,8 @@ - + + diff --git a/hrpsys_gazebo_tutorials/robot_models/SampleRobot/SampleRobot.urdf.xacro b/hrpsys_gazebo_tutorials/robot_models/SampleRobot/SampleRobot.urdf.xacro index a0f1feb2..8f66556e 100644 --- a/hrpsys_gazebo_tutorials/robot_models/SampleRobot/SampleRobot.urdf.xacro +++ b/hrpsys_gazebo_tutorials/robot_models/SampleRobot/SampleRobot.urdf.xacro @@ -30,6 +30,7 @@ + 0 0 0 1.5708 0 3.14159 diff --git a/hrpsys_ros_bridge_tutorials/models/samplerobot.yaml b/hrpsys_ros_bridge_tutorials/models/samplerobot.yaml index ad24e04c..e7a82ef3 100644 --- a/hrpsys_ros_bridge_tutorials/models/samplerobot.yaml +++ b/hrpsys_ros_bridge_tutorials/models/samplerobot.yaml @@ -108,3 +108,13 @@ replace_xmls: attribute_name: type attribute_value: continuous replaced_attribute_value: revolute + - match_rule: + tag: dynamics + attribute_name: friction + attribute_value: 0 + replaced_attribute_value: 1 + - match_rule: + tag: dynamics + attribute_name: damping + attribute_value: 0.2 + replaced_attribute_value: 1