Tweak autocollision threshold in JointInit

[ymollard]

We should be able to tweak the autocollision threshold from the dynamic reconfigure server so that the robot does not inerrrupts by itself during some high speed motions. Info from Comau engineers:

This mechanism is used to detect a collision and calibrating all the currents shall be correctly detected and thus an false collision shall not be generated. If the robot is not calibrated in vertical position the gravity can be seen by the collision algorithm as a real collision. If the client get false collisions can also mean that some axes have mechanical problems....

anyway, this code example in python (but can be used also by command line with rostopic pub -1 /machine_int.....) is used to remove or modulate the collision

machine_init = rospy.Publisher('machine_init', JointInit, queue_size=1)
# first param: mode 3 - manage collision thresold
# second param: axes mask (63 affect all axes)
# third param: current threshold  
imsg = JointInit(3, 63, 10.0)
# publish init to remove collision 
machine_init.publish(imsg)

consider that, depending on the axes, the current thresold is between 0.3 and 0,7 so putting all the axes to 10 means disabling the collsion. Depending on the mask you can change the thresold of one or multiple axes.

IMPORTANT: after this command the collision is disabled e.DO won't stop if it collides with something.

[lucarinelli]

Hi Yoan, I don't if it is still usefull but I think that the topic for this message should be /bridge_init instead of /machine_init. Using /bridge_init I'm able to see the ACK from the joints.
Anyway this does not solve the problem for me: after the execution of a trajectory the robot starts falling and then it brakes with some joints blinking red.
I tracked it down to some code in StateManager.cpp from edo_core_pkg that references some kind of undervoltage.
Have you experienced anything like this? Your edo was behaving differently?
I'm using version 3.0.0 of the controller (raspberry image) and your packages with some 'tweaks (I don't have the gripper).

[ymollard]

This piece of code was sent by a Comau engineer working on eDo so I highly doubt that he made a mistake.
I no longer work with that robot, but in my memories this piece of code was actually disabling collision detection.
Have you tried with machine_init anyway? Getting an ACK from joints doesn't mean they disabled collision detection, they just replied to some JointInit message somehow.

your packages with some 'tweaks (I don't have the gripper).

If you have some time, that would be very interesting to code a nice gripper:=false parameter in the launchfiles that will replace all occurrences of the "127" mask by yours (63, I assume), start and to document how to deal with a robot with no gripper in the README.

[Bracewind]

Hi Yoan,
I currently have the exact same issue than lucarinelli. I tried to apply the code given by Comau, but the problem was not solved.
When you disabled the collision detection, did it work fine ? If yes, how can I be sure it has been disabled ? I've already check that the message is publish on the topic machine_init.
By the way, the issue was still there with version 2.5, even though I use the version 3.0 now.

[lucarinelli]

Hi @Bracewind
I have solved the problem by adding this

        # before disabling edo_algorithms we need to "disable" collision checking on joints and raspberry
        msg_dis_coll_js = JointInit()
        msg_dis_coll_js.mode = 3
        msg_dis_coll_js.joints_mask = 127
        msg_dis_coll_js.reduction_factor = 10.0

        msg_dis_coll_algo = CollisionThreshold()
        msg_dis_coll_algo.joints_mask = 127
        msg_dis_coll_algo.threshold = 10.0

        self._joint_init_command_pub.publish(msg_dis_coll_js)
        self._algo_col_thr_pub = rospy.Publisher('/algo_coll_thr', CollisionThreshold, queue_size=1, latch=True) 
        self._algo_col_thr_pub.publish(msg_dis_coll_algo)

before self.switch_algo_service(enable_algorithm_node) in the init of EdoStates.

And changing the spin method of JointTrajectoryActionServer to:

    def spin(self):
        while not rospy.is_shutdown():
            self.states.update()
            # Necessary to hold still to our position if no trajectory is being executed?
            self.states.joint_control_pub.publish(self.states.msg_jca)
            self._update_rate_spinner.sleep()

I tested it on eDO without gripper version 3.0, I also modified the code from Yoan as you did in your pull request. Anyway I'm going to test it on an eDO with a gripper tomorrow morning probably.

[Bracewind]

