Moveit servo with Universal_Robots_ROS_Driver and simulated UR5 controller

[EspenTeigen]

Hi. i have struggled for a while now with implementing moveit servo on my UR-robot. When i try to publish a message to move the robot, I only get messages about halting for collision and Stale command(full log further down). I have confirmed that communication between ROS and UR CB-controller is working by running a path planning with moveit.

My configuration:

ROS melodic on Ubuntu 18.04 in WSL2

I use the Universal_Robots_ROS_Driver to communicate with controller

I have a virtual machine running the CB-controller

ros-melodic-moveit-servo is installed

Here is how I start my setup

roslaunch telecoffee bringup_ur.launch

roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=false

roslaunch telecoffee servo.launch

After everything is booted up i switch controller with

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

and confirms that controller is running

name: "joint_group_vel_controller"
state: "running"
type: "velocity_controllers/JointGroupVelocityController"
claimed_resources:
  -
    hardware_interface: "hardware_interface::VelocityJointInterface"
    resources: [elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

This is servo.launch

<?xml version="1.0"?>

 <launch>
     <node name="servo_server" pkg="telecoffee" type="servo_server" output="screen" >
     <param name="parameter_ns" type="string" value="optional_parameter_namespace" />
     <rosparam ns="optional_parameter_namespace" command="load" file="$(find telecoffee)/config                                                               /ur_servo.yaml" />
     </node>
 </launch>

And this is my servo configuration file ur_servo.yaml

use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
command_in_type: "speed_units" 
scale: # Scale parameters are only used if command_in_type=="unitless"
linear:  0.0006  
rotational:  0.003 
joint: 0.001 
low_pass_filter_coeff: 2.  # Larger --> trust the filtered data more, trust the measurements less.
publish_period: 0.01  # 1/Nominal publish rate [seconds]
publish_delay: 0.005  # Delay between calculation and execution start of command
command_out_type: std_msgs/Float64MultiArray
publish_joint_positions: false
publish_joint_velocities: true
publish_joint_accelerations: false
move_group_name:  manipulator  # Often 'manipulator' or 'arm'
planning_frame: world
ee_frame_name: wrist_3_link  
robot_link_command_frame:  base_link 
incoming_command_timeout:  1  # Stop servoing if X seconds elapse without a new command
num_outgoing_halt_msgs_to_publish: 0
lower_singularity_threshold:  17  
hard_stop_singularity_threshold: 30 # Stop when the condition number hits this
joint_limit_margin: 0.15 
cartesian_command_in_topic: delta_twist_cmds  # Topic for incoming Cartesian twist commands
joint_command_in_topic: delta_joint_cmds # Topic for incoming joint angle commands
joint_topic:  joint_states
status_topic: status # Publish status to this topic
command_out_topic: /joint_group_vel_controller/command # Publish outgoing commands here
check_collisions: true # Check collisions?
collision_check_rate: 40 # [Hz] Collision-checking can easily bog down a CPU if done too often.
collision_check_type: stop_distance
self_collision_proximity_threshold: 0.02 # Start decelerating when a self-collision is this far [m]
scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m]
collision_distance_safety_factor: 1000 
min_allowable_collision_distance: 0.02 # Stop if a collision is closer than this [m]

When everything is up and running, I try to publish

rostopic pub -r 125 -s /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto
    twist:
      linear:
        x: 0.0
        y: 0.0
        z: 0.1
     angular:
        x: 0.0
        y: 0.0
        z: 0.0"

And the response I get from servo_server is

[ INFO] [1620828759.131166000]: Loading robot model 'ur5'...
[ WARN] [1620828759.132970700]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the  URDF frame 'world'
[ INFO] [1620828759.133301200]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1620828759.290105300]: 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] [1620828759.356228300]: Starting planning scene monitor
[ INFO] [1620828759.360651700]: Listening to '/planning_scene'
[ INFO] [1620828759.360754800]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1620828759.365325300]: Listening to '/collision_object'
[ INFO] [1620828759.369331000]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1620828759.378176000]: Listening to '/attached_collision_object' for attached collision objects
[ WARN] [1620828763.655916800]: Halting for collision!
[ WARN] [1620828763.656199500]: Stale command. Try a larger 'incoming_command_timeout' parameter?
[ WARN] [1620828766.655929600]: Halting for collision!
[ WARN] [1620828844.209257700]: last_duration: 0.109173 (0.025)

