Using MoveIt! and ros_control to control hardware
I am trying to use MoveIt! to control a robot, but have a hard time wrapping my head around how to integrate it with ros_control.
I have a working controller which i have created with the help of ros_control_boilerplate. The robot is running with position control. Currently I am just mirroring the set-point as feedback in the HW interface.
I am able to move the robot by sending trajectory_msgs::JointTrajectory to the robot through actionlib::SimpleActionClient.
I struggle to see how i can use MoveIt! to send desired trajectory to the robot.
The MoveIt! Controller Configuration Tutorial states " We will assume that your robot offers a FollowJointTrajectory action service for the arms on your robot." Does this require me to write a new action service form scratch, or can it use the SimpleActionClient used in ros_control_boilerplate?
I have tried adding a FollowJointTrajectory action controller to my controller file.
joint_trajectory_controller:
type: FollowJointTrajectory
default: true
joints:
- Joint_1
- Joint_2
- Joint_3
- Joint_4
- Joint_5
- Joint_6
- Joint_7
When i try to execute MoveIt! planned path I get the following error:
[ERROR] [1625559855.661295665]: Unable to identify any set of controllers that can actuate the specified joints: [ Joint_1 Joint_2 Joint_3 Joint_4 Joint_5 Joint_6 Joint_7 ]
[ERROR] [1625559855.661416865]: Known controllers and their joints:
What am I missing? Any help pointing me in the right direction would be greatly appreciated.
My controller.yaml file looks like this:
# ros_control_boilerplate Settings -----------------------
# Settings for ros_control control loop
generic_hw_control_loop:
loop_hz: 300
cycle_time_error_threshold: 0.01
# Settings for ros_control hardware interface
hardware_interface:
joints:
- Joint_1
- Joint_2
- Joint_3
- Joint_4
- Joint_5
- Joint_6
- Joint_7
sim_control_mode: 0 # 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: 50
# Joint Trajectory Controller -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
position_trajectory_controller:
type: position_controllers/JointTrajectoryController
# These joints can likely just be copied from the hardware_interface list above
joints:
- Joint_1
- Joint_2
- Joint_3
- Joint_4
- Joint_5
- Joint_6
- Joint_7
constraints:
goal_time: 5.0
#stopped_position_tolerance: 0.4 # Defaults to 0.01
Joint_1:
trajectory: 0.60
goal: 0.15
Joint_2:
trajectory: 0.60
goal: 0.15
Joint_3:
trajectory: 0.60
goal: 0.15
Joint_4:
trajectory: 0.60
goal: 0.15
Joint_5:
trajectory: 0.60
goal: 0.15
Joint_6:
trajectory: 0.60
goal: 0.15
Joint_7:
trajectory: 0.60
goal: 0.15
# gains:
# joint1: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
# joint2: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#hold_trajectory_duration: 0 # Defaults to 0.5
# Individual Position Controllers ---------------------------------------
# Allows to send individual ROS msg of Float64 to each joint separately
Joint_1_position_controller:
type: position_controllers/JointPositionController
joint: Joint_1
pid: {p: 100.0, i: 0.01, d: 10.0}
Joint_2_position_controller:
type: position_controllers/JointPositionController
joint: Joint_2
pid: {p: 100.0, i: 0.01, d: 10.0}
Joint_3_position_controller:
type: position_controllers/JointPositionController
joint: Joint_3
pid: {p: 100.0, i: 0.01, d: 10.0}
Joint_4_position_controller:
type: position_controllers/JointPositionController
joint: Joint_4
pid: {p: 100.0, i: 0.01, d: 10.0}
Joint_5_position_controller:
type: position_controllers/JointPositionController ...