Skip to content

F #src 4998 bimanual hand ctrl #538

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 10 commits into
base: melodic-devel
Choose a base branch
from
Open

F #src 4998 bimanual hand ctrl #538

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
e2e4c97
WIP
ethanfowler Aug 4, 2020
dff92cd
WIP
ethanfowler Aug 6, 2020
7738c30
Fixing controller spawning bug in which WRJ1+2 would not work when wr…
ethanfowler Aug 6, 2020
4c100cc
Merge remote-tracking branch 'origin/F_#SRC-4998_include_wrist_in_arm…
ethanfowler Aug 10, 2020
3c8347b
WIP
ethanfowler Aug 10, 2020
e3cc3a1
Fixing left hand controller spawner config
ethanfowler Aug 10, 2020
020c8fa
Merge branch 'F_#SRC-4998_include_wrist_in_arm' into F_#SRC-4998_bima…
ethanfowler Aug 10, 2020
e584d89
Merge branch 'melodic-devel' into F_#SRC-4998_bimanual_hand_ctrl
ethanfowler Aug 12, 2020
89ac8a7
Merge branch 'F_#SRC-4998_bimanual_hand_ctrl' of github.com:shadow-ro…
ethanfowler Aug 14, 2020
ef98a28
Merge branch 'melodic-devel' into F_#SRC-4998_bimanual_hand_ctrl
ethanfowler Sep 16, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 31 additions & 20 deletions sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,20 +2,26 @@
<arg name="debug" default="false"/>
<arg name="robot_state_pub_frequency" default="250"/>
<arg name="joint_state_pub_frequency" default="125"/>
<!-- Specify if the system has "both" hands, only "right", only "left", or "none"-->
<arg name="hands" default="both"/>
<arg name="arm_ctrl" default="true"/>

<!-- ROBOT CONFIG-->
<arg name="robot_model" default="ur10"/>
<!-- Whether there are arms. -->
<arg name="arms" default="true"/>
<!-- Specify if the system has "both" hands, only "right", only "left", or "none"-->
<arg name="hands" default="both"/>
<!-- Whether to run arm controllers. -->
<arg name="arm_ctrl" default="$(arg arms)"/>
<!-- Whether to run hand controllers. -->
<arg name="hand_ctrl" default="$(eval hands != 'none')"/>
<!-- HANDS AND ARMS -->
<arg name="robot_description" if="$(eval not arg('hands') == 'none' and arg('arm_ctrl'))" default="'$(find sr_multi_description)/urdf/bimanual_srhand_$(arg robot_model).urdf.xacro'"/>
<arg name="robot_config_file" if="$(eval not arg('hands') == 'none' and arg('arm_ctrl'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_sh_$(arg robot_model).yaml"/>
<arg name="robot_description" if="$(eval arg('hands') != 'none' and arg('arms'))" default="'$(find sr_multi_description)/urdf/bimanual_srhand_$(arg robot_model).urdf.xacro'"/>
<arg name="robot_config_file" if="$(eval arg('hands') != 'none' and arg('arms'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_sh_$(arg robot_model).yaml"/>
<!-- ARMS BUT NO HANDS -->
<arg name="robot_description" if="$(eval arg('hands') == 'none' and arg('arm_ctrl'))" default="'$(find sr_multi_description)/urdf/bimanual_$(arg robot_model).urdf.xacro'"/>
<arg name="robot_config_file" if="$(eval arg('hands') == 'none' and arg('arm_ctrl'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_$(arg robot_model).yaml"/>
<arg name="robot_description" if="$(eval arg('hands') == 'none' and arg('arms'))" default="'$(find sr_multi_description)/urdf/bimanual_$(arg robot_model).urdf.xacro'"/>
<arg name="robot_config_file" if="$(eval arg('hands') == 'none' and arg('arms'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_$(arg robot_model).yaml"/>
<!-- HANDS BUT NO ARMS -->
<arg name="robot_description" if="$(eval not arg('hands') == 'none' and not arg('arm_ctrl'))" default="'$(find sr_description)/robots/bimanual_shadowhand_motor_plus.urdf.xacro'"/>
<arg name="robot_config_file" if="$(eval not arg('hands') == 'none' and not arg('arm_ctrl'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_sh.yaml"/>
<arg name="robot_description" if="$(eval arg('hands') != 'none' and not arg('arms'))" default="'$(find sr_description)/robots/bimanual_shadowhand_motor_plus.urdf.xacro'"/>
<arg name="robot_config_file" if="$(eval arg('hands') != 'none' and not arg('arms'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_sh.yaml"/>