Here is my file tree

├── CMakeLists.txt
├── README.md
├── config
│ ├── unity.yaml
│ └── ur_servo.yaml
├── include
├── launch
│ ├── bringup_unity.launch
│ ├── bringup_ur.launch
│ └── servo.launch
├── package.xml
├── scripts
│ ├── unityServer.py
│ └── ur_control.py
└── src
├── cpp_interface_example.cpp
└── servo_server.cpp

This is my first post, so please give feedback how I can make the posts better in the future. All feedback that can help me are really appreciated.

Espen

[welcome]

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

[AndyZe]

Hi Espen,

Lol at the name telecoffee.

It's great that all your parameters and the controller seem to be loaded correctly.

I think the error is pretty clear: Halting for collision!

Can you please change collision_check_type to threshold_distance instead of stop_distance? stop_distance never worked well -- probably should remove it as an option.

[EspenTeigen]

Hehe, yeah. We are working on a project with a universal robot, senseGlove and a Shadowhand to remotely make a cup of espresso ;)

I tried your suggestion, and the server gave this feedback when i tried to publish a message to /servo_server/delta_twist_cmds.

process[servo_server-1]: started with pid [9597]
[ INFO] [1621428129.563957300]: Loading robot model 'ur5'...
[ WARN] [1621428129.566979100]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1621428129.567307100]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1621428129.721076900]: 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] [1621428129.832757300]: Starting planning scene monitor
[ INFO] [1621428129.841986600]: Listening to '/planning_scene'
[ INFO] [1621428129.842106400]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1621428129.850952800]: Listening to '/collision_object'
[ INFO] [1621428129.859530300]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1621428129.876484100]: Listening to '/attached_collision_object' for attached collision objects
[ WARN] [1621428132.633438000]: Close to a collision, decelerating
[ WARN] [1621428132.633695300]: Stale command. Try a larger 'incoming_command_timeout' parameter?

The robot is in no way near a self collision or singularity, and I tried different positions as well.

I hope my work let me publish the code when it is done :)

[AndyZe]

Maybe the end effector (the shadow hand) is too close to collision with the robot?

You can open the SRDF and disable collision checking between those 2 bodies.

Or make self_collision_proximity_threshold very small, like 0.001. That will essentially not slow down until the robot is actually in collision with itself.

[EspenTeigen]

hmm. I changed it to self_collision_proximity_treshold: 0.0001, but I still get the [ WARN] [1621428680.316102500]: Stale command. Try a larger 'incoming_command_timeout' parameter? and the robot are not moving.

Can it be that I am using the wrong ee_frame_name?

[AndyZe]

OK, that's good. The collision issue seems to be resolved.

No, ee_frame_name wouldn't cause that.

It must be that your gamepad controller (or whatever it is) is publishing on the wrong topic. It should be publishing on /servo_server/delta_twist_cmds I think.

Use rosnode info to debug

[EspenTeigen]

I do not have a gamepad for this. I am going to send the commands from Unity to ROS when I get the glove as a controller. For the moment I am trying to publish this

rostopic pub -r 100 /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto
twist:
  linear:
    x: 0.0
    y: 0.0
    z: -0.1
  angular:
    x: 0.0
    y: 0.0
    z: 0.0"

I only get the stale command warning when I try to publish.

this is the servo topics

rostopic list | grep servo
/servo_server/delta_joint_cmds
/servo_server/delta_twist_cmds
/servo_server/internal/collision_velocity_scale
/servo_server/internal/worst_case_stop_time
/servo_server/planning_scene_monitor/parameter_descriptions
/servo_server/planning_scene_monitor/parameter_updates
/servo_server/status

