Feat/robot fixes

docs/tutorials/custom_robot_import.md

@@ -494,28 +494,6 @@ Now that we have the USD file for the robot, let's write our own robot class. Fo
         def wheel_axle_length(self):
             return 0.330
 
-        @property
-        def finger_lengths(self):
-            return {self.default_arm: 0.04}
-
-        @property
-        def assisted_grasp_start_points(self):
-            return {
-                self.default_arm: [
-                    GraspingPoint(link_name="link_gripper_finger_right", position=th.tensor([0.013, 0.0, 0.01])),
-                    GraspingPoint(link_name="link_gripper_finger_right", position=th.tensor([-0.01, 0.0, 0.009])),
-                ]
-            }
-
-        @property
-        def assisted_grasp_end_points(self):
-            return {
-                self.default_arm: [
-                    GraspingPoint(link_name="link_gripper_finger_left", position=th.tensor([0.013, 0.0, 0.01])),
-                    GraspingPoint(link_name="link_gripper_finger_left", position=th.tensor([-0.01, 0.0, 0.009])),
-                ]
-            }
-
         @property
         def disabled_collision_pairs(self):
             return [

docs/tutorials/demo_collection.md

@@ -101,7 +101,7 @@ playback_env = DataPlaybackWrapper.create_from_hdf5(
 )
 
 # Playback the entire dataset and record observations
-playback_env.playback_dataset(record=True)
+playback_env.playback_dataset(record_data=True)
 
 # Save the recorded playback data
 playback_env.save_data()

omnigibson/action_primitives/starter_semantic_action_primitives.py

@@ -102,7 +102,7 @@
 m.LOW_PRECISION_DIST_THRESHOLD = 0.1
 m.LOW_PRECISION_ANGLE_THRESHOLD = 0.2
 
-m.JOINT_POS_DIFF_THRESHOLD = 0.01
+m.JOINT_POS_DIFF_THRESHOLD = 0.005
 m.LOW_PRECISION_JOINT_POS_DIFF_THRESHOLD = 0.05
 m.JOINT_CONTROL_MIN_ACTION = 0.0
 m.MAX_ALLOWED_JOINT_ERROR_FOR_LINEAR_MOTION = math.radians(45)
@@ -506,7 +506,10 @@ def _sample_grasp_pose(self, obj):
         # Identity quaternion for top-down grasping (x-forward, y-right, z-down)
         approach_dir = T.quat2mat(grasp_quat) @ th.tensor([0.0, 0.0, -1.0])
 
-        pregrasp_offset = self.robot.finger_lengths[self.arm] / 2.0 + m.GRASP_APPROACH_DISTANCE
+        avg_finger_offset = th.mean(
+            th.tensor([length for length in self.robot.eef_to_fingertip_lengths[self.arm].values()])
+        )
+        pregrasp_offset = avg_finger_offset + m.GRASP_APPROACH_DISTANCE
 
         pregrasp_pos = grasp_pos - approach_dir * pregrasp_offset
 
@@ -581,7 +584,7 @@ def _grasp(self, obj):
             # By default, it's NOT the z-axis of the world frame unless `project_pose_to_goal_frame=False` is set in curobo.
             # For sticky grasping, we also need to ignore the object during motion planning because the fingers are already closed.
             yield from self._move_hand(
-                grasp_pose, motion_constraint=[1, 1, 1, 1, 1, 0], stop_on_contact=True, ignore_objects=[obj]
+                grasp_pose, motion_constraint=[1, 1, 1, 1, 1, 0], stop_on_ag=True, ignore_objects=[obj]
             )
         elif self.robot.grasping_mode == "assisted":
             indented_print("Assisted grasping: approach")
@@ -853,6 +856,7 @@ def _move_hand(
         self,
         target_pose,
         stop_on_contact=False,
+        stop_on_ag=False,
         motion_constraint=None,
         low_precision=False,
         lock_auxiliary_arm=False,
@@ -864,6 +868,7 @@ def _move_hand(
         Args:
             target_pose (Tuple[th.tensor, th.tensor]): target pose to reach for the default end-effector in the world frame
             stop_on_contact (bool): Whether to stop executing motion plan if contact is detected
+            stop_on_ag (bool): Whether to stop executing motion plan if assisted grasping is activated
             motion_constraint (MotionConstraint): Motion constraint for the motion
             low_precision (bool): Whether to use low precision for the motion
             lock_auxiliary_arm (bool): Whether to lock the other arm in place
@@ -906,7 +911,9 @@ def _move_hand(
         )
 
         indented_print(f"Plan has {len(q_traj)} steps")
-        yield from self._execute_motion_plan(q_traj, stop_on_contact=stop_on_contact, low_precision=low_precision)
+        yield from self._execute_motion_plan(
+            q_traj, stop_on_contact=stop_on_contact, stop_on_ag=stop_on_ag, low_precision=low_precision
+        )
 
     def _plan_joint_motion(
         self,
@@ -972,7 +979,13 @@ def _plan_joint_motion(
         return q_traj
 
     def _execute_motion_plan(
-        self, q_traj, stop_on_contact=False, ignore_failure=False, low_precision=False, ignore_physics=False
+        self,
+        q_traj,
+        stop_on_contact=False,
+        stop_on_ag=False,
+        ignore_failure=False,
+        low_precision=False,
+        ignore_physics=False,
     ):
         for i, joint_pos in enumerate(q_traj):
             # indented_print(f"Executing motion plan step {i + 1}/{len(q_traj)}")
@@ -992,6 +1005,12 @@ def _execute_motion_plan(
                 articulation_control_idx = th.cat(articulation_control_idx)
                 for j in range(m.MAX_STEPS_FOR_JOINT_MOTION):
                     # indented_print(f"Step {j + 1}/{m.MAX_STEPS_FOR_JOINT_MOTION}")
+
+                    # We need to call @q_to_action for every step because as the robot base moves, the same base joint_pos will be
+                    # converted to different actions, since HolonomicBaseJointController accepts an action in the robot local frame.
+                    action = self.robot.q_to_action(joint_pos)
+                    yield self._postprocess_action(action)
+
                     current_joint_pos = self.robot.get_joint_positions()
                     joint_pos_diff = joint_pos - current_joint_pos
                     base_joint_diff = joint_pos_diff[self.robot.base_control_idx]
@@ -1014,14 +1033,10 @@ def _execute_motion_plan(
 
                     collision_detected = detect_robot_collision_in_sim(self.robot)
                     if stop_on_contact and collision_detected:
-                        indented_print(f"Collision detected at step {i + 1}/{len(q_traj)}")
                         return
 
-                    # We need to call @q_to_action for every step because as the robot base moves, the same base joint_pos will be
-                    # converted to different actions, since HolonomicBaseJointController accepts an action in the robot local frame.
-                    action = self.robot.q_to_action(joint_pos)
-
-                    yield self._postprocess_action(action)
+                    if stop_on_ag and self._get_obj_in_hand() is not None:
+                        return
 
                 if not ignore_failure:
                     if not base_target_reached:
@@ -1334,13 +1349,13 @@ def _move_fingers_to_limit(self, limit_type):
         target_joint_positions = self._get_joint_position_with_fingers_at_limit(limit_type)
         action = self.robot.q_to_action(target_joint_positions)
         for _ in range(m.MAX_STEPS_FOR_GRASP_OR_RELEASE):
-            current_joint_positinos = self.robot.get_joint_positions()
-            if th.allclose(current_joint_positinos, target_joint_positions, atol=0.005):
+            yield self._postprocess_action(action)
+            current_joint_positions = self.robot.get_joint_positions()
+            if th.allclose(current_joint_positions, target_joint_positions, atol=m.JOINT_POS_DIFF_THRESHOLD):
                 break
             elif limit_type == "lower" and self._get_obj_in_hand() is not None:
                 # If we are grasping an object, we should stop when object is detected in hand
                 break
-            yield self._postprocess_action(action)
 
     def _execute_grasp(self):
         """

omnigibson/configs/fetch_behavior.yaml

@@ -40,6 +40,8 @@ scene:
 robots:
   - type: Fetch
     obs_modalities: [scan, rgb, depth]
+    include_sensor_names: null
+    exclude_sensor_names: null
     scale: 1.0
     self_collision: false
     action_normalize: true

omnigibson/configs/r1_primitives.yaml

@@ -32,6 +32,8 @@ scene:
 robots:
   - type: R1
     obs_modalities: [rgb]
+    include_sensor_names: null
+    exclude_sensor_names: null
     scale: 1.0
     self_collisions: true
     action_normalize: false

omnigibson/configs/robots/fetch.yaml

@@ -17,6 +17,8 @@ robot:
   self_collision: true
   grasping_mode: physical
   default_arm_pose: vertical
+  include_sensor_names: null
+  exclude_sensor_names: null
   sensor_config:
     VisionSensor:
       sensor_kwargs:

omnigibson/configs/robots/freight.yaml

@@ -10,6 +10,8 @@ robot:
   base_name: null
   scale: 1.0
   self_collision: false
+  include_sensor_names: null
+  exclude_sensor_names: null
   sensor_config:
     VisionSensor:
       sensor_kwargs:

omnigibson/configs/robots/husky.yaml

@@ -10,6 +10,8 @@ robot:
   base_name: null
   scale: 1.0
   self_collision: false
+  include_sensor_names: null
+  exclude_sensor_names: null
   sensor_config:
     VisionSensor:
       sensor_kwargs:

omnigibson/configs/robots/locobot.yaml

@@ -10,6 +10,8 @@ robot:
   base_name: null
   scale: 1.0
   self_collision: false
+  include_sensor_names: null
+  exclude_sensor_names: null
   sensor_config:
     VisionSensor:
       sensor_kwargs:

omnigibson/configs/robots/turtlebot.yaml

@@ -10,6 +10,8 @@ robot:
   base_name: null
   scale: 1.0
   self_collision: false
+  include_sensor_names: null
+  exclude_sensor_names: null
   sensor_config:
     VisionSensor:
       sensor_kwargs:

omnigibson/configs/tiago_primitives.yaml

@@ -33,6 +33,8 @@ scene:
 robots:
   - type: Tiago
     obs_modalities: [rgb]
+    include_sensor_names: null
+    exclude_sensor_names: null
     scale: 1.0
     self_collisions: true
     action_normalize: false

omnigibson/configs/turtlebot_nav.yaml

@@ -30,6 +30,8 @@ scene:
 robots:
   - type: Turtlebot
     obs_modalities: [scan, rgb, depth]
+    include_sensor_names: null
+    exclude_sensor_names: null
     scale: 1.0
     self_collision: false
     action_normalize: true

omnigibson/controllers/ik_controller.py

@@ -53,7 +53,7 @@ def __init__(
         pos_kp=None,
         pos_damping_ratio=None,
         vel_kp=None,
-        use_impedances=True,
+        use_impedances=False,
         mode="pose_delta_ori",
         smoothing_filter_size=None,
         workspace_pose_limiter=None,

omnigibson/controllers/joint_controller.py

@@ -13,7 +13,7 @@
 )
 from omnigibson.macros import create_module_macros
 from omnigibson.utils.backend_utils import _compute_backend as cb
-from omnigibson.utils.backend_utils import _ComputeBackend, _ComputeNumpyBackend, _ComputeTorchBackend
+from omnigibson.utils.backend_utils import add_compute_function
 from omnigibson.utils.python_utils import assert_valid_key
 from omnigibson.utils.ui_utils import create_module_logger
 
@@ -223,7 +223,7 @@ def compute_control(self, goal_dict, control_dict):
             else:  # effort
                 u = target
 
-            u = cb.compute_joint_torques(u, control_dict["mass_matrix"], self.dof_idx)
+            u = cb.get_custom_method("compute_joint_torques")(u, control_dict["mass_matrix"], self.dof_idx)
 
             # Add gravity compensation
             if self._use_gravity_compensation:
@@ -341,6 +341,6 @@ def _compute_joint_torques_numpy(
 
 
 # Set these as part of the backend values
-setattr(_ComputeBackend, "compute_joint_torques", None)
-setattr(_ComputeTorchBackend, "compute_joint_torques", _compute_joint_torques_torch)
-setattr(_ComputeNumpyBackend, "compute_joint_torques", _compute_joint_torques_numpy)
+add_compute_function(
+    name="compute_joint_torques", np_function=_compute_joint_torques_numpy, th_function=_compute_joint_torques_torch
+)

omnigibson/controllers/multi_finger_gripper_controller.py

@@ -126,8 +126,15 @@ def _generate_default_command_output_limits(self):
         if self._mode == "binary":
             command_output_limits = (-1.0, 1.0)
         # If we're in smoothing mode, output limits should be the average of the independent limits
-        elif self._mode == "smoothing":
-            command_output_limits = cb.mean(command_output_limits[0]), cb.mean(command_output_limits[1])
+        elif self._mode == "smooth":
+            command_output_limits = (
+                cb.mean(command_output_limits[0]),
+                cb.mean(command_output_limits[1]),
+            )
+        elif self._mode == "independent":
+            pass
+        else:
+            raise ValueError(f"Invalid mode {self._mode}")
 
         return command_output_limits