Moveit with Gazebo - FollowJointTrajectory does not work

edit

Hello developers!

I'm using Manipulator-H from Robotis github and I'm trying to create a go to goal in gazebo using moveit. By now I made:

• Created with moveit setup assistant the package
• Control it in RViz using Plan and Execute

But my problem is connect to moveit with gazebo. I'm trying to use FollowJointTrajectory without sucess... The ERRORS change between:

• Failed to load mh_arm_controller

OR

• Could not load controller 'mh_arm_controller' because the type was not specified. Did you load the controller configuration on the parameter server (namespace: '/robotis_manipulator_h/mh_arm_controller')?

PS: I listed those two errors most because when I solved one, the other appears.

I will present now the solutions that I tried:

• I followed this tutorial and created my own yaml and launch files with a different pipeline (because the files created by moveit setup assistant does not work by the same error): low level controllers moveit
• Checked import packages for ros control following this video: Which ROS controllers required for PR2 ?
• Try this other tutorial How to control a Gazebo simulated robot with MoveIt! but my error now changes to: MoveItSimpleControllerManager: Action client not connected
• Already looked those solutions too: link1, gazebo controller spawner warming and so on...
• Uninstalled ros a installed again

I think the problem are between: namespace or FollowJointTrajectory most because the ROBOTIS provide a position controller that works properly (I tried to follow the codes model without sucess). I already install every single controller in ros even pr2 controller (I really don't know what is FollowJointTrajectory in google).

WHAT I MADE SO FAR (REMEMBER THAT I TRIED OTHERS FORMATS, BUT THIS IS THE MAIN PIPELINE THAT I TRIED 90% OF THE TIME):

1 - Create a package for manipulator-h in moveit, adding FollowJointTrajectory

2 - manipulator_h_description folder has the manipulator_h.gazebo that I did not change. You can find where the piece of code:

  <!-- ros_control plugin -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/robotis_manipulator_h</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>

3 - moveit setup assistant created

ros_controllers_yaml

robotis_manipulator_h:
# 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: 300
    cycle_time_error_threshold: 0.01
# Settings for ros_control hardware interface
  hardware_interface:
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    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: 50
  controller_list:
    - name: mh_arm_controller
      action_ns: follow_joint_trajectory
      default: True
      type: FollowJointTrajectory
      joints:
        - joint1
        - joint2
        - joint3
        - joint4
        - joint5
        - joint6
        - world_fixed

ros_controllers.launch

<?xml version="1.0"?>
<launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find manipulator_h_moveit)/config/ros_controllers.yaml" command="load"/>

  <!-- Load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="robotis_manipulator_h" args="mh_arm_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="robotis_manipulator_h/joint_states" />
  </node>

</launch>

PS: In this launch, I just changed ns from robotis_manipulator_h to /robotis_manipulator_h.

4 - After this, in gazebo launch called ... (more)