From 8d06756ce7baee5bb67a34032718876147de8133 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 12 Apr 2020 21:45:40 +0900 Subject: [PATCH 1/6] [hrpsys_gazebo_tutorials] enable hrp2 demos --- hrpsys_gazebo_tutorials/config/HRP2JSK.yaml | 68 +++++++++---------- hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml | 32 ++++----- .../config/HRP2JSKNTS.yaml | 32 ++++----- .../robot_models/HRP2JSK/HRP2JSK.urdf.xacro | 1 + .../HRP2JSKNT/HRP2JSKNT.urdf.xacro | 1 + .../HRP2JSKNTS/HRP2JSKNTS.urdf.xacro | 1 + .../models/hrp2jsk.yaml | 23 +++++++ .../models/hrp2jsknt.yaml | 12 +++- .../models/hrp2jsknts.yaml | 12 +++- 9 files changed, 114 insertions(+), 68 deletions(-) diff --git a/hrpsys_gazebo_tutorials/config/HRP2JSK.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSK.yaml index 332715c0..ffcbbd22 100644 --- a/hrpsys_gazebo_tutorials/config/HRP2JSK.yaml +++ b/hrpsys_gazebo_tutorials/config/HRP2JSK.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: 250 ### loose synchronization default true @@ -47,38 +47,38 @@ hrpsys_gazebo_configuration: - LARM_JOINT6 - LARM_JOINT7 gains: - CHEST_JOINT0: {p: 3000.0, d: 8.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} - CHEST_JOINT1: {p: 1000.0, d: 4.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} - HEAD_JOINT0: {p: 150.0, d: 0.5, i: 0.0, i_clamp: 0.0, p_v: 350.0} - HEAD_JOINT1: {p: 700.0, d: 1.5, i: 0.0, i_clamp: 0.0, p_v: 350.0} - LARM_JOINT0: {p: 2000.0, d: 4.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} - LARM_JOINT1: {p: 1000.0, d: 2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} - LARM_JOINT2: {p: 200.0, d: 0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0} - LARM_JOINT3: {p: 1500.0, d: 3.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} - LARM_JOINT4: {p: 200.0, d: 0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0} - LARM_JOINT5: {p: 400.0, d: 0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0} - LARM_JOINT6: {p: 400.0, d: 0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0} - LARM_JOINT7: {p: 20.0, d: 0.02, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RARM_JOINT0: {p: 2000.0, d: 4.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RARM_JOINT1: {p: 1000.0, d: 2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RARM_JOINT2: {p: 200.0, d: 0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RARM_JOINT3: {p: 1500.0, d: 3.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RARM_JOINT4: {p: 200.0, d: 0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RARM_JOINT5: {p: 400.0, d: 0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RARM_JOINT6: {p: 400.0, d: 0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RARM_JOINT7: {p: 20.0, d: 0.02, i: 0.0, i_clamp: 0.0, p_v: 350.0} - LLEG_JOINT0: {p: 1500.0, d: 2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} # CRTOCH-Y - LLEG_JOINT1: {p: 2000.0, d: 4.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0} # CRTOCH-R - LLEG_JOINT2: {p: 3800.0, d: 15.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0} # CRTOCH-P - LLEG_JOINT3: {p: 3400.0, d: 12.5, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} # KNEE-P - LLEG_JOINT4: {p: 2800.0, d: 10.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} # ANKLE-P - LLEG_JOINT5: {p: 2000.0, d: 4.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} # ANKLE-R - RLEG_JOINT0: {p: 1500.0, d: 2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RLEG_JOINT1: {p: 2000.0, d: 4.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0} - RLEG_JOINT2: {p: 3800.0, d: 15.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0} - RLEG_JOINT3: {p: 3400.0, d: 12.5, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} - RLEG_JOINT4: {p: 2800.0, d: 10.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} - RLEG_JOINT5: {p: 2000.0, d: 2.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} + CHEST_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + CHEST_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + HEAD_JOINT0: {p: 400.0, d: 0.040, i: 0.0, i_clamp: 0.0, p_v: 350.0} + HEAD_JOINT1: {p: 400.0, d: 0.040, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT7: {p: 20.0, d: 0.020, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT7: {p: 20.0, d: 0.020, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LLEG_JOINT0: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # CRTOCH-Y + LLEG_JOINT1: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, vp: 0.0, p_v: 350.0} # CRTOCH-R + LLEG_JOINT2: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, vp: 0.0, p_v: 350.0} # CRTOCH-P + LLEG_JOINT3: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, vp: 0.0, p_v: 350.0} # KNEE-P + LLEG_JOINT4: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, vp: 0.0, p_v: 350.0} # ANKLE-P + LLEG_JOINT5: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, vp: 0.0, p_v: 350.0} # ANKLE-R + RLEG_JOINT0: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT1: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, vp: 0.0, p_v: 350.0} + RLEG_JOINT2: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, vp: 0.0, p_v: 350.0} + RLEG_JOINT3: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, vp: 0.0, p_v: 350.0} + RLEG_JOINT4: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, vp: 0.0, p_v: 350.0} + RLEG_JOINT5: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, vp: 0.0, p_v: 350.0} force_torque_sensors: - rfsensor @@ -94,5 +94,5 @@ hrpsys_gazebo_configuration: imu_sensors: - imu_sensor0 imu_sensors_config: -## TODO: add translation and rotation +## pose of IMU sensor should be written in URDF imu_sensor0: {ros_name: 'waist_imu', link_name: 'CHEST_LINK1', frame_id: 'CHEST_LINK1'} diff --git a/hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml index 61da644c..b0ed9245 100644 --- a/hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml +++ b/hrpsys_gazebo_tutorials/config/HRP2JSKNT.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: 250 ### loose synchronization default true @@ -80,20 +80,20 @@ hrpsys_gazebo_configuration: RARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} RARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} RARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} - LLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # CRTOCH-Y - LLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # CRTOCH-R - LLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} # CRTOCH-P - LLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} # KNEE-P - LLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-P - LLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-R - LLEG_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # TOE-R - RLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} - RLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} - RLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} - RLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RLEG_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + LLEG_JOINT0: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # CRTOCH-Y + LLEG_JOINT1: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # CRTOCH-R + LLEG_JOINT2: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} # CRTOCH-P + LLEG_JOINT3: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} # KNEE-P + LLEG_JOINT4: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-P + LLEG_JOINT5: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-R + LLEG_JOINT6: {p: 120.0, d: 0.013, i: 0.0, i_clamp: 0.0, p_v: 250.0} # TOE-R + RLEG_JOINT0: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + RLEG_JOINT1: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT2: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} + RLEG_JOINT3: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} + RLEG_JOINT4: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT5: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT6: {p: 120.0, d: 0.013, i: 0.0, i_clamp: 0.0, p_v: 250.0} L_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} L_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} L_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} @@ -121,5 +121,5 @@ hrpsys_gazebo_configuration: imu_sensors: - imu_sensor0 imu_sensors_config: -## TODO: add translation and rotation +## pose of IMU sensor should be written in URDF imu_sensor0: {ros_name: 'waist_imu', link_name: 'CHEST_LINK1', frame_id: 'CHEST_LINK1'} diff --git a/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml index 232e7e44..67ad5115 100644 --- a/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml +++ b/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.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: 250 ### loose synchronization default true @@ -79,21 +79,21 @@ hrpsys_gazebo_configuration: RARM_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} RARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} RARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} - RARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} - LLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # CRTOCH-Y - LLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # CRTOCH-R - LLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} # CRTOCH-P - LLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} # KNEE-P - LLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-P - LLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-R + RARM_JOINT6: {p: 120.0, d: 0.013, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LLEG_JOINT0: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # CRTOCH-Y + LLEG_JOINT1: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # CRTOCH-R + LLEG_JOINT2: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} # CRTOCH-P + LLEG_JOINT3: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} # KNEE-P + LLEG_JOINT4: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-P + LLEG_JOINT5: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-R LLEG_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # TOE-R - RLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} - RLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} - RLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} - RLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} - RLEG_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + RLEG_JOINT0: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + RLEG_JOINT1: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT2: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} + RLEG_JOINT3: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} + RLEG_JOINT4: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT5: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT6: {p: 120.0, d: 0.013, i: 0.0, i_clamp: 0.0, p_v: 250.0} L_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} L_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} L_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} @@ -121,5 +121,5 @@ hrpsys_gazebo_configuration: imu_sensors: - imu_sensor0 imu_sensors_config: -## TODO: add translation and rotation +## pose of IMU sensor should be written in URDF imu_sensor0: {ros_name: 'waist_imu', link_name: 'CHEST_LINK1', frame_id: 'CHEST_LINK1'} diff --git a/hrpsys_gazebo_tutorials/robot_models/HRP2JSK/HRP2JSK.urdf.xacro b/hrpsys_gazebo_tutorials/robot_models/HRP2JSK/HRP2JSK.urdf.xacro index 7b5d5ccb..4f4cd20e 100644 --- a/hrpsys_gazebo_tutorials/robot_models/HRP2JSK/HRP2JSK.urdf.xacro +++ b/hrpsys_gazebo_tutorials/robot_models/HRP2JSK/HRP2JSK.urdf.xacro @@ -29,6 +29,7 @@ + -0.13 0 0.118 0 0 0 diff --git a/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNT/HRP2JSKNT.urdf.xacro b/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNT/HRP2JSKNT.urdf.xacro index 84541444..74a31d9b 100644 --- a/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNT/HRP2JSKNT.urdf.xacro +++ b/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNT/HRP2JSKNT.urdf.xacro @@ -29,6 +29,7 @@ + -0.13 0 0.118 0 0 0 diff --git a/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro b/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro index 327cd1af..91fe1018 100644 --- a/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro +++ b/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro @@ -29,6 +29,7 @@ + -0.13 0 0.118 0 0 0 diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsk.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsk.yaml index 85dcba40..b1cd69da 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsk.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsk.yaml @@ -128,3 +128,26 @@ replace_xmls: attribute_name: velocity attribute_value: 0.5 replaced_attribute_value: 10.0 + - 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 + # for hands + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: RARM_LINK7 + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: LARM_LINK7 + replaced_xml: '' diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml index 01b0fbf5..fd338a87 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml @@ -158,7 +158,7 @@ replace_xmls: tag: inertia attribute_name: ixx attribute_value: '0' - replaced_xml: '' + replaced_xml: '' - match_rule: tag: gazebo attribute_name: reference @@ -189,6 +189,16 @@ replace_xmls: attribute_name: effort attribute_value: 100 replaced_attribute_value: 1000 + - 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 # for hands - match_rule: tag: inertia diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml index 4258f589..fa043925 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml @@ -164,7 +164,7 @@ replace_xmls: tag: inertia attribute_name: ixx attribute_value: '0' - replaced_xml: '' + replaced_xml: '' - match_rule: tag: gazebo attribute_name: reference @@ -195,6 +195,16 @@ replace_xmls: attribute_name: effort attribute_value: 100 replaced_attribute_value: 1000 + - 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 # for hands - match_rule: tag: inertia From a75004238060a8e6a73bfd1bfa66c8de33deaea8 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 14 Apr 2020 15:55:28 +0900 Subject: [PATCH 2/6] add _indigo.yaml, _indigo.launch --- README.md | 3 +- .../config/HRP2JSKNTS_indigo.yaml | 125 ++++++++++++++++++ .../config/HRP2JSKNT_indigo.yaml | 125 ++++++++++++++++++ .../config/HRP2JSK_indigo.yaml | 98 ++++++++++++++ ...azebo_hrp2jsk_no_controllers_indigo.launch | 18 +++ ...ebo_hrp2jsknt_no_controllers_indigo.launch | 23 ++++ ...bo_hrp2jsknts_no_controllers_indigo.launch | 23 ++++ 7 files changed, 414 insertions(+), 1 deletion(-) create mode 100644 hrpsys_gazebo_tutorials/config/HRP2JSKNTS_indigo.yaml create mode 100644 hrpsys_gazebo_tutorials/config/HRP2JSKNT_indigo.yaml create mode 100644 hrpsys_gazebo_tutorials/config/HRP2JSK_indigo.yaml create mode 100644 hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsk_no_controllers_indigo.launch create mode 100644 hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknt_no_controllers_indigo.launch create mode 100644 hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers_indigo.launch diff --git a/README.md b/README.md index eb63f264..8abdd67a 100644 --- a/README.md +++ b/README.md @@ -38,7 +38,8 @@ roseus samplerobot-interface.l Open Terminal and run gazebo ``` -roslaunch hrpsys_gazebo_tutorials gazebo_hrp2jsknt_no_controllers.launch +roslaunch hrpsys_gazebo_tutorials gazebo_hrp2jsknt_no_controllers.launch #kinetic or melodic +roslaunch hrpsys_gazebo_tutorials gazebo_hrp2jsknt_no_controllers_indigo.launch #indigo ``` Launch another terminal and start hrpsys-base ``` diff --git a/hrpsys_gazebo_tutorials/config/HRP2JSKNTS_indigo.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSKNTS_indigo.yaml new file mode 100644 index 00000000..232e7e44 --- /dev/null +++ b/hrpsys_gazebo_tutorials/config/HRP2JSKNTS_indigo.yaml @@ -0,0 +1,125 @@ +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: 250 +### 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: HRP2JSKNTS +### 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, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45] +### joints : list used in gazebo, sizeof(joint_id_list) == sizeof(joints) + joints: + - RLEG_JOINT0 + - RLEG_JOINT1 + - RLEG_JOINT2 + - RLEG_JOINT3 + - RLEG_JOINT4 + - RLEG_JOINT5 + - RLEG_JOINT6 + - LLEG_JOINT0 + - LLEG_JOINT1 + - LLEG_JOINT2 + - LLEG_JOINT3 + - LLEG_JOINT4 + - LLEG_JOINT5 + - LLEG_JOINT6 + - CHEST_JOINT0 + - CHEST_JOINT1 + - HEAD_JOINT0 + - HEAD_JOINT1 + - RARM_JOINT0 + - RARM_JOINT1 + - RARM_JOINT2 + - RARM_JOINT3 + - RARM_JOINT4 + - RARM_JOINT5 + - RARM_JOINT6 + - RARM_JOINT7 + - LARM_JOINT0 + - LARM_JOINT1 + - LARM_JOINT2 + - LARM_JOINT3 + - LARM_JOINT4 + - LARM_JOINT5 + - LARM_JOINT6 + - LARM_JOINT7 + - L_THUMBCM_Y + - L_THUMBCM_P + - L_INDEXMP_R + - L_INDEXMP_P + - L_INDEXPIP_R + - L_MIDDLEPIP_R + - R_THUMBCM_Y + - R_THUMBCM_P + - R_INDEXMP_R + - R_INDEXMP_P + - R_INDEXPIP_R + - R_MIDDLEPIP_R +## jointid: + gains: + CHEST_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + CHEST_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + HEAD_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + HEAD_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # CRTOCH-Y + LLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # CRTOCH-R + LLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} # CRTOCH-P + LLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} # KNEE-P + LLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-P + LLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-R + LLEG_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # TOE-R + RLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + RLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} + RLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} + RLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + L_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + + force_torque_sensors: + - rfsensor + - lfsensor + - rhsensor + - lhsensor + force_torque_sensors_config: +## TODO: add translation and rotation + lfsensor: {joint_name: 'LLEG_JOINT5', frame_id: 'LLEG_LINK5', translation: [0, 0, -0.105], rotation: [1, 0, 0, 0]} + rfsensor: {joint_name: 'RLEG_JOINT5', frame_id: 'RLEG_LINK5', translation: [0, 0, -0.105], rotation: [1, 0, 0, 0]} + lhsensor: {joint_name: 'LARM_JOINT6', frame_id: 'LARM_LINK6', translation: [0, 0, -0.0775], rotation: [1, 0, 0, 0]} + rhsensor: {joint_name: 'RARM_JOINT6', frame_id: 'RARM_LINK6', translation: [0, 0, -0.0775], rotation: [1, 0, 0, 0]} + imu_sensors: + - imu_sensor0 + imu_sensors_config: +## TODO: add translation and rotation + imu_sensor0: {ros_name: 'waist_imu', link_name: 'CHEST_LINK1', frame_id: 'CHEST_LINK1'} diff --git a/hrpsys_gazebo_tutorials/config/HRP2JSKNT_indigo.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSKNT_indigo.yaml new file mode 100644 index 00000000..61da644c --- /dev/null +++ b/hrpsys_gazebo_tutorials/config/HRP2JSKNT_indigo.yaml @@ -0,0 +1,125 @@ +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: 250 +### 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: HRP2JSKNT +### 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, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45] +### joints : list used in gazebo, sizeof(joint_id_list) == sizeof(joints) + joints: + - RLEG_JOINT0 + - RLEG_JOINT1 + - RLEG_JOINT2 + - RLEG_JOINT3 + - RLEG_JOINT4 + - RLEG_JOINT5 + - RLEG_JOINT6 + - LLEG_JOINT0 + - LLEG_JOINT1 + - LLEG_JOINT2 + - LLEG_JOINT3 + - LLEG_JOINT4 + - LLEG_JOINT5 + - LLEG_JOINT6 + - CHEST_JOINT0 + - CHEST_JOINT1 + - HEAD_JOINT0 + - HEAD_JOINT1 + - RARM_JOINT0 + - RARM_JOINT1 + - RARM_JOINT2 + - RARM_JOINT3 + - RARM_JOINT4 + - RARM_JOINT5 + - RARM_JOINT6 + - RARM_JOINT7 + - LARM_JOINT0 + - LARM_JOINT1 + - LARM_JOINT2 + - LARM_JOINT3 + - LARM_JOINT4 + - LARM_JOINT5 + - LARM_JOINT6 + - LARM_JOINT7 + - L_THUMBCM_Y + - L_THUMBCM_P + - L_INDEXMP_R + - L_INDEXMP_P + - L_INDEXPIP_R + - L_MIDDLEPIP_R + - R_THUMBCM_Y + - R_THUMBCM_P + - R_INDEXMP_R + - R_INDEXMP_P + - R_INDEXPIP_R + - R_MIDDLEPIP_R +## jointid: + gains: + CHEST_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + CHEST_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + HEAD_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + HEAD_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + RARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0} + LLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # CRTOCH-Y + LLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # CRTOCH-R + LLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} # CRTOCH-P + LLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} # KNEE-P + LLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-P + LLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-R + LLEG_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # TOE-R + RLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + RLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} + RLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} + RLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} + L_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + + force_torque_sensors: + - rfsensor + - lfsensor + - rhsensor + - lhsensor + force_torque_sensors_config: +## TODO: add translation and rotation + lfsensor: {joint_name: 'LLEG_JOINT5', frame_id: 'LLEG_LINK5', translation: [0, 0, -0.105], rotation: [1, 0, 0, 0]} + rfsensor: {joint_name: 'RLEG_JOINT5', frame_id: 'RLEG_LINK5', translation: [0, 0, -0.105], rotation: [1, 0, 0, 0]} + lhsensor: {joint_name: 'LARM_JOINT6', frame_id: 'LARM_LINK6', translation: [0, 0, -0.0775], rotation: [1, 0, 0, 0]} + rhsensor: {joint_name: 'RARM_JOINT6', frame_id: 'RARM_LINK6', translation: [0, 0, -0.0775], rotation: [1, 0, 0, 0]} + imu_sensors: + - imu_sensor0 + imu_sensors_config: +## TODO: add translation and rotation + imu_sensor0: {ros_name: 'waist_imu', link_name: 'CHEST_LINK1', frame_id: 'CHEST_LINK1'} diff --git a/hrpsys_gazebo_tutorials/config/HRP2JSK_indigo.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSK_indigo.yaml new file mode 100644 index 00000000..332715c0 --- /dev/null +++ b/hrpsys_gazebo_tutorials/config/HRP2JSK_indigo.yaml @@ -0,0 +1,98 @@ +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: 250 +### 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: HRP2JSK +### 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, 29, 30, 31] +### joints : list used in gazebo, sizeof(joint_id_list) == sizeof(joints) + joints: + - RLEG_JOINT0 + - RLEG_JOINT1 + - RLEG_JOINT2 + - RLEG_JOINT3 + - RLEG_JOINT4 + - RLEG_JOINT5 + - LLEG_JOINT0 + - LLEG_JOINT1 + - LLEG_JOINT2 + - LLEG_JOINT3 + - LLEG_JOINT4 + - LLEG_JOINT5 + - CHEST_JOINT0 + - CHEST_JOINT1 + - HEAD_JOINT0 + - HEAD_JOINT1 + - RARM_JOINT0 + - RARM_JOINT1 + - RARM_JOINT2 + - RARM_JOINT3 + - RARM_JOINT4 + - RARM_JOINT5 + - RARM_JOINT6 + - RARM_JOINT7 + - LARM_JOINT0 + - LARM_JOINT1 + - LARM_JOINT2 + - LARM_JOINT3 + - LARM_JOINT4 + - LARM_JOINT5 + - LARM_JOINT6 + - LARM_JOINT7 + gains: + CHEST_JOINT0: {p: 3000.0, d: 8.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} + CHEST_JOINT1: {p: 1000.0, d: 4.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} + HEAD_JOINT0: {p: 150.0, d: 0.5, i: 0.0, i_clamp: 0.0, p_v: 350.0} + HEAD_JOINT1: {p: 700.0, d: 1.5, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT0: {p: 2000.0, d: 4.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT1: {p: 1000.0, d: 2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT2: {p: 200.0, d: 0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT3: {p: 1500.0, d: 3.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT4: {p: 200.0, d: 0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT5: {p: 400.0, d: 0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT6: {p: 400.0, d: 0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LARM_JOINT7: {p: 20.0, d: 0.02, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT0: {p: 2000.0, d: 4.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT1: {p: 1000.0, d: 2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT2: {p: 200.0, d: 0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT3: {p: 1500.0, d: 3.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT4: {p: 200.0, d: 0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT5: {p: 400.0, d: 0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT6: {p: 400.0, d: 0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RARM_JOINT7: {p: 20.0, d: 0.02, i: 0.0, i_clamp: 0.0, p_v: 350.0} + LLEG_JOINT0: {p: 1500.0, d: 2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} # CRTOCH-Y + LLEG_JOINT1: {p: 2000.0, d: 4.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0} # CRTOCH-R + LLEG_JOINT2: {p: 3800.0, d: 15.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0} # CRTOCH-P + LLEG_JOINT3: {p: 3400.0, d: 12.5, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} # KNEE-P + LLEG_JOINT4: {p: 2800.0, d: 10.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} # ANKLE-P + LLEG_JOINT5: {p: 2000.0, d: 4.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} # ANKLE-R + RLEG_JOINT0: {p: 1500.0, d: 2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} + RLEG_JOINT1: {p: 2000.0, d: 4.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0} + RLEG_JOINT2: {p: 3800.0, d: 15.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0} + RLEG_JOINT3: {p: 3400.0, d: 12.5, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} + RLEG_JOINT4: {p: 2800.0, d: 10.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} + RLEG_JOINT5: {p: 2000.0, d: 2.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} + + force_torque_sensors: + - rfsensor + - lfsensor + - rhsensor + - lhsensor + force_torque_sensors_config: +## TODO: add translation and rotation + lfsensor: {joint_name: 'LLEG_JOINT5', frame_id: 'LLEG_LINK5', translation: [0, 0, -0.105], rotation: [1, 0, 0, 0]} + rfsensor: {joint_name: 'RLEG_JOINT5', frame_id: 'RLEG_LINK5', translation: [0, 0, -0.105], rotation: [1, 0, 0, 0]} + lhsensor: {joint_name: 'LARM_JOINT6', frame_id: 'LARM_LINK6', translation: [0, 0, -0.0775], rotation: [1, 0, 0, 0]} + rhsensor: {joint_name: 'RARM_JOINT6', frame_id: 'RARM_LINK6', translation: [0, 0, -0.0775], rotation: [1, 0, 0, 0]} + imu_sensors: + - imu_sensor0 + imu_sensors_config: +## TODO: add translation and rotation + imu_sensor0: {ros_name: 'waist_imu', link_name: 'CHEST_LINK1', frame_id: 'CHEST_LINK1'} diff --git a/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsk_no_controllers_indigo.launch b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsk_no_controllers_indigo.launch new file mode 100644 index 00000000..004819fa --- /dev/null +++ b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsk_no_controllers_indigo.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknt_no_controllers_indigo.launch b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknt_no_controllers_indigo.launch new file mode 100644 index 00000000..77a80688 --- /dev/null +++ b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknt_no_controllers_indigo.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers_indigo.launch b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers_indigo.launch new file mode 100644 index 00000000..b91fe029 --- /dev/null +++ b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers_indigo.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + From 1670f61121f599466b5eac425f6beb5288ed3be3 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Thu, 16 Apr 2020 09:48:06 +0900 Subject: [PATCH 3/6] 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 From 503432f5bf65b503bbe7ea30452bfe1ded9a0aae Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 12 May 2020 18:00:08 +0900 Subject: [PATCH 4/6] [hrpsys_gazebo_tutorials] fix README.md for samplerobot(indigo) --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 8abdd67a..69e27caa 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,8 @@ If you use closed models, you have to compile `hrpsys_ros_bridge_tutorials` afte Open Terminal and run gazebo ``` -roslaunch hrpsys_gazebo_tutorials gazebo_samplerobot_no_controllers.launch +roslaunch hrpsys_gazebo_tutorials gazebo_samplerobot_no_controllers.launch #kinetic or melodic +roslaunch hrpsys_gazebo_tutorials gazebo_hrp2jsknt_no_controllers_indigo.launch #indigo ``` Launch another terminal and start hrpsys-base ``` From cf9ba0b38c67f0169dd7ba3414f2f77dfeb85126 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 12 May 2020 18:54:11 +0900 Subject: [PATCH 5/6] [hrpsys_g azebo_tutorials]add HRPSYS_RATE --- hrpsys_gazebo_tutorials/launch/hrp2jsk_hrpsys_bringup.launch | 1 + hrpsys_gazebo_tutorials/launch/hrp2jsknt_hrpsys_bringup.launch | 1 + hrpsys_gazebo_tutorials/launch/hrp2jsknts_hrpsys_bringup.launch | 1 + hrpsys_gazebo_tutorials/launch/samplerobot_hrpsys_bringup.launch | 1 + 4 files changed, 4 insertions(+) diff --git a/hrpsys_gazebo_tutorials/launch/hrp2jsk_hrpsys_bringup.launch b/hrpsys_gazebo_tutorials/launch/hrp2jsk_hrpsys_bringup.launch index 358d527a..f4a4cf5e 100644 --- a/hrpsys_gazebo_tutorials/launch/hrp2jsk_hrpsys_bringup.launch +++ b/hrpsys_gazebo_tutorials/launch/hrp2jsk_hrpsys_bringup.launch @@ -22,5 +22,6 @@ + diff --git a/hrpsys_gazebo_tutorials/launch/hrp2jsknt_hrpsys_bringup.launch b/hrpsys_gazebo_tutorials/launch/hrp2jsknt_hrpsys_bringup.launch index a42fbb5e..b910f029 100644 --- a/hrpsys_gazebo_tutorials/launch/hrp2jsknt_hrpsys_bringup.launch +++ b/hrpsys_gazebo_tutorials/launch/hrp2jsknt_hrpsys_bringup.launch @@ -22,5 +22,6 @@ + diff --git a/hrpsys_gazebo_tutorials/launch/hrp2jsknts_hrpsys_bringup.launch b/hrpsys_gazebo_tutorials/launch/hrp2jsknts_hrpsys_bringup.launch index 5a4dffca..48290830 100644 --- a/hrpsys_gazebo_tutorials/launch/hrp2jsknts_hrpsys_bringup.launch +++ b/hrpsys_gazebo_tutorials/launch/hrp2jsknts_hrpsys_bringup.launch @@ -23,5 +23,6 @@ + diff --git a/hrpsys_gazebo_tutorials/launch/samplerobot_hrpsys_bringup.launch b/hrpsys_gazebo_tutorials/launch/samplerobot_hrpsys_bringup.launch index 849915f7..275396b6 100644 --- a/hrpsys_gazebo_tutorials/launch/samplerobot_hrpsys_bringup.launch +++ b/hrpsys_gazebo_tutorials/launch/samplerobot_hrpsys_bringup.launch @@ -26,5 +26,6 @@ + From 1b51bcb0291a3764f18fc960493b8c0e77c2674d Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 12 May 2020 20:04:26 +0900 Subject: [PATCH 6/6] fix typo of README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 69e27caa..9ff52b78 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ Open Terminal and run gazebo ``` roslaunch hrpsys_gazebo_tutorials gazebo_samplerobot_no_controllers.launch #kinetic or melodic -roslaunch hrpsys_gazebo_tutorials gazebo_hrp2jsknt_no_controllers_indigo.launch #indigo +roslaunch hrpsys_gazebo_tutorials gazebo_samplerobot_no_controllers_indigo.launch #indigo ``` Launch another terminal and start hrpsys-base ```