From 510252b85e5fe4c8045f8de1d9f730d7bb928e06 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 27 Oct 2023 11:49:23 -0700 Subject: [PATCH] More updates --- .../starter_semantic_action_primitives.py | 20 +++++++++---------- omnigibson/controllers/ik_controller.py | 2 ++ omnigibson/robots/tiago.py | 1 + omnigibson/utils/control_utils.py | 1 + omnigibson/utils/motion_planning_utils.py | 14 ++++++------- 5 files changed, 19 insertions(+), 19 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index b4b4a750b..e35646afe 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/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 diff --git a/omnigibson/controllers/ik_controller.py b/omnigibson/controllers/ik_controller.py index 9f43677d5..214e03e99 100644 --- a/omnigibson/controllers/ik_controller.py +++ b/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, diff --git a/omnigibson/robots/tiago.py b/omnigibson/robots/tiago.py index 630c34d61..a2c4943e8 100644 --- a/omnigibson/robots/tiago.py +++ b/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"), diff --git a/omnigibson/utils/control_utils.py b/omnigibson/utils/control_utils.py index 9935439d8..6ac070ebd 100644 --- a/omnigibson/utils/control_utils.py +++ b/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) diff --git a/omnigibson/utils/motion_planning_utils.py b/omnigibson/utils/motion_planning_utils.py index 60e5fbb05..6a5f99bb1 100644 --- a/omnigibson/utils/motion_planning_utils.py +++ b/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