Moveit servo with Universal_Robots_ROS: Very close to a singularity, emergency stop. Stale command. Try a larger 'incoming_command_timeout' parameter?

[caimingxue]

When I use moveit servo to control ur5 in gazebo and when i publish the twist according to the tutorial : https://ros-planning.github.io/moveit_tutorials/doc/realtime_servo/realtime_servo_tutorial.html. It will occur : NODES / servo_server (moveit_servo/servo_server)

ROS_MASTER_URI=http://localhost:11311

process[servo_server-1]: started with pid [8290] [ INFO] [1650695836.257584152]: Loading robot model 'ur5'... [ WARN] [1650695836.257884177]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world' [ INFO] [1650695836.257897989]: No root/virtual joint specified in SRDF. Assuming fixed joint [ WARN] [1650695836.276761883]: Kinematics solver doesn't support #attempts anymore, but only a timeout. Please remove the parameter '/robot_description_kinematics/manipulator/kinematics_solver_attempts' from your configuration. [ INFO] [1650695836.298708822]: Starting planning scene monitor [ INFO] [1650695836.299459937]: Listening to '/planning_scene' [ INFO] [1650695836.299473662]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. [ INFO] [1650695836.300126467]: Listening to '/collision_object' [ INFO] [1650695836.300849959]: Listening to '/planning_scene_world' for planning scene world geometry [ INFO] [1650695836.302268702]: Listening to '/attached_collision_object' for attached collision objects [ WARN] [1650695879.747646847, 1021.064000000]: Very close to a singularity, emergency stop [ WARN] [1650695879.747686285, 1021.064000000]: Stale command. Try a larger 'incoming_command_timeout' parameter? [ WARN] [1650695889.750920119, 1031.064000000]: Stale command. Try a larger 'incoming_command_timeout' parameter?

Moreover, when I see the twist pub node: Node [/rostopic_8564_1650695879361] Publications:

• /servo_server/delta_twist_cmds [geometry_msgs/TwistStamped]

Subscriptions: None

Services:

• /rostopic_8564_1650695879361/get_loggers
• /rostopic_8564_1650695879361/set_logger_level

contacting node http://cmx-Z590-AORUS-PRO-AX:33265/ ... Pid: 8564 Connections:

• topic: /servo_server/delta_twist_cmds
  • to: /servo_server
  • direction: outbound (38459 - 127.0.0.1:48400) [6]
  • transport: TCPROS

The Subscriptions is none. It is very strange. No matter how i change the parameter in the moveit servo parameters, it still does not work and the arm does not move. Could you give me some advices. Thanks

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[AndyZe]

It may be that your workspace is not sourced correctly since you say the parameters are not updated. Can you tell exactly what commands you used to launch things?

[alimardanov]

@AndyZe I am also facing the same problem. My commands to launch are the same as the Moveit_Servo tutorials: roslaunch ur_gazebo ur5.launch

roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true

roslaunch ur5_moveit_config moveit_rviz.launch config:=true

rosservice call /controller_manager/switch_controller "start_controllers: ['joint_group_position_controller'] stop_controllers: ['arm_controller'] strictness: 0 start_asap: false timeout: 0.0"

roslaunch moveit_servo spacenav_cpp.launch

I am publishing TwistStamped from Unity. The only thing that I changed is the spacenav_to_twist.cpp. Now the script receives messages from Unity instead of the joystick node.

[AndyZe]

Let me try to summarize the problem to check my understanding. The spacenav_to_twist node does not show any subscriptions when you do rosnode info spacenav_to_twist?

It looks like the code where that subscriber ought to be created is very short and simple: https://github.com/ros-planning/moveit/blob/a63580edd05b01d9480c333645036e5b2b222da9/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp#L54

Possibly it would help to increase NUM_SPINNERS here. That can avoid deadlock sometimes.

[alimardanov]

@AndyZe rostopic info spacenav_to_twist does show me its subscriber. There are both a publisher and a subscriber. However, when I start publishing from Unity, I get 'Stale command. Try a larger 'incoming_command_timeout'. There is also a very close to singularity warning even non of the robot joints are close to 0 value. I will attach an image of the robot configuration that produces close to singularity warning,

[AndyZe]

Well, I think the simplest explanation is correct here. The robot probably is very close to a singularity. (That can happen at other joint values besides all-zeros.)

You can verify by a print statement right here: https://github.com/ros-planning/moveit/blob/a63580edd05b01d9480c333645036e5b2b222da9/moveit_ros/moveit_servo/src/servo_calcs.cpp#L705

You might be able to work around this by increasing these singularity thresholds in the yaml file:

lower_singularity_threshold: 17 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30 # Stop when the condition number hits this

Of course, that can be dangerous. You don't want to make these values too large.