-
Notifications
You must be signed in to change notification settings - Fork 8
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
Conversation
There was a problem hiding this 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.
Unfortunately the iCub model, being autogenerated from the CAD file, has a peculiar kinematic tree that breaks the rule I reported above:
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 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.pyfrom 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:
|
a2a8e5c
to
5b8d45d
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
lgtm
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):and it seems working fine (requires robotology-legacy/gym-ignition#212). However, contact detection is not optimal. Check in the gif below:
This is a problem with DART's contact detection algorithm, we cannot do much here. I cc also @chapulina and @azeey for reference.