Merge pull request #905 from StanfordVL/eric-release-fixes
Eric's release fixes
This commit is contained in:
Chengshu Li
GitHub
commit
8659a0d1eb
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ class KinematicsMixin(BaseObjectState):
|
|||
info[self.obj] = {"q": self.obj.states[Joint].get_value(), "p": self.obj.states[Pose].get_value()}
|
||||
for arg in get_value_args:
|
||||
if isinstance(arg, StatefulObject):
|
||||
info[arg] = {"q": self.obj.states[Joint].get_value(), "p": arg.states[Pose].get_value()}
|
||||
info[arg] = {"q": arg.states[Joint].get_value(), "p": arg.states[Pose].get_value()}
|
||||
|
||||
return info
|
||||
|
||||
|
|
|
|||
|
|
@ -4,9 +4,8 @@ from abc import ABC, abstractmethod
|
|||
import omnigibson as og
|
||||
import omnigibson.lazy as lazy
|
||||
from omnigibson.utils.python_utils import Recreatable, Serializable
|
||||
from omnigibson.utils.sim_utils import check_deletable_prim
|
||||
from omnigibson.utils.ui_utils import create_module_logger
|
||||
from omnigibson.utils.usd_utils import scene_relative_prim_path_to_absolute
|
||||
from omnigibson.utils.usd_utils import delete_or_deactivate_prim, scene_relative_prim_path_to_absolute
|
||||
|
||||
# Create module logger
|
||||
log = create_module_logger(module_name=__name__)
|
||||
|
|
@ -133,9 +132,9 @@ class BasePrim(Serializable, Recreatable, ABC):
|
|||
if not self._loaded:
|
||||
raise ValueError("Cannot remove a prim that was never loaded.")
|
||||
|
||||
# Remove prim if it can be deleted
|
||||
if check_deletable_prim(self.prim_path):
|
||||
lazy.omni.isaac.core.utils.prims.delete_prim(self.prim_path)
|
||||
# Remove or deactivate prim if it's possible
|
||||
if not delete_or_deactivate_prim(self.prim_path):
|
||||
log.warning(f"Prim {self.name} at prim_path {self.prim_path} could not be deleted or deactivated.")
|
||||
|
||||
def _load(self):
|
||||
"""
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -55,34 +55,6 @@ def set_carb_setting(carb_settings, setting, value):
|
|||
raise TypeError(f"Value of type {type(value)} is not supported.")
|
||||
|
||||
|
||||
def check_deletable_prim(prim_path):
|
||||
"""
|
||||
Checks whether the prim defined at @prim_path can be deleted.
|
||||
|
||||
Args:
|
||||
prim_path (str): Path defining which prim should be checked for deletion
|
||||
|
||||
Returns:
|
||||
bool: Whether the prim can be deleted or not
|
||||
"""
|
||||
if not lazy.omni.isaac.core.utils.prims.is_prim_path_valid(prim_path):
|
||||
return False
|
||||
if lazy.omni.isaac.core.utils.prims.is_prim_no_delete(prim_path):
|
||||
return False
|
||||
if lazy.omni.isaac.core.utils.prims.is_prim_ancestral(prim_path):
|
||||
return False
|
||||
if lazy.omni.isaac.core.utils.prims.get_prim_type_name(prim_path=prim_path) == "PhysicsScene":
|
||||
return False
|
||||
if prim_path == "/World":
|
||||
return False
|
||||
if prim_path == "/":
|
||||
return False
|
||||
# Don't remove any /Render prims as that can cause crashes
|
||||
if prim_path.startswith("/Render"):
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def prims_to_rigid_prim_set(inp_prims):
|
||||
"""
|
||||
Converts prims @inp_prims into its corresponding set of rigid prims
|
||||
|
|
|
|||
|
|
@ -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 []
|
||||
|
||||
|
|
@ -1743,3 +1752,39 @@ def deep_copy_prim(source_root_prim, dest_stage, dest_root_path):
|
|||
for child in source_prim.GetAllChildren():
|
||||
new_dest_path = dest_path + "/" + child.GetName()
|
||||
queue.append((child, new_dest_path))
|
||||
|
||||
|
||||
def delete_or_deactivate_prim(prim_path):
|
||||
"""
|
||||
Attept to delete or deactivate the prim defined at @prim_path.
|
||||
|
||||
Args:
|
||||
prim_path (str): Path defining which prim should be deleted or deactivated
|
||||
|
||||
Returns:
|
||||
bool: Whether the operation was successful or not
|
||||
"""
|
||||
if not lazy.omni.isaac.core.utils.prims.is_prim_path_valid(prim_path):
|
||||
return False
|
||||
if lazy.omni.isaac.core.utils.prims.is_prim_no_delete(prim_path):
|
||||
return False
|
||||
if lazy.omni.isaac.core.utils.prims.get_prim_type_name(prim_path=prim_path) == "PhysicsScene":
|
||||
return False
|
||||
if prim_path == "/World":
|
||||
return False
|
||||
if prim_path == "/":
|
||||
return False
|
||||
# Don't remove any /Render prims as that can cause crashes
|
||||
if prim_path.startswith("/Render"):
|
||||
return False
|
||||
|
||||
# If the prim is not ancestral, we can delete it.
|
||||
if not lazy.omni.isaac.core.utils.prims.is_prim_ancestral(prim_path):
|
||||
lazy.omni.usd.commands.DeletePrimsCommand([prim_path], destructive=True).do()
|
||||
|
||||
# Otherwise, we can only deactivate it, which essentially serves the same purpose.
|
||||
# All objects that are originally in the scene are ancestral because we add the pre-build scene to the stage.
|
||||
else:
|
||||
lazy.omni.usd.commands.DeletePrimsCommand([prim_path], destructive=False).do()
|
||||
|
||||
return True
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue