diff --git a/config/simple_moveit_controllers.yaml b/config/simple_moveit_controllers.yaml index ac804cc4b0d0416e6464c021cc5e8aa9bd969ec1..454ca4230068ca37eb9e0de1fc8bee7f56f6070e 100644 --- a/config/simple_moveit_controllers.yaml +++ b/config/simple_moveit_controllers.yaml @@ -1,5 +1,5 @@ controller_list: - - name: joint_trajectory_controller + - name: $(arg transmission)_joint_trajectory_controller action_ns: follow_joint_trajectory type: FollowJointTrajectory default: True @@ -17,4 +17,3 @@ controller_list: default: True joints: - panda_finger_joint1 - diff --git a/launch/demo.launch b/launch/demo.launch index 05eb309ca8526ef97d34a5eb0ab4b8a58bab64fa..3a6db415227257bcf602e62783e658247271f652 100644 --- a/launch/demo.launch +++ b/launch/demo.launch @@ -21,6 +21,8 @@ <arg name="moveit_controller_manager" default="fake" /> <!-- Set execution mode for fake execution controllers --> <arg name="fake_execution_type" default="interpolate" /> + <!-- Transmission used for joint control: position, velocity, or effort --> + <arg name="transmission" /> <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode --> <arg name="use_gui" default="false" /> @@ -50,15 +52,9 @@ </group> <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) --> - <include file="$(dirname)/move_group.launch"> - <arg name="allow_trajectory_execution" value="true"/> - <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" /> - <arg name="fake_execution_type" value="$(arg fake_execution_type)"/> - <arg name="info" value="true"/> - <arg name="debug" value="$(arg debug)"/> - <arg name="pipeline" value="$(arg pipeline)"/> - <arg name="load_gripper" value="$(arg load_gripper)"/> - <arg name="load_robot_description" value="$(arg load_robot_description)"/> + <include file="$(dirname)/move_group.launch" pass_all_args="true"> + <arg name="allow_trajectory_execution" value="true" /> + <arg name="info" value="true" /> </include> <!-- Run Rviz and load the default config to see the state of the move_group node --> diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch index 2f63ceb1888d79e58a184c93efb9cf86feb031eb..2907f8f355960777f7498793ffde0a86599e00d0 100644 --- a/launch/demo_gazebo.launch +++ b/launch/demo_gazebo.launch @@ -14,7 +14,7 @@ <include file="$(find franka_gazebo)/launch/panda.launch" pass_all_args="true"> <arg name="headless" value="$(eval not arg('gazebo_gui'))" /> <arg name="use_gripper" default="$(arg load_gripper)" /> - <arg name="controller" default="joint_trajectory_controller" /> + <arg name="controller" default="$(arg transmission)_joint_trajectory_controller" /> </include> <include file="$(dirname)/demo.launch" pass_all_args="true"> diff --git a/launch/franka_control.launch b/launch/franka_control.launch index 953446181f8fbfedf6819bdd11860e0dccfc80a1..efb0c206bf541249ebcad92481106340d93b47f2 100644 --- a/launch/franka_control.launch +++ b/launch/franka_control.launch @@ -2,9 +2,8 @@ <launch> <!-- Launch real-robot control --> <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true" /> - - <!-- Load joint-trajectory controller into ROS controller_manager --> - <node name="trajectory_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_trajectory_controller" /> + <!-- By default use joint position controllers --> + <arg name="transmission" default="position" /> <!-- as well as MoveIt demo --> <include file="$(dirname)/demo.launch" pass_all_args="true"> diff --git a/launch/move_group.launch b/launch/move_group.launch index 1dc4a5b1b1f6b2711138071af25db09782746789..7cb7e8efa3e406de31355b82dd332e43addfaf8d 100644 --- a/launch/move_group.launch +++ b/launch/move_group.launch @@ -74,10 +74,8 @@ </group> <!-- Trajectory Execution Functionality --> - <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)"> + <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" pass_all_args="true" if="$(arg allow_trajectory_execution)"> <arg name="moveit_manage_controllers" value="true" /> - <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" /> - <arg name="fake_execution_type" value="$(arg fake_execution_type)" /> </include> <!-- Sensors Functionality --> diff --git a/launch/ros_controllers.launch b/launch/ros_controllers.launch index 91a84de358e02aa429e2336538a214625b68d7d5..0354d488e1166c32d770b0added0d517a0b8cca8 100644 --- a/launch/ros_controllers.launch +++ b/launch/ros_controllers.launch @@ -4,8 +4,7 @@ <!-- Load joint controller configurations from YAML file to parameter server --> <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml" command="load"/> - <!-- Load the controllers --> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" - output="screen" args="franka_gripper "/> - + <!-- Load and start the controllers --> + <node ns="/" name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" + args="$(arg transmission)_joint_trajectory_controller franka_gripper" /> </launch> diff --git a/launch/simple_moveit_controller_manager.launch.xml b/launch/simple_moveit_controller_manager.launch.xml index 5b18aca3b68a510e5f46a4e0f6dd8896afae028e..3640908bc4556cba8aebfb365c48fed9ba66c3cc 100644 --- a/launch/simple_moveit_controller_manager.launch.xml +++ b/launch/simple_moveit_controller_manager.launch.xml @@ -3,6 +3,7 @@ <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" /> <!-- Load controller list to the parameter server --> - <rosparam file="$(find panda_moveit_config)/config/simple_moveit_controllers.yaml" /> - <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml" /> + <rosparam subst_value="true" file="$(find panda_moveit_config)/config/simple_moveit_controllers.yaml" /> + <!-- Start ROS controllers --> + <include file="$(dirname)/ros_controllers.launch" pass_all_args="true" /> </launch>