Talos with simple collision boxes

[dalinel]

This adds two talos model :

• talos_fast_collision.urdf (talos_fast.urdf visual + boxes for collision shapes)
• talos_box.urdf (boxes for visual and collision shapes)

I have taken one box per significant body node
The boxes are entirely covering the talos mesh shapes
Hence the boxes are bigger than the talos mesh shape and collision will happen sooner than what we expect with meshes

You can see the box model here on rviz and robot_dart:

In my code I have added a collisionFilter to the world ConstraintSolver with the following bodynodes pairs:

body_pairs["leg_right_6_link"] = "leg_right_4_link";
body_pairs["leg_left_6_link"] = "leg_left_4_link";
body_pairs["leg_left_1_link"] = "leg_left_3_link";
body_pairs["leg_right_1_link"] = "leg_right_3_link";
body_pairs["arm_left_7_link"] = "arm_left_5_link";
body_pairs["arm_right_7_link"] = "arm_right_5_link";
body_pairs["torso_2_link"] = "base_link";
body_pairs["leg_right_2_link"] = "base_link";
body_pairs["leg_left_2_link"] = "base_link";
body_pairs["head_2_link"] = "torso_2_link";

This is because disableAdjacentBodyCheck(); is only taking directly adjacent bodynodes and it is not enough on some links.

[jbmouret]

Could we add the collision filters to the TalosFast class in robot_dart? (or to a new TalosFastCollisions class)?

[costashatz]

@dalinel thanks for this. A few comments:

• Please merge with latest master, I had a small bug in the CI script (this is why it is failing)
• You haven't added any example code in this PR/branch
• robot_dart already has collision filters (see here). Can't you use those inside a new class (as @jbmouret mentions)?

[costashatz]

Ah and by the way, great work!

[dalinel]

Ok so I have added a TalosFastCollision class
It includes a collision_vec() function https://github.com/resibots/robot_dart/blob/a528f67bb9f870fb71e38b291df06bb165e793eb/src/robot_dart/robots/talos.cpp#L77

It returns a vector of tuple with the body_name, category_mask and collision_maks to be used with
https://github.com/resibots/robot_dart/blob/2f60a43d945be8ec5bec3b6ca90576c02a09157f/src/robot_dart/robot_dart_simu.hpp#L122

I have used this in the examples talos_box.cpp and talos_fast_collision.cpp

[costashatz]

I have used this in the examples talos_box.cpp and talos_fast_collision.cpp

To make this transparent to the user, I'd do this inside the _post_addition function (see here).

Otherwise this is looking niiiice! Thanks!

[dalinel]

Your welcome! I hope it will help to have better Talos simulation !
Now the collision masks are set directly in _post_addition

[costashatz]

@dalinel thanks for the work! I made a few minor fixes and clean-up. Once the tests/build pass, I will merge.. ;)