Improve primitive shapes in iCubGazeboSimpleCollisionsV2_5

[diegoferigo]

• Use better primitive shapes than #15
• Added collisions also for upper arms and upper legs

Keep in mind that collisions are disabled by default between parent-child links of a joint.

Images in Gazebo Classic

  

I tried to run the following code in Ignition Gazebo (it requires gym_pushrecovery that is not yet open source):

import time
import gym_ignition_models
from gym_pushrecovery import models
from gym_ignition.utils import scenario

gazebo, world = scenario.init_gazebo_sim()

icub = models.icub.ICubGazeboSimpleCollisions(
    world=world,
    model_file=gym_ignition_models.get_model_file("iCubGazeboSimpleCollisionsV2_5"))

assert icub.self_collisions_enabled()

gazebo.gui()
time.sleep(3)
gazebo.run(paused=True)
time.sleep(10)

links_in_contact = icub.links_in_contact()

for _ in range(2000):
    gazebo.run()
    new = icub.links_in_contact()

    if new != links_in_contact:
        links_in_contact = new
        print(links_in_contact)
        time.sleep(0.2)

time.sleep(5)

and it seems working fine (requires https://github.com/robotology/gym-ignition/pull/212). However, contact detection is not optimal. Check in the gif below:

1) Feets are tipping and sometimes one foot get lost 2) When the base touches ground, all other contact disappear and then they come back again

This is a problem with DART's contact detection algorithm, we cannot do much here. I cc also @chapulina and @azeey for reference.

[diegoferigo]

Unfortunately the iCub model, being autogenerated from the CAD file, has a peculiar kinematic tree that breaks the rule I reported above:

Keep in mind that collisions are disabled by default between parent-child links of a joint.

