Controlling UR10e with Moveit Servo

[robertokcanale]

Hi, I am trying to set up a UR robot and control it with MoveIt2 servo. I am currently using ubuntu 22.04, Ros2 Humble. I am trying to control a UR10e. So far, I am connected to the robot and can control it via Moveit Simply. Here are the 2 terminals commands needed for me.

" os2 launch ur_robot_driver ur_control.launch.py robot_ip:=192.168.0.100 ur_type:=ur10e use_fake_hardware:=false launch_rviz:=false initial_joint_controller:=scaled_joint_trajectory_controller

ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur10e launch_rviz:=true use_fake_hardware:=false "

How do I control it with Moveit2 Servo? So far, moveit launches a defalt servo node, but how do I activate it, what do i do with it, how to actually control the robot with moveit servo? Is there any guide? the basic page doesnt really work.

[RobertWilbrandt]

Hey, yeah we should probably provide more documentation on that. In order to start moving with servo, you need to do two things:

1. Switch controllers such that forward_position_controller is running, e.g. by using ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller --activate forward_position_controller or changing your robot launch parameters.
2. Starting servo: ros2 service call /servo_node/start_servo std_srvs/srv/Trigger

You should now be able to send commands to it and see the robot move, for example: ros2 topic pub /servo_node/delta_twist_cmds geometry_msgs/msg/TwistStamped "{ header: { stamp: 'now' }, twist: {linear: {x: -0.1}, angular: { }}}" -r 10

Can you verify if this works for you? For general information on how to interact with servo further, see the MoveIt tutorial on the topic.

[robertokcanale]

Hello. thanks for the reply. I followed your instructions and did a few tests and trials. It seems that everything works up to the service call to the servo node.

When I start the servo node, it crashes or nothing happens. I try to send the twist command but nothing happens (I tried sending the twist command before and after calling the servo start. Nothing works). At the same time, I also tried to call other services, like _/servo_node/stopservo exactly as you mentioned above, and this other services work. The _/startservo/ unfortunately doesn't work.

Controller side it looks good:

io_and_status_controller[ur_controllers/GPIOController] active    
force_torque_sensor_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active    
joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active    
joint_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] inactive  
speed_scaling_state_broadcaster[ur_controllers/SpeedScalingStateBroadcaster] active    
forward_position_controller[position_controllers/JointGroupPositionController] active 

The Servo node seems to starts correctly:

[servo_node_main-2] extra_arguments=[{'use_intra_process_comms' : True}]
[servo_node_main-2] to the Servo composable node in the launch file
[servo_node_main-2] [INFO] [1682655944.399326047] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00371013 seconds
[servo_node_main-2] [INFO] [1682655944.399364162] [moveit_robot_model.robot_model]: Loading robot model 'ur'...
[servo_node_main-2] [INFO] [1682655944.461372368] [moveit_ros.current_state_monitor]: Listening to joint states on topic '/joint_states'
[servo_node_main-2] [INFO] [1682655944.475557574] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[servo_node_main-2] [INFO] [1682655944.475653924] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[servo_node_main-2] [INFO] [1682655944.478181060] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[servo_node_main-2] [INFO] [1682655944.478651199] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on '/servo_node/publish_planning_scene'

However, I after I call the service call start node, the servo node dies. (I do ros2 node list and its not there anymore). When I close the termina, I get this error from the servo node:

