[hab3_merge] Updates in humanoid controller

habitat-lab/habitat/articulated_agent_controllers/humanoid_rearrange_controller.py

@@ -53,6 +53,9 @@ def __init__(self, joints_quat_array, transform_array, displacement, fps):
     20  # The maximum angle we should be rotating at a given step
 )
 THRESHOLD_ROTATE_NOT_MOVE = 20  # The rotation angle above which we should only walk as if rotating in place
+DIST_TO_STOP = (
+    1e-9  # If the amout to move is this distance, just stop the character
+)
 
 
 class HumanoidRearrangeController:
@@ -109,10 +112,24 @@ def __init__(
         self.prev_orientation = None
         self.walk_mocap_frame = 0
 
-    def reset(self, position) -> None:
+    def set_framerate_for_linspeed(self, lin_speed, ang_speed, ctrl_freq):
+        """Set the speed of the humanoid according to the simulator speed"""
+        seconds_per_step = 1.0 / ctrl_freq
+        meters_per_step = lin_speed * seconds_per_step
+        frames_per_step = meters_per_step / self.dist_per_step_size
+        self.draw_fps = self.walk_motion.fps / frames_per_step
+        rotate_amount = ang_speed * seconds_per_step
+        rotate_amount = rotate_amount * 180.0 / np.pi
+        self.turning_step_amount = rotate_amount
+        self.threshold_rotate_not_move = rotate_amount
+
+    def reset(self, base_transformation) -> None:
         """Reset the joints on the human. (Put in rest state)"""
         self.obj_transform_offset = mn.Matrix4()
-        self.obj_transform_base.translation = position + self.base_offset
+        self.obj_transform_base = base_transformation
+        self.prev_orientation = base_transformation.transform_vector(
+            mn.Vector3(1.0, 0.0, 0.0)
+        )
 
     def calculate_stop_pose(self):
         """
@@ -138,9 +155,13 @@ def calculate_walk_pose(
         """
         deg_per_rads = 180.0 / np.pi
         forward_V = target_position
-        if forward_V.length() == 0.0:
+        if (
+            forward_V.length() < DIST_TO_STOP
+            or np.isnan(target_position).any()
+        ):
             self.calculate_stop_pose()
             return
+
         distance_to_walk = np.linalg.norm(forward_V)
         did_rotate = False
 
@@ -219,13 +240,19 @@ def calculate_walk_pose(
         joint_pose, obj_transform = new_pose.joints, new_pose.root_transform
 
         # We correct the object transform
+
+        forward_V_norm = mn.Vector3(
+            [forward_V[2], forward_V[1], -forward_V[0]]
+        )
         look_at_path_T = mn.Matrix4.look_at(
             self.obj_transform_base.translation,
-            self.obj_transform_base.translation + forward_V.normalized(),
+            self.obj_transform_base.translation + forward_V_norm.normalized(),
             mn.Vector3.y_axis(),
         )
 
         # Remove the forward component, and orient according to forward_V
+        add_rot = mn.Matrix4.rotation(mn.Rad(np.pi), mn.Vector3(0, 1, 0))
+        obj_transform = add_rot @ obj_transform
         obj_transform.translation *= mn.Vector3.x_axis() + mn.Vector3.y_axis()
 
         # This is the rotation and translation caused by the current motion pose
@@ -238,7 +265,10 @@ def calculate_walk_pose(
         forward_V_dist = forward_V * dist_diff * distance_multiplier
         obj_transform_base.translation += forward_V_dist
 
-        self.obj_transform_base = obj_transform_base
+        rot_offset = mn.Matrix4.rotation(
+            mn.Rad(-np.pi / 2), mn.Vector3(1, 0, 0)
+        )
+        self.obj_transform_base = obj_transform_base @ rot_offset
         self.joint_pose = joint_pose
 
     def get_pose(self):

habitat-lab/habitat/articulated_agents/humanoids/kinematic_humanoid.py

@@ -2,6 +2,7 @@
 # This source code is licensed under the MIT license found in the
 # LICENSE file in the root directory of this source tree.
 
+import pickle as pkl
 from typing import List
 
 import magnum as mn
@@ -13,6 +14,7 @@
     MobileManipulator,
     MobileManipulatorParams,
 )
+from habitat_sim.utils.common import orthonormalize_rotation_shear
 
 
 class KinematicHumanoid(MobileManipulator):
@@ -73,6 +75,26 @@ def __init__(
         # a path, whereas the offset transform will be changing the position
         # to simulate different gaits
         self.offset_transform = mn.Matrix4()
+        self.offset_rot = -np.pi / 2
+        add_rot = mn.Matrix4.rotation(
+            mn.Rad(self.offset_rot), mn.Vector3(0, 1.0, 0)
+        )
+        perm = mn.Matrix4.rotation(
+            mn.Rad(self.offset_rot), mn.Vector3(0, 0, 1.0)
+        )
+        self.offset_transform_base = perm @ add_rot
+
+        self.rest_joints = None
+
+    def set_rest_pose_path(self, rest_pose_path):
+        """Sets the parameters that indicate the reset state of the agent. Note that this function overrides
+        _get_X_params, which is used to set parameters of the robots, but the parameters are so large that
+        it is better to put that on a file
+        """
+        with open(rest_pose_path, "rb") as f:
+            rest_pose = pkl.load(f)
+            rest_pose = rest_pose["stop_pose"]
+        self.rest_joints = list(rest_pose["joints"].reshape(-1))
 
     @property
     def inverse_offset_transform(self):
@@ -82,16 +104,18 @@ def inverse_offset_transform(self):
 
     @property
     def base_transformation(self):
-        return self.sim_obj.transformation @ self.inverse_offset_transform
+        return (
+            self.sim_obj.transformation
+            @ self.inverse_offset_transform
+            @ self.offset_transform_base
+        )
 
     @property
     def base_pos(self):
         """Get the humanoid base ground position"""
         # via configured local offset from origin
         base_transform = self.base_transformation
-        return base_transform.translation + base_transform.transform_vector(
-            self.params.base_offset
-        )
+        return base_transform.translation + self.params.base_offset
 
     @base_pos.setter
     def base_pos(self, position: mn.Vector3):
@@ -102,34 +126,106 @@ def base_pos(self, position: mn.Vector3):
         if len(position) != 3:
             raise ValueError("Base position needs to be three dimensions")
         base_transform = self.base_transformation
-        base_pos = position - base_transform.transform_vector(
-            self.params.base_offset
-        )
+        base_pos = position - self.params.base_offset
         base_transform.translation = base_pos
-        final_transform = base_transform @ self.offset_transform
-
+        add_rot = self.offset_transform_base.inverted()
+        final_transform = base_transform @ add_rot @ self.offset_transform
         self.sim_obj.transformation = final_transform
 
     @property
     def base_rot(self) -> float:
-        return self.base_transformation.rotation.angle()
+        return self.sim_obj.rotation.angle() + mn.Rad(self.offset_rot)
 
     @base_rot.setter
     def base_rot(self, rotation_y_rad: float):
         if self._base_type == "mobile" or self._base_type == "leg":
+            angle_rot = -self.offset_rot
             self.sim_obj.rotation = mn.Quaternion.rotation(
-                mn.Rad(rotation_y_rad), mn.Vector3(0, 1, 0)
+                mn.Rad(rotation_y_rad + angle_rot), mn.Vector3(0, 1, 0)
             )
         else:
             raise NotImplementedError("The base type is not implemented.")
 
+    def set_rest_position(self) -> None:
+        """Sets the agents in a resting position"""
+        if self.rest_joints is None:
+            joint_list = self.sim_obj.joint_positions
+        else:
+            joint_list = self.rest_joints
+
+        offset_transform = mn.Matrix4()  # self.rest_matrix
+        self.set_joint_transform(
+            joint_list, offset_transform, self.base_transformation
+        )
+
+    def update(self) -> None:
+        """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
+            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]
+
+                    if cam_info.attached_link_id == -1:
+                        link_trans = self.sim_obj.transformation
+                    elif cam_info.attached_link_id == -2:
+                        rot_offset = self.offset_transform_base.inverted()
+                        link_trans = self.base_transformation @ rot_offset
+                    else:
+                        link_trans = self.sim_obj.get_link_scene_node(
+                            cam_info.attached_link_id
+                        ).transformation
+
+                    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 = Mt @ Mz @ My @ Mx
+                    else:
+                        cam_transform = 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
+                        @ cam_info.relative_transform
+                    )
+                    cam_transform = inv_T @ cam_transform
+
+                    sens_obj.node.transformation = (
+                        orthonormalize_rotation_shear(cam_transform)
+                    )
+
+        if self._fix_joint_values is not None:
+            self.arm_joint_pos = self._fix_joint_values
+
+        self.sim_obj.awake = True
+
     def reconfigure(self) -> None:
         """Instantiates the human in the scene. Loads the URDF, sets initial state of parameters, joints, motors, etc..."""
         super().reconfigure()
         self.sim_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC
         # remove any remaining joint motors
         for motor_id in self.sim_obj.existing_joint_motor_ids:
             self.sim_obj.remove_joint_motor(motor_id)
+        self.update()
+        self.set_rest_position()
+
+    def reset(self) -> None:
+        super().reset()
+        self.update()
+        self.set_rest_position()
 
     def set_joint_transform(
         self,
@@ -141,7 +237,9 @@ def set_joint_transform(
         # TODO: should this go into articulated agent?
         self.sim_obj.joint_positions = joint_list
         self.offset_transform = offset_transform
-        final_transform = base_transform @ offset_transform
+
+        add_rot = self.offset_transform_base.inverted()
+        final_transform = (base_transform @ add_rot) @ offset_transform
 
         self.sim_obj.transformation = final_transform
 

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

@@ -37,7 +37,9 @@ def __init__(self, *args, task, **kwargs):
 
         elif self.motion_type == "human_joints":
             HumanoidJointAction.__init__(self, *args, **kwargs)
-            self.humanoid_controller = self.lazy_inst_humanoid_controller(task)
+            self.humanoid_controller = self.lazy_inst_humanoid_controller(
+                task, config
+            )
 
         else:
             raise ValueError("Unrecognized motion type for oracle nav  action")
@@ -58,7 +60,7 @@ def _compute_turn(rel, turn_vel, robot_forward):
             vel = [0, turn_vel]
         return vel
 
-    def lazy_inst_humanoid_controller(self, task):
+    def lazy_inst_humanoid_controller(self, task, config):
         # Lazy instantiation of humanoid controller
         # We assign the task with the humanoid controller, so that multiple actions can
         # use it.
@@ -76,6 +78,9 @@ def lazy_inst_humanoid_controller(self, task):
             ].motion_data_path
 
             humanoid_controller = HumanoidRearrangeController(walk_pose_path)
+            humanoid_controller.set_framerate_for_linspeed(
+                config["lin_speed"], config["ang_speed"], self._sim.ctrl_freq
+            )
             task.humanoid_controller = humanoid_controller
         return task.humanoid_controller
 
@@ -115,7 +120,7 @@ def _get_target_for_idx(self, nav_to_target_idx: int):
             )
             if self.motion_type == "human_joints":
                 self.humanoid_controller.reset(
-                    self.cur_articulated_agent.base_pos
+                    self.cur_articulated_agent.base_transformation
                 )
             self._targets[nav_to_target_idx] = (start_pos, np.array(obj_pos))
         return self._targets[nav_to_target_idx]

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

@@ -77,6 +77,8 @@ def __init__(self, cfg, sim):
             agent_cfg = cfg.agents[agent_name]
             agent_cls = eval(agent_cfg.articulated_agent_type)
             agent = agent_cls(agent_cfg.articulated_agent_urdf, sim)
+            if agent_cfg.motion_data_path != "":
+                agent.set_rest_pose_path(agent_cfg.motion_data_path)
             grasp_managers = []
             for grasp_manager_id in range(agent_cfg.grasp_managers):
                 graps_mgr = RearrangeGraspManager(