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 manual mode with xarm_controller #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:

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:

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!