and if I run rosnode info on that topic when I am publishing, I get.

Node [/rostopic_14193_1621429621337]
Publications:
 * /servo_server/delta_twist_cmds [geometry_msgs/TwistStamped]

Subscriptions: None

Services:
 * /rostopic_14193_1621429621337/get_loggers
 * /rostopic_14193_1621429621337/set_logger_level


contacting node http://pc7275:60820/ ...
Pid: 14193
Connections:
 * topic: /servo_server/delta_twist_cmds
    * to: /servo_server
    * direction: outbound (60821 - 127.0.0.1:60826) [7]
    * transport: TCPROS

roswtf gives me this

WARNING The following node subscriptions are unconnected:
 * /servo_server:
   * /collision_object
   * /planning_scene_world
   * /attached_collision_object
   * /servo_server/delta_joint_cmds
   * /planning_scene
 * /move_group:
   * /collision_object
   * /place/cancel
   * /trajectory_execution_event
   * /pickup/goal
   * /move_group/goal
   * /execute_trajectory/cancel
   * /planning_scene_world
   * /execute_trajectory/goal
   * /attached_collision_object
   * /place/goal
   * /move_group/cancel
   * /planning_scene
   * /pickup/cancel
 * /ur_hardware_interface:
   * /pos_joint_traj_controller/follow_joint_trajectory/goal
   * /scaled_pos_joint_traj_controller/command
   * /pos_joint_traj_controller/command
   * /pos_joint_traj_controller/follow_joint_trajectory/cancel
   * /ur_hardware_interface/script_command
 * /ur_hardware_interface/ur_robot_state_helper:
   * /ur_hardware_interface/set_mode/goal
   * /ur_hardware_interface/set_mode/cancel

[AndyZe]

* /servo_server/delta_twist_cmds [geometry_msgs/TwistStamped]

Subscriptions: None

^ that's a problem. Can you do rosnode info servo_server to see what topic it subscribes to?

[EspenTeigen]

Here it is

Node [/servo_server]
Publications:
 * /joint_group_vel_controller/command [std_msgs/Float64MultiArray]
 * /rosout [rosgraph_msgs/Log]
 * /servo_server/internal/collision_velocity_scale [std_msgs/Float64]
 * /servo_server/internal/worst_case_stop_time [std_msgs/Float64]
 * /servo_server/planning_scene_monitor/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /servo_server/planning_scene_monitor/parameter_updates [dynamic_reconfigure/Config]
 * /servo_server/status [std_msgs/Int8]

Subscriptions:
 * /attached_collision_object [unknown type]
 * /collision_object [unknown type]
 * /joint_states [sensor_msgs/JointState]
 * /planning_scene [unknown type]
 * /planning_scene_world [unknown type]
 * /servo_server/delta_joint_cmds [unknown type]
 * /servo_server/delta_twist_cmds [geometry_msgs/TwistStamped]
 * /servo_server/internal/collision_velocity_scale [std_msgs/Float64]
 * /servo_server/internal/worst_case_stop_time [std_msgs/Float64]

Services:
 * /servo_server/change_control_dimensions
 * /servo_server/change_drift_dimensions
 * /servo_server/get_loggers
 * /servo_server/planning_scene_monitor/set_parameters
 * /servo_server/reset_servo_status
 * /servo_server/set_logger_level


