Cannot control the real robot through custom hardware interface by moveit!
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 ...