0
$\begingroup$

Rosanswers logo

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
   joint: Joint_5
   pid: {p: 100.0, i: 0.01, d: 10.0}
Joint_6_position_controller:
   type: position_controllers/JointPositionController
   joint: Joint_6
   pid: {p: 100.0, i: 0.01, d: 10.0}
Joint_7_position_controller:
   type: position_controllers/JointPositionController
   joint: Joint_7
   pid: {p: 100.0, i: 0.01, d: 10.0}

# Group Position Controllers ---------------------------------------
# Allows to send single ROS msg of Float64MultiArray to all joints
joint_position_controller:
   type: position_controllers/JointGroupPositionController
   joints:
      - Joint_1
      - Joint_2
      - Joint_3
      - Joint_4
      - Joint_5
      - Joint_6
      - Joint_7

Originally posted by MRRobot on ROS Answers with karma: 60 on 2021-07-06

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I figured it out, so I will leave this answer in case it helps anyone else.

There is no need to implement new action server. The key is to point MoveIt towards the joint trajectory controller and load the controller.

position_trajectory_controller:
   type: position_controllers/JointTrajectoryController
   joints:
      - Joint_1
      - Joint_2
      - Joint_3
      - Joint_4
      - Joint_5
      - Joint_6
      - Joint_7

/follow_joint_trajectory needs to be remapped to the controller. The controller also needs to be loaded by the controller manager.

  <!-- Run the main MoveIt! executable without trajectory execution-->
  <remap from="/follow_joint_trajectory" to="manipulator_joint_trajectory_controller/follow_joint_trajectory"/>

<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="spawn joint_state_controller position_trajectory_controller" />

Finally MoveIt needs to read params from the same controller filte. robot_name_moveit_controller_manager.launch.xml:

<launch>
  <!-- loads moveit_controller_manager on the parameter server which is taken as argument
    if no argument is passed, moveit_simple_controller_manager will be set -->
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

  <!-- loads ros_controllers to the param server -->
  <rosparam file="$(find robot_name)/config/ros_controllers.yaml"/>
</launch>

The final launch file will look something like this:

<launch>
  <!-- By default, we will load or override the robot_description -->
  <arg name="load_robot_description" default="true"/>

  <!-- Given the published joint states, publish tf for the robot links -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

  <!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
  <remap from="/follow_joint_trajectory" to="position_trajectory_controller/follow_joint_trajectory"/>

  <include file="$(find robot_name)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="false"/>
    <arg name="execution_type" value="interpolate"/>
    <arg name="info" value="true"/>
    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
  </include>


  <!-- Launch HW interface -->
  <!-- Load URDF -->
  <arg name="urdf_path" default="$(find robot_name_description)/urdf/robot_name_description.urdf"/>
  <param name="robot_description" textfile="$(arg urdf_path)" />

  <!-- Load hardware interface -->
    <node name="robot_hardware_interface" pkg="ros_control_boilerplate" type="sim_hw_main"
          output="screen" launch-prefix=""/>

  <!-- Load controller settings -->
    <rosparam file="$(find robot_name)/config/ros_controllers.yaml" command="load"/>

  <!-- Load controller manager -->
    <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
      output="screen" args="spawn joint_state_controller position_trajectory_controller" />
</launch>

Originally posted by MRRobot with karma: 60 on 2021-07-08

This answer was ACCEPTED on the original site

Post score: 4

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.