From c386a5e1f4f373a80ad3c7b2918da4681a638cd6 Mon Sep 17 00:00:00 2001 From: dnandha Date: Fri, 2 Jul 2021 15:23:37 +0200 Subject: [PATCH] add tiago robot, configs and examples --- gibson2/envs/env_base.py | 11 +- .../configs/tiago_dual_point_nav.yaml | 83 +++++++++++ gibson2/examples/configs/tiago_reaching.yaml | 84 +++++++++++ .../configs/tiago_single_point_nav.yaml | 73 ++++++++++ gibson2/examples/demo/tiago_dual_ik.py | 122 ++++++++++++++++ gibson2/examples/demo/tiago_example.py | 70 ++++++++++ gibson2/robots/tiago_dual_robot.py | 132 ++++++++++++++++++ gibson2/robots/tiago_single_robot.py | 103 ++++++++++++++ 8 files changed, 677 insertions(+), 1 deletion(-) create mode 100644 gibson2/examples/configs/tiago_dual_point_nav.yaml create mode 100644 gibson2/examples/configs/tiago_reaching.yaml create mode 100644 gibson2/examples/configs/tiago_single_point_nav.yaml create mode 100644 gibson2/examples/demo/tiago_dual_ik.py create mode 100644 gibson2/examples/demo/tiago_example.py create mode 100644 gibson2/robots/tiago_dual_robot.py create mode 100644 gibson2/robots/tiago_single_robot.py diff --git a/gibson2/envs/env_base.py b/gibson2/envs/env_base.py index d1dd0b955..8cb329302 100644 --- a/gibson2/envs/env_base.py +++ b/gibson2/envs/env_base.py @@ -7,6 +7,8 @@ from gibson2.robots.freight_robot import Freight from gibson2.robots.fetch_robot import Fetch from gibson2.robots.locobot_robot import Locobot +from gibson2.robots.tiago_single_robot import Tiago_Single +from gibson2.robots.tiago_dual_robot import Tiago_Dual from gibson2.simulator import Simulator from gibson2.scenes.empty_scene import EmptyScene from gibson2.scenes.stadium_scene import StadiumScene @@ -174,6 +176,9 @@ def load(self): if first_n != -1: scene._set_first_n_objects(first_n) self.simulator.import_ig_scene(scene) + else: + raise Exception( + 'Unknown scene type: {}'.format(self.config['scene'])) if self.config['robot'] == 'Turtlebot': robot = Turtlebot(self.config) @@ -193,9 +198,13 @@ def load(self): robot = Fetch(self.config) elif self.config['robot'] == 'Locobot': robot = Locobot(self.config) + elif self.config['robot'] == 'Tiago_Single': + robot = Tiago_Single(self.config) + elif self.config['robot'] == 'Tiago_Dual': + robot = Tiago_Dual(self.config) else: raise Exception( - 'unknown robot type: {}'.format(self.config['robot'])) + 'Unknown robot type: {}'.format(self.config['robot'])) self.scene = scene self.robots = [robot] diff --git a/gibson2/examples/configs/tiago_dual_point_nav.yaml b/gibson2/examples/configs/tiago_dual_point_nav.yaml new file mode 100644 index 000000000..fe938f1c0 --- /dev/null +++ b/gibson2/examples/configs/tiago_dual_point_nav.yaml @@ -0,0 +1,83 @@ +# scene +scene: gibson +scene_id: Rs +#scene: igibson +#scene_id: Rs_int +is_interactive: false +build_graph: true +load_texture: true +trav_map_resolution: 0.1 +trav_map_erosion: 3 + +# robot +robot: Tiago_Dual +is_discrete: false +wheel_velocity: 1.0 +torso_lift_velocity: 1.0 +head_velocity: 0.8 +arm_left_velocity: 0.9 +arm_right_velocity: 0.9 +gripper_velocity: 1.0 +hand_velocity: 0.5 + +# task, observation and action +#task: pointgoal # pointgoal|objectgoal|areagoal|reaching +#target_dist_min: 1.0 +#target_dist_max: 10.0 +#initial_pos_z_offset: 0.1 +#additional_states_dim: 4 +task: point_nav_random +target_dist_min: 1.0 +target_dist_max: 10.0 +goal_format: polar +task_obs_dim: 4 + +# reward +reward_type: geodesic +success_reward: 10.0 +slack_reward: -0.01 +potential_reward_weight: 1.0 +collision_reward_weight: -0.1 +collision_ignore_link_a_ids: [0, 1, 2] # ignore collisions with these robot links + +# discount factor +discount_factor: 0.99 + +# termination condition +dist_tol: 0.5 # body width +max_step: 500 +max_collisions_allowed: 500 +goal_format: polar + +# sensor spec +#output: [sensor, rgb, depth, scan] +output: [task_obs, rgb, depth, scan, occupancy_grid] +# image +# Primesense Carmine 1.09 short-range RGBD sensor +# http://xtionprolive.com/primesense-carmine-1.09 +fisheye: false +image_width: 160 +image_height: 120 +vertical_fov: 45 +# depth +depth_low: 0.35 +depth_high: 3.0 +# scan +# SICK TIM571 scanning range finder +# https://docs.fetchrobotics.com/robot_hardware.html +# n_horizontal_rays is originally 661, sub-sampled 1/3 +n_horizontal_rays: 220 +n_vertical_beams: 1 +laser_linear_range: 25.0 +laser_angular_range: 220.0 +min_laser_dist: 0.05 +laser_link_name: base_laser_link + +# sensor noise +depth_noise_rate: 0.0 +scan_noise_rate: 0.0 + +# visual objects +visual_object_at_initial_target_pos: true +target_visual_object_visible_to_agent: false + diff --git a/gibson2/examples/configs/tiago_reaching.yaml b/gibson2/examples/configs/tiago_reaching.yaml new file mode 100644 index 000000000..df474a83f --- /dev/null +++ b/gibson2/examples/configs/tiago_reaching.yaml @@ -0,0 +1,84 @@ +# scene +scene: igibson +scene_id: Rs_int +build_graph: true +load_texture: true +pybullet_load_texture: true +trav_map_type: no_obj +trav_map_resolution: 0.1 +trav_map_erosion: 3 +should_open_all_doors: true + +# domain randomization +texture_randomization_freq: null +object_randomization_freq: null + +# robot +robot: Tiago_Dual +wheel_velocity: 0.8 +torso_lift_velocity: 0.8 +arm_velocity: 0.8 + +# task +task: grasp_task +load_object_categories: [ + bottom_cabinet, + bottom_cabinet_no_top, + top_cabinet, + dishwasher, + fridge, + microwave, + oven, + washer + dryer, +] + + +# reward +reward_type: l2 +success_reward: 10.0 +potential_reward_weight: 1.0 +collision_reward_weight: -0.1 + +# discount factor +discount_factor: 0.99 + +# termination condition +dist_tol: 0.36 # body width +max_step: 500 +max_collisions_allowed: 500 + +# misc config +initial_pos_z_offset: 0.1 +collision_ignore_link_a_ids: [0, 1, 2] # ignore collisions with these robot links + +# sensor spec +output: [task_obs, rgb, depth, seg, scan, occupancy_grid] +# image +# Primesense Carmine 1.09 short-range RGBD sensor +# http://xtionprolive.com/primesense-carmine-1.09 +fisheye: false +image_width: 640 +image_height: 480 +vertical_fov: 45 +# depth +depth_low: 0.35 +depth_high: 3.0 +# scan +# SICK TIM571 scanning range finder +# https://docs.fetchrobotics.com/robot_hardware.html +# n_horizontal_rays is originally 661, sub-sampled 1/3 +n_horizontal_rays: 220 +n_vertical_beams: 1 +laser_linear_range: 25.0 +laser_angular_range: 220.0 +min_laser_dist: 0.05 +laser_link_name: base_laser_link + +# sensor noise +depth_noise_rate: 0.0 +scan_noise_rate: 0.0 + +# visual objects +visual_object_at_initial_target_pos: true +target_visual_object_visible_to_agent: false diff --git a/gibson2/examples/configs/tiago_single_point_nav.yaml b/gibson2/examples/configs/tiago_single_point_nav.yaml new file mode 100644 index 000000000..0061a156e --- /dev/null +++ b/gibson2/examples/configs/tiago_single_point_nav.yaml @@ -0,0 +1,73 @@ +# scene +scene: stadium +model_id: Rs +is_interactive: false +build_graph: true +load_texture: true +trav_map_resolution: 0.1 +trav_map_erosion: 3 + +# robot +robot: Tiago_Single +is_discrete: false +wheel_velocity: 1.0 +torso_lift_velocity: 1.0 +head_velocity: 0.8 +arm_velocity: 0.9 +gripper_velocity: 1.0 + +# task, observation and action +task: pointgoal # pointgoal|objectgoal|areagoal|reaching +target_dist_min: 1.0 +target_dist_max: 10.0 +initial_pos_z_offset: 0.1 +additional_states_dim: 4 + +# reward +reward_type: geodesic +success_reward: 10.0 +slack_reward: -0.01 +potential_reward_weight: 1.0 +collision_reward_weight: -0.1 +collision_ignore_link_a_ids: [0, 1, 2] # ignore collisions with these robot links + +# discount factor +discount_factor: 0.99 + +# termination condition +dist_tol: 0.5 # body width +max_step: 500 +max_collisions_allowed: 500 +goal_format: polar + +# sensor spec +output: [sensor, rgb, depth, scan] +# image +# Primesense Carmine 1.09 short-range RGBD sensor +# http://xtionprolive.com/primesense-carmine-1.09 +fisheye: false +image_width: 160 +image_height: 120 +vertical_fov: 45 +# depth +depth_low: 0.35 +depth_high: 3.0 +# scan +# SICK TIM571 scanning range finder +# https://docs.fetchrobotics.com/robot_hardware.html +# n_horizontal_rays is originally 661, sub-sampled 1/3 +n_horizontal_rays: 220 +n_vertical_beams: 1 +laser_linear_range: 25.0 +laser_angular_range: 220.0 +min_laser_dist: 0.05 +laser_link_name: laser_link + +# sensor noise +depth_noise_rate: 0.0 +scan_noise_rate: 0.0 + +# visual objects +visual_object_at_initial_target_pos: true +target_visual_object_visible_to_agent: false + diff --git a/gibson2/examples/demo/tiago_dual_ik.py b/gibson2/examples/demo/tiago_dual_ik.py new file mode 100644 index 000000000..66a97720d --- /dev/null +++ b/gibson2/examples/demo/tiago_dual_ik.py @@ -0,0 +1,122 @@ +from gibson2.robots.tiago_dual_robot import Tiago_Dual +from gibson2.simulator import Simulator +from gibson2.scenes.empty_scene import EmptyScene +from gibson2.utils.utils import parse_config +from gibson2.render.profiler import Profiler + +import pybullet as p +from gibson2.external.pybullet_tools.utils import set_joint_positions, joints_from_names, get_joint_positions, \ + get_max_limits, get_min_limits, get_sample_fn, get_movable_joints + +import numpy as np +import gibson2 +import os + +def main(): + config = parse_config(os.path.join(gibson2.example_config_path, 'tiago_dual_point_nav.yaml')) + s = Simulator(mode='gui', physics_timestep=1 / 240.0) + scene = EmptyScene() + s.import_scene(scene) + tiago = Tiago_Dual(config) + s.import_robot(tiago) + + robot_id = tiago.robot_ids[0] + + arm_joints = joints_from_names(robot_id, ['arm_left_1_joint', + 'arm_left_2_joint', + 'arm_left_3_joint', + 'arm_left_4_joint', + 'arm_left_5_joint', + 'arm_left_6_joint', + 'arm_left_7_joint']) + gripper_joints = joints_from_names(robot_id, [ + 'gripper_left_right_finger_joint', + 'gripper_left_left_finger_joint']) + + tiago.robot_body.reset_position([0, 0, 0]) + tiago.robot_body.reset_orientation([0, 0, 1, 0]) + x, y, z = tiago.get_end_effector_position() + + visual_marker = p.createVisualShape(p.GEOM_SPHERE, radius=0.02) + marker = p.createMultiBody(baseVisualShapeIndex=visual_marker) + + all_joints = get_movable_joints(robot_id) + + max_limits = get_max_limits(robot_id, all_joints) + min_limits = get_min_limits(robot_id, all_joints) + rest_position = get_joint_positions(robot_id, all_joints) + joint_range = list(np.array(max_limits) - np.array(min_limits)) + jd = [0.1 for item in joint_range] + + valid_joints = [j.joint_index for j in tiago.ordered_joints] + joint_mask = [] + for j in all_joints: + if j in valid_joints: + joint_mask += [True] + else: + joint_mask += [False] + + def accurateCalculateInverseKinematics(robotid, endEffectorId, targetPos, threshold, maxIter): + #sample_fn = get_sample_fn(robotid, arm_joints) + #set_joint_positions(robotid, arm_joints, sample_fn()) + it = 0 + while it < maxIter: + jointPoses = p.calculateInverseKinematics( + robotid, + endEffectorId, + targetPos, + lowerLimits=min_limits, + upperLimits=max_limits, + jointRanges=joint_range, + restPoses=rest_position, + jointDamping=jd) + jointPoses = np.asarray(jointPoses) + set_joint_positions(robotid, valid_joints, jointPoses[joint_mask]) + ls = p.getLinkState(robotid, endEffectorId) + newPos = ls[4] + + dist = np.linalg.norm(np.array(targetPos) - np.array(newPos)) + if dist < threshold: + break + + it += 1 + + print("Num iter: " + str(it) + ", residual: " + str(dist)) + return jointPoses + + while True: + with Profiler("Simulation step"): + tiago.robot_body.reset_position([0, 0, 0]) + tiago.robot_body.reset_orientation([0, 0, 1, 0]) + threshold = 0.01 + maxIter = 100 + joint_pos = accurateCalculateInverseKinematics( + robot_id, + tiago.end_effector_part_index(), + #tiago.parts['gripper_link'].body_part_index, + [x, y, z], + threshold, + maxIter)[2:10] + + s.step() + keys = p.getKeyboardEvents() + for k, v in keys.items(): + if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_IS_DOWN)): + x += 0.01 + if (k == p.B3G_LEFT_ARROW and (v & p.KEY_IS_DOWN)): + x -= 0.01 + if (k == p.B3G_UP_ARROW and (v & p.KEY_IS_DOWN)): + y += 0.01 + if (k == p.B3G_DOWN_ARROW and (v & p.KEY_IS_DOWN)): + y -= 0.01 + if (k == ord('z') and (v & p.KEY_IS_DOWN)): + z += 0.01 + if (k == ord('x') and (v & p.KEY_IS_DOWN)): + z -= 0.01 + p.resetBasePositionAndOrientation(marker, [x, y, z], [0, 0, 0, 1]) + + s.disconnect() + + +if __name__ == '__main__': + main() diff --git a/gibson2/examples/demo/tiago_example.py b/gibson2/examples/demo/tiago_example.py new file mode 100644 index 000000000..20e8b03d3 --- /dev/null +++ b/gibson2/examples/demo/tiago_example.py @@ -0,0 +1,70 @@ +from gibson2.robots.tiago_single_robot import Tiago_Single +from gibson2.robots.tiago_dual_robot import Tiago_Dual +from gibson2.utils.utils import parse_config +import os +import time +import numpy as np +import pybullet as p +import pybullet_data + +def main(): + p.connect(p.GUI) + p.setGravity(0,0,-9.8) + p.setTimeStep(1./240.) + + floor = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml") + p.loadMJCF(floor) + + + robots = [] + config = parse_config('../configs/tiago_single_point_nav.yaml') + tiago = Tiago_Single(config) + robots.append(tiago) + + config = parse_config('../configs/tiago_dual_point_nav.yaml') + tiago = Tiago_Dual(config) + robots.append(tiago) + + positions = [ + [1, 0, 0], + [0, 1, 0], + ] + + for robot, position in zip(robots, positions): + robot.load() + robot.set_position(position) + robot.robot_specific_reset() + robot.keep_still() + + secs = 2 + print("Keep still for {} seconds".format(secs)) + for _ in range(240 * secs): + p.stepSimulation() + time.sleep(1./240.) + + secs = 30 + print("Small movements for {} seconds".format(secs)) + for _ in range(240 * secs): # move with small random actions for 10 seconds + for robot, position in zip(robots, positions): + action = np.random.uniform(-1, 1, robot.action_dim) + #action = np.zeros(robot.action_dim) + #x = 0 + #y = robot.wheel_dim + #action[x:y] = 0.1 + #x = y + #y += robot.torso_lift_dim + #action[x:y] = 0.2 + #x = y + #y += robot.head_dim + #action[x:y] = 0.3 + + robot.apply_action(action) + p.stepSimulation() + time.sleep(1./240.0) + + p.disconnect() + + +if __name__ == '__main__': + main() + diff --git a/gibson2/robots/tiago_dual_robot.py b/gibson2/robots/tiago_dual_robot.py new file mode 100644 index 000000000..0cf62ad8c --- /dev/null +++ b/gibson2/robots/tiago_dual_robot.py @@ -0,0 +1,132 @@ +import gym +import numpy as np +import pybullet as p + +from gibson2.external.pybullet_tools.utils import joints_from_names, set_joint_positions, get_movable_joints +from gibson2.robots.robot_locomotor import LocomotorRobot + + +class Tiago_Dual(LocomotorRobot): + def __init__(self, config, with_hand=False): + self.with_hand = with_hand + self.wheel_velocity = config.get('wheel_velocity', 1.0) + self.torso_lift_velocity = config.get('torso_lift_velocity', 1.0) + self.head_velocity = config.get('head_velocity', 1.0) + self.arm_left_velocity = config.get('arm_left_velocity', 1.0) + self.arm_right_velocity = config.get('arm_right_velocity', 1.0) + self.gripper_velocity = config.get('gripper_velocity', 1.0) + self.hand_velocity = config.get('hand_velocity', 1.0) + self.wheel_dim = 2 + self.torso_lift_dim = 1 + self.head_dim = 2 + self.arm_left_dim = 7 + self.arm_right_dim = 7 + self.gripper_dim = 2 + self.hand_dim = 3 + 5 + 4 * 7 # 3 base joints + 5 thumb joints + 7 finger joints + self.rest_position = [0, 0, 0, 0, 0, + -np.pi/6, np.pi/2, 2*np.pi/3, np.pi/2, 0, -np.pi/3, 0, + 0, 0, + -np.pi/6, np.pi/2, 2*np.pi/3, np.pi/2, 0, 0, 0 + ] + + self.problem_parts = [] # filled on load + self.joint_mask = [] # filled on load + + action_dim = self.wheel_dim + self.torso_lift_dim\ + + self.head_dim + self.arm_left_dim\ + + self.arm_right_dim + self.gripper_dim\ + + (self.hand_dim if self.with_hand else self.gripper_dim) + LocomotorRobot.__init__(self, + "tiago/tiago_dual_hand.urdf" if self.with_hand\ + else "tiago/tiago_dual.urdf", + action_dim=action_dim, + scale=config.get("robot_scale", 1.0), + is_discrete=config.get("is_discrete", False), + control="velocity", + self_collision=True) + + def set_up_continuous_action_space(self): + self.action_high = np.array( + [self.wheel_velocity] * self.wheel_dim + + [self.torso_lift_velocity] * self.torso_lift_dim + + [self.head_velocity] * self.head_dim + + [self.arm_left_velocity] * self.arm_left_dim + + [self.gripper_velocity] * self.gripper_dim + + [self.arm_right_velocity] * self.arm_right_dim + + ([self.hand_velocity] * self.hand_dim if self.with_hand\ + else [self.gripper_velocity] * self.gripper_dim) + ) + self.action_low = -self.action_high + self.action_space = gym.spaces.Box(shape=(self.action_dim,), + low=-1.0, + high=1.0, + dtype=np.float32) + + def set_up_discrete_action_space(self): + assert False, "Tiago_Dual does not support discrete actions" + + def robot_specific_reset(self): + super(Tiago_Dual, self).robot_specific_reset() + + # roll the arm to its body + robot_id = self.robot_ids[0] + torso_joints = joints_from_names(robot_id, ['head_1_joint', 'head_2_joint', 'torso_lift_joint']) + arm_left_joints = joints_from_names(robot_id, + [ + 'arm_left_1_joint', + 'arm_left_2_joint', + 'arm_left_3_joint', + 'arm_left_4_joint', + 'arm_left_5_joint', + 'arm_left_6_joint', + 'arm_left_7_joint', + ]) + + arm_right_joints = joints_from_names(robot_id, + [ + 'arm_right_1_joint', + 'arm_right_2_joint', + 'arm_right_3_joint', + 'arm_right_4_joint', + 'arm_right_5_joint', + 'arm_right_6_joint', + 'arm_right_7_joint', + ]) + + rest_pos_torso = [-0.07, -0.80, 0.33] + rest_pos_left = [0.22, 0.48, 1.52, 1.76, 0.04, -0.49, 0] + #rest_pos_left = [-np.pi/6, np.pi/2, 2*np.pi/3, np.pi/2, 0, -np.pi/3, 0] + rest_pos_right = [-np.pi/6, np.pi/2, 2*np.pi/3, np.pi/2, 0, 0, 0] + + set_joint_positions(robot_id, torso_joints, rest_pos_torso) + set_joint_positions(robot_id, arm_left_joints, rest_pos_left) + set_joint_positions(robot_id, arm_right_joints, rest_pos_right) + + def get_end_effector_position(self): + return self.parts['gripper_left_grasping_frame'].get_position() + + def end_effector_part_index(self): + return self.parts['gripper_left_grasping_frame'].body_part_index + + def load(self): + ids = super(Tiago_Dual, self).load() + robot_id = self.robot_ids[0] + + # get problematic links + moving_parts = ["arm", "gripper", "wrist"] + for part in self.parts: + for x in moving_parts: + if x not in part: + self.problem_parts.append(self.parts[part]) + + # disable self collision + for a in self.problem_parts: + for b in self.problem_parts: + p.setCollisionFilterPair(robot_id, robot_id, a.body_part_index, b.body_part_index, 0) + + # calculate joint mask + all_joints = get_movable_joints(robot_id) + valid_joints = [j.joint_index for j in self.ordered_joints] + self.joint_mask = [j in valid_joints for j in all_joints] + + return ids diff --git a/gibson2/robots/tiago_single_robot.py b/gibson2/robots/tiago_single_robot.py new file mode 100644 index 000000000..2b2063e06 --- /dev/null +++ b/gibson2/robots/tiago_single_robot.py @@ -0,0 +1,103 @@ +import gym +import numpy as np +import pybullet as p + +from gibson2.external.pybullet_tools.utils import joints_from_names, set_joint_positions, get_movable_joints +from gibson2.robots.robot_locomotor import LocomotorRobot + + +class Tiago_Single(LocomotorRobot): + def __init__(self, config): + self.wheel_velocity = config.get('wheel_velocity', 1.0) + self.torso_lift_velocity = config.get('torso_lift_velocity', 1.0) + self.head_velocity = config.get('head_velocity', 1.0) + self.arm_velocity = config.get('arm_velocity', 1.0) + self.gripper_velocity = config.get('gripper_velocity', 1.0) + self.wheel_dim = 2 + self.torso_lift_dim = 1 + self.head_dim = 2 + self.arm_dim = 7 + self.gripper_dim = 2 + self.rest_position = [0, 0, 0, 0, 0, + 0, np.pi, -np.pi/2, 0, np.pi/2, 0, 0, 0, + 0, 0] + + self.problem_parts = [] # filled on load + self.joint_mask = [] # filled on load + + action_dim = self.wheel_dim + self.torso_lift_dim + self.head_dim\ + + self.arm_dim + self.gripper_dim + LocomotorRobot.__init__(self, + "tiago/tiago_single.urdf", + action_dim=action_dim, + scale=config.get("robot_scale", 1.0), + is_discrete=config.get("is_discrete", False), + control="velocity", + self_collision=True) + + def set_up_continuous_action_space(self): + self.action_high = np.array( + [self.wheel_velocity] * self.wheel_dim + + [self.torso_lift_velocity] * self.torso_lift_dim + + [self.head_velocity] * self.head_dim + + [self.arm_velocity] * self.arm_dim + + [self.gripper_velocity] * self.gripper_dim + ) + self.action_low = -self.action_high + self.action_space = gym.spaces.Box(shape=(self.action_dim,), + low=-1.0, + high=1.0, + dtype=np.float32) + + def set_up_discrete_action_space(self): + assert False, "Tiago_Single does not support discrete actions" + + def robot_specific_reset(self): + super(Tiago_Single, self).robot_specific_reset() + + # roll the arm to its body + robot_id = self.robot_ids[0] + arm_joints = joints_from_names(robot_id, + [ + 'torso_lift_joint', + 'arm_1_joint', + 'arm_2_joint', + 'arm_3_joint', + 'arm_4_joint', + 'arm_5_joint', + 'arm_6_joint', + 'arm_7_joint', + ]) + + rest_position = [0, np.pi, -np.pi/2, 0, np.pi/2, 0, 0, 0] + + set_joint_positions(robot_id, arm_joints, rest_position) + + def get_end_effector_position(self): + return self.parts['gripper_grasping_frame'].get_position() + + def get_end_effector_index(self): + return self.parts['gripper_grasping_frame'].body_part_index + + def load(self): + ids = super(Tiago_Single, self).load() + robot_id = self.robot_ids[0] + + # get problematic links + moving_parts = ["arm", "gripper", "wrist"] + for part in self.parts: + for x in moving_parts: + if x not in part: + self.problem_parts.append(self.parts[part]) + + # disable self collision + for a in self.problem_parts: + for b in self.problem_parts: + p.setCollisionFilterPair(robot_id, robot_id, a.body_part_index, b.body_part_index, 0) + + # calculate joint mask + all_joints = get_movable_joints(robot_id) + valid_joints = [j.joint_index for j in self.ordered_joints] + self.joint_mask = [j in valid_joints for j in all_joints] + + return ids