More updates

omnigibson/action_primitives/starter_semantic_action_primitives.py

@@ -136,6 +136,7 @@ def _assemble_robot_copy(self):
             robot_urdf_path=self.robot.urdf_path,
         )
 
+        # TODO: Remove the need for this after refactoring the FK / descriptors / etc.
         arm_links = self.robot.manipulation_link_names
 
         if TORSO_FIXED:
@@ -506,23 +507,23 @@ def _open_or_close(self, obj, should_open):
                 yield from self._navigate_if_needed(obj, pose_on_obj=approach_pose)
 
                 if should_open:
-                    yield from self._move_hand_direct_cartesian(approach_pose, ignore_failure=False, stop_on_contact=True, stop_if_stuck=True)
+                    yield from self._move_hand_linearly_cartesian(approach_pose, ignore_failure=False, stop_on_contact=True, stop_if_stuck=True)
                 else:
-                    yield from self._move_hand_direct_cartesian(approach_pose, ignore_failure=False, stop_if_stuck=True)
+                    yield from self._move_hand_linearly_cartesian(approach_pose, ignore_failure=False, stop_if_stuck=True)
 
                 # Step once to update
                 empty_action = self._empty_action()
                 yield self._postprocess_action(empty_action)
 
                 for i, target_pose in enumerate(target_poses):
-                    yield from self._move_hand_direct_cartesian(target_pose, ignore_failure=False, stop_if_stuck=True)
+                    yield from self._move_hand_linearly_cartesian(target_pose, ignore_failure=False, stop_if_stuck=True)
 
                 # Moving to target pose often fails. This might leave the robot's motors with torques that
                 # try to get to a far-away position thus applying large torques, but unable to move due to
                 # the sticky grasp joint. Thus if we release the joint, the robot might suddenly launch in an
                 # arbitrary direction. To avoid this, we command the hand to apply torques with its current
                 # position as its target. This prevents the hand from jerking into some other position when we do a release.
