Calling Driver Services while Servoing

[Fneylon]

Hello,

We are working with xarm7 and have a few questions about accessing the services in the xarm driver. There are two instances in which we want to uniquely access these services. We are using the humble branch of the xarm_ros2 repo.

Case 1:

We are looking to achieve the following:

1. The user is able to servo the robot.
2. We are able to enable manual mode via a keypress to call the appropriate services.
3. We are able to switch back to servo mode via a keypress to call the appropriate services. We have looked into git issue #58 and have been able to recreate the behavior. For reference this is what we called:

   
   # Terminal 1: 
   ros2 launch xarm_moveit_servo xarm_moveit_servo_realmove.launch.py robot_ip:=XXX.XXX.X.XXX dof:=7 add_gripper:=true

Terminal 2:

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}" 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 xarm7_traj_controller active # change 2 is here

However, we are unable to servo again after this. 

As a follow up, we investigated the services found in `xarm_driver_service.cpp`. It appears when we launch ` ros2 launch xarm_moveit_servo xarm_moveit_servo_realmove.launch.py robot_ip:=XXX.XXX.X.XXX dof:=7 add_gripper:=true` none of these services are launched. To create these services we made edits to `xarm_params.yaml` and changed line 14 `debug` to `True`:

![Screenshot from 2024-07-12 10-50-17](https://github.com/user-attachments/assets/6b186dea-8cf5-453f-a547-bff3e865232a) 

There appears to be a note in this file saying NOT to edit this file but to instead edit `xarm_user_params.yaml`. We were unable to find this file, though it is in the `.gitignore`? 

Our questions for this use case: 
1. How can we toggle between manual mode and servoing 
2. Do the services laid out in the xarm_driver_services.cpp actually do anything when they are created? How can we get them to do something? and why do we not call the '/xarm/ft_sensor_app_set` service to enable manual mode?

#### **Case 2**
The other major issue we are having while seroving is the robot appears to enter a `singularity` even though, in this figure for example, it is not obvious why the robot is having difficulty moving around itself: 
![Screenshot from 2024-07-02 14-18-00](https://github.com/user-attachments/assets/7279981a-3b32-48b2-8c08-53ecb500542b)

We are looking to achieve the following: 
1. While servoing, the robot is able to plan around itself to avoid singularity that disable the control box which forces us to have to restart our entire pipeline (several times) during an experimental session. 

We contacted support a few weeks ago and received the following response: 

> Did you check set_allow_approx_motion option? Did you enable 'bypassing singularities'？
> There is a set_allow_approx_motion method to set it on ROS2 as well.

We looked into this issue and we do NOT want to set this via the GUI, rather this is something we need to set in our ROS environment. 

Our questions for this use case: 
1. How can we "bypass" singularities via changing parameters/config files/calling services etc...in the ros2 environment?
2. We enabled the `set_allow_approx_motion` service (via command line) and this appeared to do nothing. This is therefore an extension of the previous use case questions. 
3. We tried using the setParameter service calls as well and this did nothing. What does this do? Is this the correct way to "bypass" singularities and joint limits? It appears the joint limits are set quite high as the robot will hault once it reaches about within 25% of the actual listed limit (as observed in the UFactory Studio GUI)

Thank you for your help! 

[penglongxiang]

Hi @Fneylon , actually it is not encouraged to switch or mix control sources together with ros controller operating, sometimes unexpected behavior may occur since we have not yet fully studied moveit servo exception dealing logic behind the scene.

For your inquiry, we just found out if moved to a different pose by another mode (like manual mode), if use keyboard as servo joystick type, the arm will automatically jump back to the last joint configuration under Moveit Servo control when the mode is switched back to servo mode. It seems in this case, the actual joint state is not updated to moveit servo server. We are looking into this issue and see if there is a way to prevent this unexpected behavior. Please be aware of this and try NOT to switch mode under moveit servo application at this moment.

Secondly, for the singularity question, since you are using moveit servo, the inverse kinematics calculation, singularity handling and trajectory planning are all done inside this third party module and our internal set_allow_approx_motion interface will not be effective in this case. However, I just did some search online, maybe bio_ik has the ability to use approximated solution and have singularity avoidance capability, it can be the alternative kinematics plugin for moveit. You can try it first and we will also take a look at it later.

[Fneylon]

Hi @penglongxiang thanks for the insight! Is there additional unexpected behavior we should be aware of outside of the jumping? We were able to fix the jumping behavior when we were switching between planning and servoing. Meaning we are able to servo, plan to a different position, and resume servoing from the final planned position without the robot jumping. We were initially seeing that jumping behavior, but fixed it. We were thinking we would be able to apply the same fix when switching between force control and servoing we just need to know how to access force control via scripts.

As for the singularity question, we've been looking into bio_ik over the last few weeks and we agree it seems like it has the ability to avoid singularities. We tried implementing it and were unsuccessful in seeing any changes in how the robot behaved. Are you aware of additional changes that need to be made outside of getting the bio_ik plug in and making the change in the kinmeatics.yaml? Again, thanks for the help!