p04798526
/
OmniGibson
mirror of https://github.com/StanfordVL/OmniGibson.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

temporarily fix attachment (demo and test pass), and fix bugs in RigidContactAPI)

This commit is contained in:
Chengshu Li 2024-09-25 17:22:45 -07:00
parent c4383a8aee
commit 458d32a0c2
6 changed files with 66 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
omnigibson/examples/object_states/attachment_demo.py
View File

@ -37,7 +37,6 @@ def main(random_selection=False, headless=False, short_exec=False):
category="shelf_back_panel",
model="gjsnrt",
position=[0, 0, 0.01],
fixed_base=True,
abilities={"attachable": {}},
)
)
@ -99,7 +98,7 @@ def main(random_selection=False, headless=False, short_exec=False):
name=f"shelf_baseboard",
category="shelf_baseboard",
model="hlhneo",
position=[0, -0.97884506, base_z + delta_z * idx],
position=[0, -10.97884506, base_z + delta_z * idx],
abilities={"attachable": {}},
)
)
@ -119,13 +118,10 @@ def main(random_selection=False, headless=False, short_exec=False):
env.step([])
shelf_baseboard = env.scene.object_registry("name", "shelf_baseboard")
shelf_baseboard.set_position_orientation(position=[0, -0.979, 0.26], orientation=[0, 0, 0, 1])
shelf_baseboard.set_position_orientation(position=[0, -0.979, 0.21], orientation=[0, 0, 0, 1])
shelf_baseboard.keep_still()
shelf_baseboard.set_linear_velocity(th.tensor([-0.2, 0, 0]))
shelf_side_left = env.scene.object_registry("name", "shelf_side_left")
shelf_side_left.set_position_orientation(position=[-0.4, 0.0, 0.2], orientation=[0, 0, 0, 1])
shelf_side_left.keep_still()
# Lower the mass of the baseboard - otherwise, the gravity will create enough torque to break the joint
shelf_baseboard.root_link.mass = 0.1
input(
"\n\nShelf parts fall to their correct poses and get automatically attached to the back panel.\n"

3
omnigibson/object_states/attached_to.py
View File

@ -316,7 +316,7 @@ class AttachedTo(
_, parent_local_quat = T.relative_pose_transform([0, 0, 0], child_quat, [0, 0, 0], parent_quat)
# Disable collision between the parent and child objects
self._disable_collision_between_child_and_parent(child=self.obj, parent=other)
# self._disable_collision_between_child_and_parent(child=self.obj, parent=other)
# Set the parent references
self.parent = other
@ -341,6 +341,7 @@ class AttachedTo(
joint_frame_in_parent_frame_quat=parent_local_quat,
joint_frame_in_child_frame_pos=th.zeros(3),
joint_frame_in_child_frame_quat=th.tensor([0.0, 0.0, 0.0, 1.0]),
exclude_from_articulation=True,
**kwargs,
)

53
omnigibson/robots/manipulation_robot.py
View File

@ -273,32 +273,38 @@ class ManipulationRobot(BaseRobot):
set of unique robot link prim_paths that it is in contact with
"""
arm = self.default_arm if arm == "default" else arm
# Get robot contact links
# Get robot finger links
finger_paths = set([link.prim_path for link in self.finger_links[arm]])
# Get robot links
link_paths = set(self.link_prim_paths)
if not return_contact_positions:
raw_contact_data = {
(row, col)
for row, col in GripperRigidContactAPI.get_contact_pairs(self.scene.idx, column_prim_paths=link_paths)
for row, col in GripperRigidContactAPI.get_contact_pairs(self.scene.idx, column_prim_paths=finger_paths)
if row not in link_paths
}
else:
raw_contact_data = {
(row, col, point)
for row, col, force, point, normal, sep in GripperRigidContactAPI.get_contact_data(
self.scene.idx, column_prim_paths=finger_paths
)
if row not in link_paths
}
# Translate that to robot contact data
robot_contact_links = {}
for con_data in raw_contact_data:
# Translate to robot contact data
robot_contact_links = dict()
contact_data = set()
for con_data in raw_contact_data:
if not return_contact_positions:
other_contact, link_contact = con_data
if other_contact not in robot_contact_links:
robot_contact_links[other_contact] = set()
robot_contact_links[other_contact].add(link_contact)
return {other for other, _ in raw_contact_data}, robot_contact_links
# Otherwise, we rely on the simpler, but more costly, get_contact_data API.
contacts = GripperRigidContactAPI.get_contact_data(self.scene.idx, column_prim_paths=link_paths)
contact_data = {(contact[0], contact[3]) for contact in contacts}
robot_contact_links = {}
for con_data in contacts:
other_contact, link_contact = con_data[:2]
contact_data.add(other_contact)
else:
other_contact, link_contact, point = con_data
contact_data.add((other_contact, point))
if other_contact not in robot_contact_links:
robot_contact_links[other_contact] = set()
robot_contact_links[other_contact].add(link_contact)
@ -867,7 +873,7 @@ class ManipulationRobot(BaseRobot):
if candidate_obj is None or link_name not in candidate_obj.links:
continue
candidate_link = candidate_obj.links[link_name]
dist = th.norm(th.tensor(candidate_link.get_position_orientation()[0]) - th.tensor(gripper_center_pos))
dist = th.norm(candidate_link.get_position_orientation()[0] - gripper_center_pos)
candidate_data.append((prim_path, dist))
if not candidate_data:
@ -1240,9 +1246,14 @@ class ManipulationRobot(BaseRobot):
force_data, _ = self._find_gripper_contacts(arm=arm, return_contact_positions=True)
for c_link_prim_path, c_contact_pos in force_data:
if c_link_prim_path == ag_link.prim_path:
contact_pos = th.tensor(c_contact_pos)
contact_pos = c_contact_pos
break
assert contact_pos is not None
assert contact_pos is not None, (
f"contact_pos in self._find_gripper_contacts(return_contact_positions=True) is not found in "
f"self._find_gripper_contacts(return_contact_positions=False). This is likely because "
f"GripperRigidContactAPI.get_contact_pairs and get_contact_data return inconsistent results."
)
# Joint frame set at the contact point
# Need to find distance between robot and contact point in robot link's local frame and
@ -1266,6 +1277,7 @@ class ManipulationRobot(BaseRobot):
body0=self.eef_links[arm].prim_path,
body1=ag_link.prim_path,
enabled=True,
exclude_from_articulation=True,
joint_frame_in_parent_frame_pos=parent_frame_pos / self.scale,
joint_frame_in_parent_frame_quat=parent_frame_orn,
joint_frame_in_child_frame_pos=child_frame_pos / ag_obj.scale,
@ -1448,6 +1460,7 @@ class ManipulationRobot(BaseRobot):
body0=ag_link.prim_path,
body1=None,
enabled=False,
exclude_from_articulation=True,
joint_frame_in_child_frame_pos=attachment_point_pos,
)

6
omnigibson/utils/physx_utils.py
View File

@ -262,10 +262,16 @@ def create_physx_particleset_pointinstancer(
def apply_force_at_pos(prim, force, pos):
if isinstance(force, th.Tensor):
force = force.cpu().numpy()
if isinstance(pos, th.Tensor):
pos = pos.cpu().numpy()
prim_id = lazy.pxr.PhysicsSchemaTools.sdfPathToInt(prim.prim_path)
og.sim.psi.apply_force_at_pos(og.sim.stage_id, prim_id, force, pos)
def apply_torque(prim, foward_vect, roll_torque_scalar):
if isinstance(foward_vect, th.Tensor):
foward_vect = foward_vect.cpu().numpy()
prim_id = lazy.pxr.PhysicsSchemaTools.sdfPathToInt(prim.prim_path)
og.sim.psi.apply_torque(og.sim.stage_id, prim_id, foward_vect * roll_torque_scalar)

13
omnigibson/utils/usd_utils.py
View File

@ -89,6 +89,7 @@ def create_joint(
body0=None,
body1=None,
enabled=True,
exclude_from_articulation=False,
joint_frame_in_parent_frame_pos=None,
joint_frame_in_parent_frame_quat=None,
joint_frame_in_child_frame_pos=None,
@ -107,6 +108,7 @@ def create_joint(
body0 (str or None): absolute path to the first body's prim. At least @body0 or @body1 must be specified.
body1 (str or None): absolute path to the second body's prim. At least @body0 or @body1 must be specified.
enabled (bool): whether to enable this joint or not.
exclude_from_articulation (bool): whether to exclude this joint from the articulation or not.
joint_frame_in_parent_frame_pos (th.tensor or None): relative position of the joint frame to the parent frame (body0).
joint_frame_in_parent_frame_quat (th.tensor or None): relative orientation of the joint frame to the parent frame (body0).
joint_frame_in_child_frame_pos (th.tensor or None): relative position of the joint frame to the child frame (body1).
@ -168,6 +170,9 @@ def create_joint(
# Possibly (un-/)enable this joint
joint_prim.GetAttribute("physics:jointEnabled").Set(enabled)
# Possibly exclude this joint from the articulation
joint_prim.GetAttribute("physics:excludeFromArticulation").Set(exclude_from_articulation)
# We update the simulation now without stepping physics if sim is playing so we can bypass the snapping warning from PhysicsUSD
if og.sim.is_playing():
with suppress_omni_log(channels=["omni.physx.plugin"]):
@ -389,12 +394,14 @@ class RigidContactAPIImpl:
# Get all of the (row, col) pairs where the impulse is greater than 0
return {
(interesting_row_paths[row], interesting_col_paths[col])
for row, col in th.nonzero(interesting_impulses > 0, as_tuple=True)
for row, col in zip(*th.nonzero(interesting_impulses > 0, as_tuple=True))
}
def get_contact_data(self, scene_idx, row_prim_paths=None, column_prim_paths=None):
# First check if the object has any contacts
impulses = self.get_all_impulses(scene_idx)
impulses = th.norm(self.get_all_impulses(scene_idx), dim=-1)
assert impulses.ndim == 2, f"Impulse matrix should be 2D, found shape {impulses.shape}"
row_idx = (
list(range(impulses.shape[0]))
if row_prim_paths is None
@ -406,6 +413,8 @@ class RigidContactAPIImpl:
else [self.get_body_col_idx(path)[1] for path in column_prim_paths]
)
relevant_impulses = impulses[row_idx][:, col_idx]
# Early return if not in contact.
if not th.any(relevant_impulses > 0):
return []

11
tests/test_object_states.py
View File

@ -716,19 +716,24 @@ def test_toggled_on(env):
assert not stove.states[ToggledOn].get_value()
@pytest.mark.skip(reason="skipping attachment for now")
@og_test
def test_attached_to(env):
shelf_back_panel = env.scene.object_registry("name", "shelf_back_panel")
shelf_shelf = env.scene.object_registry("name", "shelf_shelf")
shelf_baseboard = env.scene.object_registry("name", "shelf_baseboard")
# Lower the mass of the shelf - otherwise, the gravity will create enough torque to break the joint
shelf_shelf.root_link.mass = 0.1
shelf_back_panel.set_position_orientation(position=[0, 0, 0.01], orientation=[0, 0, 0, 1])
shelf_back_panel.keep_still()
shelf_shelf.set_position_orientation(position=[0, 0.03, 0.17], orientation=[0, 0, 0, 1])
shelf_shelf.keep_still()
og.sim.step()
# The shelf should not be attached to the back panel (no contact yet)
assert not shelf_shelf.states[Touching].get_value(shelf_back_panel)
assert not shelf_shelf.states[AttachedTo].get_value(shelf_back_panel)
# Let the shelf fall
@ -736,16 +741,20 @@ def test_attached_to(env):
og.sim.step()
# The shelf should be attached to the back panel
assert shelf_shelf.states[Touching].get_value(shelf_back_panel)
assert shelf_shelf.states[AttachedTo].get_value(shelf_back_panel)
# Try to attach again (should be no-op)
assert shelf_shelf.states[AttachedTo].set_value(shelf_back_panel, True)
# The shelf should still be attached to the back panel
assert shelf_shelf.states[AttachedTo].get_value(shelf_back_panel)
# Detach
assert shelf_shelf.states[AttachedTo].set_value(shelf_back_panel, False)
# The shelf should not be attached to the back panel
assert not shelf_shelf.states[AttachedTo].get_value(shelf_back_panel)
# Attach again
assert shelf_shelf.states[AttachedTo].set_value(shelf_back_panel, True)
# shelf should be attached to the back panel
assert shelf_shelf.states[AttachedTo].get_value(shelf_back_panel)