1
$\begingroup$

I use ubuntu 20.04 and ROS noetic. And, I want to connect Moveit with Gazebo simulation and Rviz. The robot model is auboi5 robot arm. I call /aubo_i5/controller_manager/list_controllers and the result is in the following.

controller: 
  - 
    name: "manipulator_i5_controller"
    state: "running"
    type: "position_controllers/JointTrajectoryController"
    claimed_resources: 
      - 
        hardware_interface: "hardware_interface::PositionJointInterface"
        resources: 
          - foreArm_joint
          - shoulder_joint
          - upperArm_joint
          - wrist1_joint
          - wrist2_joint
          - wrist3_joint
  - 
    name: "gripper_controller"
    state: "running"
    type: "position_controllers/JointTrajectoryController"
    claimed_resources: 
      - 
        hardware_interface: "hardware_interface::PositionJointInterface"
        resources: 
          - gripper_finger1_joint

But, when I press plan and execute button for one of the movegroups (i.e. manipulator_i5) in the Rviz, I got the following error message:

[ WARN] [1711986151.244228319, 9.694000000]: Failed to read controllers from /controller_manager/list_controllers
[ERROR] [1711984845.865042658, 780.646000000]: Unable to identify any set of controllers that can actuate the specified joints: [ foreArm_joint shoulder_joint upperArm_joint wrist1_joint wrist2_joint wrist3_joint ]
[ERROR] [1711986151.244323874, 9.694000000]: Known controllers and their joints:

This is my ros_controllers.yaml


aubo_i5:
  moveit_sim_hw_interface:
    joint_model_group: controllers_initial_group_
    joint_model_group_pose: controllers_initial_pose_

  generic_hw_control_loop:
    loop_hz: 300
    cycle_time_error_threshold: 0.01

  hardware_interface:
    joints:
      - shoulder_joint
      - upperArm_joint
      - foreArm_joint
      - wrist1_joint
      - wrist2_joint
      - wrist3_joint
    sim_control_mode: 1  # 0: position, 1: velocity

  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

  manipulator_i5_controller: 
    type: position_controllers/JointTrajectoryController
    joints: 
      - shoulder_joint
      - upperArm_joint
      - foreArm_joint
      - wrist1_joint
      - wrist2_joint
      - wrist3_joint
    gains:
      shoulder_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
      upperArm_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
      foreArm_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
      wrist1_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
      wrist2_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
      wrist3_joint: {p: 100, d: 1, i: 1, i_clamp: 1}

  gripper_controller: 
    type: position_controllers/JointTrajectoryController
    joints: 
        - gripper_finger1_joint
    gains:
      gripper_finger1_joint: {p: 100, d: 1, i: 1, i_clamp: 1}

controller_list:
  - name: aubo_i5/manipulator_i5_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints: 
      - shoulder_joint
      - upperArm_joint
      - foreArm_joint
      - wrist1_joint
      - wrist2_joint
      - wrist3_joint
      
  - name: aubo_i5/gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints: 
      - gripper_finger1_joint

This is my ros_controllers.launch:



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

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

  <!-- Load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" ns="/aubo_i5" output="screen" 
    args="--namespace=/aubo_i5 
    manipulator_i5_controller 
    gripper_controller 
    --timeout 30"/>

</launch>

This is ros_control_moveit_controller_manager.launch.xml

<launch>
    <!-- Define MoveIt controller manager plugin -->
    <param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
    <rosparam file="$(find aubo_i5_gripper_moveit_config)/config/ros_controllers.yaml"/>
</launch>

This is my demo_gazebo.launch


<?xml version="1.0"?>
<launch>
  <!-- MoveIt options -->
  <arg name="pipeline" default="ompl" doc="Planning pipeline to use with MoveIt"/>

  <arg name="use_gui" default="false" />
  
  <!-- By default, we are not in debug mode -->
  <arg name="debug" default="false" />

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

  <!-- Gazebo options -->
  <arg name="gazebo_gui" default="true" doc="Start Gazebo GUI"/>
  <arg name="paused" default="false" doc="Start Gazebo paused"/>
  <arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>
  <arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
<!-- <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> -->
<rosparam param="source_list">[aubo_i5/joint_states]</rosparam>
  </node>  
  <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
    <!-- <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> -->
    <rosparam param="source_list">[aubo_i5/joint_states]</rosparam>
  </node>

  <!-- Launch Gazebo and spawn the robot -->
  <include file="$(dirname)/gazebo.launch" pass_all_args="true"/>

  <!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find aubo_i5_gripper_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
    <arg name="moveit_controller_manager" value="ros_control" />
  </include>  

  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find aubo_i5_gripper_moveit_config)/launch/moveit_rviz.launch">
<arg name="rviz_config" value="$(find aubo_i5_gripper_moveit_config)/launch/moveit.rviz"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

</launch>

This is gazebo.launch

<?xml version="1.0"?>
<launch>
  <!-- Gazebo options -->
  <arg name="gazebo_gui" default="true" doc="Start Gazebo GUI"/>
  <arg name="paused" default="false" doc="Start Gazebo paused"/>
  <arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>
  <arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>
  <arg name="initial_joint_positions" default=" -J foreArm_joint 0 -J shoulder_joint 0 -J upperArm_joint 0 -J wrist1_joint 0 -J wrist2_joint 0 -J wrist3_joint 0" doc="Initial joint configuration of the robot"/>

  <!-- Start Gazebo paused to allow the controllers to pickup the initial pose -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch" pass_all_args="true">
    <arg name="paused" value="true"/>
  </include>

  <!-- Set the robot urdf on the parameter server -->
  <param name="robot_description" textfile="$(find aubo_i5_gripper_moveit_config)/config/gazebo_aubo_i5.urdf" />

  <!-- Unpause the simulation after loading the robot model -->
  <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />

  <!-- Spawn the robot in Gazebo -->
  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
    respawn="false" output="screen" />

  <!-- Load the controller parameters onto the parameter server -->
  <rosparam file="$(find aubo_i5_gripper_moveit_config)/config/gazebo_controllers.yaml" />
  <include file="$(dirname)/aubo_i5_controllers.launch"/>

  <!-- Spawn the Gazebo ROS controllers -->
  <node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />

  <!-- 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">
      <remap from="/joint_states" to="aubo_i5/joint_states" />
  </node>
</launch>

I have spent many hours on how to configure it but still cannot get it work and really have no clues. Any advice would be appreciated.

$\endgroup$

0

Your Answer

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

Browse other questions tagged or ask your own question.