diff --git a/examples/hitl/isaacsim_viewer/isaacsim_viewer.py b/examples/hitl/isaacsim_viewer/isaacsim_viewer.py index 24fccbc840..f034ff2d00 100644 --- a/examples/hitl/isaacsim_viewer/isaacsim_viewer.py +++ b/examples/hitl/isaacsim_viewer/isaacsim_viewer.py @@ -86,7 +86,7 @@ def reset(self): self._spot_wrapper._target_arm_joint_positions = [0.0, -2.36, 0.0, 2.25, 0.0, 1.67, 0.0, 0.0] - pos_usd = isaac_prim_utils.habitat_to_usd_position([random.uniform(-2.0, -4.0), 0.8, -4.8]) + pos_usd = isaac_prim_utils.habitat_to_usd_position([-4.5, 0.1, -3.5]) self._spot_wrapper._robot.set_world_pose(pos_usd, [1.0, 0.0, 0.0, 0.0]) @@ -479,13 +479,15 @@ def __init__(self, app_service: AppService): isaac_world.set_simulation_dt(physics_dt = self._isaac_physics_dt, rendering_dt = self._isaac_physics_dt) # asset_path = "/home/eric/projects/habitat-lab/data/usd/scenes/102817140.usda" - asset_path = "/home/eric/projects/habitat-lab/data/usd/scenes/102344193.usda" + asset_path = "/home/joanne/habitat-lab/data/usd/scenes/fremont_static_objects.usda" from omni.isaac.core.utils.stage import add_reference_to_stage add_reference_to_stage(usd_path=asset_path, prim_path="/World/test_scene") self._usd_visualizer.on_add_reference_to_stage(usd_path=asset_path, prim_path="/World/test_scene") from habitat.isaac_sim._internal.spot_robot_wrapper import SpotRobotWrapper - self._spot_wrapper = SpotRobotWrapper(self._isaac_wrapper.service) + from habitat.isaac_sim._internal.murp_robot_wrapper import MurpRobotWrapper + + self._spot_wrapper = MurpRobotWrapper(self._isaac_wrapper.service) from habitat.isaac_sim._internal.metahand_robot_wrapper import MetahandRobotWrapper self._metahand_wrapper = MetahandRobotWrapper(self._isaac_wrapper.service) diff --git a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py new file mode 100644 index 0000000000..34f83748c1 --- /dev/null +++ b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py @@ -0,0 +1,171 @@ +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +from typing import Dict, List, Optional, Set + +import attr +import magnum as mn +import numpy as np + +from habitat.articulated_agents.mobile_manipulator import ( + ArticulatedAgentCameraParams, + MobileManipulator, +) + + +@attr.s(auto_attribs=True, slots=True) +class MurpParams: + """Data to configure a mobile manipulator. + + :property arm_joints: The joint ids of the arm joints. + :property gripper_joints: The habitat sim joint ids of any grippers. + :property arm_init_params: The starting joint angles of the arm. If None, + resets to 0. + :property gripper_init_params: The starting joint positions of the gripper. If None, + resets to 0. + :property ee_offset: The 3D offset from the end-effector link to the true + end-effector position. + :property ee_link: The Habitat Sim link ID of the end-effector. + :property ee_constraint: A (2, 3) shaped array specifying the upper and + lower limits for the 3D end-effector position. + :property cameras: The cameras and where they should go. The key is the + prefix to match in the sensor names. For example, a key of `"head"` + will match sensors `"head_rgb"` and `"head_depth"` + :property gripper_closed_state: All gripper joints must achieve this + state for the gripper to be considered closed. + :property gripper_open_state: All gripper joints must achieve this + state for the gripper to be considered open. + :property gripper_state_eps: Error margin for detecting whether gripper is closed. + :property arm_mtr_pos_gain: The position gain of the arm motor. + :property arm_mtr_vel_gain: The velocity gain of the arm motor. + :property arm_mtr_max_impulse: The maximum impulse of the arm motor. + :property base_offset: The offset of the root transform from the center ground point for navmesh kinematic control. + :property base_link_names: The name of the links + :property ee_count: how many end effectors + """ + + arm_joints: List[int] + gripper_joints: List[int] + + arm_init_params: Optional[List[float]] + gripper_init_params: Optional[List[float]] + + ee_offset: List[mn.Vector3] + ee_links: List[int] + ee_constraint: np.ndarray + + cameras: Dict[str, ArticulatedAgentCameraParams] + + gripper_closed_state: List[float] + gripper_open_state: List[float] + gripper_state_eps: float + + arm_mtr_pos_gain: float + arm_mtr_vel_gain: float + arm_mtr_max_impulse: float + + base_offset: mn.Vector3 + base_link_names: Set[str] + + ik_pb_link_idx: int + ik_arm_len: int + ik_arm_start_idx: int + + ee_count: Optional[int] = 1 + + +class MurpRobot(MobileManipulator): + @classmethod + def _get_murp_params(cls): + return MurpParams( + arm_joints=[0, 2, 4, 6, 8, 10, 12], + gripper_joints=[19], + arm_init_params=[ + 2.6116285, + 1.5283098, + 1.0930868, + -0.50559217, + 0.48147443, + 2.628784, + -1.3962275, + ], + gripper_init_params=[-1.56], + ee_offset=[mn.Vector3(0.08, 0, 0)], + ee_links=[7], + ee_constraint=np.array([[[0.4, 1.2], [-0.7, 0.7], [0.25, 1.5]]]), + cameras={ + "articulated_agent_arm_depth": ArticulatedAgentCameraParams( + cam_offset_pos=mn.Vector3(0.166, 0.0, 0.018), + cam_orientation=mn.Vector3(0.0, -1.571, 0.0), + attached_link_id=36, + relative_transform=mn.Matrix4.rotation_z(mn.Deg(-90)), + ), + "articulated_agent_arm_rgb": ArticulatedAgentCameraParams( + cam_offset_pos=mn.Vector3(0.166, 0.023, 0.03), + cam_orientation=mn.Vector3(0, -1.571, 0.0), + attached_link_id=36, + relative_transform=mn.Matrix4.rotation_z(mn.Deg(-90)), + ), + "articulated_agent_arm_panoptic": ArticulatedAgentCameraParams( + cam_offset_pos=mn.Vector3(0.166, 0.0, 0.018), + cam_orientation=mn.Vector3(0, -1.571, 0.0), + attached_link_id=6, + relative_transform=mn.Matrix4.rotation_z(mn.Deg(-90)), + ), + "head_stereo_right": ArticulatedAgentCameraParams( + cam_offset_pos=mn.Vector3( + 0.4164822634134684, 0.0, 0.03614789234067159 + ), + cam_orientation=mn.Vector3( + 0.0290787, -0.940569, -0.38998877 + ), + attached_link_id=-1, + ), + "head_stereo_left": ArticulatedAgentCameraParams( + cam_offset_pos=mn.Vector3( + 0.4164822634134684, 0.0, -0.03740343144695029 + ), + cam_orientation=mn.Vector3( + -3.1125141, -0.940569, 2.751605 + ), + attached_link_id=-1, + ), + "third": ArticulatedAgentCameraParams( + cam_offset_pos=mn.Vector3(0.5, 1.9, 0.0), + cam_look_at_pos=mn.Vector3(1, 0.0, -0.75), + attached_link_id=-1, + ), + }, + gripper_closed_state=[0.0], + gripper_open_state=[-1.56], + gripper_state_eps=0.01, + arm_mtr_pos_gain=0.3, + arm_mtr_vel_gain=0.3, + arm_mtr_max_impulse=10.0, + base_offset=mn.Vector3(0.0, -0.48, 0.0), + base_link_names={"base_link", "spine"}, + # for left arm + ik_pb_link_idx=9, # link to use for IK + ik_arm_len=7, # num links in arm URDF for IK + ik_arm_start_idx=3, # starting link for arm in URDF for IK + ) + + @property + def base_transformation(self): + add_rot = mn.Matrix4.rotation( + mn.Rad(-np.pi / 2), mn.Vector3(1.0, 0, 0) + ) + return self.sim_obj.transformation # @ add_rot + + def __init__( + self, agent_cfg, sim, limit_robo_joints=True, fixed_base=True + ): + super().__init__( + self._get_murp_params(), + agent_cfg, + sim, + limit_robo_joints, + fixed_base, + base_type="mobile", + ) \ No newline at end of file diff --git a/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py new file mode 100644 index 0000000000..d7e156724a --- /dev/null +++ b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py @@ -0,0 +1,606 @@ +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +import magnum as mn +import numpy as np +import omni +import omni.physx.scripts.utils as physxUtils + +# todo: add guard to ensure SimulatorApp is created, or give nice error message, so we don't get weird import errors here +from omni.isaac.core import World +from omni.isaac.core.objects import DynamicCuboid +from omni.isaac.core.prims.rigid_prim import RigidPrim +from omni.isaac.core.prims.rigid_prim_view import RigidPrimView +from omni.isaac.core.robots import Robot +from omni.isaac.core.utils.stage import add_reference_to_stage +from omni.isaac.core.utils.types import ArticulationAction +from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics +from scipy.spatial.transform import Rotation as R + +from habitat.isaac_sim import isaac_prim_utils + + +class MurpRobotWrapper: + """Isaac-internal wrapper for a robot. + + The goal with this wrapper is convenience but not encapsulation. See also (public) IsaacMobileManipulator, which has the goal of exposing a minimal public interface to the rest of Habitat-lab. + """ + + def __init__(self, isaac_service, instance_id=0): + + self._isaac_service = isaac_service + asset_path = "./data/usd/robots/franka_with_hand_2.usda" #Lambda Machine Change + robot_prim_path = f"/World/env_{instance_id}/Murp" + self._robot_prim_path = robot_prim_path + + add_reference_to_stage(usd_path=asset_path, prim_path=robot_prim_path) + self._isaac_service.usd_visualizer.on_add_reference_to_stage( + usd_path=asset_path, prim_path=robot_prim_path + ) + + robot_prim = self._isaac_service.world.stage.GetPrimAtPath( + robot_prim_path + ) + test_scene_root_xform = UsdGeom.Xform(robot_prim) + # test_scene_root_xform.AddRotateZOp().Set(-90) + + if not robot_prim.IsValid(): + raise ValueError(f"Prim at {robot_prim_path} is not valid.") + + # Traverse only the robot's prim hierarchy + for prim in Usd.PrimRange(robot_prim): + + if prim.HasAPI(PhysxSchema.PhysxJointAPI): + joint_api = PhysxSchema.PhysxJointAPI(prim) + joint_api.GetMaxJointVelocityAttr().Set(200.0) + + if prim.HasAPI(UsdPhysics.DriveAPI): + # Access the existing DriveAPI + drive_api = UsdPhysics.DriveAPI(prim, "angular") + if drive_api: + + # Modify drive parameters + drive_api.GetStiffnessAttr().Set(10.0) # Position gain + drive_api.GetDampingAttr().Set(0.1) # Velocity gain + drive_api.GetMaxForceAttr().Set(50) # Maximum force/torque + + drive_api = UsdPhysics.DriveAPI(prim, "linear") + if drive_api: + + drive_api = UsdPhysics.DriveAPI.Get(prim, "linear") + drive_api.GetStiffnessAttr().Set( + 1000 + ) # Example for linear stiffness + + if prim.HasAPI(UsdPhysics.RigidBodyAPI): + + # UsdPhysics.RigidBodyAPI doesn't support damping but PhysxRigidBodyAPI does + if prim.HasAPI(PhysxSchema.PhysxRigidBodyAPI): + physx_api = PhysxSchema.PhysxRigidBodyAPI(prim) + else: + physx_api = PhysxSchema.PhysxRigidBodyAPI.Apply(prim) + + # todo: decide hard-coded values here + physx_api.CreateLinearDampingAttr(50.0) + physx_api.CreateAngularDampingAttr(10.0) + + # todo: investigate if this is needed for kinematic base + # todo: resolve off-by-100 scale issue + # self.scale_prim_mass_and_inertia(f"{robot_prim_path}/base", 100.0) + + self._name = f"murp_{instance_id}" + self._robot = self._isaac_service.world.scene.add( + Robot(prim_path=robot_prim_path, name=self._name) + ) + self._robot_controller = self._robot.get_articulation_controller() + + self._step_count = 0 + self._lateral_vel = [0.0, 0.0] + self._instance_id = instance_id + + self.arm_target_joint_pos = None + + # beware this poses the object + self._xform_prim_view = self._create_xform_prim_view() + + @property + def robot(self) -> Robot: + """Get Isaac Sim Robot""" + return self._robot + + def get_prim_path(self): + return self._robot_prim_path + + def set_root_pose(self, pos, rot, convention="hab"): + + rot = [rot.scalar] + list(rot.vector) + if convention == "hab": + pos = isaac_prim_utils.habitat_to_usd_position(pos) + rot = isaac_prim_utils.habitat_to_usd_rotation(rot) + self._robot.set_world_pose(pos, rot) + + def get_root_pose(self, convention="hab"): + + pos_usd, rot_usd = self._robot.get_world_pose() + if convention == "hab": + pos = mn.Vector3(isaac_prim_utils.usd_to_habitat_position(pos_usd)) + rot = isaac_prim_utils.rotation_wxyz_to_magnum_quat( + isaac_prim_utils.usd_to_habitat_rotation(rot_usd) + ) + else: + pos = mn.Vector3(pos_usd) + rot = isaac_prim_utils.rotation_wxyz_to_magnum_quat(rot_usd) + return pos, rot + + def get_link_world_poses(self, convention="hab"): + + positions = [] + positions_usd, rotations_usd = self._xform_prim_view.get_world_poses() + for pos in positions_usd: + if convention == "hab": + pos = isaac_prim_utils.usd_to_habitat_position(pos) + pos = mn.Vector3(pos) + positions.append(pos) + rotations = [] + for rot in rotations_usd: + if convention == "hab": + rot = isaac_prim_utils.usd_to_habitat_rotation(rot) + rot = isaac_prim_utils.rotation_wxyz_to_magnum_quat(rot) + rotations.append(rot) + + # perf todo: consider RobotView.get_body_coms instead + + return positions, rotations + + def _create_xform_prim_view(self): + + root_prim_path = self._robot_prim_path + root_prim = self._isaac_service.world.stage.GetPrimAtPath( + root_prim_path + ) + + prim_paths = [] + + # lazy import + from pxr import Usd, UsdPhysics + + prim_range = Usd.PrimRange(root_prim) + it = iter(prim_range) + for prim in it: + prim_path = str(prim.GetPath()) + + if not prim.HasAPI(UsdPhysics.RigidBodyAPI): + continue + + # we found a rigid body, so let's ignore children + it.PruneChildren() + prim_paths.append(prim_path) + + assert len(prim_paths) + print("prim_paths: ", prim_paths, len(prim_paths)) + + self._body_prim_paths = prim_paths + + from omni.isaac.core.prims.xform_prim_view import XFormPrimView + + return XFormPrimView(prim_paths) + + def reset_arm(self): + # todo: specify this in isaac_spot_robot.py + left_arm_joint_names = [ + "left_fr3_joint1", + "left_fr3_joint2", + "left_fr3_joint3", + "left_fr3_joint4", + "left_fr3_joint5", + "left_fr3_joint6", + "left_fr3_joint7", + ] + + right_arm_joint_names = [ + "right_fr3_joint1", + "right_fr3_joint2", + "right_fr3_joint3", + "right_fr3_joint4", + "right_fr3_joint5", + "right_fr3_joint6", + "right_fr3_joint7", + ] + + self.ee_link_name = left_arm_joint_names[-1].replace("joint", "link") + self.right_ee_link_name = right_arm_joint_names[-1].replace( + "joint", "link" + ) + + self.ee_link_id = self.get_link_id(self.ee_link_name) + self.right_ee_link_id = self.get_link_id(self.right_ee_link_name) + left_arm_joint_indices = [] + right_arm_joint_indices = [] + dof_names = self._robot.dof_names + print("dof names: ", dof_names) + assert len(dof_names) > 0 + for arm_joint_name in left_arm_joint_names: + left_arm_joint_indices.append(dof_names.index(arm_joint_name)) + + for arm_joint_name in right_arm_joint_names: + right_arm_joint_indices.append(dof_names.index(arm_joint_name)) + + self._arm_joint_indices = np.array(left_arm_joint_indices) + self._right_arm_joint_indices = np.array(right_arm_joint_indices) + rest_positions = [ + 2.6116285, + 1.5283098, + 1.0930868, + -0.50559217, + 0.48147443, + 2.628784, + -1.3962275, + ] + self._target_arm_joint_positions = rest_positions + self._target_right_arm_joint_positions = rest_positions + + def get_link_id(self, link_str): + return [ + i for i, s in enumerate(self._body_prim_paths) if link_str in s + ][0] + + def reset_hand(self): + # todo: specify this in isaac_spot_robot.py + # only list the revolute joints + right_hand_joint_names = [ + "joint_0_0", + "joint_1_0", + "joint_2_0", + "joint_3_0", + "joint_4_0", + "joint_5_0", + "joint_6_0", + "joint_7_0", + "joint_8_0", + "joint_9_0", + "joint_10_0", + "joint_11_0", + "joint_12_0", + "joint_13_0", + "joint_14_0", + "joint_15_0", + ] + left_hand_joint_names = [ + "joint_l_0_0", + "joint_l_1_0", + "joint_l_2_0", + "joint_l_3_0", + "joint_l_4_0", + "joint_l_5_0", + "joint_l_6_0", + "joint_l_7_0", + "joint_l_8_0", + "joint_l_9_0", + "joint_l_10_0", + "joint_l_11_0", + "joint_l_12_0", + "joint_l_13_0", + "joint_l_14_0", + "joint_l_15_0", + ] + + left_hand_joint_indices = [] + right_hand_joint_indices = [] + dof_names = self._robot.dof_names + assert len(dof_names) > 0 + for hand_joint_name in left_hand_joint_names: + left_hand_joint_indices.append(dof_names.index(hand_joint_name)) + + for hand_joint_name in right_hand_joint_names: + right_hand_joint_indices.append(dof_names.index(hand_joint_name)) + + self._hand_joint_indices = np.array(left_hand_joint_indices) + self._right_hand_joint_indices = np.array(right_hand_joint_indices) + + n_hand_joints = len(left_hand_joint_names) + closed_positions = np.array([3.14159] * n_hand_joints) + open_positions = np.zeros(n_hand_joints) + self._target_hand_joint_positions = open_positions + self._target_right_hand_joint_positions = open_positions + + def post_reset(self): + # todo: just do a single callback + self._isaac_service.world.add_physics_callback( + f"{self._name}_physics_callback", callback_fn=self.physics_callback + ) + self.reset_arm() + self.reset_hand() + + def scale_prim_mass_and_inertia(self, path, scale): + + prim = self._isaac_service.world.stage.GetPrimAtPath(path) + assert prim.HasAPI(UsdPhysics.MassAPI) + mass_api = UsdPhysics.MassAPI(prim) + mass_api.GetMassAttr().Set(mass_api.GetMassAttr().Get() * scale) + mass_api.GetDiagonalInertiaAttr().Set( + mass_api.GetDiagonalInertiaAttr().Get() * scale + ) + + def fix_base_orientation_via_angular_vel( + self, step_size, base_position, base_orientation + ): + + curr_angular_velocity = self._robot.get_angular_velocity() + + # Constants + max_angular_velocity = 3.0 # Maximum angular velocity (rad/s) + + # wxyz to xyzw + base_orientation_xyzw = np.array( + [ + base_orientation[1], + base_orientation[2], + base_orientation[3], + base_orientation[0], + ] + ) + base_rotation = R.from_quat(base_orientation_xyzw) + + # Define the local "up" axis and transform it to world space + local_up = np.array([0, 0, 1]) # Object's local up axis (hack: -1?) + world_up = base_rotation.apply(local_up) # Local up in world space + + # Define the global up axis + global_up = np.array([0, 0, 1]) # Global up direction + + # Compute the axis of rotation to align world_up to global_up + rotation_axis = np.cross(world_up, global_up) + rotation_axis_norm = np.linalg.norm(rotation_axis) + + # Handle special cases where no rotation is needed + if rotation_axis_norm < 1e-6: # Already aligned + desired_angular_velocity = np.array( + [0, 0, 0] + ) # No correction needed + else: + # Normalize the rotation axis + rotation_axis /= rotation_axis_norm + + # Compute the angle of rotation using the dot product + rotation_angle = np.arccos( + np.clip(np.dot(world_up, global_up), -1.0, 1.0) + ) + + # Calculate the angular velocity to correct the tilt in one step + hack_scale = 1.0 + tilt_correction_velocity = ( + (rotation_axis * rotation_angle) / step_size + ) * hack_scale + + # Cap the angular velocity to the maximum allowed value + angular_velocity_magnitude = np.linalg.norm( + tilt_correction_velocity + ) + if angular_velocity_magnitude > max_angular_velocity: + tilt_correction_velocity *= ( + max_angular_velocity / angular_velocity_magnitude + ) + + desired_angular_velocity = tilt_correction_velocity + + # don't change rotation about z + desired_angular_velocity[2] = curr_angular_velocity[2] + + self._robot.set_angular_velocity(desired_angular_velocity) + + def fix_base_height_via_linear_vel_z( + self, step_size, base_position, base_orientation + ): + + curr_linear_velocity = self._robot.get_linear_velocity() + + z_target = 0.1 # todo: get from navmesh or assume ground_z==0 + max_linear_vel = 3.0 + + # Extract the vertical position and velocity + z_current = base_position[2] + + # Compute the position error + position_error = z_target - z_current + + desired_linear_vel_z = position_error / step_size + desired_linear_vel_z = max( + -max_linear_vel, min(max_linear_vel, desired_linear_vel_z) + ) + + self._robot.set_linear_velocity( + [ + curr_linear_velocity[0], + curr_linear_velocity[1], + desired_linear_vel_z, + ] + ) + + def drive_arm(self, step_size): + + if np.array(self._target_arm_joint_positions).any(): + assert len(self._target_arm_joint_positions) == len( + self._arm_joint_indices + ) + self._robot_controller.apply_action( + ArticulationAction( + joint_positions=self._target_arm_joint_positions, + joint_indices=self._arm_joint_indices, + ) + ) + + def drive_right_arm(self, step_size): + + if np.array(self._target_right_arm_joint_positions).any(): + assert len(self._target_right_arm_joint_positions) == len( + self._right_arm_joint_indices + ) + self._robot_controller.apply_action( + ArticulationAction( + joint_positions=self._target_right_arm_joint_positions, + joint_indices=self._right_arm_joint_indices, + ) + ) + + def drive_hand(self, step_size): + + if np.array(self._target_hand_joint_positions).any(): + assert len(self._target_hand_joint_positions) == len( + self._hand_joint_indices + ) + self._robot_controller.apply_action( + ArticulationAction( + joint_positions=self._target_hand_joint_positions, + joint_indices=self._hand_joint_indices, + ) + ) + + def drive_right_hand(self, step_size): + + if np.array(self._target_right_hand_joint_positions).any(): + assert len(self._target_right_hand_joint_positions) == len( + self._right_hand_joint_indices + ) + self._robot_controller.apply_action( + ArticulationAction( + joint_positions=self._target_right_hand_joint_positions, + joint_indices=self._right_hand_joint_indices, + ) + ) + + def fix_base(self, step_size, base_position, base_orientation): + + self.fix_base_height_via_linear_vel_z( + step_size, base_position, base_orientation + ) + self.fix_base_orientation_via_angular_vel( + step_size, base_position, base_orientation + ) + + def physics_callback(self, step_size): + base_position, base_orientation = self._robot.get_world_pose() + trans, rot = self.get_prim_transform( + "_urdf_kitchen_FREMONT_KITCHENSET_FREMONT_KITCHENSET_CLEANED_urdf/kitchenset_fridgedoor2" + ) + self.fix_base(step_size, base_position, base_orientation) + # self.drive_arm(step_size) + self.drive_right_arm(step_size) + eepose,ee_rot=self.ee_pose() + # print("EE POSE",eepose) + # print("EE ROT",ee_rot) + # self.drive_hand(step_size) + self.drive_right_hand(step_size) + self._step_count += 1 + + @property + def arm_joint_pos(self): + """Get the current arm joint positions.""" + robot_joint_positions = self._robot.get_joint_positions() + arm_joint_positions = np.array( + [robot_joint_positions[i] for i in self._arm_joint_indices], + dtype=np.float32, + ) + + return arm_joint_positions + + @property + def right_arm_joint_pos(self): + """Get the current arm joint positions.""" + robot_joint_positions = self._robot.get_joint_positions() + arm_joint_positions = np.array( + [robot_joint_positions[i] for i in self._right_arm_joint_indices], + dtype=np.float32, + ) + + return arm_joint_positions + + @property + def hand_joint_pos(self): + """Get the current arm joint positions.""" + robot_joint_positions = self._robot.get_joint_positions() + hand_joint_positions = np.array( + [robot_joint_positions[i] for i in self._hand_joint_indices], + dtype=np.float32, + ) + + return hand_joint_positions + + @property + def right_hand_joint_pos(self): + """Get the current arm joint positions.""" + robot_joint_positions = self._robot.get_joint_positions() + hand_joint_positions = np.array( + [robot_joint_positions[i] for i in self._right_hand_joint_indices], + dtype=np.float32, + ) + + return hand_joint_positions + + def ee_pose(self, convention="hab"): + """Get the current ee position and rotation.""" + link_poses = self.get_link_world_poses(convention=convention) + + ee_pos = link_poses[0][self.right_ee_link_id] + ee_rot = link_poses[1][self.right_ee_link_id] + + return ee_pos, ee_rot + + def get_prim_transform(self, asset_path=None): + """Get Scene prim's position and rotation.""" + if asset_path is None: + asset_path = os.path.abspath( + "data/usd/scenes/fremont_static_objects.usda" + ) + prim_path = f"/World/test_scene/{asset_path}" + prim = self._isaac_service.world.stage.GetPrimAtPath(prim_path) + matrix: Gf.Matrix4d = omni.usd.get_world_transform_matrix(prim) + translate: Gf.Vec3d = matrix.ExtractTranslation() + rotation: Gf.Rotation = matrix.ExtractRotation() + quat_rotation: Gf.Quatd = matrix.ExtractRotationQuat() + euler_rotation = rotation.GetAngle() + # # translate=isaac_prim_utils.usd_to_habitat_position(list(translate)) + # # Convert quaternion to NumPy array (Isaac uses wxyz format) + # isaac_quat = np.array([quat_rotation.GetReal(), *quat_rotation.GetImaginary()]) + + # # Apply transformation to Habitat's coordinate system + # habitat_quat = isaac_prim_utils.apply_isaac_to_habitat_orientation(isaac_quat.reshape(1, -1))[0] + + # # Convert translation from Isaac Sim to Habitat coordinates + # translate = isaac_prim_utils.usd_to_habitat_position(list(translate)) + + # # Format quaternion as [scalar, vector[0], vector[1], vector[2]] + # quat_door = [habitat_quat[0], habitat_quat[1], habitat_quat[2], habitat_quat[3]] + + return translate, rotation + + def get_articulation_links(self, prim_path: str): + """Function to get link names which articulation can be applied + Sample call: get_articulation_links(prim_path="/World/test_scene")""" + prim = self._isaac_service.world.stage.GetPrimAtPath(prim_path) + + articulation_links = {} + + for child_prim in prim.GetChildren(): + if UsdPhysics.ArticulationRootAPI.CanApply(child_prim): + articulation_api = UsdPhysics.ArticulationRootAPI(child_prim) + + link_names = [] + + for link_prim in child_prim.GetChildren(): + if link_prim.GetTypeName() == "Xform": + link_names.append(link_prim.GetName()) + + articulation_links[child_prim.GetName()] = link_names + + return articulation_links + + def hand_pose(self, convention="hab"): + """Get the current hand base link position and rotation.""" + link_poses = self.get_link_world_poses(convention=convention) + hand_base_id = self.get_link_id("link_l0_0") + + ee_pos = link_poses[0][hand_base_id] + ee_rot = link_poses[1][hand_base_id] + + return ee_pos, ee_rot diff --git a/habitat-lab/habitat/isaac_sim/data_conversion_utils.py b/habitat-lab/habitat/isaac_sim/data_conversion_utils.py index a816076d38..86d783acfc 100644 --- a/habitat-lab/habitat/isaac_sim/data_conversion_utils.py +++ b/habitat-lab/habitat/isaac_sim/data_conversion_utils.py @@ -1,12 +1,9 @@ - - -import os +import argparse import asyncio +import json +import os import re import xml.etree.ElementTree as ET -import argparse -import json - from omni.isaac.lab.app import AppLauncher @@ -15,14 +12,15 @@ AppLauncher.add_app_launcher_args(parser) # parse the arguments args_cli = parser.parse_args() +print("args_cli: ", args_cli) # launch omniverse app app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app +import omni.physx.scripts.utils from omni.isaac.core.utils.extensions import enable_extension from pxr import Usd, UsdGeom, UsdPhysics, PhysxSchema, Gf -import omni.physx.scripts.utils def add_habitat_visual_to_usd_root(usd_filepath, render_asset_filepath, render_asset_scale): # Open the USD file @@ -89,7 +87,6 @@ def convert_meshes_to_static_colliders(stage, root_path): root_prim = stage.GetPrimAtPath(root_path) if not root_prim.IsValid(): raise ValueError(f"The specified root path '{root_path}' is not valid.") - # Iterate over all child prims in the subtree for prim in Usd.PrimRange(root_prim): # Check if the prim is a mesh @@ -222,7 +219,7 @@ def convert_object_to_usd(object_config_filepath, out_usd_path, project_root_fol object_config_folder, _ = os.path.split(object_config_filepath) object_config_json_data = None - with open(object_config_filepath, 'r') as file: + with open(object_config_filepath, "r") as file: object_config_json_data = json.load(file) # By convention, we fall back to render_asset if collision_asset is not set. See Habitat-sim Simulator::getJoinedMesh. collision_asset_rel_filepath = object_config_json_data["collision_asset"] if "collision_asset" in object_config_json_data else object_config_json_data["render_asset"] @@ -333,7 +330,7 @@ def convert_hab_scene(scene_filepath, project_root_folder, enable_collision_for_ objects_root_folder = scenes_folder + "/../objects" assert os.path.exists(objects_root_folder) and os.path.isdir(objects_root_folder) - with open(scene_filepath, 'r') as file: + with open(scene_filepath, "r") as file: scene_json_data = json.load(file) object_counts = {} @@ -344,19 +341,17 @@ def convert_hab_scene(scene_filepath, project_root_folder, enable_collision_for_ assert "stage_instance" in scene_json_data if True: stage_json_data = scene_json_data["stage_instance"] - stage_template_name = stage_json_data['template_name'] + stage_template_name = stage_json_data["template_name"] base_stage_name = stage_template_name[stage_template_name.rfind('/') + 1:] stage_config_filepath = os.path.abspath(os.path.join(stages_folder, base_stage_name + ".stage_config.json")) assert os.path.exists(stage_config_filepath) out_usd_path = f"./data/usd/stages/{base_stage_name}.usda" if not os.path.exists(out_usd_path): - convert_object_to_usd(stage_config_filepath, out_usd_path, project_root_folder, enable_collision=enable_collision_for_stage) + convert_object_to_usd(stage_config_filepath,out_usd_path,project_root_folder,enable_collision=enable_collision_for_stage,) relative_usd_path = os.path.relpath(out_usd_path, start=scene_usd_folder) - object_xform = UsdGeom.Xform.Define( - usd_stage, f"/Scene/stage_{base_stage_name}" - ) + object_xform = UsdGeom.Xform.Define(usd_stage, f"/Scene/stage_{base_stage_name}") prim = object_xform.GetPrim() prim.GetReferences().AddReference(relative_usd_path) @@ -404,6 +399,99 @@ def convert_hab_scene(scene_filepath, project_root_folder, enable_collision_for_ xform_op_order.append(translate_op) assert "object_instances" in scene_json_data + # TODO: Cleanup this format due to high repetitive content + #Handling articulated objects + for obj_instance_json in scene_json_data["articulated_object_instances"]: + urdf_filename = f"{obj_instance_json['template_name']}.object_config.json" + urdf_filepath = find_file(objects_root_folder,urdf_filename) + urdf_filepath=os.path.join(objects_root_folder,urdf_filename) + + with open(urdf_filepath, "r") as file: + object_config_json_data = json.load(file) + # By convention, we fall back to render_asset if collision_asset is not set. See Habitat-sim Simulator::getJoinedMesh. + collision_asset_rel_filepath = (object_config_json_data["collision_asset"] if "collision_asset" in object_config_json_data else object_config_json_data["render_asset"]) + if urdf_filepath: + # Step 1: Convert the URDF to USD using the UrdfConverter + base_object_name = os.path.splitext(os.path.basename(collision_asset_rel_filepath))[0] + base_object_name = f"OBJECT_{base_object_name}" + base_object_name = sanitize_usd_name(collision_asset_rel_filepath) + out_usd_path = f"./data/usd/objects/{base_object_name}.usda" + + if not os.path.exists(out_usd_path): + try: + convert_object_urdf(collision_asset_rel_filepath,out_usd_path) + except Exception as e: + print(f"Error converting URDF to USD for {urdf_filename}: {e}") + continue + # relative_usd_path = out_usd_path.removeprefix("./data/usd/") + relative_usd_path = os.path.relpath(out_usd_path, start=scene_usd_folder) + + object_xform = UsdGeom.Xform.Define(usd_stage, f"/Scene/{base_object_name}") + prim = object_xform.GetPrim() + prim.GetReferences().AddReference(relative_usd_path) + # UsdPhysics.ArticulationRootAPI.Apply(prim) + # omni.physx.scripts.utils.setStaticCollider(prim, "convexHull") + # set purpose to "guide" so that this mesh doesn't render in Isaac? + if False: + mesh_geom = UsdGeom.Imageable(prim) + mesh_geom.CreatePurposeAttr(UsdGeom.Tokens.guide) + + + # Collect or create transform ops in the desired order: scale, rotation, translation + xform_op_order = [] + + + # Ensure scale op exists + scale = [1.0, 1.0, 1.0] # obj_instance_json.get("non_uniform_scale", [1.0, 1.0, 1.0]) + scale_op = next( + (op for op in object_xform.GetOrderedXformOps() if op.GetName() == "xformOp:scale"), + None, + ) + if scale_op is None: + scale_op = object_xform.AddScaleOp() + scale_op.Set(Gf.Vec3f(*scale)) + xform_op_order.append(scale_op) + + + # Ensure rotation op exists + # rotation = habitat_to_usd_rotation([0.0, 0.0, 0.0, 1.0]) + # rotation = [1.0, 0.0, 0.0, 0.0] + # 90 degrees about x in wxyz format + # rotation = [0.7071068,0.7071067,0.0,0.0] + rotation = habitat_to_usd_rotation( + obj_instance_json.get("rotation") + ) + orient_op = next( + (op for op in object_xform.GetOrderedXformOps() if op.GetName() == "xformOp:orient"),None, + ) + if orient_op is None: + orient_op = object_xform.AddOrientOp(precision=UsdGeom.XformOp.PrecisionDouble) + orient_op.Set(Gf.Quatd(*rotation)) + xform_op_order.append(orient_op) + + # Ensure translation op exists + position = habitat_to_usd_position( + obj_instance_json.get("translation") + ) + translate_op = next( + (op for op in object_xform.GetOrderedXformOps() if op.GetName() == "xformOp:translate"), + None, + ) + if translate_op is None: + translate_op = object_xform.AddTranslateOp() + translate_op.Set(Gf.Vec3f(*position)) + xform_op_order.append(translate_op) + + + # Let's not change the xform order. I don't know what's going on with Xform order. + # object_xform.SetXformOpOrder(xform_op_order) + + + count += 1 + if count == max_count: + break + + for obj_instance_json in scene_json_data["object_instances"]: # todo: assert collision_asset_size is (1,1,1) or not present @@ -426,11 +514,19 @@ def convert_hab_scene(scene_filepath, project_root_folder, enable_collision_for_ # todo: gather these up and do them later with multiprocessing if not os.path.exists(out_usd_path): - - object_config_filepath = find_file(objects_root_folder, object_config_filename) - assert object_config_filepath - - convert_object_to_usd(object_config_filepath, out_usd_path, project_root_folder) + try: + object_config_filepath = find_file( + objects_root_folder, object_config_filename + ) + assert object_config_filepath + + convert_object_to_usd( + object_config_filepath, out_usd_path, project_root_folder + ) + except: + print( + "skipping: ", objects_root_folder, object_config_filename + ) # relative_usd_path = out_usd_path.removeprefix("./data/usd/") relative_usd_path = os.path.relpath(out_usd_path, start=scene_usd_folder) @@ -531,9 +627,9 @@ def convert_urdf(urdf_filepath, out_usd_filepath): print(f"USD file generated at: {usd_path}") -from pxr import Usd, Sdf, Gf -import xml.etree.ElementTree as ET import os +import xml.etree.ElementTree as ET +from pxr import Gf, Sdf, Usd def add_habitat_visual_metadata_for_articulation(usd_filepath, reference_urdf_filepath, out_usd_filepath, project_root_folder): # Parse the URDF file @@ -561,17 +657,30 @@ def add_habitat_visual_metadata_for_articulation(usd_filepath, reference_urdf_fi scale = (1.0, 1.0, 1.0) # Default scale # Check for scale in the element - scale_element = mesh.find("scale") + scale_element = mesh.get("scale") if scale_element is not None: - scale = tuple(map(float, scale_element.text.split())) + scale = tuple(map(float, scale_element.split())) # Replace periods with underscores for USD-safe names - # todo: use a standard get_sanitized_usd_name function here safe_link_name = link_name.replace(".", "_") visual_metadata[safe_link_name] = { "assetPath": asset_path, "assetScale": scale } + else: + print(f"Warning: No mesh found for visual in link {link_name}") + else: + print(f"Warning: No visual found for link {link_name}") + + # Extract damping values from URDF joints + joint_damping_data = {} + for joint in urdf_root.findall("joint"): + joint_name = joint.get("name") + dynamics = joint.find("dynamics") + if dynamics is not None: + damping = dynamics.get("damping") + if damping is not None: + joint_damping_data[joint_name] = float(damping) # Open the USD file stage = Usd.Stage.Open(usd_filepath) @@ -584,19 +693,34 @@ def add_habitat_visual_metadata_for_articulation(usd_filepath, reference_urdf_fi prim = stage.GetPrimAtPath(prim_path) if prim: # Add assetPath - asset_path_attr = prim.CreateAttribute("habitatVisual:assetPath", Sdf.ValueTypeNames.String) + asset_path_attr = prim.CreateAttribute( + "habitatVisual:assetPath", Sdf.ValueTypeNames.String + ) asset_path_attr.Set(metadata["assetPath"]) # Add assetScale - asset_scale_attr = prim.CreateAttribute("habitatVisual:assetScale", Sdf.ValueTypeNames.Float3) + asset_scale_attr = prim.CreateAttribute( + "habitatVisual:assetScale", Sdf.ValueTypeNames.Float3 + ) asset_scale_attr.Set(Gf.Vec3f(*metadata["assetScale"])) else: print(f"Warning: Prim not found for link: {link_name}") + + # Update joints with damping values + for joint_name, damping in joint_damping_data.items(): + joint_path = f"/{robot_name}/{joint_name}" + joint_prim = stage.GetPrimAtPath(joint_path) + if joint_prim: + damping_attr = joint_prim.CreateAttribute("drive:angular:physics:damping", Sdf.ValueTypeNames.Float) + damping_attr.Set(damping) + else: + print(f"Warning: Joint not found in USD: {joint_name}") # Save the updated USD to the output file stage.GetRootLayer().Export(out_usd_filepath) print(f"Updated USD file written to: {out_usd_filepath}") + def convert_urdf_test(): base_urdf_name = "hab_spot_arm" base_urdf_folder = "data/robots/hab_spot_arm/urdf" @@ -612,7 +736,26 @@ def convert_urdf_test(): temp_usd_filepath = f"data/usd/robots/{base_urdf_name}.usda" out_usd_filepath = f"data/usd/robots/{base_urdf_name}.usda" convert_urdf(clean_urdf_filepath, temp_usd_filepath) - add_habitat_visual_metadata_for_articulation(temp_usd_filepath, source_urdf_filepath, out_usd_filepath, project_root_folder="./") + add_habitat_visual_metadata_for_articulation(temp_usd_filepath,source_urdf_filepath,out_usd_filepath,project_root_folder="./",) + +def convert_object_urdf(path,out_usd_path): + + base_urdf_name = "FREMONT-KITCHENISLAND" + base_urdf_folder = path + base_directory = "/home/joanne/habitat-lab/data/Fremont-Knuckles" + base_urdf_folder=os.path.normpath(os.path.join(base_directory,path)) + source_urdf_filepath = f"{base_urdf_folder}" + clean_urdf_filepath = f"{base_urdf_folder}" + temp_usd_filepath = out_usd_path + out_usd_filepath = out_usd_path + convert_urdf(clean_urdf_filepath, temp_usd_filepath) + add_habitat_visual_metadata_for_articulation( + temp_usd_filepath, + source_urdf_filepath, + out_usd_filepath, + project_root_folder="./", + ) + def convert_objects_folder_to_usd(objects_root_folder, out_usd_folder, project_root_folder): @@ -636,6 +779,29 @@ def convert_objects_folder_to_usd(objects_root_folder, out_usd_folder, project_r if __name__ == "__main__": # example usage: - # convert_urdf_test() - convert_hab_scene("data/scene_datasets/hssd-hab/scenes-uncluttered/102344193.scene_instance.json", project_root_folder="./", enable_collision_for_stage=True) - # convert_objects_folder_to_usd("data/objects/ycb", "data/usd/objects/ycb/configs", "./") \ No newline at end of file + convert_urdf_test() + # convert_hab_scene("data/scene_datasets/hssd-hab/scenes-uncluttered/102344193.scene_instance.json", project_root_folder="./", enable_collision_for_stage=True) + # convert_objects_folder_to_usd( + # "data/objects/ycb", "data/usd/objects/ycb/configs_v2", "./" + # ) + # convert_object_glb_to_usd( + # "data/Fremont-Knuckles/objects/other/plush4/", + # "data/usd/objects/fremont/other/plush4/OBJECT_plush4.usda", + # "./", + # ) + # convert_hab_scene( + # "data/scene_datasets/hssd-hab/scenes-uncluttered/102344529.scene_instance.json", + # project_root_folder="./", + # enable_collision_for_stage=True, + # ) + + # convert_hab_scene( + # "data/Fremont-Knuckles/configs/scenes/fremont_static_v2.scene_instance.json", + # project_root_folder="./", + # enable_collision_for_stage=True, + # ) +# convert_hab_scene( +# "data/Fremont-Knuckles/configs/scenes/fremont_static_objects.scene_instance.json", +# project_root_folder="./", +# enable_collision_for_stage=True, +# ) \ No newline at end of file diff --git a/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py b/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py new file mode 100644 index 0000000000..339bc33709 --- /dev/null +++ b/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py @@ -0,0 +1,109 @@ +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +from typing import Dict, List, Optional, Set, Tuple + +import magnum as mn +import numpy as np +import quaternion + +from habitat.articulated_agents.mobile_manipulator import ( + ArticulatedAgentCameraParams, + MobileManipulatorParams, +) +from habitat.articulated_agents.robots.murp_robot import MurpRobot +from habitat.isaac_sim._internal.murp_robot_wrapper import MurpRobotWrapper +from habitat.isaac_sim.isaac_mobile_manipulator import IsaacMobileManipulator + + +class IsaacMurpRobot(IsaacMobileManipulator): + """Isaac-internal wrapper for a robot. + + + The goal with this wrapper is convenience but not encapsulation. See also (public) IsaacMobileManipulator, which has the goal of exposing a minimal public interface to the rest of Habitat-lab. + """ + + # todo: put most of this logic in IsaacMobileManipulator + @property + def base_transformation(self): + add_rot = mn.Matrix4.rotation(mn.Rad(np.pi / 2), mn.Vector3(1.0, 0, 0)) + base_position, base_rotation = self._robot_wrapper.get_root_pose() + pose = mn.Matrix4.from_(base_rotation.to_matrix(), base_position) + return pose @ add_rot + + @base_transformation.setter + def base_transformation(self, base_transformation): + rot = mn.Matrix4.rotation(mn.Rad(-np.pi / 2), mn.Vector3(1.0, 0, 0)) + base_transformation = base_transformation @ rot + rot = mn.Quaternion.from_matrix(base_transformation.rotation()) + self._robot_wrapper.set_root_pose(base_transformation.translation, rot) + # pose = mn.Matrix4.from_(base_rotation.to_matrix(), base_position + + def get_link_transform(self, link_id): + link_positions, link_rotations = ( + self._robot_wrapper.get_link_world_poses() + ) + position, rotation = link_positions[link_id], link_rotations[link_id] + # breakpoint() + + pose = mn.Matrix4.from_(rotation.to_matrix(), position) + add_rot = mn.Matrix4.rotation( + mn.Rad(-np.pi / 2), mn.Vector3(1.0, 0, 0) + ) + return pose + + def get_ee_local_pose( + self, ee_index: int = 0 + ) -> Tuple[np.ndarray, np.ndarray]: + """Get the local pose of the end-effector. + + :param ee_index: the end effector index for which we want the link transform + """ + if ee_index >= len(self.params.ee_links): + raise ValueError( + "The current manipulator does not have enough end effectors" + ) + + assert False # todo + return None + + # ee_transform = self.ee_transform() + # base_transform = self.base_transformation + # # Get transformation + # base_T_ee_transform = base_transform.inverted() @ ee_transform + + # # Get the local ee location (x,y,z) + # local_ee_location = base_T_ee_transform.translation + + # # Get the local ee orientation (roll, pitch, yaw) + # local_ee_quat = quaternion.from_rotation_matrix( + # base_T_ee_transform.rotation() + # ) + + # return np.array(local_ee_location), local_ee_quat + + def __init__(self, agent_cfg, isaac_service, sim=None): + # TODO: This should be obtained from _target_arm_joint_positions but it is not intialized here yet. + ee_index = 19 + # arm_joints = [0, 2, 4, 6, 8, 10, 12] + arm_joints = list(range(0, 13)) + + murp_params = MurpRobot._get_murp_params() + murp_params.arm_joints = arm_joints + murp_params.gripper_joints = [ee_index] + murp_params.arm_init_params = [ + 2.6116285, + 1.5283098, + 1.0930868, + -0.50559217, + 0.48147443, + 2.628784, + -1.3962275, + ] + robot_wrapper = MurpRobotWrapper( + isaac_service=isaac_service, instance_id=0 + ) + super().__init__( + murp_params, agent_cfg, isaac_service, robot_wrapper, sim=sim + )