Modifications for launching UR5e ros_control with new MoveIt! config
I am using the MoveIt! Setup Assistant in ROS Kinetic to generate a new MoveIt! configuration with the goal of trying to continuously control a UR5e with a Robotiq 2f-140 gripper, but the configuration is not working as expected. After generating the configuration, I have modifed my_robot_moveit_config/config/controllers.yaml
according to the instructions according to ros_control with the ur_modern_driver package and GripperCommand, but encounter the warning below before the spawner finshes and
[INFO] [1557268989.540753]: Controller Spawner: Waiting for service controller_manager/load_controller
[WARN] [1557262201.240509]: Controller Spawner couldn't find the expected controller_manager ROS interface.
My my_robot_moveit_config/launch/ros_controllers.launch
file is the same as created by the Setup Assistant:
<?xml version="1.0"?>
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find my_robot_moveit_config)/config/ros_controllers.yaml" command="load"/>
<!-- Load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="my_robot" args="pos_based_pos_traj_controller joint_state_controller "/>
<!-- Convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="my_robot/joint_states" />
</node>
</launch>
I have also tried replacing config/ros_controllers.yaml
with config/controllers.yaml
according to the ur_modern_driver
README.md, but it results in the same behavior.
Here is config/controllers.yaml
for reference:
my_robot:
# MoveIt-specific simulation settings
moveit_sim_hw_interface:
joint_model_group: controllers_initial_group_
joint_model_group_pose: controllers_initial_pose_
# Settings for ros_control control loop
generic_hw_control_loop:
loop_hz: 500
cycle_time_error_threshold: 0.01
# Settings for ros_control hardware interface
hardware_interface:
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
- finger_joint
sim_control_mode: 1 # 0: position, 1: velocity
# Publish all joint states
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 500
controller_list:
- name: ""
action_ns: gripper
type: GripperCommand
default: true
joints:
- finger_joint
- name: /pos_based_pos_traj_controller
action_ns: follow_joint_trajectory
default: True
type: FollowJointTrajectory
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
pos_based_pos_traj_controller:
type: position_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10
gains:
shoulder_pan_joint:
p: 100
d: 1
i: 1
i_clamp: 1
shoulder_lift_joint:
p: 100
d: 1
i: 1
i_clamp: 1
elbow_joint:
p: 100
d: 1
i: 1
i_clamp: 1
wrist_1_joint:
p: 100
d: 1
i: 1
i_clamp: 1
wrist_2_joint:
p: 100
d: 1
i: 1
i_clamp: 1
wrist_3_joint:
p: 100
d: 1
i: 1
i_clamp: 1
What am I doing wrong?
This appears to be a cross-post of ros-industrial/ur_modern_driver#305.