[servo_node_main-2]   what():  Invalid argument
[servo_node_main-2] Stack trace (most recent call last):
[servo_node_main-2] #23   Object "", at 0xffffffffffffffff, in 
[servo_node_main-2] #22   Object "/opt/ros/humble/lib/moveit_servo/servo_node_main", at 0x55d1f7376ca4, in _start
[servo_node_main-2] #21   Source "../csu/libc-start.c", line 392, in __libc_start_main_impl [0x7f3caca90e3f]
[servo_node_main-2] #20   Source "../sysdeps/nptl/libc_start_call_main.h", line 58, in __libc_start_call_main [0x7f3caca90d8f]
[servo_node_main-2] #19   Source "./src/servo_node_main.cpp", line 50, in main [0x55d1f7376b4d]
[servo_node_main-2] #18   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f3cacfdf154, in rclcpp::spin(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)
[servo_node_main-2] #17   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f3cacfdef3f, in rclcpp::executors::SingleThreadedExecutor::spin()
[servo_node_main-2] #16   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f3cacfd76e5, in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&)
[servo_node_main-2] #15   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f3cacfd7379, in rclcpp::Executor::execute_service(std::shared_ptr<rclcpp::ServiceBase>)
[servo_node_main-2] #14   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f3cacfd9965, in 
[servo_node_main-2] #13   Object "/home/asr/Desktop/CartinLFD/install/ur10eservo/lib/libservo_node.so", at 0x7f3cad443783, in rclcpp::Service<std_srvs::srv::Trigger>::handle_request(std::shared_ptr<rmw_request_id_s>, std::shared_ptr<void>)
[servo_node_main-2] #12   Object "/home/asr/Desktop/CartinLFD/install/ur10eservo/lib/libservo_node.so", at 0x7f3cad443190, in rclcpp::AnyServiceCallback<std_srvs::srv::Trigger>::dispatch(std::shared_ptr<rclcpp::Service<std_srvs::srv::Trigger> > const&, std::shared_ptr<rmw_request_id_s> const&, std::shared_ptr<std_srvs::srv::Trigger_Request_<std::allocator<void> > >)
[servo_node_main-2] #11   Object "/home/asr/Desktop/CartinLFD/install/ur10eservo/lib/libservo_node.so", at 0x7f3cad437230, in moveit_servo::ServoNode::startCB(std::shared_ptr<std_srvs::srv::Trigger_Request_<std::allocator<void> > > const&, std::shared_ptr<std_srvs::srv::Trigger_Response_<std::allocator<void> > > const&)
[servo_node_main-2] #10   Source "./src/servo.cpp", line 72, in start [0x7f3cad360b7a]
[servo_node_main-2] #9    Source "./src/servo_calcs.cpp", line 244, in start [0x7f3cad363d7f]
[servo_node_main-2] #8  | Source "./src/servo_calcs.cpp", line 296, in lock_guard
[servo_node_main-2]     | Source "/usr/include/c++/11/bits/std_mutex.h", line 229, in lock
[servo_node_main-2]     |   228:       explicit lock_guard(mutex_type& __m) : _M_device(__m)
[servo_node_main-2]     | > 229:       { _M_device.lock(); }
[servo_node_main-2]     |   230: 
[servo_node_main-2]     |   231:       lock_guard(mutex_type& __m, adopt_lock_t) noexcept : _M_device(__m)
[servo_node_main-2]       Source "/usr/include/c++/11/mutex", line 112, in stop [0x7f3cad363820]
[servo_node_main-2]         110:       // EINVAL, EAGAIN, EBUSY, EINVAL, EDEADLK(may)
[servo_node_main-2]         111:       if (__e)
[servo_node_main-2]       > 112:    __throw_system_error(__e);
[servo_node_main-2]         113:     }
[servo_node_main-2]         114: 
[servo_node_main-2]         115:     bool
[servo_node_main-2] #7    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f3cacd5489e, in std::__throw_system_error(int)
[servo_node_main-2] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f3cacd5d517, in __cxa_throw
[servo_node_main-2] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f3cacd5d2b6, in std::terminate()
[servo_node_main-2] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f3cacd5d24b, in 
[servo_node_main-2] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f3cacd51bbd, in 
[servo_node_main-2] #2    Source "./stdlib/abort.c", line 79, in abort [0x7f3caca8f7f2]
[servo_node_main-2] #1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x7f3cacaa9475]
[servo_node_main-2] #0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
[servo_node_main-2]     | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
[servo_node_main-2]       Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x7f3cacafda7c]
[servo_node_main-2] Aborted (Signal sent by tkill() 10588 1000)

[Pomorondza]

[servo_node_main-2] what(): Invalid argument If you ask me I would check the parameters you are passing to the servo node.

# Servo node for realtime control
 servo_yaml = load_yaml("ur_moveit_config", "config/ur_servo.yaml")
 servo_params = {"moveit_servo": servo_yaml}
 servo_node = Node(
      package="moveit_servo",
      condition=IfCondition(launch_servo),
      executable="servo_node_main",
      parameters=[
          **servo_params,
          robot_description,
          robot_description_semantic,**
      ],
      output="screen",
  )

[robertokcanale]

Yea I managed to get that to work I believe, but the servo node is still not working... You can have a look at my RQT graph here.... i compared it with the ones from the tutorials. First of all, my servo node is taking in twist commands but it is not throwing anything out: it says it publishes something, but it doesnt.. so it feels like the UR robot is not reading controls form anywhere... should I maye try with another controller?

[Pomorondza]

Since you want to use forward_position_controller with MoveIt servo, it seems like you didn't load and activate this controller yet. On the other hand, joint_trajectory_controller is active, and this one you will need to deactivate so only forward_position_controller is active.

[robertokcanale]

Yea I made the controller switch but still no , nothing happens.

Would you mind sharing your RQT_Graph to have a look at how it works?

[eladpar]

manged to move the robot a bit:

• there is no service /servo_node/start_servo
• changing to forward position controller using ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller --activate forward_position_controller

before:

