Update habitat-lab submodule

examples/hitl/rearrange_v2/config/language_rearrange.yaml

@@ -10,7 +10,7 @@ defaults:
 
   - /habitat/dataset/rearrangement: hssd
 
-  - /habitat/task/[email protected].agent_0_base_velocity: base_velocity
+  - /habitat/task/[email protected].: base_velocity
   - /habitat/task/[email protected]_0_rearrange_stop: rearrange_stop
   - /habitat/task/[email protected]_0_humanoidjoint_action: humanoidjoint_action
 

habitat-lab/habitat/articulated_agents/articulated_agent_base.py

@@ -106,6 +106,7 @@ def reconfigure(self) -> None:
             # pylint: disable=not-an-iterable
             for i in self.params.wheel_joints:
                 self.sim_obj.update_joint_motor(self.joint_motors[i][0], jms)
+                print("_wheel_jointsposupdate_joint_motor")
         self._update_motor_settings_cache()
 
         # set correct gains for legs
@@ -123,6 +124,7 @@ def reconfigure(self) -> None:
             # pylint: disable=not-an-iterable
             for i in self.params.leg_joints:
                 self.sim_obj.update_joint_motor(self.joint_motors[i][0], jms)
+                print("leg_jointsupdate_joint_motor")
             self.leg_joint_pos = self.params.leg_init_params
         self._update_motor_settings_cache()
 
@@ -258,6 +260,7 @@ def leg_joint_pos(self, ctrl: List[float]):
                 joint_positions[self.joint_pos_indices[jidx]] = ctrl[i]
             self.sim_obj.joint_positions = joint_positions
         else:
+            print("xytestself._base_type",self._base_type)
             raise NotImplementedError(
                 "There are no leg motors other than leg robots"
             )

habitat-lab/habitat/articulated_agents/articulated_agent_interface.py

@@ -2,7 +2,8 @@
 # This source code is licensed under the MIT license found in the
 # LICENSE file in the root directory of this source tree.
 
-from abc import ABC, abstractmethod
+from abc import ABC, abstractmethod 
+from habitat_sim.physics import JointMotorSettings, MotionType #xychange
 
 from habitat_sim.physics import ManagedBulletArticulatedObject
 
@@ -14,6 +15,7 @@ def __init__(self):
         """Initializes this wrapper, but does not instantiate the robot."""
         # the Habitat ArticulatedObject API access wrapper
         self.sim_obj: ManagedBulletArticulatedObject = None
+        # self.sim_obj.motion_type = MotionType.DYNAMIC #xychange
 
     def get_robot_sim_id(self) -> int:
         """Get the unique id for referencing the robot."""

habitat-lab/habitat/articulated_agents/create_urdf.py

