Cannot control the real robot through custom hardware interface by moveit!

asked 2021-08-17 15:54:33 -0500

lsattf gravatar image

updated 2021-08-17 15:55:59 -0500

Hi. I am going to control my custom 4 joint robot with the inverse kinematic solover in moveit! The package generated by moveit_setup_assistant can be runned by demo_gazebo.launch and the performance in gazebo doesn't have any problem. But when I am running the demo.launch file and my custom finger_hardware_interface_node for real robot. In demo.launch file, I only change <arg name="fake_execution" value="true"/> to <arg name="fake_execution" value="false"/>.And I also do the command roslaunch one_finger_design_03_moveit_config ros_controllers.launch to make sure all the controllers are running properly.

But when I press plan and execute, it fail and this error came up:

[ERROR] [1629230909.569088658]: Unable to identify any set of controllers that can actuate the specified joints: [ joint1 joint2 joint3 joint4 ]
[ERROR] [1629230909.569158674]: Known controllers and their joints:

[ERROR] [1629230909.569230915]: Apparently trajectory initialization failed

I know there are some similar questions online, but all of that didn't work on me. Here is my ros_controller.yaml file:

# Simulation settings for using moveit_sim_controllers
moveit_sim_hw_interface:
  joint_model_group: finger
  joint_model_group_pose: home
# Settings for ros_control_boilerplate 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


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: finger_controller
        action_ns: follow_joint_trajectory
        default: True
        type: FollowJointTrajectory
        joints:
      - joint1
      - joint2
      - joint3
      - joint4


finger_controller:
    type: position_controllers/JointTrajectoryController
      joints:
        - joint1
        - joint2
        - joint3
        - joint4
      gains:
        joint1:
          p: 100
      d: 1
      i: 1
      i_clamp: 1
    joint2:
      p: 100
      d: 1
  i: 1
  i_clamp: 1
joint3:
  p: 100
  d: 1
  i: 1
  i_clamp: 1
joint4:
  p: 100
  d: 1
  i: 1
  i_clamp: 1

And my ros_controllers.launch file:

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

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

  <!-- Load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="joint_state_controller finger_controller  "/>
</launch>

Actually when i run the demo_gazebo.launch, the gazebo can subscribe the goal topic in action following sending by /move_group

 * /finger_controller/follow_joint_trajectory/goal

like:

Type: control_msgs/FollowJointTrajectoryActionGoal

Publishers: 
 * /move_group (http://lsf-HP-ZBook-15-G6:39815/)

Subscribers: 
 * /gazebo (http://lsf-HP-ZBook-15-G6:43811/)

But when I doing it in real robot, the /move_group doesn't send the goal anymore:

Type: control_msgs/FollowJointTrajectoryActionGoal

Publishers: None

Subscribers: 
 * /finger_hardware_interface_node (http://lsf-HP-ZBook-15-G6:45587/)

But the weird thing is it can receive the /joint_states results and feedback:

rosnode info /finger_hardware_interface_node 
--------------------------------------------------------------------------------
Node [/finger_hardware_interface_node]
Publications: 
 * /finger_controller/follow_joint_trajectory/feedback [control_msgs/FollowJointTrajectoryActionFeedback]
 * /finger_controller/follow_joint_trajectory/result [control_msgs/FollowJointTrajectoryActionResult]
 * /finger_controller/follow_joint_trajectory/status [actionlib_msgs/GoalStatusArray]
 * /finger_controller/state [control_msgs/JointTrajectoryControllerState]
 * /joint_states [sensor_msgs/JointState]
 * /rosout [rosgraph_msgs/Log]
 * /set_sync_position [one_finger_project_v3/SyncSetPosition]

    Subscriptions: 

 * /finger_controller/command [unknown type]
 * /finger_controller/follow_joint_trajectory/cancel [unknown type]
 * /finger_controller/follow_joint_trajectory/goal [unknown type]

Services: 
 * /controller_manager/list_controller_types
 * /controller_manager/list_controllers
 * /controller_manager/load_controller



* /controller_manager/reload_controller_libraries
 * /controller_manager/switch_controller
 * /controller_manager/unload_controller
 * /finger_controller/query_state
 * /finger_hardware_interface_node/get_loggers
 * /finger_hardware_interface_node/set_logger_level


contacting node http://lsf-HP-ZBook-15-G6:45587/ ...
Pid: 15504
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound (37063 - 127.0.0.1 ...
(more)
edit retag flag offensive close merge delete