ros2 control list_controllers 
force_torque_sensor_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active    
joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active    
forward_position_controller[position_controllers/JointGroupPositionController] inactive  
io_and_status_controller[ur_controllers/GPIOController] active    
scaled_joint_trajectory_controller[ur_controllers/ScaledJointTrajectoryController] active    
speed_scaling_state_broadcaster[ur_controllers/SpeedScalingStateBroadcaster] active

after:

ros2 control list_controllers 
force_torque_sensor_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active    
joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active    
forward_position_controller[position_controllers/JointGroupPositionController] active    
io_and_status_controller[ur_controllers/GPIOController] active    
scaled_joint_trajectory_controller[ur_controllers/ScaledJointTrajectoryController] inactive  
speed_scaling_state_broadcaster[ur_controllers/SpeedScalingStateBroadcaster] active

• iv'e updated the command type using : ros2 service call /servo_node/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"
• published ` ros2 topic pub /servo_node/delta_twist_cmds geometry_msgs/msg/TwistStamped "{ header: { stamp: 'now', frame_id: 'tool0'}, twist: {linear: {x: -1.3, y: 0.5}, angular: { }}}

the whole process:

change type to twist:

ros2 service call /servo_node/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"

check cforawrd position controller is active:

ros2 control list_controllers

if not , change it by

ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller --activate forward_position_controller

check where the tool 0 is located by:

ros2 run tf2_ros tf2_echo world tool0

look at the Translation and decide where to move it : - Translation: [-0.588, -0.040, 0.832]

pub command

ros2 topic pub /servo_node/delta_twist_cmds geometry_msgs/msg/TwistStamped "{ header: { stamp: 'now', frame_id: 'tool0'}, twist: {linear: {x: -1.3, y: 0.5}, angular: { }}}"

Rqt Graph that works:

I think this should be a tutorial

[shrutichakraborty]

Hey, yeah we should probably provide more documentation on that. In order to start moving with servo, you need to do two things:

1. Switch controllers such that forward_position_controller is running, e.g. by using ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller --activate forward_position_controller or changing your robot launch parameters.
2. Starting servo: ros2 service call /servo_node/start_servo std_srvs/srv/Trigger

You should now be able to send commands to it and see the robot move, for example: ros2 topic pub /servo_node/delta_twist_cmds geometry_msgs/msg/TwistStamped "{ header: { stamp: 'now' }, twist: {linear: {x: -0.1}, angular: { }}}" -r 10

Can you verify if this works for you? For general information on how to interact with servo further, see the MoveIt tutorial on the topic.

Hello @RobertWilbrandt, I am working on the same setup, I have a UR10e with ROS2 Humble. I have followed the installation on moveit2 tutorials and I have installed it in a ws_moveit2. I am able to launch the robot and swithc controllers. On calling the servo_start server ros2 service call /servo_node/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}" and publishing commands to /delta_twist_commands my robot does not move as there is nothing published to "forwrd_position_controller/commands topic. Any ideas?

Thanks!

[IrvingF7]

Hey, yeah we should probably provide more documentation on that. In order to start moving with servo, you need to do two things:

1. Switch controllers such that forward_position_controller is running, e.g. by using ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller --activate forward_position_controller or changing your robot launch parameters.
2. Starting servo: ros2 service call /servo_node/start_servo std_srvs/srv/Trigger

You should now be able to send commands to it and see the robot move, for example: ros2 topic pub /servo_node/delta_twist_cmds geometry_msgs/msg/TwistStamped "{ header: { stamp: 'now' }, twist: {linear: {x: -0.1}, angular: { }}}" -r 10 Can you verify if this works for you? For general information on how to interact with servo further, see the MoveIt tutorial on the topic.

Hello @RobertWilbrandt, I am working on the same setup, I have a UR10e with ROS2 Humble. I have followed the installation on moveit2 tutorials and I have installed it in a ws_moveit2. I am able to launch the robot and swithc controllers. On calling the servo_start server ros2 service call /servo_node/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}" and publishing commands to /delta_twist_commands my robot does not move as there is nothing published to "forwrd_position_controller/commands topic. Any ideas?

Thanks!

Hi!

If you don't have any specific reason that you have to use forwrd_position_controller to do the teleoperation, then I have a working package (at least in RViz) that may be helpful. I directly publish velocities to scaled_joint_trajectory_controller. I will test this on the hardware in a day or two and update it properly as well.

A similar package I wrote works fine for a real-world Xarm robot.

[minku1219]

Hello everyone. Following the discussion i am able to use servo with the forward_position_control with twist_stamp msgs, but my goal is to use velocity control on UR5. So on trying to send the velocity commands to the robot, the robot moves but in wrong direction. Any suggestions or any example for the same is highly appreciated.