<!-- setting this parameter to false allows to load the robot_description from a higher level -->
Expand All @@ -31,14 +37,15 @@
<arg name="eth_port" default="$(optenv ETHERCAT_PORT eth0)"/>
<!-- The control mode PWM (true) or torque (false) -->
<arg name="pwm_control" default="true"/>
<arg name="hand_trajectory" unless="$(eval hands == 'none')" default="true"/>
<arg name="hand_trajectory" if="$(eval hands == 'none')" default="false"/>
<!-- Set to true if you want to use grasp controller -->
<arg name="grasp_controller" default="false"/>
<arg name="hand_trajectory" default="true"/>

<!-- ARMS CONFIG-->
<arg name="arm_1_z" default="0.7551"/>
<arg name="arm_2_z" default="0.7551"/>
<arg name="arm_x_separation" default="-0.4" if="$(eval not arg('hands') == 'none' and not arg('arm_ctrl'))"/>
<arg name="arm_x_separation" default="0.0" unless="$(eval not arg('hands') == 'none' and not arg('arm_ctrl'))"/>
<arg name="arm_x_separation" default="-0.4" if="$(eval not arg('hands') == 'none' and not arg('arms'))"/>
<arg name="arm_x_separation" default="0.0" unless="$(eval not arg('hands') == 'none' and not arg('arms'))"/>
<arg name="arm_y_separation" default="1.5"/>
<arg name="arm_robot_hw_1" if="$(eval not arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/right_ur_arm_robot_hw.yaml"/>
<arg name="arm_robot_hw_1" if="$(eval arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/right_ur10e_arm_robot_hw.yaml"/>
Expand Down Expand Up @@ -85,8 +92,13 @@

<include file="$(find sr_robot_launch)/launch/bimanual_controller_stopper.launch"/>

<!-- Default hand controller groups -->
<arg if="$(arg grasp_controller)" name="hand_controller_group" default="grasp"/>
<arg if="$(eval hand_trajectory and not grasp_controller)" name="hand_controller_group" default="trajectory"/>
<arg if="$(eval not hand_trajectory and not grasp_controller)" name="hand_controller_group" default="position"/>

