add tiago robot, configs and examples

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]

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
+

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

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
+

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()