In order to ensure proper self collision handling, I had to resort to bitmask (luckily implemented in ignitionrobotics/ign-physics#116 and https://github.com/ignitionrobotics/ign-gazebo/pull/160). It would have been easier if also the category_bitmask would be supported, but I managed to find a workaround playing with the masks.

I am a bit ashamed for the quality of the following script, but I will post it anyway. It's quite messy and can be much (much!) improved, but it fulfils its purpose for the time being:

collision_bitmask.py

```python from typing import Dict, List link_names = ( 'head', 'chest', 'base', 'r_upper_leg', 'l_upper_leg', 'r_lower_leg', 'l_lower_leg', 'r_foot', 'l_foot', 'r_upper_arm', 'l_upper_arm', 'r_lower_arm', 'l_lower_arm', 'r_hand', 'l_hand', ) # Collision constraints do_not_collision_map = { 'head': ['chest'], 'chest': ['head', 'base', 'r_upper_arm', 'l_upper_arm'], 'base': ['chest', 'r_upper_leg', 'l_upper_leg'], 'r_upper_leg': ['base', 'r_lower_leg'], 'l_upper_leg': ['base', 'l_lower_leg'], 'r_lower_leg': ['r_upper_leg', 'r_foot'], 'l_lower_leg': ['l_upper_leg', 'l_foot'], 'r_foot': ['r_lower_leg'], 'l_foot': ['l_lower_leg'], 'r_upper_arm': ['r_lower_arm', 'chest'], 'l_upper_arm': ['l_lower_arm', 'chest'], 'r_lower_arm': ['r_upper_arm', 'r_hand'], 'l_lower_arm': ['l_upper_arm', 'l_hand'], 'r_hand': ['r_lower_arm'], 'l_hand': ['l_lower_arm'], } # Initialize collision matrix with all None entries collision_bitmasks = {link_name: [None] * len(link_names) for link_name in link_names} # Initialize the main diagonal of the collision matrix for link in collision_bitmasks.keys(): link_idx = link_names.index(link) collision_bitmasks[link][link_idx] = 1 # Initialize the collision matrix with collision constraints for link, others in do_not_collision_map.items(): for other_link in others: other_idx = link_names.index(other_link) collision_bitmasks[link][other_idx] = 0 link_idx = link_names.index(link) collision_bitmasks[other_link][link_idx] = 0 def collide(mask1: List[int], mask2: List[int]) -> bool: assert len(mask1) == len(mask2) for i, el in enumerate(mask1): if el == 1 and mask2[i] == 1: return True return False def broken_constraint(link_name: str, attempt: List[int], link_names: List[str], bitmasks: Dict[str, List[int]]) -> bool: link_index = list(do_not_collision_map.keys()).index(link_name) link_subset = link_names[:link_index] for other_link in link_subset: if other_link in do_not_collision_map[link_name]: if collide(attempt, bitmasks[other_link]): return True return False for link_name in link_names: # Iterate only on all previous links of the link_names list link_index = list(do_not_collision_map.keys()).index(link_name) link_subset = link_names[:link_index] for other_name in link_subset: # If the two links don't have to collide, continue if other_name in do_not_collision_map[link_name]: assert not collide(collision_bitmasks[link_name], collision_bitmasks[other_name]) continue # If the links already collide, continue if collide(collision_bitmasks[link_name], collision_bitmasks[other_name]): continue other_idx = link_names.index(other_name) # 1. Try to modify the bitmask of the processed link if collision_bitmasks[link_name][other_idx] is None: attempt = collision_bitmasks[link_name].copy() attempt[other_idx] = 1 if not broken_constraint(link_name, attempt, link_names, collision_bitmasks): collision_bitmasks[link_name] = attempt # Update constraint for l in do_not_collision_map[link_name]: assert collision_bitmasks[l][other_idx] != 1 collision_bitmasks[l][other_idx] = 0 continue # Put a 0 so that this constraint is enfoced the next times else: collision_bitmasks[link_name][other_idx] = 0 # Update constraint for l in do_not_collision_map[link_name]: assert collision_bitmasks[l][other_idx] != 0 collision_bitmasks[l][other_idx] = 1 # 2. Try to modify the bitmask of the other link if collision_bitmasks[other_name][link_index] is None: attempt = collision_bitmasks[other_name].copy() attempt[link_index] = 1 if not broken_constraint(other_name, attempt, link_names, collision_bitmasks): collision_bitmasks[other_name] = attempt for l in do_not_collision_map[other_name]: assert collision_bitmasks[l][link_index] != 1 collision_bitmasks[l][link_index] = 0 continue # 3. Try to find the first None in the link bitmask found = False for idx, el in enumerate(collision_bitmasks[link_name]): if el is None: attempt = collision_bitmasks[link_name].copy() attempt[idx] = 1 # print("a", attempt) if collide(attempt, collision_bitmasks[other_name]) and not \ broken_constraint(link_name, attempt, link_names, collision_bitmasks): collision_bitmasks[link_name] = attempt found = True for l in do_not_collision_map[link_name]: assert collision_bitmasks[l][idx] != 1 collision_bitmasks[l][idx] = 0 break if found: continue # 4. Try to find the first None in the other bitmask for idx, el in enumerate(collision_bitmasks[other_name]): if el is None: attempt = collision_bitmasks[other_name].copy() attempt[idx] = 1 if collide(attempt, collision_bitmasks[link_name]) and not \ broken_constraint(other_name, attempt, link_names, collision_bitmasks): collision_bitmasks[other_name] = attempt found = True # update contraints for l in do_not_collision_map[other_name]: assert collision_bitmasks[l][idx] != 1 collision_bitmasks[l][idx] = 0 break if found: continue # 5. Try to find the first location where both link and other are None for idx in range(len(link_subset)): el1 = collision_bitmasks[link_name][idx] el2 = collision_bitmasks[other_name][idx] if el1 is None and el2 is None: attempt1 = collision_bitmasks[link_name].copy() attempt1[idx] = 1 attempt2 = collision_bitmasks[other_name].copy() attempt2[idx] = 1 if not broken_constraint(link_name, attempt1, link_names, collision_bitmasks) and \ not broken_constraint(other_name, attempt2, link_names, collision_bitmasks): collision_bitmasks[link_name] = attempt1 collision_bitmasks[other_name] = attempt2 print(idx) print(link_name, attempt1) print(other_name, attempt2) found = True for l in do_not_collision_map[link_name]: assert collision_bitmasks[l][idx] != 1 collision_bitmasks[l][idx] = 0 for l in do_not_collision_map[other_name]: print(l) assert collision_bitmasks[l][idx] != 1 collision_bitmasks[l][idx] = 0 break if found: continue raise RuntimeError("This is a pathological case not yet supported") # Check that colliding constraints are met for name in set(link_subset) - set(link_name) - set(do_not_collision_map[link_name]): assert collide(collision_bitmasks[link_name], collision_bitmasks[name]) # Check that non-colliding constraints are met for name in do_not_collision_map[link_name]: assert not collide(collision_bitmasks[link_name], collision_bitmasks[name]) # Convert bitmask to integer for link, bitmask in collision_bitmasks.items(): value = 0 for idx, el in enumerate(bitmask): el = 0 if el is None else el value += 2**idx * el print(f"{link}\t", value, bitmask) ``` Output: ``` ❯ python collision_bitmask.py head 6553 [1, 0, None, 1, 1, 0, 0, 1, 1, None, None, 1, 1, 0, 0] chest 24674 [0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1] base 5 [1, 0, 1, 0, 0, 0, 0, None, None, None, None, None, None, 0, 0] r_upper_leg 10 [0, 1, 0, 1, 0, 0, None, None, None, None, None, None, None, None, None] l_upper_leg 18 [0, 1, 0, 0, 1, None, 0, None, None, None, None, None, None, None, None] r_lower_leg 49 [1, 0, 0, 0, 1, 1, 0, 0, None, None, None, None, None, None, None] l_lower_leg 73 [1, 0, 0, 1, 0, 0, 1, None, 0, None, None, None, None, None, None] r_foot 198 [0, 1, 1, None, 0, 0, 1, 1, None, None, None, None, None, None, None] l_foot 294 [0, 1, 1, 0, None, 1, 0, None, 1, None, None, None, None, None, None] r_upper_arm 921 [1, 0, 0, 1, 1, 0, 0, 1, 1, 1, 0, 0, None, 0, 0] l_upper_arm 1433 [1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 1, None, 0, 0, 0] r_lower_arm 3174 [0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 1, 1, 0, 0, None] l_lower_arm 4710 [0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 0, 0, 1, None, 0] r_hand 12697 [1, 0, 0, 1, 1, 0, 0, 1, 1, None, 0, 0, 1, 1, None] l_hand 18841 [1, 0, 0, 1, 1, 0, 0, 1, 1, 0, None, 1, 0, None, 1] ```