Commit 28d7a6a4 authored by Joseph Mirabel's avatar Joseph Mirabel

Move generation of list of joint names to a separate launch file.

parent 7654f791
sot_controller:
libname: libsot-tiago-steel-controller.so
joint_names: [
torso_lift_joint,
arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, gripper_finger_joint,
head_1_joint, head_2_joint ]
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
torques: torques, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0,
......@@ -26,11 +22,20 @@ sot_controller:
ros_control_mode: POSITION
arm_7_joint:
ros_control_mode: POSITION
gripper_finger_joint:
ros_control_mode: POSITION
head_1_joint:
ros_control_mode: POSITION
head_2_joint:
ros_control_mode: POSITION
gripper_finger_joint:
ros_control_mode: POSITION
hand_thumb_joint:
ros_control_mode: POSITION
hand_index_joint:
ros_control_mode: POSITION
hand_mrl_joint:
ros_control_mode: POSITION
dt: 0.01
jitter: 0.004
sot_controller:
joint_names: [
wheel_left_joint, wheel_right_joint,
torso_lift_joint,
arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, gripper_finger_joint,
head_1_joint, head_2_joint ]
control_mode:
wheel_left_joint:
ros_control_mode: VELOCITY
......
<launch>
<arg name="use_mobile_base" doc="Whether SoT should control the mobile base"/>
<arg name="end_effector" doc="one of false, pal-gripper, pal-hey5, schunk-wsg"/>
<arg name="simulation" default="false"/>
<param name="/sot_controller/use_mobile_base" value="$(arg use_mobile_base)"/>
<include file="$(find sot_tiago_bringup)/launch/sot_params.launch">
<arg name="end_effector" value="$(arg end_effector)"/>
<arg name="use_mobile_base" value="$(arg use_mobile_base)"/>
</include>
<!-- Sot Controller configuration -->
<rosparam command="load" file="$(find roscontrol_sot_tiago)/config/sot_params.yaml"/>
<rosparam if="$(arg use_mobile_base)" command="load" file="$(find roscontrol_sot_tiago)/config/sot_params_mobile_base.yaml"/>
......
......@@ -53,5 +53,6 @@
<run_depend>cmake_modules</run_depend>
<run_depend>roscontrol_sot</run_depend>
<run_depend>controller_interface</run_depend>
<run_depend>sot_tiago_bringup</run_depend>
<!-- The export tag contains other, unspecified, tags -->
</package>
......@@ -13,18 +13,16 @@
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find tiago_description)/robots/tiago.urdf.xacro' end_effector:=$(arg end_effector)"/>
<arg if="$(arg use_mobile_base)" name="mobile_base_joint_names" value="'wheel_left_joint', 'wheel_right_joint', " />
<arg unless="$(arg use_mobile_base)" name="mobile_base_joint_names" value="" />
<arg if="$(eval arg('end_effector') == 'schunk-wsg')" name="gripper_joint_names" value="'gripper_finger_joint', " />
<arg if="$(eval arg('end_effector') == 'pal-hey5' )" name="gripper_joint_names" value="'hand_index_joint', 'hand_mrl_joint', 'hand_thumb_joint', " />
<include file="$(find sot_tiago_bringup)/launch/sot_params.launch">
<arg name="end_effector" value="$(arg end_effector)"/>
<arg name="use_mobile_base" value="$(arg use_mobile_base)"/>
</include>
<!-- Load robot sot params. -->
<group ns="sot">
<rosparam param="tf_base_link">base_footprint</rosparam>
<rosparam param="state_vector_map" subst_value="True">[ $(arg mobile_base_joint_names) "torso_lift_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint", "arm_7_joint", $(arg gripper_joint_names) "head_1_joint", "head_2_joint" ]</rosparam>
</group>
<group ns="sot_controller">
<rosparam param="joint_names" subst_value="True">[ $(arg mobile_base_joint_names) "torso_lift_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint", "arm_7_joint", $(arg gripper_joint_names) "head_1_joint", "head_2_joint" ]</rosparam>
<rosparam param="dt">0.01</rosparam>
</group>
......
<!-- -*-xml-*-
Set parameters /sot/state_vector_map and /sot_controller/joint_names according to the robot model.
-->
<launch>
<arg name="end_effector" doc="one of false, pal-gripper, pal-hey5, schunk-wsg"/>
<arg name="use_mobile_base" doc="Whether SoT should control the mobile base"/>
<arg if="$(arg use_mobile_base)" name="mobile_base_joint_names" value="'wheel_left_joint', 'wheel_right_joint', " />
<arg unless="$(arg use_mobile_base)" name="mobile_base_joint_names" value="" />
<arg if="$(eval arg('end_effector') == 'schunk-wsg')" name="gripper_joint_names" value="'gripper_finger_joint', " />
<arg if="$(eval arg('end_effector') == 'pal-hey5' )" name="gripper_joint_names" value="'hand_index_joint', 'hand_mrl_joint', 'hand_thumb_joint', " />
<!-- Load robot sot params. -->
<group ns="sot">
<rosparam param="state_vector_map" subst_value="True">[ $(arg mobile_base_joint_names) "torso_lift_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint", "arm_7_joint", $(arg gripper_joint_names) "head_1_joint", "head_2_joint" ]</rosparam>
</group>
<group ns="sot_controller">
<rosparam param="joint_names" subst_value="True">[ $(arg mobile_base_joint_names) "torso_lift_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint", "arm_7_joint", $(arg gripper_joint_names) "head_1_joint", "head_2_joint" ]</rosparam>
</group>
</launch>
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment