manual mode with xarm_controller

[mcres]

The goal of this issue is to clarify how or even if it is currently possible to switch between these two behaviors:

1. Use the joint_trajectory_controller/JointTrajectoryController controllers.
2. Use the robot Manual Mode.

This would be really useful to quickly test applications while actually controlling the robot with ros2_control.

Case 1: Manual mode with xarm_api ✅

Using only the xarm_api, I'm able to successfuly switch between manual and position modes:

## Terminal 1

# Launch the ROS 2 API for the xArm5
ros2 launch xarm_api xarm5_driver.launch.py robot_ip:=X.X.X.X

## Terminal 2

# Use the `set_mode` service to set manual mode
ros2 service call /xarm/set_mode xarm_msgs/srv/SetInt16 "{data: 2}"

# Go back to position control mode
ros2 service call /xarm/set_mode xarm_msgs/srv/SetInt16 "{data: 0}"

# The robot can still be moved with the UFACTORY Studio at this point

Case 2: Manual mode with xarm_controller ❌

However, after launching the joint_trajectory_controller/JointTrajectoryController, a manual mode request seems to break some things. The solution is to relaunch everything, which is not desired.

Here's how to reproduce the behavior:

## Terminal 1

# Launch MoveIt and ros2_control for the xArm5
ros2 launch xarm_moveit_config xarm5_moveit_realmove.launch.py robot_ip:=X.X.X.X

# Everything works at this point, and I can move the robot using RViz

Make a set_mode request. This makes moving the robot through RViz stop working:

## Terminal 2

# Use the `set_mode` service to set manual mode
ros2 service call /xarm/set_mode xarm_msgs/srv/SetInt16 "{data: 2}"

The following message appears on terminal 1 after setting the manual mode:

[ros2_control_node-6] [set_state], xArm is not ready to move
[ros2_control_node-6] [ERROR] [1701702810.167133511] [UFACTORY.RobotHW]: [X.X.X.X] Robot State detected! State: 5

Possible workarounds

Is there perhaps a set of steps to achieve what I want? E.g., perhaps I should use the xarm/set_state and xarm/set_mode services, and/or ros2 control commands before/after setting the manual mode?

[penglongxiang]

Hi @mcres, while it is not recommended to switch to manual mode when using ros-controller and Moveit planning, there is a way to achieve your expectation, however please test with enough caution.

The ufactory robotic arm will operate in mode 1 for external servo controller. When you wish to switch to manual mode 2, you may need to call set_mode(2) twice since the first mode change command will first reset mode to 0. Then a set_state(0) service call to enable the drag and teach function. Current mode can be monitored in /xarm/robot_states topic. Before doing this mode change, please make sure the payload and mounting configurations are correct.

If you finished hand teaching and would like to restore back to ros-controller, you need to call set_mode(1) service followed by a set_state(0) service. After that, you must update the current robot state (starting position for next planning) in Moveit since it was no longer the same with last planning finish position. This can be done by selecting/clicking current again for Start State in Moveit Rviz GUI.

[mcres]

Thanks for the prompt reply @penglongxiang.

I'll give it a try and let you know.

[mcres]

@penglongxiang I've tried your suggestion but I'm not able to restore the ros2_controller back:

ros2 service call /xarm/set_mode xarm_msgs/srv/SetInt16 "{data: 2}"
ros2 service call /xarm/set_mode xarm_msgs/srv/SetInt16 "{data: 2}"  # it is indeed needed to call this twice
ros2 service call /xarm/set_state xarm_msgs/srv/SetInt16 "{data: 0}"

# At this point, the robot can be moved in manual mode

ros2 service call /xarm/set_mode xarm_msgs/srv/SetInt16 "{data: 1}"
ros2 service call /xarm/set_state xarm_msgs/srv/SetInt16 "{data: 0}"
# RViz: update the Start State to "Current"

# I'm not able to use the ros2_controller again through RViz

Here's the console output of the process in case it helps. At line 166 I executed the previous commands. Note: the Plan and Execute request complete! attempts correspond to executions with the same start and goal position, but those are not successes.

Any suggestions?

[mcres]

Monitoring the /xarm/robot_states I see that initially, the robot is at {mode, state} = {4,1}.

I can successfully switch between manual mode and the ros2_controller by modifying the commands above:

ros2 service call /xarm/set_mode xarm_msgs/srv/SetInt16 "{data: 2}"
ros2 service call /xarm/set_mode xarm_msgs/srv/SetInt16 "{data: 2}"
ros2 service call /xarm/set_state xarm_msgs/srv/SetInt16 "{data: 0}"

# At this point, the robot can be moved in manual mode

ros2 service call /xarm/set_mode xarm_msgs/srv/SetInt16 "{data: 4}" # change 1 is here
ros2 service call /xarm/set_state xarm_msgs/srv/SetInt16 "{data: 0}"

ros2 control set_controller_state xarm5_traj_controller active # change 2 is here

# At this point, the robot can be moved using RViz again

As an aditional comment, the services requested at /xarm/set_mode and /xarm/set_state, and the corresponding changes in the /xarm/robot_states topic are not very coherent. For instance, the state will never be 0 even though the controller is requested so.

@penglongxiang I could only find this documentation on modes and states. Is there any other place where this is explained? It'd be interesting having a list of the error codes as well.

[penglongxiang]

Hi, @mcres, so I guess you are using joint velocity interface, which will require setting to mode 4 instead of mode 1 for position interface. Sorry I forgot to mention this difference in previous reply.

About the Mode and State explanation, we are working on an online document which will address this topic but it is not released yet, I will post the content here first for your reference.

Mode (setting and feedback):
Mode 0 : xArm controller Position mode (Default mode).
Mode 1 : External trajectory planner servo (position) mode.
Mode 2 : Free-Drive (zero gravity) mode.
Mode 3 : Reserved.
Mode 4 : Joint velocity control mode.
Mode 5 : Cartesian velocity control mode.
Mode 6 : Joint space dynamic online planning mode. (Firmware >= v1.10.0)
Mode 7 : Cartesian space dynamic online planning mode. (Firmware >= v1.11.0)

Mode (only in feedback)
Mode 11: In the process of recorded trajectory playback;

----------------------------------------------------------------------------------------

State (For setting)
state 0: Configure the robot to be STANDBY state in corresponding Mode, as well as clearing the error code. Note: After the setting, feedback state will switch to 2 (READY) automatically.
state 3: Setting the robot to a PAUSED state when executing motion commands, the motion can be resumed by setting state 0.
state 4: Setting the robot to STOP state, it will terminate any execution immediately and will not receive or execute any new command until the sate is set back to STANDBY(0).
state 6: Perform decelerated stop immediately in Mode 0. 

State (For Feedback)
state 1: robot is in motion.
state 2: robot is ready to receive and execute command.
state 3: robot is in PAUSE state.
state 4: robot is in STOP state, can not receive and execute command. Will automatically switch to this state when any error occurs.
state 5: MODE_CHANGED state, will automatically switch to this state if some critical configurations (mode, payload, TCP offset, collision sensitivity, etc) have been changed. Robot will not receive or execute any command until the state is set back to STANDBY(0).
state 6: robot is performing decelerated stop.

As for the Error Code explanation, please refer to our latest user manual and check the 1.2.1 Control Box Error Code section.

[mcres]

Thanks for the info and support @penglongxiang!

[penglongxiang]

@mcres No problem, feel free to contact us again if there are any further issues.