[Do not merge] Discussion on integration with habitat

examples/hitl/isaacsim_viewer/isaacsim_viewer.py

@@ -219,7 +219,7 @@ def update(self, dt):
 class SpotPickHelper:
 
     APPROACH_DIST = 0.16
-    APPROACH_DURATION = 2.0
+    APPROACH_DURATION = 50.0
 
     def __init__(self, num_dof):
 
@@ -320,7 +320,7 @@ def update(self, dt, target_rel_pos):
         # target_arm_joint_positions[7] = -1.0
 
         if approach_progress > 0.0:
-            print(f"approach_progress: {approach_progress}")
+            print(f"approach_progress: {approach_progress}", target_arm_joint_positions[7])
 
         return target_arm_joint_positions
 

habitat-lab/habitat/articulated_agents/robots/spot_robot.py

@@ -87,7 +87,10 @@ class SpotParams:
 
 
 class SpotRobot(MobileManipulator):
-    def _get_spot_params(self):
+    @classmethod
+
+
+    def _get_spot_params(cls):
         return SpotParams(
             arm_joints=list(range(0, 7)),
             gripper_joints=[7],

habitat-lab/habitat/isaac_sim/_internal/spot_robot_wrapper.py

@@ -100,6 +100,15 @@ def robot(self) -> Robot:
 
     def get_prim_path(self):
         return self._robot_prim_path
+
+
+    def set_root_pose(self, pos, rot):
+
+        rot = [rot.scalar] + list(rot.vector)
+        pos_usd = isaac_prim_utils.habitat_to_usd_position(pos)
+        rot_usd = isaac_prim_utils.habitat_to_usd_rotation(rot)
+        self._robot.set_world_pose(pos_usd, rot_usd)
+
 
     def get_root_pose(self):
 
@@ -167,7 +176,8 @@ def post_reset(self):
             arm_joint_indices.append(dof_names.index(arm_joint_name))
 
         self._arm_joint_indices = np.array(arm_joint_indices)
-        self._target_arm_joint_positions = None
+        # self._target_arm_joint_positions = None
+        self._target_arm_joint_positions = [0.0, -2.36, 0.0, 2.25, 0.0, 1.67, 0.0, -1.67]
 
 
     def scale_prim_mass_and_inertia(self, path, scale):

habitat-lab/habitat/isaac_sim/actions.py

@@ -0,0 +1,62 @@
+from habitat.core.registry import registry
+from habitat.tasks.rearrange.actions.actions import BaseVelAction, ArticulatedAgentAction
+import magnum as mn
+import numpy as np
+import habitat_sim
+from examples.hitl.isaacsim_viewer.isaacsim_viewer import SpotPickHelper
+@registry.register_task_action
+class BaseVelIsaacAction(BaseVelAction):
+    def step(self, *args, **kwargs):
+        lin_vel, ang_vel = kwargs[self._action_arg_prefix + "base_vel"]
+        lin_vel = np.clip(lin_vel, -1, 1) * self._lin_speed
+        ang_vel = np.clip(ang_vel, -1, 1) * self._ang_speed
+        if not self._allow_back:
+            lin_vel = np.maximum(lin_vel, 0)
+
+        self.base_vel_ctrl.linear_velocity = mn.Vector3(lin_vel, 0, 0)
+        self.base_vel_ctrl.angular_velocity = mn.Vector3(0, ang_vel, 0)
+        self.cur_articulated_agent._robot_wrapper._robot.set_angular_velocity([0, 0, ang_vel])
+        self.cur_articulated_agent._robot_wrapper._robot.set_linear_velocity([lin_vel, 0, 0])
+
+
+
+@registry.register_task_action
+class ArmReachAction(ArticulatedAgentAction):
+    def __init__(self, *args, **kwargs):
+        super().__init__(self, *args, **kwargs)
+
+        self._spot_wrapper = self.cur_articulated_agent._robot_wrapper
+        self._spot_pick_helper = SpotPickHelper(len(self._spot_wrapper._arm_joint_indices))
+
+    def step(self, *args, **kwargs):
+        target_pos = kwargs[self._action_arg_prefix + "target_pos"]
+        base_pos, base_rot = self._spot_wrapper.get_root_pose()
+        def inverse_transform(pos_a, rot_b, pos_b):
+            inv_pos = rot_b.inverted().transform_vector(pos_a - pos_b)    
+            return inv_pos
+        target_rel_pos = inverse_transform(target_pos, base_rot, base_pos)
+
+        dt = 0.5
+        self._spot_wrapper._target_arm_joint_positions = self._spot_pick_helper.update(dt, target_rel_pos)
+
+
+
+@registry.register_task_action
+class BaseVelKinematicIsaacAction(BaseVelAction):
+
+    def update_base(self):
+        ctrl_freq = self._sim.ctrl_freq
+        trans = self.cur_articulated_agent.base_transformation
+        rigid_state = habitat_sim.RigidState(
+            mn.Quaternion.from_matrix(trans.rotation()), trans.translation
+        )
+
+        target_rigid_state = self.base_vel_ctrl.integrate_transform(
+            1 / ctrl_freq, rigid_state
+        )
+        target_trans = mn.Matrix4.from_(
+            target_rigid_state.rotation.to_matrix(), target_rigid_state.translation
+        )
+        self.cur_articulated_agent.base_transformation = target_trans         
+
+

habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py

@@ -29,12 +29,30 @@ def __init__(
         params: MobileManipulatorParams,
         agent_cfg,
         isaac_service,
+        sim=None
         # limit_robo_joints: bool = True,
         # fixed_base: bool = True,
         # maintain_link_order: bool = False,
         # base_type="mobile",
     ):
+        self._sim = sim
         self._robot_wrapper = SpotRobotWrapper(isaac_service=isaac_service, instance_id=0)
+        # Modify here the params:
+
+        self.params = params
+
+        # TODO: this should move later, cameras should not be attached to agents
+        # @alexclegg
+
+        self._cameras = None
+        if hasattr(self.params, "cameras"):
+            from collections import defaultdict
+
+            self._cameras = defaultdict(list)
+            for camera_prefix in self.params.cameras:
+                for sensor_name in self._sim._sensors:
+                    if sensor_name.startswith(camera_prefix):
+                        self._cameras[camera_prefix].append(sensor_name)
 
 
     def reconfigure(self) -> None:
@@ -46,9 +64,54 @@ def update(self) -> None:
         """Updates the camera transformations and performs necessary checks on
         joint limits and sleep states.
         """
-        # todo
-        pass
-
+        """Updates the camera transformations and performs necessary checks on
+        joint limits and sleep states.
+        """
+        if self._cameras is not None:
+            # get the transformation
+            agent_node = self._sim._default_agent.scene_node
+            inv_T = agent_node.transformation.inverted()
+            # update the cameras
+            sim = self._sim
+            look_up = mn.Vector3(0,1,0)
+
+            for cam_prefix, sensor_names in self._cameras.items():
+                for sensor_name in sensor_names:
+                    sens_obj = self._sim._sensors[sensor_name]._sensor_object
+                    cam_info = self.params.cameras[cam_prefix]
+                    agent = sim.agents_mgr._all_agent_data[0].articulated_agent
+                    look_at = sim.agents_mgr._all_agent_data[0].articulated_agent.base_pos
+
+                    if cam_info.attached_link_id == -1:
+                        link_trans = agent.base_transformation
+                    else:
+                        link_trans = agent.get_link_transform(cam_info.attached_link_id+1)
+                    if cam_info.cam_look_at_pos == mn.Vector3(0, 0, 0):
+                        pos = cam_info.cam_offset_pos
+                        ori = cam_info.cam_orientation
+                        Mt = mn.Matrix4.translation(pos)
+                        Mz = mn.Matrix4.rotation_z(mn.Rad(ori[2]))
+                        My = mn.Matrix4.rotation_y(mn.Rad(ori[1]))
+                        Mx = mn.Matrix4.rotation_x(mn.Rad(ori[0]))
+                        cam_transform_rel = Mt @ Mz @ My @ Mx
+                    else:
+                        cam_transform_rel = mn.Matrix4.look_at(
+                            cam_info.cam_offset_pos,
+                            cam_info.cam_look_at_pos,
+                            mn.Vector3(0, 1, 0),
+                        )
+
+                    cam_transform = (
+                        link_trans
+                        @ cam_transform_rel
+                        @ cam_info.relative_transform
+                    )
+
+
+                    sens_obj.node.transformation = (
+                        cam_transform
+                    )
+
     def reset(self) -> None:
         """Reset the joints on the existing robot.
         NOTE: only arm and gripper joint motors (not gains) are reset by default, derived class should handle any other changes.

habitat-lab/habitat/isaac_sim/isaac_spot_robot.py

@@ -8,23 +8,53 @@
 import numpy as np
 import quaternion
 
+from habitat.articulated_agents.mobile_manipulator import ArticulatedAgentCameraParams
 from habitat.articulated_agents.mobile_manipulator import MobileManipulatorParams
 from habitat.isaac_sim.isaac_mobile_manipulator import IsaacMobileManipulator
 from habitat.articulated_agents.robots.spot_robot import SpotRobot
 
+
+
 class IsaacSpotRobot(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_position, base_orientation = self._robot_wrapper.robot.get_world_pose()
-        # todo: get Hab transform from pos and orient
-        assert False
-        return None
+        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]:
@@ -58,10 +88,21 @@ def get_ee_local_pose(
     def __init__(
         self,
         agent_cfg,
-        isaac_service
+        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,  5, 10, 15, 16, 17, 18]
+        leg_joints = [jid for jid in range(19) if jid not in arm_joints]
+
+        spot_params = SpotRobot._get_spot_params()
+        spot_params.arm_joints = arm_joints
+        spot_params.gripper_joints = [ee_index]
+        spot_params.leg_joints = leg_joints
         super().__init__(
-            SpotRobot.get_spot_params(),
+            spot_params,
             agent_cfg,
             isaac_service,
+            sim=sim
         )

habitat-lab/habitat/isaac_sim/spot_arm_ik_helper.py

@@ -138,4 +138,5 @@ def is_valid_triangle(a, b, h):
     result[0] = yaw_angle_to_target
 
     is_ik_active = not is_out_of_range
-    return is_ik_active, result
+    return is_ik_active, result
+

habitat-lab/habitat/tasks/rearrange/actions/actions.py

@@ -428,6 +428,8 @@ def step(self, delta_pos, *args, **kwargs):
         self.cur_articulated_agent.arm_motor_pos = set_arm_pos
 
 
+
+
 @registry.register_task_action
 class BaseVelAction(ArticulatedAgentAction):
     """