<!-- Controller -->
<group unless="$(eval hands == 'none')">
<group if="$(arg hand_ctrl)">
<!-- Launch rosparam for payload if we use hand. If we only use hand and no arm the extra values wont be used-->
<rosparam file="$(arg robot_config_file)"/>
<!-- HAND (N.B. Arm robot harware is implicitly started here if ra_sr_ur_robot_hw is present in param /robot_hardware-->
Expand All @@ -109,15 +121,14 @@
<arg name="load_robot_description" value="$(arg load_robot_description)"/>
</include>
<node name="bimanual_trajectory_controller" pkg="sr_utilities" type="controller_spawner.py" output="screen">
<param if="$(arg hand_trajectory)" name="controller_group" value="trajectory"/>
<param unless="$(arg hand_trajectory)" name="controller_group" value="position"/>
<param name="controller_group" value="$(arg hand_controller_group)"/>
<param name="exclude_wrist" value="$(arg include_wrist_in_arm_controller)"/>
<param name="wait_for" value="calibrated"/>
</node>
</group>

<!-- Set rosparam if both hands and arms are being used -->
<group if="$(eval not arg('hands') == 'none' and arg('arm_ctrl'))">
<group if="$(eval not arg('hands') == 'none' and arg('arms'))">
<rosparam>
robot_hardware:
- unique_robot_hw
Expand All @@ -127,7 +138,7 @@
</group>

<!-- Set rosparam, load xacros and launch state publisher nodes if only arms are being used (no hands) -->
<group if="$(eval arg('hands') == 'none' and arg('arm_ctrl'))">
<group if="$(eval arg('hands') == 'none' and arg('arms'))">
<rosparam>
robot_hardware:
- ra_sr_ur_robot_hw
Expand All @@ -145,7 +156,7 @@
</group>

<!-- Set rosparams and load robot hardware if arms are being used -->
<group if="$(eval arg('arm_ctrl'))">
<group if="$(eval arg('arms'))">
<rosparam command="load" file="$(arg arm_robot_hw_1)"/>
<rosparam command="load" file="$(arg arm_robot_hw_2)"/>
<rosparam command="load" file="$(arg kinematics_config_right)" ns="ra_sr_ur_robot_hw"/>
Expand All @@ -163,7 +174,7 @@
</group>

<!-- Set rosparam if only bimanual hands and no arms are being used -->
<group if="$(eval not arg('hands') == 'none' and not arg('arm_ctrl'))">
<group if="$(eval not arg('hands') == 'none' and not arg('arms'))">
<rosparam>
robot_hardware:
- unique_robot_hw
Expand Down
56 changes: 28 additions & 28 deletions sr_robot_launch/launch/sr_ur_arms_hands.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@
<!-- ROBOT CONFIG-->
<!-- Specify if the system has "both" hands, only "right", only "left" or "none" hands-->
<arg name="hands" default="both"/>
<arg name="left_hand" default="$(eval hands in ['left', 'both'])"/>
<arg name="right_hand" default="$(eval hands in ['right', 'both'])"/>
<arg name="robot_model" default="ur10"/>
<arg name="robot_description" default="'$(find sr_multi_description)/urdf/bimanual_srhand_$(arg robot_model).urdf.xacro'"/>
<arg name="robot_config_file" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_sh_$(arg robot_model).yaml"/>
Expand All @@ -52,11 +54,16 @@
<!-- Set to true to spawn trajectory controllers for the hands(the trajectory controller overwrites continuously the joint position command, preventing direct control via topics-->
<arg name="hand_trajectory" default="true"/>
<!-- Set to true to spawn the position controllers for the hands-->
<arg name="hand_ctrl" unless="$(eval hands == 'none')" default="true"/>
<arg name="hand_ctrl" if="$(eval hands == 'none')" value="false"/>
<arg name="hand_ctrl" default="$(eval hands != 'none')"/>
<!-- The control mode PWM (true) or torque (false) -->
<!-- Set to true by default for now as torque control is not available yet -->
<arg name="pwm_control" default="true"/>
<!-- Set to true if you want to use grasp controller -->
<arg name="grasp_controller" default="false"/>
<!-- Default hand controller groups -->
<arg if="$(arg grasp_controller)" name="hand_controller_group" default="grasp"/>
<arg if="$(eval hand_trajectory and not grasp_controller)" name="hand_controller_group" default="trajectory"/>
<arg if="$(eval not hand_trajectory and not grasp_controller)" name="hand_controller_group" default="position"/>

<!-- ARMS CONFIG-->
<arg name="arm_1_z" default="0.0"/>
Expand All @@ -69,7 +76,7 @@
<arg name="arm_robot_hw_2" if="$(eval arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/left_bimanual_ur10e_arm_robot_hw.yaml"/>
<!-- When cyberglove is used set "include_wrist_in_arm_controller:=false" as the wrist joints are part of the arm by default-->
<!-- This will include the wrist joints in the hand controller and exclude them from the arm one -->
<arg name="include_wrist_in_arm_controller" default="true"/>
<arg name="include_wrist_in_arm_controller" default="$(eval hands != 'none')"/>
<!-- Set to true to spawn trajectory controllers for the arms (the trajectory controller overwrites continuously the joint position command, preventing direct control via topics-->
<arg name="arm_trajectory" default="true"/>
<!-- Set to true to spawn group position controllers for the arms -->
Expand Down Expand Up @@ -99,12 +106,12 @@
</include>
<param name="robot_description" command="xacro $(arg robot_description) arm_1_z:=$(arg arm_1_z) arm_2_z:=$(arg arm_2_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation)"/>
<!-- HAND -->
<group if="$(eval hands == 'both' or hands == 'right')">
<group if="$(arg right_hand)">
<param name="/hand/mapping/$(arg rh_serial)" value="rh"/>
<param name="/hand/joint_prefix/$(arg rh_serial)" value="rh_"/>
<rosparam command="load" file="$(find sr_description)/hand/config/rh_controller_gazebo.yaml"/>
</group>
<group if="$(eval hands == 'both' or hands == 'left')">
<group if="$(arg left_hand)">
<param name="/hand/mapping/$(arg lh_serial)" value="lh"/>
<param name="/hand/joint_prefix/$(arg lh_serial)" value="lh_"/>
<rosparam command="load" file="$(find sr_description)/hand/config/lh_controller_gazebo.yaml"/>
Expand All @@ -117,30 +124,22 @@
</include>
<!-- ARM -->
<group if="$(arg arm_trajectory)">
<group if="$(arg include_wrist_in_arm_controller)">
<rosparam if="$(eval hands == 'both' or hands == 'right')" file="$(find sr_robot_launch)/config/gazebo/controller/ra_trajectory_controller.yaml" command="load"/>
<rosparam if="$(eval hands == 'left'or hands == 'none')" file="$(find sr_robot_launch)/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml" command="load"/>
<rosparam if="$(eval hands == 'both' or hands == 'left')" file="$(find sr_robot_launch)/config/gazebo/controller/la_trajectory_controller.yaml" command="load"/>
<rosparam if="$(eval hands == 'right' or hands == 'none')" file="$(find sr_robot_launch)/config/gazebo/controller/la_trajectory_controller_no_wrist.yaml" command="load"/>
</group>
<group unless="$(arg include_wrist_in_arm_controller)">
<rosparam file="$(find sr_robot_launch)/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml" command="load"/>
<rosparam file="$(find sr_robot_launch)/config/gazebo/controller/la_trajectory_controller_no_wrist.yaml" command="load"/>
</group>
<arg if="$(eval include_wrist_in_arm_controller and right_hand)" name="ra_gazebo_trajectory_controller_config" default="ra_trajectory_controller"/>
<arg if="$(eval include_wrist_in_arm_controller and left_hand)" name="la_gazebo_trajectory_controller_config" default="la_trajectory_controller"/>
<arg unless="$(eval include_wrist_in_arm_controller and right_hand)" name="ra_gazebo_trajectory_controller_config" default="ra_trajectory_controller_no_wrist"/>
<arg unless="$(eval include_wrist_in_arm_controller and left_hand)" name="la_gazebo_trajectory_controller_config" default="la_trajectory_controller_no_wrist"/>
<rosparam file="$(find sr_robot_launch)/config/gazebo/controller/$(arg ra_gazebo_trajectory_controller_config).yaml" command="load"/>
<rosparam file="$(find sr_robot_launch)/config/gazebo/controller/$(arg la_gazebo_trajectory_controller_config).yaml" command="load"/>
<node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="ra_trajectory_controller la_trajectory_controller"/>
</group>
<group if="$(arg arm_position)">
<group if="$(arg include_wrist_in_arm_controller)">
<rosparam if="$(eval hands == 'both' or hands == 'right')" file="$(find sr_robot_launch)/config/gazebo/controller/ra_group_position_controller.yaml" command="load"/>
<rosparam if="$(hands == 'left' or hands == 'none')" file="$(find sr_robot_launch)/config/gazebo/controller/ra_group_position_controller_no_wrist.yaml" command="load"/>
<rosparam if="$(eval hands == 'both' or hands == 'left')" file="$(find sr_robot_launch)/config/gazebo/controller/la_group_position_controller.yaml" command="load"/>
<rosparam if="$(hands == 'right' or hands == 'none')" file="$(find sr_robot_launch)/config/gazebo/controller/la_group_position_controller_no_wrist.yaml" command="load"/>
</group>
<group unless="$(arg include_wrist_in_arm_controller)">
<rosparam file="$(find sr_robot_launch)/config/gazebo/controller/ra_group_position_controller_no_wrist.yaml" command="load"/>
<rosparam file="$(find sr_robot_launch)/config/gazebo/controller/la_group_position_controller_no_wrist.yaml" command="load"/>
</group>
<node name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="ra_trajectory_controller la_trajectory_controller"/>
<arg if="$(eval include_wrist_in_arm_controller and right_hand)" name="ra_gazebo_group_position_controller_config" default="ra_group_position_controller"/>
<arg if="$(eval include_wrist_in_arm_controller and left_hand)" name="la_gazebo_group_position_controller_config" default="la_group_position_controller"/>
<arg unless="$(eval include_wrist_in_arm_controller and right_hand)" name="ra_gazebo_group_position_controller_config" default="ra_group_position_controller_no_wrist"/>
<arg unless="$(eval include_wrist_in_arm_controller and left_hand)" name="la_gazebo_group_position_controller_config" default="la_group_position_controller_no_wrist"/>
<rosparam file="$(find sr_robot_launch)/config/gazebo/controller/$(arg ra_gazebo_group_position_controller_config).yaml" command="load"/>
<rosparam file="$(find sr_robot_launch)/config/gazebo/controller/$(arg la_gazebo_group_position_controller_config).yaml" command="load"/>
<node name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="ra_group_position_controller la_group_position_controller"/>
</group>
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
Expand All @@ -155,8 +154,7 @@
</group>
<group if="$(arg hand_ctrl)">
<node name="sr_controller_spawner" pkg="sr_utilities" type="controller_spawner.py" output="screen">
<param if="$(arg hand_trajectory)" name="controller_group" value="trajectory"/>
<param unless="$(arg hand_trajectory)" name="controller_group" value="position"/>
<param name="controller_group" value="$(arg hand_controller_group)"/>
<param name="exclude_wrist" value="$(arg include_wrist_in_arm_controller)"/>
</node>
</group>
Expand All @@ -172,6 +170,8 @@
<arg name="robot_description" value="$(arg robot_description)"/>
<arg name="robot_config_file" value="$(arg robot_config_file)"/>
<arg name="hands" value="$(arg hands)"/>
<arg name="hand_ctrl" value="$(arg hand_ctrl)"/>
<arg name="hand_controller_group" value="$(arg hand_controller_group)"/>
<arg name="rh_serial" value="$(arg rh_serial)"/>
<arg name="lh_serial" value="$(arg lh_serial)"/>
<arg name="rh_mapping_path" value="$(arg rh_mapping_path)"/>
Expand Down