[fetch] Exclude specific collision-check-pairs to ignore insignificant collisions

[708yamaguchi]

Before

When I send fetch :reset-pose and calculate :self-collision-check in euslisp, some collision pairs are always output like below.

2.irteusgl$ (send *fetch* :reset-pose)
#f(20.0 75.6304 80.2141 -11.4592 98.5487 0.0 95.111 0.0 0.0 0.0)
3.irteusgl$ (send *fetch* :self-collision-check)
((#<bodyset-link #X56d8ca8 base_link  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X57bc388 bellows_link2  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X56d8ca8 base_link  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X57b7788 bellows_link  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X57a00b0 torso_fixed_link  -86.875 0.0 377.425 / 0.0 0.0 0.0> . #<bodyset-link #X56d9a28 torso_lift_link  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X57a00b0 torso_fixed_link  -86.875 0.0 377.425 / 0.0 0.0 0.0> . #<bodyset-link #X57bc388 bellows_link2  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X57a00b0 torso_fixed_link  -86.875 0.0 377.425 / 0.0 0.0 0.0> . #<bodyset-link #X57b7788 bellows_link  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X57bc388 bellows_link2  -86.875 0.0 397.43 / 0.0 0.0 0.0> . #<bodyset-link #X57b7788 bellows_link  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X58b6458 r_gripper_finger_link  35.009 -126.734 756.756 / 1.961 -1.492 -0.445> . #<bodyset-link #X58ad070 l_gripper_finger_link  65.797 -128.387 757.797 / 1.961 -1.492 -0.445>))

Figure of reset-pose of fetch is shown below.
The output of :self-collision-check says that base link, bellows links and torso link collide, but this information seems to be meaningless.

After

In this Pull Request, I exclude the specific pairs from :collision-check-pairs. With this Pull Request, output of :self-collision-check is below.

2.irteusgl$ (send *fetch* :reset-pose)
#f(20.0 75.6304 80.2141 -11.4592 98.5487 0.0 95.111 0.0 0.0 0.0)
3.irteusgl$ (send *fetch* :self-collision-check)
nil

[708yamaguchi]

thank you for your cooperation, @itohdak

[Affonso-Gui]

Have you considered extracting the list of pairs to ignore from the moveit configuration file?
https://github.com/fetchrobotics/fetch_ros/blob/melodic-devel/fetch_moveit_config/config/fetch.srdf

It seems that you have all the pairs marked as Default + the gripper one

EDIT: Default is "Collision in default", i.e. "Always in collision"

[708yamaguchi]

@Affonso-Gui
I have not considered the srdf file you mentioned, and your idea seems to be good for me.
However, unfortunately, I do not know how to parse the srdf file in euslisp.
Can you tell me how to do that?

[Affonso-Gui]

I do not believe we have any kind of utility to parse xml in euslisp (@k-okada please correct me if I am mistaken).
Being so, you could try one of the below:

1. Make your own parser
2. Parse in other language and provide the result through FFI / ROS/ save to file
3. Try to reproduce other xml-parsing common lisp packages (not very recommended, though)

This case should be simple enough to go for a custom parser, reading strings line-by-line or by delimiter

EDIT: Actually custom-parsing might be tricky in cases where newlines are missing/ the corresponding part is commented out (since the .srdf is usually an automatically-generated file it might be ok to ignore such cases).

Also take a look in the robot_description_semantic rosparam. It should have the contents of the .srdf file

[Affonso-Gui]

Could also be interesting to give the .srdf config file as an optional parameter in euscollada and include collision information directly in the robot.l

[knorth55]

SRDF might be not enough because euslisp makes convex for each link as below link.
SRDF uses collision link written in URDF, and euslisp makes convex from euslisp robot model.
start-jsk/jsk_apc#2624

I think writing SRDF parser is not a good idea, and just manually write and check various angle-vectors for checking.

[knorth55]

See also here #895

[Affonso-Gui]

Something like checking the collision pairs in reset-pose and adding them to collision-ignore (probably in the instance :init) ?

Since there should be no collision links in the reset-pose all pairs appearing in collision are interpreted as Always in collision

[k-okada]

@708yamaguchi please add test code, something like
https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_baxter_robot/baxtereus/test/test-baxter.l#L7-L18

[k-okada]

@708yamaguchi please resolve conflicts

[708yamaguchi]

I force-pushed the commit and resolved the conflicts.