I tried your code and it did not work for me. When I put a mask of 127, an error occur and the robot must be reboot. When I put a mask of 63, It works as before, i.e it works but there is times when the robot falls. I put the code between self._joint_calibration_command_pub and rospy.Subscriber('/machine_movement_ack'...

[lucarinelli]

I have done some tests this morning on a model with the gripper, it seems that msg_dis_coll_js.joints_mask should be 63, otherwise it will complain about the gripper not returning an ACK, and msg_dis_coll_algo.joints_mask = 127.

Have you modified also the spin method as I wrote in the previous comment? In my previous tests on a model without the gripper it seemed necessary.

[Bracewind]

Yes, and I just tried with your value, and the issue is not solved for me :/. The robot is still falling at times.
I'm interested to know how you debug because I do not have the slightest idea on how to find the source. I'm not even sure the false collisions are the problem here cause /algo_collision never publish True.

[lucarinelli]

Don't know if it makes any difference but I had the code to disable collisions among rospy.Subscriber('/machine_movement_ack', MovementFeedback, self.move_ack_callback) and self.switch_algo_service(enable_algorithm_node).

I have modified the ministarter.sh on the raspberry to don't launch the control software at the startup and I have made another script to start the control software in the ssh shell in order to see easily the output. I modified some flags in the source of edo_core_pkg that you can find on the rapsberry to enable debug prints. If you have the 3.0.0 installed the source is more or less similar (there are some subtle differences) to what is published on the Comau github profile. By the way you can always extract the source from the sd.

About the robot falling I don't really know what else could be causing the problem at this point. On an edo without gripper it has been working well for a long time now.

Are you using the tablet app to initialize and calibrate edo?

Does the app prompt something about brakes check?

Are you connected by cable to edo?

Offtopic(?): Comau has just published an sdk to interact with edo using python

[Bracewind]

no difference :/
Yeah I'm using the tablet to do all the initialization and calibration, and no, there is just a message about brake being inserted that allows me unlock them to finish the current move, but after that I can't ask the robot to do another move.

I'm using wifi, I doubt that it is the cause though.
When I control the robot manually in cartesian mode, the robot also falls sometimes, so the collision is definitely buggy.

Anyway, thanks for all the help you have given, if you can push your edo_control/edo_core package code somewhere i will track the differences, if not I will try something else

[lucarinelli]

On Friday I couldn't do the tests on the model with the gripper.

I did the tests today and I have again the same problems that you are describing. It seems like the solution that was working for the version w/o gripper for some reason does not work with a gripper attached.

Eventually I will try to reach to Comau if I can't find an obvious solution in a reasonable time.

if you can push your edo_control/edo_core package code somewhere i will track the differences, if not I will try something else

I can't share the modified code right now, but I'm working on a way to share it.

[lucarinelli]

@Bracewind this code works for me on eDO with gripper:

        # before disabling edo_algorithms we need to "disable" collision checking on joints and raspberry
        msg_dis_coll_js = JointInit()
        msg_dis_coll_js.mode = 3
        msg_dis_coll_js.joints_mask = 63
        msg_dis_coll_js.reduction_factor = 10.0

        msg_dis_coll_js_m = JointInit()
        msg_dis_coll_js_m.mode = 3
        msg_dis_coll_js_m.joints_mask = 127
        msg_dis_coll_js_m.reduction_factor = 10.0

        msg_dis_coll_algo = CollisionThreshold()
        msg_dis_coll_algo.joints_mask = 127
        msg_dis_coll_algo.threshold = 10.0

        self._joint_init_command_pub.publish(msg_dis_coll_js)

        self._machine_init_pub = rospy.Publisher('/machine_init', JointInit, queue_size=1, latch=True) 
        self._machine_init_pub.publish(msg_dis_coll_js_m)

        self._algo_col_thr_pub = rospy.Publisher('/algo_coll_thr', CollisionThreshold, queue_size=1, latch=True) 
        self._algo_col_thr_pub.publish(msg_dis_coll_algo)

        self.switch_algo_service(enable_algorithm_node)    

[Bracewind]

It works perfectly, thanks a lot

[ymollard]

If you have some spare time, it would be nice to integrate this to a /edo/enable_collisions service accepting a boolean input and doing all the magic.

Also, it looks like the last eDo messages have some collision-related messages here Comau/eDO_core_msgs@0daad46#diff-af3b638bc2a3e6c650974192a53c7291R75
Does this compile, though? The corresponding .msg files do not exist.

[Bracewind]

I have the edo message package contained in the robot, so I don't know it it compiles with the one from the git repository.
For disabling the collisions, the code I am currently using will not work on v2.5.
I can do it for v3.0 though.
To enable it again, I suppose reduction_factor and threshold should be put at 0.5, right ?

[ymollard]

To enable it again, I suppose reduction_factor and threshold should be put at 0.5, right ?
I assume it's a fair way to make it working again, although this value looks different for each joint (between 0.3 and 0.7) so it would be nice to restore the default values instead (if only they are available somewhere).

[khalid4747]

@Bracewind this code works for me on eDO with gripper:

        # before disabling edo_algorithms we need to "disable" collision checking on joints and raspberry
        msg_dis_coll_js = JointInit()
        msg_dis_coll_js.mode = 3
        msg_dis_coll_js.joints_mask = 63
        msg_dis_coll_js.reduction_factor = 10.0

        msg_dis_coll_js_m = JointInit()
        msg_dis_coll_js_m.mode = 3
        msg_dis_coll_js_m.joints_mask = 127
        msg_dis_coll_js_m.reduction_factor = 10.0

        msg_dis_coll_algo = CollisionThreshold()
        msg_dis_coll_algo.joints_mask = 127
        msg_dis_coll_algo.threshold = 10.0

        self._joint_init_command_pub.publish(msg_dis_coll_js)

        self._machine_init_pub = rospy.Publisher('/machine_init', JointInit, queue_size=1, latch=True) 
        self._machine_init_pub.publish(msg_dis_coll_js_m)

        self._algo_col_thr_pub = rospy.Publisher('/algo_coll_thr', CollisionThreshold, queue_size=1, latch=True) 
        self._algo_col_thr_pub.publish(msg_dis_coll_algo)

        self.switch_algo_service(enable_algorithm_node)    

I can confirm that this works on e.DO with gripper on firmware v2.5. Only thing is that edo_core_msgs must be obtained from the e.DO itself for anyone else trying this because CollisionThreshold message is missing from the Comau github. Thanks a lot for the fix!