From 3cd597832ccf335a2a0a56de2367aa598720fbc3 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Sun, 5 Nov 2023 16:18:15 -0800 Subject: [PATCH 1/5] wip --- .../starter_semantic_action_primitives.py | 8 ++++---- omnigibson/objects/controllable_object.py | 4 +++- tests/test_primitives.py | 1 + 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index eecce4954..d39f86a49 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -5,7 +5,7 @@ See provided tiago_primitives.yaml config file for an example. See examples/action_primitives for runnable examples. """ -from functools import cached_property +# from functools import cached_property import inspect import logging import random @@ -829,7 +829,7 @@ def _target_in_reach_of_robot_relative(self, relative_target_pose): """ return self._ik_solver_cartesian_to_joint_space(relative_target_pose) is not None - @cached_property + # @cached_property def _manipulation_control_idx(self): """The appropriate manipulation control idx for the current settings.""" if isinstance(self.robot, Tiago): @@ -842,7 +842,7 @@ def _manipulation_control_idx(self): # Otherwise just return the default arm control idx return self.robot.arm_control_idx[self.arm] - @cached_property + # @cached_property def _manipulation_descriptor_path(self): """The appropriate manipulation descriptor for the current settings.""" if isinstance(self.robot, Tiago) and m.TIAGO_TORSO_FIXED: @@ -1739,7 +1739,7 @@ def _test_pose(self, pose_2d, context, pose_on_obj=None): Args: pose_2d (Iterable): (x, y, yaw) 2d pose - context (Context): Undoable context reference + context (Context): Planning context reference pose_on_obj (Iterable of arrays): Pose on the object in the world frame Returns: diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index 5b51281ce..2e129d03e 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -172,7 +172,9 @@ def _load_controllers(self): # If we're using normalized action space, override the inputs for all controllers if self._action_normalize: cfg["command_input_limits"] = "default" # default is normalized (-1, 1) - + if name == "gripper_left": + from IPython import embed; embed() + print(name) # Create the controller controller = create_controller(**cfg) # Verify the controller's DOFs can all be driven diff --git a/tests/test_primitives.py b/tests/test_primitives.py index cdfd52368..1ef00839c 100644 --- a/tests/test_primitives.py +++ b/tests/test_primitives.py @@ -242,3 +242,4 @@ def test_open_revolute(): assert primitive_tester(categories, objects, primitives, primitives_args) +test_navigate() \ No newline at end of file From dc2a9cec2932bd9a1a459c4cc07c17e871b749de Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Sun, 5 Nov 2023 19:10:00 -0800 Subject: [PATCH 2/5] fix primitive tests --- .../starter_semantic_action_primitives.py | 16 +++--- omnigibson/controllers/joint_controller.py | 7 ++- omnigibson/objects/controllable_object.py | 3 -- omnigibson/utils/motion_planning_utils.py | 1 + tests/test_primitives.py | 50 +++++++------------ 5 files changed, 34 insertions(+), 43 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index d39f86a49..e77039a95 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -64,7 +64,7 @@ m.MAX_STEPS_FOR_HAND_MOVE_JOINT = 500 m.MAX_STEPS_FOR_HAND_MOVE_IK = 1000 m.MAX_STEPS_FOR_GRASP_OR_RELEASE = 30 -m.MAX_STEPS_FOR_WAYPOINT_NAVIGATION = 200 +m.MAX_STEPS_FOR_WAYPOINT_NAVIGATION = 500 m.MAX_ATTEMPTS_FOR_OPEN_CLOSE = 20 m.MAX_ATTEMPTS_FOR_SAMPLING_POSE_WITH_OBJECT_AND_PREDICATE = 20 @@ -295,7 +295,6 @@ def _postprocess_action(self, action): action_type = "nav" context = action_type + context_function - # print(context) return action, context def _load_robot_copy(self): @@ -865,9 +864,9 @@ def _ik_solver_cartesian_to_joint_space(self, relative_target_pose): - np.array: Indices for joints in the robot """ ik_solver = IKSolver( - robot_description_path=self._manipulation_descriptor_path, + robot_description_path=self._manipulation_descriptor_path(), robot_urdf_path=self.robot.urdf_path, - default_joint_pos=self.robot.default_joint_pos[self._manipulation_control_idx], + default_joint_pos=self.robot.default_joint_pos[self._manipulation_control_idx()], eef_name=self.robot.eef_link_names[self.arm], ) # Grab the joint positions in order to reach the desired pose target @@ -1008,7 +1007,7 @@ def _move_hand_direct_joint(self, joint_pos, stop_on_contact=False, ignore_failu prev_eef_pos = np.zeros(3) for _ in range(m.MAX_STEPS_FOR_HAND_MOVE_JOINT): - current_joint_pos = self.robot.get_joint_positions()[self._manipulation_control_idx] + current_joint_pos = self.robot.get_joint_positions()[self._manipulation_control_idx()] diff_joint_pos = np.array(current_joint_pos) - np.array(joint_pos) if np.max(np.abs(diff_joint_pos)) < m.JOINT_POS_DIFF_THRESHOLD: return @@ -1168,10 +1167,10 @@ def _move_hand_linearly_cartesian(self, target_pose, stop_on_contact=False, igno for joint_pos in joint_space_data: # Check if the movement can be done roughly linearly. - current_joint_positions = self.robot.get_joint_positions()[self._manipulation_control_idx] + current_joint_positions = self.robot.get_joint_positions()[self._manipulation_control_idx()] failed_joints = [] - for joint_idx, target_joint_pos, current_joint_pos in zip(self._manipulation_control_idx, joint_pos, current_joint_positions): + for joint_idx, target_joint_pos, current_joint_pos in zip(self._manipulation_control_idx(), joint_pos, current_joint_positions): if np.abs(target_joint_pos - current_joint_pos) > m.MAX_ALLOWED_JOINT_ERROR_FOR_LINEAR_MOTION: failed_joints.append(joints[joint_idx].joint_name) @@ -1355,7 +1354,7 @@ def _reset_hand(self): yield from self._move_hand_direct_ik(reset_eef_pose, ignore_failure=True, in_world_frame=False) else: indented_print("Resetting hand") - reset_pose = self._get_reset_joint_pos()[self._manipulation_control_idx] + reset_pose = self._get_reset_joint_pos()[self._manipulation_control_idx()] try: yield from self._move_hand_joint(reset_pose) except ActionPrimitiveError: @@ -1507,6 +1506,7 @@ def _navigate_to_obj(self, obj, pose_on_obj=None, **kwargs): np.array or None: Action array for one step for the robot to navigate in range or None if it is done navigating """ pose = self._sample_pose_near_object(obj, pose_on_obj=pose_on_obj, **kwargs) + print(pose) yield from self._navigate_to_pose(pose) def _navigate_to_pose_direct(self, pose_2d, low_precision=False): diff --git a/omnigibson/controllers/joint_controller.py b/omnigibson/controllers/joint_controller.py index 902fb948b..2cfbd66fb 100644 --- a/omnigibson/controllers/joint_controller.py +++ b/omnigibson/controllers/joint_controller.py @@ -27,6 +27,7 @@ def __init__( command_output_limits="default", use_delta_commands=False, compute_delta_in_quat_space=None, + use_single_command=False, ): """ Args: @@ -56,6 +57,8 @@ def __init__( joints that need to be processed in quaternion space to avoid gimbal lock issues normally faced by 3 DOF rotation joints. Each group needs to consist of three idxes corresponding to the indices in the input space. This is only used in the delta_commands mode. + use_single_command (bool): whether all related joints should be controlled with one (scaled) input or + independent values individually. If True, then the inputted command should be a single value. """ # Store arguments assert_valid_key(key=motor_type.lower(), valid_keys=ControlType.VALID_TYPES_STR, name="motor_type") @@ -63,6 +66,8 @@ def __init__( self._use_delta_commands = use_delta_commands self._compute_delta_in_quat_space = [] if compute_delta_in_quat_space is None else compute_delta_in_quat_space + self._use_single_command = use_single_command + # When in delta mode, it doesn't make sense to infer output range using the joint limits (since that's an # absolute range and our values are relative). So reject the default mode option in that case. assert not ( @@ -146,4 +151,4 @@ def control_type(self): @property def command_dim(self): - return len(self.dof_idx) + return 1 if self._use_single_command else len(self.dof_idx) diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index 2e129d03e..7933dea03 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -172,9 +172,6 @@ def _load_controllers(self): # If we're using normalized action space, override the inputs for all controllers if self._action_normalize: cfg["command_input_limits"] = "default" # default is normalized (-1, 1) - if name == "gripper_left": - from IPython import embed; embed() - print(name) # Create the controller controller = create_controller(**cfg) # Verify the controller's DOFs can all be driven diff --git a/omnigibson/utils/motion_planning_utils.py b/omnigibson/utils/motion_planning_utils.py index 6a5f99bb1..3e3bef40e 100644 --- a/omnigibson/utils/motion_planning_utils.py +++ b/omnigibson/utils/motion_planning_utils.py @@ -187,6 +187,7 @@ def remove_unnecessary_rotations(path): ss.setStartAndGoalStates(start, goal) if not state_valid_fn(start()) or not state_valid_fn(goal()): + print('fail') return solved = ss.solve(planning_time) diff --git a/tests/test_primitives.py b/tests/test_primitives.py index 1ef00839c..d7271d0cf 100644 --- a/tests/test_primitives.py +++ b/tests/test_primitives.py @@ -66,6 +66,7 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg "command_input_limits": [-1, 1], "command_output_limits": None, "use_delta_commands": True, + "use_single_command": True }, "gripper_right": { "name": "JointController", @@ -73,10 +74,13 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg "command_input_limits": [-1, 1], "command_output_limits": None, "use_delta_commands": True, + "use_single_command": True }, "camera": { "name": "JointController", - "motor_type": "velocity", + "motor_type": "position", + "command_input_limits": None, + "command_output_limits": None, "use_delta_commands": False } } @@ -103,11 +107,11 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg og.sim.step() controller = StarterSemanticActionPrimitives(env) + set_start_pose(robot) for primitive, args in zip(primitives, primitives_args): try: - set_start_pose(robot) execute_controller(controller.apply_ref(primitive, *args), env) - except: + except Exception as e: og.sim.clear() return False @@ -116,17 +120,16 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg return True def test_navigate(): - categories = ["floors", "ceilings", "walls", "coffee_table"] + categories = ["floors", "ceilings", "walls"] objects = [] obj_1 = { "object": DatasetObject( - name="table", - category="breakfast_table", - model="rjgmmy", - scale=[0.3, 0.3, 0.3] - ), - "position": [-0.7, -2.0, 0.2], + name="cologne", + category="bottle_of_cologne", + model="lyipur" + ), + "position": [-0.3, -0.8, 0.5], "orientation": [0, 0, 0, 1] } objects.append(obj_1) @@ -141,30 +144,18 @@ def test_grasp(): objects = [] obj_1 = { - "object": DatasetObject( - name="table", - category="breakfast_table", - model="rjgmmy", - scale=[0.3, 0.3, 0.3] - ), - "position": [-0.7, 0.5, 0.2], - "orientation": [0, 0, 0, 1] - } - obj_2 = { "object": DatasetObject( name="cologne", - category="cologne", - model="lyipur", - scale=[0.01, 0.01, 0.01] + category="bottle_of_cologne", + model="lyipur" ), "position": [-0.3, -0.8, 0.5], "orientation": [0, 0, 0, 1] } objects.append(obj_1) - objects.append(obj_2) primitives = [StarterSemanticActionPrimitiveSet.GRASP] - primitives_args = [(obj_2['object'],)] + primitives_args = [(obj_1['object'],)] assert primitive_tester(categories, objects, primitives, primitives_args) @@ -185,9 +176,8 @@ def test_place(): obj_2 = { "object": DatasetObject( name="cologne", - category="cologne", - model="lyipur", - scale=[0.01, 0.01, 0.01] + category="bottle_of_cologne", + model="lyipur" ), "position": [-0.3, -0.8, 0.5], "orientation": [0, 0, 0, 1] @@ -240,6 +230,4 @@ def test_open_revolute(): primitives = [StarterSemanticActionPrimitiveSet.OPEN] primitives_args = [(obj_1['object'],)] - assert primitive_tester(categories, objects, primitives, primitives_args) - -test_navigate() \ No newline at end of file + assert primitive_tester(categories, objects, primitives, primitives_args) \ No newline at end of file From bcd1a3fe37469167c7516f2f1df8c9a54c05e61f Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Sun, 5 Nov 2023 19:20:31 -0800 Subject: [PATCH 3/5] clean up code --- .../starter_semantic_action_primitives.py | 19 +++++++++---------- omnigibson/objects/controllable_object.py | 1 + omnigibson/utils/motion_planning_utils.py | 1 - tests/test_primitives.py | 2 +- 4 files changed, 11 insertions(+), 12 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index e77039a95..e7a8d111c 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -5,7 +5,7 @@ See provided tiago_primitives.yaml config file for an example. See examples/action_primitives for runnable examples. """ -# from functools import cached_property +from functools import cached_property import inspect import logging import random @@ -828,7 +828,7 @@ def _target_in_reach_of_robot_relative(self, relative_target_pose): """ return self._ik_solver_cartesian_to_joint_space(relative_target_pose) is not None - # @cached_property + @cached_property def _manipulation_control_idx(self): """The appropriate manipulation control idx for the current settings.""" if isinstance(self.robot, Tiago): @@ -841,7 +841,7 @@ def _manipulation_control_idx(self): # Otherwise just return the default arm control idx return self.robot.arm_control_idx[self.arm] - # @cached_property + @cached_property def _manipulation_descriptor_path(self): """The appropriate manipulation descriptor for the current settings.""" if isinstance(self.robot, Tiago) and m.TIAGO_TORSO_FIXED: @@ -864,9 +864,9 @@ def _ik_solver_cartesian_to_joint_space(self, relative_target_pose): - np.array: Indices for joints in the robot """ ik_solver = IKSolver( - robot_description_path=self._manipulation_descriptor_path(), + robot_description_path=self._manipulation_descriptor_path, robot_urdf_path=self.robot.urdf_path, - default_joint_pos=self.robot.default_joint_pos[self._manipulation_control_idx()], + default_joint_pos=self.robot.default_joint_pos[self._manipulation_control_idx], eef_name=self.robot.eef_link_names[self.arm], ) # Grab the joint positions in order to reach the desired pose target @@ -1007,7 +1007,7 @@ def _move_hand_direct_joint(self, joint_pos, stop_on_contact=False, ignore_failu prev_eef_pos = np.zeros(3) for _ in range(m.MAX_STEPS_FOR_HAND_MOVE_JOINT): - current_joint_pos = self.robot.get_joint_positions()[self._manipulation_control_idx()] + current_joint_pos = self.robot.get_joint_positions()[self._manipulation_control_idx] diff_joint_pos = np.array(current_joint_pos) - np.array(joint_pos) if np.max(np.abs(diff_joint_pos)) < m.JOINT_POS_DIFF_THRESHOLD: return @@ -1167,10 +1167,10 @@ def _move_hand_linearly_cartesian(self, target_pose, stop_on_contact=False, igno for joint_pos in joint_space_data: # Check if the movement can be done roughly linearly. - current_joint_positions = self.robot.get_joint_positions()[self._manipulation_control_idx()] + current_joint_positions = self.robot.get_joint_positions()[self._manipulation_control_idx] failed_joints = [] - for joint_idx, target_joint_pos, current_joint_pos in zip(self._manipulation_control_idx(), joint_pos, current_joint_positions): + for joint_idx, target_joint_pos, current_joint_pos in zip(self._manipulation_control_idx, joint_pos, current_joint_positions): if np.abs(target_joint_pos - current_joint_pos) > m.MAX_ALLOWED_JOINT_ERROR_FOR_LINEAR_MOTION: failed_joints.append(joints[joint_idx].joint_name) @@ -1354,7 +1354,7 @@ def _reset_hand(self): yield from self._move_hand_direct_ik(reset_eef_pose, ignore_failure=True, in_world_frame=False) else: indented_print("Resetting hand") - reset_pose = self._get_reset_joint_pos()[self._manipulation_control_idx()] + reset_pose = self._get_reset_joint_pos()[self._manipulation_control_idx] try: yield from self._move_hand_joint(reset_pose) except ActionPrimitiveError: @@ -1506,7 +1506,6 @@ def _navigate_to_obj(self, obj, pose_on_obj=None, **kwargs): np.array or None: Action array for one step for the robot to navigate in range or None if it is done navigating """ pose = self._sample_pose_near_object(obj, pose_on_obj=pose_on_obj, **kwargs) - print(pose) yield from self._navigate_to_pose(pose) def _navigate_to_pose_direct(self, pose_2d, low_precision=False): diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index 7933dea03..709bde2e2 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -172,6 +172,7 @@ def _load_controllers(self): # If we're using normalized action space, override the inputs for all controllers if self._action_normalize: cfg["command_input_limits"] = "default" # default is normalized (-1, 1) + # Create the controller controller = create_controller(**cfg) # Verify the controller's DOFs can all be driven diff --git a/omnigibson/utils/motion_planning_utils.py b/omnigibson/utils/motion_planning_utils.py index 3e3bef40e..6a5f99bb1 100644 --- a/omnigibson/utils/motion_planning_utils.py +++ b/omnigibson/utils/motion_planning_utils.py @@ -187,7 +187,6 @@ def remove_unnecessary_rotations(path): ss.setStartAndGoalStates(start, goal) if not state_valid_fn(start()) or not state_valid_fn(goal()): - print('fail') return solved = ss.solve(planning_time) diff --git a/tests/test_primitives.py b/tests/test_primitives.py index d7271d0cf..b0459093a 100644 --- a/tests/test_primitives.py +++ b/tests/test_primitives.py @@ -111,7 +111,7 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg for primitive, args in zip(primitives, primitives_args): try: execute_controller(controller.apply_ref(primitive, *args), env) - except Exception as e: + except: og.sim.clear() return False From af782289b4cbdae4d725fa0845fda648d6597c57 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Sun, 5 Nov 2023 19:21:45 -0800 Subject: [PATCH 4/5] clean up code --- omnigibson/objects/controllable_object.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index 709bde2e2..5b51281ce 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -172,7 +172,7 @@ def _load_controllers(self): # If we're using normalized action space, override the inputs for all controllers if self._action_normalize: cfg["command_input_limits"] = "default" # default is normalized (-1, 1) - + # Create the controller controller = create_controller(**cfg) # Verify the controller's DOFs can all be driven From 09ea6b5ddd587778641b81313d8882d228dac08e Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Sun, 5 Nov 2023 21:52:21 -0800 Subject: [PATCH 5/5] remove command dim assertion and revert joint controller --- omnigibson/controllers/joint_controller.py | 7 +------ omnigibson/robots/manipulation_robot.py | 5 ----- tests/test_primitives.py | 6 ++---- 3 files changed, 3 insertions(+), 15 deletions(-) diff --git a/omnigibson/controllers/joint_controller.py b/omnigibson/controllers/joint_controller.py index 2cfbd66fb..902fb948b 100644 --- a/omnigibson/controllers/joint_controller.py +++ b/omnigibson/controllers/joint_controller.py @@ -27,7 +27,6 @@ def __init__( command_output_limits="default", use_delta_commands=False, compute_delta_in_quat_space=None, - use_single_command=False, ): """ Args: @@ -57,8 +56,6 @@ def __init__( joints that need to be processed in quaternion space to avoid gimbal lock issues normally faced by 3 DOF rotation joints. Each group needs to consist of three idxes corresponding to the indices in the input space. This is only used in the delta_commands mode. - use_single_command (bool): whether all related joints should be controlled with one (scaled) input or - independent values individually. If True, then the inputted command should be a single value. """ # Store arguments assert_valid_key(key=motor_type.lower(), valid_keys=ControlType.VALID_TYPES_STR, name="motor_type") @@ -66,8 +63,6 @@ def __init__( self._use_delta_commands = use_delta_commands self._compute_delta_in_quat_space = [] if compute_delta_in_quat_space is None else compute_delta_in_quat_space - self._use_single_command = use_single_command - # When in delta mode, it doesn't make sense to infer output range using the joint limits (since that's an # absolute range and our values are relative). So reject the default mode option in that case. assert not ( @@ -151,4 +146,4 @@ def control_type(self): @property def command_dim(self): - return 1 if self._use_single_command else len(self.dof_idx) + return len(self.dof_idx) diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py index b223d9363..1468bbf0f 100644 --- a/omnigibson/robots/manipulation_robot.py +++ b/omnigibson/robots/manipulation_robot.py @@ -1093,11 +1093,6 @@ def _handle_assisted_grasping(self, action): """ # Loop over all arms for arm in self.arm_names: - # Make sure gripper action dimension is only 1 - cmd_dim = self._controllers[f"gripper_{arm}"].command_dim - assert cmd_dim == 1, \ - f"Gripper {arm} controller command dim must be 1 to use assisted grasping, got: {cmd_dim}." - # We apply a threshold based on the control rather than the command here so that the behavior # stays the same across different controllers and control modes (absolute / delta). This way, # a zero action will actually keep the AG setting where it already is. diff --git a/tests/test_primitives.py b/tests/test_primitives.py index b0459093a..03a3e6a48 100644 --- a/tests/test_primitives.py +++ b/tests/test_primitives.py @@ -65,16 +65,14 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg "motor_type": "position", "command_input_limits": [-1, 1], "command_output_limits": None, - "use_delta_commands": True, - "use_single_command": True + "use_delta_commands": True }, "gripper_right": { "name": "JointController", "motor_type": "position", "command_input_limits": [-1, 1], "command_output_limits": None, - "use_delta_commands": True, - "use_single_command": True + "use_delta_commands": True }, "camera": { "name": "JointController",