-                yield from self._move_hand_direct_cartesian(
+                yield from self._move_hand_linearly_cartesian(
                     self.robot.eef_links[self.arm].get_position_orientation(), 
                     ignore_failure=True,
                     stop_if_stuck=True
@@ -649,7 +650,7 @@ def _grasp(self, obj):
         # Since the grasp pose is slightly off the object, we want to move towards the object, around 5cm.
         # It's okay if we can't go all the way because we run into the object.
         indented_print("Performing grasp approach")
-        yield from self._move_hand_direct_cartesian(approach_pose, stop_on_contact=True)
+        yield from self._move_hand_linearly_cartesian(approach_pose, stop_on_contact=True)
 
         # Step once to update
         empty_action = self._empty_action()
@@ -960,14 +961,13 @@ def _add_linearly_interpolated_waypoints(self, plan, max_inter_dist):
         interpolated_plan.append(plan[-1].tolist())
         return interpolated_plan
 
-    def _compute_delta_command(self, controller_name, diff_joint_pos, gain=1.0, min_action=0.0):
+    def _compute_delta_command(self, diff_joint_pos, gain=1.0, min_action=0.0):
         # for joints not within thresh, set a minimum action value
         # for joints within thres, set action to zero
         delta_action = gain * diff_joint_pos
         delta_action[abs(delta_action) < min_action] = np.sign(delta_action[abs(delta_action) < min_action]) * min_action
         delta_action[abs(diff_joint_pos) < JOINT_POS_DIFF_THRESHOLD] = 0.0
-        # TODO - this is temporary
-        if TORSO_FIXED:
+        if not TORSO_FIXED:
             delta_action = np.concatenate([np.zeros(1), delta_action])
 
         return delta_action
@@ -1042,9 +1042,7 @@ def _move_hand_direct_ik(self, target_pose, stop_on_contact=False, ignore_failur
         # make sure controller is InverseKinematicsController and in expected mode
         controller_config = self.robot._controller_config["arm_" + self.arm]
         assert controller_config["name"] == "InverseKinematicsController", "Controller must be InverseKinematicsController"
-        # assert controller_config["motor_type"] == "velocity", "Controller must be in velocity mode"
         assert controller_config["mode"] == "pose_absolute_ori", "Controller must be in pose_delta_ori mode"
-        # target pose = (position, quat) IN WORLD FRAME
         if in_world_frame:
             target_pose = self._get_pose_in_robot_frame(target_pose)
         target_pos = target_pose[0]
@@ -1093,7 +1091,7 @@ def _move_hand_direct_ik(self, target_pose, stop_on_contact=False, ignore_failur
                 "Your hand was obstructed from moving to the desired joint position"
             )      
 
-    def _move_hand_direct_cartesian(self, target_pose, stop_on_contact=False, ignore_failure=False, stop_if_stuck=False):
+    def _move_hand_linearly_cartesian(self, target_pose, stop_on_contact=False, ignore_failure=False, stop_if_stuck=False):
         """
         Yields action for the robot to move its arm to reach the specified target pose by moving the eef along a line in cartesian
         space from its current pose

omnigibson/controllers/ik_controller.py

@@ -274,6 +274,8 @@ def _command_to_control(self, command, control_dict):
 
         # Calculate and return IK-backed out joint angles
         current_joint_pos = control_dict["joint_position"][self.dof_idx]
+
+        # TODO: Put anti-drift code in here
         target_joint_pos = self.solver.solve(
             target_pos=target_pos,
             target_quat=target_quat,

omnigibson/robots/tiago.py

@@ -678,6 +678,7 @@ def simplified_mesh_usd_path(self):
 
     @property
     def robot_arm_descriptor_yamls(self):
+        # TODO: Remove the need to do this by making the arm descriptor yaml files generated automatically
         return {"left": os.path.join(gm.ASSET_PATH, "models/tiago/tiago_dual_omnidirectional_stanford_left_arm_descriptor.yaml"),
                 "left_fixed": os.path.join(gm.ASSET_PATH, "models/tiago/tiago_dual_omnidirectional_stanford_left_arm_fixed_trunk_descriptor.yaml"),
                 "right": os.path.join(gm.ASSET_PATH, "models/tiago/tiago_dual_omnidirectional_stanford_right_arm_fixed_trunk_descriptor.yaml"),

omnigibson/utils/control_utils.py

@@ -35,6 +35,7 @@ def get_link_poses(
         Returns:
             link_poses (dict): Dictionary mapping each robot link name to its pose
         """
+        # TODO: Refactor this to go over all links at once
         link_poses = {}
         for link_name in link_names:
             pose3_lula = self.kinematics.pose(joint_positions, link_name)

omnigibson/utils/motion_planning_utils.py

@@ -2,11 +2,15 @@
 from math import ceil
 
 import omnigibson as og
+from omnigibson.macros import create_module_macros
 from omnigibson.object_states import ContactBodies
 import omnigibson.utils.transform_utils as T
 from omnigibson.utils.control_utils import IKSolver
 from pxr import PhysicsSchemaTools, Gf
 
+m = create_module_macros(module_path=__file__)
+m.ANGLE_DIFF = 0.3
+m.DIST_DIFF = 0.1
 
 def _wrap_angle(theta):
     """"
@@ -26,7 +30,6 @@ def plan_base_motion(
     end_conf,
     context,
     planning_time=15.0,
-    **kwargs
 ):
     """
     Plans a base motion to a 2d pose
@@ -43,9 +46,6 @@ def plan_base_motion(
     from ompl import base as ob
     from ompl import geometric as ompl_geo
 
-    ANGLE_DIFF = 0.3
-    DIST_DIFF = 0.1
-
     class CustomMotionValidator(ob.MotionValidator):
 
         def __init__(self, si, space):
@@ -67,7 +67,7 @@ def checkMotion(self, s1, s2):
 
             # Navigation
             dist = np.linalg.norm(goal[:2] - start[:2])
-            num_points = ceil(dist / DIST_DIFF) + 1
+            num_points = ceil(dist / m.DIST_DIFF) + 1
             nav_x = np.linspace(start[0], goal[0], num_points).tolist()
             nav_y = np.linspace(start[1], goal[1], num_points).tolist()
             for i in range(num_points):
@@ -86,7 +86,7 @@ def is_valid_rotation(si, start_conf, final_orientation):
             diff = _wrap_angle(final_orientation - start_conf[2])
             direction = np.sign(diff)
             diff = abs(diff)
-            num_points = ceil(diff / ANGLE_DIFF) + 1
+            num_points = ceil(diff / m.ANGLE_DIFF) + 1
             nav_angle = np.linspace(0.0, diff, num_points) * direction
             angles = nav_angle + start_conf[2]
             for i in range(num_points):
@@ -210,7 +210,6 @@ def plan_arm_motion(
     context,
     planning_time=15.0,
     torso_fixed=True,
-    **kwargs
 ):
     """
     Plans an arm motion to a final joint position
@@ -311,7 +310,6 @@ def plan_arm_motion_ik(
     context,
     planning_time=15.0,
     torso_fixed=True,
-    **kwargs
 ):
     """
     Plans an arm motion to a final end effector pose