UR5 , ursim, TwistStamped is not moving

[PedroMoreo]

I'm quite new to ROS 2 and MoveIt, and I'm currently trying to launch the URSim simulator (ur cap external controller), the UR driver, MoveIt, and the Servo node in order to send a twist command from another terminal.
For now, I'm trying to avoid using launch files and keeping everything manual from the command line.
Any advice or guidance would be greatly appreciated!
Thanks in advance.
ur5_simulated_config.yaml
attached the config.yaml

This is what I am launching in each terminal:

shell1

source /opt/ros/jazzy/setup.bash
source ~/ws_moveit/install/setup.bash

docker run -it --privileged \
  -p 29999:29999 \
  -p 30001:30001 \
  -p 30002:30002 \
  -p 30003:30003 \
  -p 30004:30004 \
  -p 5900:5900 \
  -p 6080:6080 \
  -v ${HOME}/.ursim/urcaps:/ursim/urcaps \
  -v ${HOME}/.ursim/programs:/ursim/programs \
  --name ursim \
  universalrobots/ursim_e-series

(for the next time just...)
docker start -i ursim

it cointains the EXTERNAL URCAP

shell2

source /opt/ros/jazzy/setup.bash
source ~/ws_moveit/install/setup.bash

ros2 launch ur_robot_driver ur_control.launch.py \
  ur_type:=ur5e \
  robot_ip:=172.17.0.2 \
  launch_rviz:=false

PRESS PLAY IN THE URSIM

shell3 (SERVO)

source /opt/ros/jazzy/setup.bash
source ~/ws_moveit/install/setup.bash

Change controllers

ros2 control switch_controllers   --activate forward_velocity_controller   --deactivate scaled_joint_trajectory_controller   -c /controller_manager
ros2 control list_controllers
ros2 topic info /forward_velocity_controller/commands

global parameters

URDF (URSim/real): ur_type:=ur5e o ur5
URDF_XML="$(ros2 run xacro xacro \
  $(ros2 pkg prefix ur_description)/share/ur_description/urdf/ur.urdf.xacro \
  ur_type:=ur5e name:=ur prefix:="")"

SRDF from driver folder:

SRDF_XML="$(ros2 run xacro xacro \
  $(ros2 pkg prefix ur_moveit_config)/share/ur_moveit_config/srdf/ur.srdf.xacro \
  name:=ur)"

Kinematicos in a temp file tmp/KIM.YAML for IK SOLVER

cat > /tmp/ur_kin.yaml <<'YAML'
servo_node:
  ros__parameters:
    robot_description_kinematics:
      ur_manipulator:
        kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
        kinematics_solver_search_resolution: 0.005
        kinematics_solver_timeout: 0.005
YAML

servo

ros2 run moveit_servo servo_node --ros-args \
  -p robot_description:="$URDF_XML" \
  -p robot_description_semantic:="$SRDF_XML" \
  -p use_sim_time:=true \
  --params-file /tmp/ur_kin.yaml \
  --params-file ~/ws_moveit/src/moveit2/moveit_ros/moveit_servo/config/ur5_simulated_config.yaml

we need to take care of the use_sim_time, otherwise the servo node is waiting forever the robot state

Everthing is fine unilt this point. No error, or warnings, we have signals of correct function.

shell4

source /opt/ros/jazzy/setup.bash
source ~/ws_moveit/install/setup.bash
**unset  the use_sim_time** 
ros2 param set /servo_node use_sim_time false

Change command mode TWIST
ros2 service call /servo_node/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"
publish TWIST

ros2 topic pub -r 100 /delta_twist_cmds geometry_msgs/TwistStamped "{
  header: {frame_id: 'base'},
  twist: {linear: {x: 0.01, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}
}"

---

Robot does not move.
The console doesn’t show any errors. Other times launch a warning due to singularities, but most of times, no response at all.

In another terminal:

ros2 topic hz /delta_twist_cmds 
WARNING: topic [/delta_twist_cmds] does not appear to be published yet

which I think is the problem.

All the common checks, are fine, like the node list, the node topic...

Any advice or guidance would be greatly appreciated!
Thanks in advance.

[urfeex]

To be honest, I have been struggling on getting moveit_servo running myself. A while back I implemented a PoC for a direct linear twist controller in https://github.com/urfeex/Universal_Robots_ROS2_Driver/tree/twist_controller

[PedroMoreo]

Thanks a lot.
I will try this repository as soon as I can.

For the moment we could make it works, changing the controller for the position controller instead of velocity
ros2 control switch_controllers --activate forward_position_controller --deactivate scaled_joint_trajectory_controller -c /controller_manager

I am not sure if this velocity controller is compatible with ur5e (ursim), for the moment I couldnt make it works.

On the other hand, the movement performed by this controller, doesn't stop although I press CONTROL+C and cut the instruction loop. To stop the arm, we need to send it a 0,0,0

ros2 topic pub --once /delta_twist_cmds geometry_msgs/TwistStamped "{
  header: {frame_id: 'base_link'},
  twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}
}"

which we don't know if it's correct, because this type of controller is made for continuous movement or something is still wrong in our configuration.
The answer to this question would be very helpful to me.

Thanks a lot again

[urfeex]

Yes, the forward velocity controller doesn't have a timeout or anything built-in. You'll have to manually send a zero command to make it stop. I don't know about moveit_servo, though.