@@ -0,0 +1,179 @@
+import pathlib
+import pprint
+from copy import deepcopy
+
+# import dex_teleop_parameters as dt
+import numpy as np
+from urdf_parser_py import urdf as ud
+
+def save_urdf_file(robot, file_name):
+    urdf_string = robot.to_xml_string()
+    print("Saving new URDF file to", file_name)
+    fid = open(file_name, "w")
+    fid.write(urdf_string)
+    fid.close()
+    print("Finished saving")
+
+urdf_filename = "/home/ubuntu/partnr-planner/data/robots/hab_stretch/urdf/hab_stretch.urdf"
+
+non_fixed_joints = [
+    "joint_lift",
+    "joint_arm_l0",
+    "joint_arm_l1",
+    "joint_arm_l2",
+    "joint_arm_l3",
+    "joint_wrist_yaw",
+    "joint_wrist_pitch",
+    "joint_wrist_roll",
+]
+
+ik_joint_limits = {
+        "joint_mobile_base_translation": (-1, 1),
+        # "joint_mobile_base_rotation": (-(np.pi / 2.0), np.pi / 2.0),
+        "joint_lift": (0.0, 1.1),
+        "joint_arm_l0":(0.0, 0.13),
+        "joint_arm_l1":(0.0, 0.13),
+        "joint_arm_l2":(0.0, 0.13),
+        "joint_arm_l3":(0.0, 0.13),
+        "joint_wrist_yaw": (-1.75, 4),
+        "joint_wrist_pitch": (-1.57, 0.56),
+        "joint_wrist_roll": (-3.14, 3.14),
+    }
+
+print()
+print("Loading URDF from:")
+print(urdf_filename)
+print("The specialized URDFs will be derived from this URDF.")
+robot = ud.Robot.from_xml_file(urdf_filename)
+
+# Change any joint that should be immobile for end effector IK into a fixed joint
+for j in robot.joint_map.keys():
+    if j not in non_fixed_joints:
+        joint = robot.joint_map[j]
+        # print('(joint name, joint type) =', (joint.name, joint.type))
+        joint.type = "fixed"
+
+robot_rotary = robot
+robot_prismatic = deepcopy(robot)
+
+###############################################
+# ADD VIRTUAL ROTARY JOINT FOR MOBILE BASE
+
+# Add a virtual base link
+link_virtual_base_rotary = ud.Link(
+    name="virtual_base", visual=None, inertial=None, collision=None, origin=None
+)
+
+# Add rotary joint for the mobile base
+origin_rotary = ud.Pose(xyz=[0, 0, 0], rpy=[0, 0, 0])
+
+limit_rotary = ud.JointLimit(effort=10, velocity=1, lower=-np.pi, upper=np.pi)
+
+joint_mobile_base_rotation = ud.Joint(
+    name="joint_mobile_base_rotation",
+    parent="virtual_base",
+    child="base_link",
+    joint_type="revolute",
+    axis=[0, 0, 1],
+    origin=origin_rotary,
+    limit=limit_rotary,
+    dynamics=None,
+    safety_controller=None,
+    calibration=None,
+    mimic=None,
+)
+
+robot_rotary.add_link(link_virtual_base_rotary)
+robot_rotary.add_joint(joint_mobile_base_rotation)
+###############################################
+
+
+###############################################
+# ADD VIRTUAL PRISMATIC JOINT FOR MOBILE BASE
+
+# Add a virtual base link
+link_virtual_base_prismatic = ud.Link(
+    name="virtual_base", visual=None, inertial=None, collision=None, origin=None
+)
+
+# Add rotary joint for the mobile base
+origin_prismatic = ud.Pose(xyz=[0, 0, 0], rpy=[0, 0, 0])
+
+limit_prismatic = ud.JointLimit(effort=10, velocity=1, lower=-1.0, upper=1.0)
+
+joint_mobile_base_translation = ud.Joint(
+    name="joint_mobile_base_translation",
+    parent="virtual_base",
+    child="base_link",
+    joint_type="prismatic",
+    axis=[1, 0, 0],
+    origin=origin_prismatic,
+    limit=limit_prismatic,
+    dynamics=None,
+    safety_controller=None,
+    calibration=None,
+    mimic=None,
+)
+
+robot_prismatic.add_link(link_virtual_base_prismatic)
+robot_prismatic.add_joint(joint_mobile_base_translation)
+
+###############################################
+
+# When specified, this sets more conservative joint limits than the
+# original URDF. Joint limits that are outside the originally
+# permitted range are clipped to the original range. Joint limits
+# with a value of None are set to the original limit.
+for robot in [robot_rotary, robot_prismatic]:
+    for j in ik_joint_limits:
+        joint = robot.joint_map.get(j, None)
+        if joint is not None:
+
+            original_upper = joint.limit.upper
+            requested_upper = ik_joint_limits[j][1]
+            # print()
+            # print('joint =', j)
+            # print('original_upper =', original_upper)
+            # print('requested_upper =', requested_upper)
+            if requested_upper is not None:
+                new_upper = min(requested_upper, original_upper)
+                # print('new_upper =', new_upper)
+                robot.joint_map[j].limit.upper = new_upper
+                # print()
+
+            original_lower = joint.limit.lower
+            requested_lower = ik_joint_limits[j][0]
+            if requested_lower is not None:
+                new_lower = max(requested_lower, original_lower)
+                robot.joint_map[j].limit.lower = new_lower
+
+
+# print('************************************************')
+# print('after adding link and joint: robot =', robot)
+# print('************************************************')
+
+print()
+save_urdf_file(robot_rotary, "stretch_base_rotation_ik.urdf")
+save_urdf_file(robot_prismatic, "stretch_base_translation_ik.urdf")
+
+
+# Create versions with fixed wrists
+
+for robot in [robot_rotary, robot_prismatic]:
+    print("Prepare URDF with a fixed wrist.")
+    non_fixed_joints = [
+        "joint_mobile_base_translation",
+        "joint_mobile_base_rotation",
+        "joint_lift",
+        "joint_arm_l0",
+    ]
+
+    # Change any joint that should be immobile for end effector IK into a fixed joint
+    for j in robot.joint_map.keys():
+        if j not in non_fixed_joints:
+            joint = robot.joint_map[j]
+            # print('(joint name, joint type) =', (joint.name, joint.type))
+            joint.type = "fixed"
+
+save_urdf_file(robot_rotary, "stretch_base_rotation_ik_with_fixed_wrist.urdf")
+save_urdf_file(robot_prismatic, "stretch_base_translation_ik_with_fixed_wrist.urdf")

habitat-lab/habitat/articulated_agents/manipulator.py

@@ -118,6 +118,7 @@ def reconfigure(self) -> None:
             )
             for i in self.params.arm_joints:
                 self.sim_obj.update_joint_motor(self.joint_motors[i][0], jms)
+                print("arm_jointsupdate_joint_motor")
         self._update_motor_settings_cache()
 
         # set correct gains for grippers
@@ -131,6 +132,7 @@ def reconfigure(self) -> None:
             )
             for i in self.params.gripper_joints:
                 self.sim_obj.update_joint_motor(self.joint_motors[i][0], jms)
+                print("gripper_jointsupdate_joint_motor")
         self._update_motor_settings_cache()
 
         # set initial states and targets
@@ -256,7 +258,7 @@ def calculate_ee_inverse_kinematics(
             "Currently no implementation for generic IK."
         )
 
-    def ee_transform(self, ee_index: int = 0) -> mn.Matrix4:
+    def ee_transform(self, ee_index: int = 0,xytest: int=None) -> mn.Matrix4:
         """Gets the transformation of the end-effector location. This is offset
         from the end-effector link location.
 
@@ -267,6 +269,13 @@ def ee_transform(self, ee_index: int = 0) -> mn.Matrix4:
             raise ValueError(
                 "The current manipulator does not have enough end effectors"
             )
+
+        if xytest:
+            ef_link_transform = self.sim_obj.get_link_scene_node(
+            xytest
+        ).transformation
+            return ef_link_transform
+
 
         ef_link_transform = self.sim_obj.get_link_scene_node(
             self.params.ee_links[ee_index]