diff --git a/omnigibson/examples/robots/robot_control_example.py b/omnigibson/examples/robots/robot_control_example.py index 6e5f2c964..3d51a92b2 100644 --- a/omnigibson/examples/robots/robot_control_example.py +++ b/omnigibson/examples/robots/robot_control_example.py @@ -154,6 +154,7 @@ def main(random_selection=False, headless=False, short_exec=False, quickstart=Fa # Loop control until user quits max_steps = -1 if not short_exec else 100 step = 0 + while step != max_steps: action = ( action_generator.get_random_action() if control_mode == "random" else action_generator.get_teleop_action() diff --git a/omnigibson/robots/__init__.py b/omnigibson/robots/__init__.py index 863d783d2..ebc3c25ef 100644 --- a/omnigibson/robots/__init__.py +++ b/omnigibson/robots/__init__.py @@ -2,8 +2,6 @@ from omnigibson.robots.behavior_robot import BehaviorRobot from omnigibson.robots.fetch import Fetch from omnigibson.robots.franka import FrankaPanda -from omnigibson.robots.franka_allegro import FrankaAllegro -from omnigibson.robots.franka_leap import FrankaLeap from omnigibson.robots.franka_mounted import FrankaMounted from omnigibson.robots.freight import Freight from omnigibson.robots.husky import Husky diff --git a/omnigibson/robots/franka.py b/omnigibson/robots/franka.py index b1e426b04..eee54984f 100644 --- a/omnigibson/robots/franka.py +++ b/omnigibson/robots/franka.py @@ -38,6 +38,8 @@ def __init__( sensor_config=None, # Unique to ManipulationRobot grasping_mode="physical", + # Unique to Franka + end_effector="gripper", **kwargs, ): """ @@ -84,9 +86,99 @@ def __init__( If "physical", no assistive grasping will be applied (relies on contact friction + finger force). If "assisted", will magnetize any object touching and within the gripper's fingers. If "sticky", will magnetize any object touching the gripper's fingers. + end_effector (str): type of end effector to use. One of {"gripper", "allegro", "leap_right", "leap_left", "inspire"} kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing for flexible compositions of various object subclasses (e.g.: Robot is USDObject + ControllableObject). """ + # store end effector information + self.end_effector = end_effector + if end_effector == "gripper": + self._model_name = "franka_panda" + self._gripper_control_idx = np.arange(7, 9) + self._eef_link_names = "panda_hand" + self._finger_link_names = ["panda_leftfinger", "panda_rightfinger"] + self._finger_joint_names = ["panda_finger_joint1", "panda_finger_joint2"] + self._default_robot_model_joint_pos = np.array([0.00, -1.3, 0.00, -2.87, 0.00, 2.00, 0.75, 0.00, 0.00]) + self._teleop_rotation_offset = np.array([-1, 0, 0, 0]) + self._ag_start_points = [ + GraspingPoint(link_name="panda_rightfinger", position=[0.0, 0.001, 0.045]), + ] + self._ag_end_points = [ + GraspingPoint(link_name="panda_leftfinger", position=[0.0, 0.001, 0.045]), + ] + elif end_effector == "allegro": + self._model_name = "franka_allegro" + self._eef_link_names = "base_link" + # thumb.proximal, ..., thumb.tip, ..., ring.tip + self._finger_link_names = [f"link_{i}_0" for i in range(16)] + self._finger_joint_names = [f"joint_{i}_0" for i in [12, 13, 14, 15, 8, 9, 10, 11, 4, 5, 6, 7, 0, 1, 2, 3]] + # position where the hand is parallel to the ground + self._default_robot_model_joint_pos = np.concatenate( + ([0.86, -0.27, -0.68, -1.52, -0.18, 1.29, 1.72], np.zeros(16)) + ) + self._teleop_rotation_offset = np.array([0, 0.7071, 0, 0.7071]) + self._ag_start_points = [ + GraspingPoint(link_name=f"base_link", position=[0.015, 0, -0.03]), + GraspingPoint(link_name=f"base_link", position=[0.015, 0, -0.08]), + GraspingPoint(link_name=f"link_15_0_tip", position=[0, 0.015, 0.007]), + ] + self._ag_end_points = [ + GraspingPoint(link_name=f"link_3_0_tip", position=[0.012, 0, 0.007]), + GraspingPoint(link_name=f"link_7_0_tip", position=[0.012, 0, 0.007]), + GraspingPoint(link_name=f"link_11_0_tip", position=[0.012, 0, 0.007]), + ] + elif "leap" in end_effector: + self._model_name = f"franka_{end_effector}" + self._eef_link_names = "palm_center" + # thumb.proximal, ..., thumb.tip, ..., ring.tip + self._finger_link_names = [ + f"{link}_{i}" for i in range(1, 5) for link in ["mcp_joint", "pip", "dip", "fingertip", "realtip"] + ] + self._finger_joint_names = [ + f"finger_joint_{i}" for i in [12, 13, 14, 15, 1, 0, 2, 3, 5, 4, 6, 7, 9, 8, 10, 11] + ] + # position where the hand is parallel to the ground + self._default_robot_model_joint_pos = np.concatenate( + ([0.86, -0.27, -0.68, -1.52, -0.18, 1.29, 1.72], np.zeros(16)) + ) + self._teleop_rotation_offset = np.array([-0.7071, 0.7071, 0, 0]) + self._ag_start_points = [ + GraspingPoint(link_name=f"palm_center", position=[0, -0.025, 0.035]), + GraspingPoint(link_name=f"palm_center", position=[0, 0.03, 0.035]), + GraspingPoint(link_name=f"fingertip_4", position=[-0.0115, -0.07, -0.015]), + ] + self._ag_end_points = [ + GraspingPoint(link_name=f"fingertip_1", position=[-0.0115, -0.06, 0.015]), + GraspingPoint(link_name=f"fingertip_2", position=[-0.0115, -0.06, 0.015]), + GraspingPoint(link_name=f"fingertip_3", position=[-0.0115, -0.06, 0.015]), + ] + elif end_effector == "inspire": + self._model_name = f"franka_{end_effector}" + self._eef_link_names = "palm_center" + # thumb.proximal, ..., thumb.tip, ..., ring.tip + hand_part_names = [11, 12, 13, 14, 21, 22, 31, 32, 41, 42, 51, 52] + self._finger_link_names = [f"link{i}" for i in hand_part_names] + self._finger_joint_names = [f"joint{i}" for i in hand_part_names] + # position where the hand is parallel to the ground + self._default_robot_model_joint_pos = np.concatenate( + ([0.86, -0.27, -0.68, -1.52, -0.18, 1.29, 1.72], np.zeros(12)) + ) + self._teleop_rotation_offset = np.array([0, 0, 0.707, 0.707]) + # TODO: add ag support for inspire hand + self._ag_start_points = [ + # GraspingPoint(link_name=f"base_link", position=[0, -0.025, 0.035]), + # GraspingPoint(link_name=f"base_link", position=[0, 0.03, 0.035]), + # GraspingPoint(link_name=f"link14", position=[-0.0115, -0.07, -0.015]), + ] + self._ag_end_points = [ + # GraspingPoint(link_name=f"link22", position=[-0.0115, -0.06, 0.015]), + # GraspingPoint(link_name=f"link32", position=[-0.0115, -0.06, 0.015]), + # GraspingPoint(link_name=f"link42", position=[-0.0115, -0.06, 0.015]), + # GraspingPoint(link_name=f"link52", position=[-0.0115, -0.06, 0.015]), + ] + else: + raise ValueError(f"End effector {end_effector} not supported for FrankaPanda") + # Run super init super().__init__( prim_path=prim_path, @@ -108,12 +200,15 @@ def __init__( proprio_obs=proprio_obs, sensor_config=sensor_config, grasping_mode=grasping_mode, + grasping_direction=( + "lower" if end_effector == "gripper" else "upper" + ), # gripper grasps in the opposite direction **kwargs, ) @property def model_name(self): - return "FrankaPanda" + return self._model_name @property def discrete_action_list(self): @@ -139,9 +234,18 @@ def _default_controllers(self): controllers["gripper_{}".format(self.default_arm)] = "MultiFingerGripperController" return controllers + @property + def _default_gripper_multi_finger_controller_configs(self): + conf = super()._default_gripper_multi_finger_controller_configs + # If the end effector is not a gripper, set the mode to independent + if self.end_effector != "gripper": + conf[self.default_arm]["mode"] = "independent" + conf[self.default_arm]["command_input_limits"] = None + return conf + @property def _default_joint_pos(self): - return np.array([0.00, -1.3, 0.00, -2.87, 0.00, 2.00, 0.75, 0.00, 0.00]) + return self._default_robot_model_joint_pos @property def finger_lengths(self): @@ -153,7 +257,11 @@ def arm_control_idx(self): @property def gripper_control_idx(self): - return {self.default_arm: np.arange(7, 9)} + return { + self.default_arm: np.array( + [list(self.joints.keys()).index(name) for name in self.finger_joint_names[self.default_arm]] + ) + } @property def arm_link_names(self): @@ -165,48 +273,50 @@ def arm_joint_names(self): @property def eef_link_names(self): - return {self.default_arm: "panda_hand"} + return {self.default_arm: self._eef_link_names} @property def finger_link_names(self): - return {self.default_arm: ["panda_leftfinger", "panda_rightfinger"]} + return {self.default_arm: self._finger_link_names} @property def finger_joint_names(self): - return {self.default_arm: ["panda_finger_joint1", "panda_finger_joint2"]} + return {self.default_arm: self._finger_joint_names} @property def usd_path(self): - return os.path.join(gm.ASSET_PATH, "models/franka/franka_panda.usd") + return os.path.join(gm.ASSET_PATH, f"models/franka/{self.model_name}.usd") @property def robot_arm_descriptor_yamls(self): - return {self.default_arm: os.path.join(gm.ASSET_PATH, "models/franka/franka_panda_description.yaml")} + return {self.default_arm: os.path.join(gm.ASSET_PATH, f"models/franka/{self.model_name}_description.yaml")} @property def urdf_path(self): - return os.path.join(gm.ASSET_PATH, "models/franka/franka_panda.urdf") + return os.path.join(gm.ASSET_PATH, f"models/franka/{self.model_name}.urdf") @property def eef_usd_path(self): - return {self.default_arm: os.path.join(gm.ASSET_PATH, "models/franka/franka_panda_eef.usd")} + return {self.default_arm: os.path.join(gm.ASSET_PATH, f"models/franka/{self.model_name}_eef.usd")} @property def teleop_rotation_offset(self): - return {self.default_arm: euler2quat([-np.pi, 0, 0])} + return {self.default_arm: self._teleop_rotation_offset} @property def assisted_grasp_start_points(self): - return { - self.default_arm: [ - GraspingPoint(link_name="panda_rightfinger", position=[0.0, 0.001, 0.045]), - ] - } + return {self.default_arm: self._ag_start_points} @property def assisted_grasp_end_points(self): - return { - self.default_arm: [ - GraspingPoint(link_name="panda_leftfinger", position=[0.0, 0.001, 0.045]), - ] - } + return {self.default_arm: self._ag_start_points} + + @property + def disabled_collision_pairs(self): + # some dexhand has self collisions that needs to be filtered out + if self.end_effector == "allegro": + return [["link_12_0", "part_studio_link"]] + elif self.end_effector == "inspire": + return [["base_link", "link12"]] + else: + return [] diff --git a/omnigibson/robots/franka_allegro.py b/omnigibson/robots/franka_allegro.py deleted file mode 100644 index 0eb4ed34c..000000000 --- a/omnigibson/robots/franka_allegro.py +++ /dev/null @@ -1,226 +0,0 @@ -import os - -import numpy as np - -import omnigibson.utils.transform_utils as T -from omnigibson.macros import gm -from omnigibson.robots.manipulation_robot import GraspingPoint, ManipulationRobot - - -class FrankaAllegro(ManipulationRobot): - """ - Franka Robot with Allegro hand - """ - - def __init__( - self, - # Shared kwargs in hierarchy - name, - prim_path=None, - uuid=None, - scale=None, - visible=True, - visual_only=False, - self_collisions=True, - load_config=None, - fixed_base=True, - # Unique to USDObject hierarchy - abilities=None, - # Unique to ControllableObject hierarchy - control_freq=None, - controller_config=None, - action_type="continuous", - action_normalize=True, - reset_joint_pos=None, - # Unique to BaseRobot - obs_modalities="all", - proprio_obs="default", - sensor_config=None, - # Unique to ManipulationRobot - grasping_mode="physical", - **kwargs, - ): - """ - Args: - name (str): Name for the object. Names need to be unique per scene - prim_path (None or str): global path in the stage to this object. If not specified, will automatically be - created at /World/ - uuid (None or int): Unique unsigned-integer identifier to assign to this object (max 8-numbers). - If None is specified, then it will be auto-generated - scale (None or float or 3-array): if specified, sets either the uniform (float) or x,y,z (3-array) scale - for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a - 3-array specifies per-axis scaling. - visible (bool): whether to render this object or not in the stage - visual_only (bool): Whether this object should be visual only (and not collide with any other objects) - self_collisions (bool): Whether to enable self collisions for this object - load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for - loading this prim at runtime. - abilities (None or dict): If specified, manually adds specific object states to this object. It should be - a dict in the form of {ability: {param: value}} containing object abilities and parameters to pass to - the object state instance constructor. - control_freq (float): control frequency (in Hz) at which to control the object. If set to be None, - simulator.import_object will automatically set the control frequency to be at teh render frequency by default. - controller_config (None or dict): nested dictionary mapping controller name(s) to specific controller - configurations for this object. This will override any default values specified by this class. - action_type (str): one of {discrete, continuous} - what type of action space to use - action_normalize (bool): whether to normalize inputted actions. This will override any default values - specified by this class. - reset_joint_pos (None or n-array): if specified, should be the joint positions that the object should - be set to during a reset. If None (default), self._default_joint_pos will be used instead. - Note that _default_joint_pos are hardcoded & precomputed, and thus should not be modified by the user. - Set this value instead if you want to initialize the robot with a different rese joint position. - obs_modalities (str or list of str): Observation modalities to use for this robot. Default is "all", which - corresponds to all modalities being used. - Otherwise, valid options should be part of omnigibson.sensors.ALL_SENSOR_MODALITIES. - Note: If @sensor_config explicitly specifies `modalities` for a given sensor class, it will - override any values specified from @obs_modalities! - proprio_obs (str or list of str): proprioception observation key(s) to use for generating proprioceptive - observations. If str, should be exactly "default" -- this results in the default proprioception - observations being used, as defined by self.default_proprio_obs. See self._get_proprioception_dict - for valid key choices - sensor_config (None or dict): nested dictionary mapping sensor class name(s) to specific sensor - configurations for this object. This will override any default values specified by this class. - grasping_mode (str): One of {"physical", "assisted", "sticky"}. - If "physical", no assistive grasping will be applied (relies on contact friction + finger force). - If "assisted", will magnetize any object touching and within the gripper's fingers. - If "sticky", will magnetize any object touching the gripper's fingers. - kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing - for flexible compositions of various object subclasses (e.g.: Robot is USDObject + ControllableObject). - """ - - # Run super init - super().__init__( - prim_path=prim_path, - name=name, - uuid=uuid, - scale=scale, - visible=visible, - fixed_base=fixed_base, - visual_only=visual_only, - self_collisions=self_collisions, - load_config=load_config, - abilities=abilities, - control_freq=control_freq, - controller_config=controller_config, - action_type=action_type, - action_normalize=action_normalize, - reset_joint_pos=reset_joint_pos, - obs_modalities=obs_modalities, - proprio_obs=proprio_obs, - sensor_config=sensor_config, - grasping_mode=grasping_mode, - grasping_direction="upper", - **kwargs, - ) - - @property - def model_name(self): - return "FrankaAllegro" - - @property - def discrete_action_list(self): - # Not supported for this robot - raise NotImplementedError() - - def _create_discrete_action_space(self): - # Fetch does not support discrete actions - raise ValueError("Franka does not support discrete actions!") - - @property - def controller_order(self): - return ["arm_{}".format(self.default_arm), "gripper_{}".format(self.default_arm)] - - @property - def _default_controllers(self): - controllers = super()._default_controllers - controllers["arm_{}".format(self.default_arm)] = "InverseKinematicsController" - controllers["gripper_{}".format(self.default_arm)] = "MultiFingerGripperController" - return controllers - - @property - def _default_gripper_multi_finger_controller_configs(self): - conf = super()._default_gripper_multi_finger_controller_configs - conf[self.default_arm]["mode"] = "independent" - conf[self.default_arm]["command_input_limits"] = None - return conf - - @property - def _default_joint_pos(self): - # position where the hand is parallel to the ground - return np.r_[[0.86, -0.27, -0.68, -1.52, -0.18, 1.29, 1.72], np.zeros(16)] - - @property - def finger_lengths(self): - return {self.default_arm: 0.1} - - @property - def arm_control_idx(self): - return {self.default_arm: np.arange(7)} - - @property - def gripper_control_idx(self): - # thumb.proximal, ..., thumb.tip, ..., ring.tip - return {self.default_arm: np.array([8, 12, 16, 20, 10, 14, 18, 22, 9, 13, 17, 21, 7, 11, 15, 19])} - - @property - def arm_link_names(self): - return {self.default_arm: [f"panda_link{i}" for i in range(8)]} - - @property - def arm_joint_names(self): - return {self.default_arm: [f"panda_joint_{i+1}" for i in range(7)]} - - @property - def eef_link_names(self): - return {self.default_arm: "base_link"} - - @property - def finger_link_names(self): - return {self.default_arm: [f"link_{i}_0" for i in range(16)]} - - @property - def finger_joint_names(self): - # thumb.proximal, ..., thumb.tip, ..., ring.tip - return {self.default_arm: [f"joint_{i}_0" for i in [12, 13, 14, 15, 8, 9, 10, 11, 4, 5, 6, 7, 0, 1, 2, 3]]} - - @property - def usd_path(self): - return os.path.join(gm.ASSET_PATH, "models/franka/franka_allegro.usd") - - @property - def robot_arm_descriptor_yamls(self): - return {self.default_arm: os.path.join(gm.ASSET_PATH, "models/franka/franka_allegro_description.yaml")} - - @property - def urdf_path(self): - return os.path.join(gm.ASSET_PATH, "models/franka/franka_allegro.urdf") - - @property - def disabled_collision_pairs(self): - return [ - ["link_12_0", "part_studio_link"], - ] - - @property - def assisted_grasp_start_points(self): - return { - self.default_arm: [ - GraspingPoint(link_name=f"base_link", position=[0.015, 0, -0.03]), - GraspingPoint(link_name=f"base_link", position=[0.015, 0, -0.08]), - GraspingPoint(link_name=f"link_15_0_tip", position=[0, 0.015, 0.007]), - ] - } - - @property - def assisted_grasp_end_points(self): - return { - self.default_arm: [ - GraspingPoint(link_name=f"link_3_0_tip", position=[0.012, 0, 0.007]), - GraspingPoint(link_name=f"link_7_0_tip", position=[0.012, 0, 0.007]), - GraspingPoint(link_name=f"link_11_0_tip", position=[0.012, 0, 0.007]), - ] - } - - @property - def teleop_rotation_offset(self): - return {self.default_arm: T.euler2quat(np.array([0, np.pi / 2, 0]))} diff --git a/omnigibson/robots/franka_leap.py b/omnigibson/robots/franka_leap.py deleted file mode 100644 index 21b4eb9d2..000000000 --- a/omnigibson/robots/franka_leap.py +++ /dev/null @@ -1,225 +0,0 @@ -import os -from typing import Dict, Iterable - -import numpy as np - -import omnigibson.utils.transform_utils as T -from omnigibson.macros import gm -from omnigibson.robots.manipulation_robot import GraspingPoint, ManipulationRobot - - -class FrankaLeap(ManipulationRobot): - """ - Franka Robot with Leap right hand - """ - - def __init__( - self, - # Shared kwargs in hierarchy - name, - hand="right", - prim_path=None, - uuid=None, - scale=None, - visible=True, - visual_only=False, - self_collisions=True, - load_config=None, - fixed_base=True, - # Unique to USDObject hierarchy - abilities=None, - # Unique to ControllableObject hierarchy - control_freq=None, - controller_config=None, - action_type="continuous", - action_normalize=True, - reset_joint_pos=None, - # Unique to BaseRobot - obs_modalities="all", - proprio_obs="default", - sensor_config=None, - # Unique to ManipulationRobot - grasping_mode="physical", - **kwargs, - ): - """ - Args: - name (str): Name for the object. Names need to be unique per scene - hand (str): One of {"left", "right"} - which hand to use, default is right - prim_path (None or str): global path in the stage to this object. If not specified, will automatically be - created at /World/ - uuid (None or int): Unique unsigned-integer identifier to assign to this object (max 8-numbers). - If None is specified, then it will be auto-generated - scale (None or float or 3-array): if specified, sets either the uniform (float) or x,y,z (3-array) scale - for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a - 3-array specifies per-axis scaling. - visible (bool): whether to render this object or not in the stage - visual_only (bool): Whether this object should be visual only (and not collide with any other objects) - self_collisions (bool): Whether to enable self collisions for this object - load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for - loading this prim at runtime. - abilities (None or dict): If specified, manually adds specific object states to this object. It should be - a dict in the form of {ability: {param: value}} containing object abilities and parameters to pass to - the object state instance constructor. - control_freq (float): control frequency (in Hz) at which to control the object. If set to be None, - simulator.import_object will automatically set the control frequency to be 1 / render_timestep by default. - controller_config (None or dict): nested dictionary mapping controller name(s) to specific controller - configurations for this object. This will override any default values specified by this class. - action_type (str): one of {discrete, continuous} - what type of action space to use - action_normalize (bool): whether to normalize inputted actions. This will override any default values - specified by this class. - reset_joint_pos (None or n-array): if specified, should be the joint positions that the object should - be set to during a reset. If None (default), self._default_joint_pos will be used instead. - Note that _default_joint_pos are hardcoded & precomputed, and thus should not be modified by the user. - Set this value instead if you want to initialize the robot with a different rese joint position. - obs_modalities (str or list of str): Observation modalities to use for this robot. Default is "all", which - corresponds to all modalities being used. - Otherwise, valid options should be part of omnigibson.sensors.ALL_SENSOR_MODALITIES. - Note: If @sensor_config explicitly specifies `modalities` for a given sensor class, it will - override any values specified from @obs_modalities! - proprio_obs (str or list of str): proprioception observation key(s) to use for generating proprioceptive - observations. If str, should be exactly "default" -- this results in the default proprioception - observations being used, as defined by self.default_proprio_obs. See self._get_proprioception_dict - for valid key choices - sensor_config (None or dict): nested dictionary mapping sensor class name(s) to specific sensor - configurations for this object. This will override any default values specified by this class. - grasping_mode (str): One of {"physical", "assisted", "sticky"}. - If "physical", no assistive grasping will be applied (relies on contact friction + finger force). - If "assisted", will magnetize any object touching and within the gripper's fingers. - If "sticky", will magnetize any object touching the gripper's fingers. - kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing - for flexible compositions of various object subclasses (e.g.: Robot is USDObject + ControllableObject). - """ - - self.hand = hand - # Run super init - super().__init__( - prim_path=prim_path, - name=name, - uuid=uuid, - scale=scale, - visible=visible, - fixed_base=fixed_base, - visual_only=visual_only, - self_collisions=self_collisions, - load_config=load_config, - abilities=abilities, - control_freq=control_freq, - controller_config=controller_config, - action_type=action_type, - action_normalize=action_normalize, - reset_joint_pos=reset_joint_pos, - obs_modalities=obs_modalities, - proprio_obs=proprio_obs, - sensor_config=sensor_config, - grasping_mode=grasping_mode, - grasping_direction="upper", - **kwargs, - ) - - @property - def model_name(self): - return f"FrankaLeap{self.hand.capitalize()}" - - @property - def discrete_action_list(self): - # Not supported for this robot - raise NotImplementedError() - - def _create_discrete_action_space(self): - # Fetch does not support discrete actions - raise ValueError("Franka does not support discrete actions!") - - @property - def controller_order(self): - return ["arm_{}".format(self.default_arm), "gripper_{}".format(self.default_arm)] - - @property - def _default_controllers(self): - controllers = super()._default_controllers - controllers["arm_{}".format(self.default_arm)] = "InverseKinematicsController" - controllers["gripper_{}".format(self.default_arm)] = "MultiFingerGripperController" - return controllers - - @property - def _default_gripper_multi_finger_controller_configs(self): - conf = super()._default_gripper_multi_finger_controller_configs - conf[self.default_arm]["mode"] = "independent" - conf[self.default_arm]["command_input_limits"] = None - return conf - - @property - def _default_joint_pos(self): - # position where the hand is parallel to the ground - return np.r_[[0.86, -0.27, -0.68, -1.52, -0.18, 1.29, 1.72], np.zeros(16)] - - @property - def assisted_grasp_start_points(self): - return { - self.default_arm: [ - GraspingPoint(link_name=f"palm_center", position=[0, -0.025, 0.035]), - GraspingPoint(link_name=f"palm_center", position=[0, 0.03, 0.035]), - GraspingPoint(link_name=f"fingertip_4", position=[-0.0115, -0.07, -0.015]), - ] - } - - @property - def assisted_grasp_end_points(self): - return { - self.default_arm: [ - GraspingPoint(link_name=f"fingertip_1", position=[-0.0115, -0.06, 0.015]), - GraspingPoint(link_name=f"fingertip_2", position=[-0.0115, -0.06, 0.015]), - GraspingPoint(link_name=f"fingertip_3", position=[-0.0115, -0.06, 0.015]), - ] - } - - @property - def finger_lengths(self): - return {self.default_arm: 0.1} - - @property - def arm_control_idx(self): - return {self.default_arm: np.arange(7)} - - @property - def gripper_control_idx(self): - # thumb.proximal, ..., thumb.tip, ..., ring.tip - return {self.default_arm: np.array([8, 12, 16, 20, 7, 11, 15, 19, 9, 13, 17, 21, 10, 14, 18, 22])} - - @property - def arm_link_names(self): - return {self.default_arm: [f"panda_link{i}" for i in range(8)]} - - @property - def arm_joint_names(self): - return {self.default_arm: [f"panda_joint_{i+1}" for i in range(7)]} - - @property - def eef_link_names(self): - return {self.default_arm: "palm_center"} - - @property - def finger_link_names(self): - links = ["mcp_joint", "pip", "dip", "fingertip", "realtip"] - return {self.default_arm: [f"{link}_{i}" for i in range(1, 5) for link in links]} - - @property - def finger_joint_names(self): - # thumb.proximal, ..., thumb.tip, ..., ring.tip - return {self.default_arm: [f"finger_joint_{i}" for i in [12, 13, 14, 15, 1, 0, 2, 3, 5, 4, 6, 7, 9, 8, 10, 11]]} - - @property - def usd_path(self): - return os.path.join(gm.ASSET_PATH, f"models/franka/franka_leap_{self.hand}.usd") - - @property - def robot_arm_descriptor_yamls(self): - return {self.default_arm: os.path.join(gm.ASSET_PATH, "models/franka/franka_leap_description.yaml")} - - @property - def urdf_path(self): - return os.path.join(gm.ASSET_PATH, f"models/franka/franka_leap_{self.hand}.urdf") - - @property - def teleop_rotation_offset(self): - return {self.default_arm: T.euler2quat(np.array([0, np.pi, np.pi / 2]))} diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py index f8a5e93f6..ee417102d 100644 --- a/omnigibson/robots/manipulation_robot.py +++ b/omnigibson/robots/manipulation_robot.py @@ -1160,8 +1160,8 @@ def _get_assisted_grasp_joint_type(self, ag_obj, ag_link): # A link is non-fixed if it has any non-fixed parent joints. joint_type = "FixedJoint" for edge in nx.edge_dfs(ag_obj.articulation_tree, ag_link.body_name, orientation="reverse"): - joint = ag_obj.articulation_tree.edges[edge]["joint"] - if joint.joint_type != JointType.JOINT_FIXED: + joint = ag_obj.articulation_tree.edges[edge[:2]] + if joint["joint_type"] != JointType.JOINT_FIXED: joint_type = "SphericalJoint" break