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.