contacting node http://pc7275:60520/ ...
Pid: 13741
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound (60519 - 127.0.0.1:60561) [10]
    * transport: TCPROS
 * topic: /servo_server/internal/worst_case_stop_time
    * to: /servo_server
    * direction: outbound
    * transport: INTRAPROCESS
 * topic: /joint_group_vel_controller/command
    * to: /ur_hardware_interface
    * direction: outbound (60519 - 127.0.0.1:60771) [14]
    * transport: TCPROS
 * topic: /servo_server/internal/collision_velocity_scale
    * to: /servo_server
    * direction: outbound
    * transport: INTRAPROCESS
 * topic: /joint_states
    * to: /ur_hardware_interface (http://pc7275:55225/)
    * direction: inbound (60737 - pc7275:55224) [12]
    * transport: TCPROS
 * topic: /servo_server/delta_twist_cmds
    * to: /rostopic_14956_1621430475954 (http://pc7275:61408/)
    * direction: inbound (61414 - pc7275:61409) [17]
    * transport: TCPROS
 * topic: /servo_server/internal/collision_velocity_scale
    * to: /servo_server (http://pc7275:60520/)
    * direction: inbound
    * transport: INTRAPROCESS
 * topic: /servo_server/internal/worst_case_stop_time
    * to: /servo_server (http://pc7275:60520/)
    * direction: inbound
    * transport: INTRAPROCESS

[AndyZe]

It must be that the message type of /servo_server/delta_twist_cmds has a mismatch. I'm not sure why yet.

[EspenTeigen]

I noticed that I did the rosnode info servo_server first time without publishing to servo_server/delta_twist_commands, I have updated the previous post

[AndyZe]

Well, I've about reached the end of what I'll debug for free. You could put print statements to see what Servo actually receives here:

moveit/moveit_ros/moveit_servo/src/servo_calcs.cpp

Line 1060 in 5e8bd34

void ServoCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg)

[EspenTeigen]

Thank you so much for your help so far. I really appreciate it 🥇

[EspenTeigen]

I know you won't help anymore, but I thought I will add to our discussion for future references for other, and hope that someone can push me in the right direction as well :)

So I have gotten the robot to move with moveit servo(See further down in post how I fixed it), but there are still some stuff to figure out. The problem I have now, is that when i publish

 rostopic pub /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto
twist:
  linear:
    x: 1
    y: 0.0
    z: 0.0
  angular:
    x: 0.0
    y: 0.0
    z: 0.0"

all joints of the robot goes as fast as possible straight to zero. If i publish something like this

rostopic pub -r 125 -s /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto
twist:
  linear:
    x: 90
    y: -90
    z: 0.0
  angular:
    x: 0.0
    y: 0.0
    z: 0.0"

The robot's joints all go to zero, but takes a different route. I have noticed that when echoing /joint_group_position_controller/command, everything goes to zero there.

Does anyone have a clue how to fix this?

Changes made to get connection up and running

I have added

joint_group_position_controller:
  type: position_controllers/JointGroupPositionController
  joints: *robot_joints

to the ur5_controllers.yaml file that can be found in

Universal_Robots_ROS_Driver->ur_robot_driver/config,

And also chaned

command_out_topic: /joint_group_position_controller/command

in the servo_config.yaml file

I have also changed the controller that are launched in the ur5_bringup.launch file in

Universal_Robots_ROS_Driver->ur_robot_driver/launch

with this

<arg name="controllers" default="joint_state_controller joint_group_position_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
  <arg name="stopped_controllers" default="pos_joint_traj_controller joint_group_vel_controller scaled_pos_joint_traj_controller " doc="Controllers that are initally loaded, but not started."/>

To avoid that I have to change controller every time I launch the ur5_bringup.launch

And here is the rqt_graph of the setup

[AndyZe]

I think the twist command you're sending is just really fast. 1 m/s is pretty quick.

I'm guessing the arm "stretches out" to reach that as well as it can. Then you probably see some singularity warnings, I hope?

Try a slower twist command first.

[EspenTeigen]

It happens even when I just publish a single message with a speed with 0.01 as well. But I do get singularity warnings when I publish with a rate. I must have done something wrong with the controller I implemented, everything works when I follow the moveit servo tutorial with the gazebo model and ros_industrial

[EspenTeigen]

Final update from me.

The error with the robot moving to 0 in joint angle was that i forgot to change the publish_joint variables in the servo config file. I changed it to from

publish_joint_positions: false
publish_joint_velocities: true

to

publish_joint_positions: true
publish_joint_velocities: false

And now it works

[slabua]

@EspenTeigen How did you use the Universal_Robots_ROS_Driver in the first place? I want to use it because I need to use the external control on a real robot.

[slabua]

Solved it by using joint_group_vel_controller.