Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improve primitive shapes in iCubGazeboSimpleCollisionsV2_5 #20

Merged
merged 2 commits into from
Aug 17, 2020

Conversation

diegoferigo
Copy link
Member

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

Images in Gazebo Classic

front
side
detail

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 robotology-legacy/gym-ignition#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.

out

Copy link
Contributor

@paolo-viceconte paolo-viceconte left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Much better shapes @diegoferigo! As regards noise in contact detection by DART, let's consider to gain robustness through an additional filtering layer. Checking contacts over a window of a few consecutive time instants could help, for instance, when using self-collisions for early termination.

@diegoferigo
Copy link
Member Author

diegoferigo commented Jun 22, 2020

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 gazebosim/gz-sim#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
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]

@diegoferigo diegoferigo force-pushed the feature/pimp-simple-collisions branch from a2a8e5c to 5b8d45d Compare June 22, 2020 14:32
Copy link
Contributor

@raffaello-camoriano raffaello-camoriano left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

lgtm

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants