p04798526
/
OmniGibson
mirror of https://github.com/StanfordVL/OmniGibson.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Three challenge tracks for Locobot

This commit is contained in:
Chengshu Li 2020-02-17 16:10:56 -08:00
parent 318b9aa99f
commit a5705330a9
9 changed files with 801 additions and 1087 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
examples/configs/locobot_p2p_nav_house.yaml Normal file
View File

@ -0,0 +1,63 @@
# scene
scene: building
model_id: Ohoopee
build_graph: true
load_texture: true
trav_map_erosion: 2
# robot
robot: Locobot
velocity: 0.1
# task, observation and action
task: pointgoal # pointgoal|objectgoal|areagoal|reaching
fisheye: false
is_discrete: false
additional_states_dim: 3
# 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: [1, 2, 3, 4] # ignore collision with these agent's link ids
# discount factor
discount_factor: 0.99
# termination condition
death_z_thresh: 0.2
dist_tol: 0.333 # body width
max_step: 500
# sensor
output: [sensor, rgb, depth]
resolution: 256
fov: 90
# display
use_filler: true
display_ui: false
show_diagnostics: false
ui_num: 2
ui_components: [RGB_FILLED, DEPTH]
random:
random_initial_pose: false
random_target_pose: false
random_init_x_range: [-0.1, 0.1]
random_init_y_range: [-0.1, 0.1]
random_init_z_range: [-0.1, 0.1]
random_init_rot_range: [-0.1, 0.1]
speed:
timestep: 0.001
frameskip: 10
mode: web_ui #gui|headless
verbose: false
fast_lq_render: true
# debug
debug: false

2
gibson2/core/physics/interactive_objects.py
View File

@ -208,7 +208,7 @@ class InteractiveObj(object):
org_pos, org_orn = p.getBasePositionAndOrientation(self.body_id)
p.resetBasePositionAndOrientation(self.body_id, pos, org_orn)
def set_position_rotation(self, pos, orn):
def set_position_orientation(self, pos, orn):
p.resetBasePositionAndOrientation(self.body_id, pos, orn)

75
gibson2/core/physics/robot_locomotors.py
View File

@ -95,6 +95,10 @@ class LocomotorRobot(BaseRobot):
def set_orientation(self, orn):
self.robot_body.set_orientation(orn)
def set_position_orientation(self, pos, orn):
self.robot_body.reset_position(pos)
self.robot_body.set_orientation(orn)
def move_by(self, delta):
new_pos = np.array(delta) + self.get_position()
self.robot_body.reset_position(new_pos)
@ -117,6 +121,10 @@ class LocomotorRobot(BaseRobot):
new_orn = qmult((euler2quat(delta, 0, 0)), orn)
self.robot_body.set_orientation(new_orn)
def keep_still(self):
for n, j in enumerate(self.ordered_joints):
j.set_motor_velocity(0.0)
def get_rpy(self):
return self.robot_body.get_rpy()
@ -709,3 +717,70 @@ class JR2_Kinova(LocomotorRobot):
for j in range(16, 28):
p.setCollisionFilterPair(robot_id, robot_id, joint, j, 0)
return ids
class Locobot(LocomotorRobot):
mjcf_scaling = 1
model_type = "URDF"
default_scale = 1
def __init__(self, config):
self.config = config
self.velocity = config.get('velocity', 0.1)
self.wheel_dim = 2
self.action_high = config.get("action_high", None)
self.action_low = config.get("action_low", None)
LocomotorRobot.__init__(self,
"locobot/locobot.urdf",
"base_link",
action_dim=self.wheel_dim,
sensor_dim=55, # TODO: what is sensor_dim?
power=2.5,
scale=config.get("robot_scale", self.default_scale),
resolution=config.get("resolution", 64),
is_discrete=config.get("is_discrete", True),
control="velocity",
self_collision=False)
def set_up_continuous_action_space(self):
if self.action_high is not None and self.action_low is not None:
self.action_high = np.full(shape=self.wheel_dim, fill_value=self.action_high)
self.action_low = np.full(shape=self.wheel_dim, fill_value=self.action_low)
else:
self.action_high = np.array([self.velocity] * self.wheel_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, "Locobot does not support discrete actions"
def apply_action(self, action):
real_action = self.action_to_real_action(action)
self.apply_real_action(real_action)
def calc_state(self):
base_state = LocomotorRobot.calc_state(self)
angular_velocity = self.robot_body.angular_velocity()
print(len(base_state), len(angular_velocity))
return np.concatenate((base_state, np.array(angular_velocity)))
def get_end_effector_position(self):
return self.parts['gripper_link'].get_position()
def load(self):
ids = self._load_model()
self.eyes = self.parts["eyes"]
robot_id = ids[0]
# disable collision for immediate parent
for joint in range(p.getNumJoints(robot_id)):
info = p.getJointInfo(robot_id, joint)
parent_id = info[-1]
p.setCollisionFilterPair(robot_id, robot_id, joint, parent_id, 0)
return ids

74
gibson2/core/physics/scene.py
View File

@ -6,15 +6,18 @@ parentdir = os.path.dirname(currentdir)
os.sys.path.insert(0, parentdir)
import pybullet_data
from gibson2.data.datasets import get_model_path
from gibson2.utils.utils import l2_distance
import numpy as np
from PIL import Image
import cv2
import networkx as nx
from IPython import embed
import pickle
class Scene:
def load(self):
raise (NotImplementedError())
raise NotImplementedError()
class EmptyScene(Scene):
"""
@ -23,6 +26,7 @@ class EmptyScene(Scene):
def load(self):
planeName = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
self.ground_plane_mjcf = p.loadMJCF(planeName)
p.changeDynamics(self.ground_plane_mjcf[0], -1, lateralFriction=1)
return [item for item in self.ground_plane_mjcf]
class StadiumScene(Scene):
@ -43,6 +47,9 @@ class StadiumScene(Scene):
return [item for item in self.stadium] + [item for item in self.ground_plane_mjcf]
def get_random_floor(self):
return 0
def get_random_point(self, random_height=False):
return self.get_random_point_floor(0, random_height)
@ -54,6 +61,17 @@ class StadiumScene(Scene):
np.random.uniform(0.4, 0.8) if random_height else 0.0
])
def get_floor_height(self, floor):
del floor
return 0.0
def reset_floor(self, floor=0, additional_elevation=0.05, height=None):
return
def get_shortest_path(self, floor, source_world, target_world, entire_path=False):
assert False, 'cannot compute shortest path in StadiumScene'
class BuildingScene(Scene):
"""
Gibson Environment building scenes
@ -90,9 +108,6 @@ class BuildingScene(Scene):
self.num_waypoints = num_waypoints
self.waypoint_interval = int(waypoint_resolution / trav_map_resolution)
def l2_distance(self, a, b):
return np.linalg.norm(np.array(a) - np.array(b))
def load(self):
"""
Load the mesh into pybullet
@ -141,19 +156,34 @@ class BuildingScene(Scene):
self.floors = sorted(list(map(float, f.readlines())))
print('floors', self.floors)
for f in range(len(self.floors)):
trav_map = Image.open(os.path.join(get_model_path(self.model_id), 'floor_trav_{}.png'.format(f)))
obstacle_map = Image.open(os.path.join(get_model_path(self.model_id), 'floor_{}.png'.format(f)))
trav_map = np.array(Image.open(
os.path.join(get_model_path(self.model_id), 'floor_trav_{}.png'.format(f))
))
obstacle_map = np.array(Image.open(
os.path.join(get_model_path(self.model_id), 'floor_{}.png'.format(f))
))
if self.trav_map_original_size is None:
width, height = trav_map.size
assert width == height, 'trav map is not a square'
height, width = trav_map.shape
assert height == width, 'trav map is not a square'
self.trav_map_original_size = height
self.trav_map_size = int(self.trav_map_original_size * self.trav_map_default_resolution / self.trav_map_resolution)
trav_map = np.array(trav_map.resize((self.trav_map_size, self.trav_map_size)))
obstacle_map = np.array(obstacle_map.resize((self.trav_map_size, self.trav_map_size)))
self.trav_map_size = int(self.trav_map_original_size *
self.trav_map_default_resolution /
self.trav_map_resolution)
trav_map[obstacle_map == 0] = 0
trav_map = cv2.resize(trav_map, (self.trav_map_size, self.trav_map_size))
trav_map = cv2.erode(trav_map, np.ones((self.trav_map_erosion, self.trav_map_erosion)))
trav_map[trav_map < 255] = 0
if self.build_graph:
graph_file = os.path.join(get_model_path(self.model_id), 'floor_trav_{}.p'.format(f))
if os.path.isfile(graph_file):
print("load traversable graph")
with open(graph_file, 'rb') as pfile:
g = pickle.load(pfile)
else:
print("build traversable graph")
g = nx.Graph()
for i in range(self.trav_map_size):
for j in range(self.trav_map_size):
@ -164,16 +194,18 @@ class BuildingScene(Scene):
for n in neighbors:
if 0 <= n[0] < self.trav_map_size and 0 <= n[1] < self.trav_map_size and \
trav_map[n[0], n[1]] > 0:
g.add_edge(n, (i, j), weight=self.l2_distance(n, (i, j)))
g.add_edge(n, (i, j), weight=l2_distance(n, (i, j)))
# only take the largest connected component
largest_cc = max(nx.connected_components(g), key=len)
g = g.subgraph(largest_cc).copy()
self.floor_graph.append(g)
with open(graph_file, 'wb') as pfile:
pickle.dump(g, pfile, protocol=pickle.HIGHEST_PROTOCOL)
self.floor_graph.append(g)
# update trav_map accordingly
trav_map[:, :] = 0
for node in largest_cc:
for node in g.nodes:
trav_map[node[0], node[1]] = 255
self.floor_map.append(trav_map)
@ -205,6 +237,11 @@ class BuildingScene(Scene):
def world_to_map(self, xy):
return np.flip((xy / self.trav_map_resolution + self.trav_map_size / 2.0)).astype(np.int)
def has_node(self, floor, world_xy):
map_xy = tuple(self.world_to_map(world_xy))
g = self.floor_graph[floor]
return g.has_node(map_xy)
def get_shortest_path(self, floor, source_world, target_world, entire_path=False):
# print("called shortest path", source_world, target_world)
assert self.build_graph, 'cannot get shortest path without building the graph'
@ -216,20 +253,21 @@ class BuildingScene(Scene):
if not g.has_node(target_map):
nodes = np.array(g.nodes)
closest_node = tuple(nodes[np.argmin(np.linalg.norm(nodes - target_map, axis=1))])
g.add_edge(closest_node, target_map, weight=self.l2_distance(closest_node, target_map))
g.add_edge(closest_node, target_map, weight=l2_distance(closest_node, target_map))
if not g.has_node(source_map):
nodes = np.array(g.nodes)
closest_node = tuple(nodes[np.argmin(np.linalg.norm(nodes - source_map, axis=1))])
g.add_edge(closest_node, source_map, weight=self.l2_distance(closest_node, source_map))
g.add_edge(closest_node, source_map, weight=l2_distance(closest_node, source_map))
path_map = np.array(nx.astar_path(g, source_map, target_map, heuristic=self.l2_distance))
path_map = np.array(nx.astar_path(g, source_map, target_map, heuristic=l2_distance))
path_world = self.map_to_world(path_map)
geodesic_distance = np.sum(np.linalg.norm(path_world[1:] - path_world[:-1], axis=1))
path_world = path_world[::self.waypoint_interval]
if not entire_path:
path_world = path_world[::self.waypoint_interval][:self.num_waypoints]
path_world = path_world[:self.num_waypoints]
num_remaining_waypoints = self.num_waypoints - path_world.shape[0]
if num_remaining_waypoints > 0:
remaining_waypoints = np.tile(target_world, (num_remaining_waypoints, 1))

21
gibson2/core/simulator.py
View File

@ -36,6 +36,7 @@ class Simulator:
self.gravity = gravity
self.timestep = timestep
self.mode = mode
# renderer
self.resolution = resolution
self.fov = fov
@ -63,8 +64,7 @@ class Simulator:
"""
Destroy the MeshRenderer and physics simulator and start again.
"""
self.renderer.release()
p.disconnect(self.cid)
self.disconnect()
self.load()
def load(self):
@ -91,6 +91,7 @@ class Simulator:
self.cid = p.connect(p.DIRECT)
p.setTimeStep(self.timestep)
p.setGravity(0, 0, -self.gravity)
p.setPhysicsEngineParameter(enableFileCaching=0)
if self.mode == 'gui' and not self.render_to_tensor:
self.add_viewer()
@ -101,14 +102,13 @@ class Simulator:
self.objects = []
def import_scene(self, scene, texture_scale=1.0, load_texture=True, class_id=0):
"""
Import a scene. A scene could be a synthetic one or a realistic Gibson Environment.
:param scene: Scene object
:param texture_scale: Option to scale down the texture for rendering
:param load_texture: If you don't need rgb output, texture loading could be skipped to make rendering faster
:param class_id: The class_id for background for rendering semantics.
:param class_id: Class id for rendering semantic segmentation
"""
new_objects = scene.load()
@ -153,7 +153,7 @@ class Simulator:
def import_object(self, object, class_id=0):
"""
:param object: Object to load
:param class_id: class_id to show for semantic segmentation mask
:param class_id: Class id for rendering semantic segmentation
"""
new_object = object.load()
self.objects.append(new_object)
@ -215,7 +215,7 @@ class Simulator:
Import a robot into Simulator
:param robot: Robot
:param class_id: class_id to show for semantic segmentation mask
:param class_id: Class id for rendering semantic segmentation
:return: id for robot in pybullet
"""
@ -298,7 +298,7 @@ class Simulator:
Import articulated objects into simulator
:param obj:
:param class_id: class_id to show for semantic segmentation mask
:param class_id: Class id for rendering semantic segmentation
:return: pybulet id
"""
ids = obj.load()
@ -382,6 +382,12 @@ class Simulator:
"""
p.stepSimulation()
self.sync()
def sync(self):
"""
Update positions in renderer without stepping the simulation. Usually used in the reset() function
"""
for instance in self.renderer.instances:
if instance.dynamic:
self.update_position(instance)
@ -427,5 +433,6 @@ class Simulator:
clean up the simulator
"""
if self.isconnected():
p.resetSimulation(physicsClientId=self.cid)
p.disconnect(self.cid)
self.renderer.release()

27
gibson2/envs/base_env.py
View File

@ -1,4 +1,5 @@
from gibson2.core.physics.robot_locomotors import Turtlebot, Husky, Ant, Humanoid, JR2, JR2_Kinova, Freight, Fetch
from gibson2.core.physics.robot_locomotors \
import Turtlebot, Husky, Ant, Humanoid, JR2, JR2_Kinova, Freight, Fetch, Locobot
from gibson2.core.simulator import Simulator
from gibson2.core.physics.scene import BuildingScene, StadiumScene
import gibson2
@ -16,21 +17,31 @@ class BaseEnv(gym.Env):
config_file,
model_id=None,
mode='headless',
action_timestep=1 / 10.0,
physics_timestep=1 / 240.0,
device_idx=0):
"""
:param config_file: config_file path
:param model_id: override model_id in config file
:param mode: headless or gui mode
:param device_idx: which GPU to run the simulation and rendering on
:param action_timestep: environment executes action per action_timestep second
:param physics_timestep: physics timestep for pybullet
:param device_idx: device_idx: which GPU to run the simulation and rendering on
"""
self.config = parse_config(config_file)
if model_id is not None:
self.config['model_id'] = model_id
self.mode = mode
self.action_timestep = action_timestep
self.physics_timestep = physics_timestep
self.simulator = Simulator(mode=mode,
timestep=physics_timestep,
use_fisheye=self.config.get('fisheye', False),
resolution=self.config.get('resolution', 64),
fov=self.config.get('fov', 90),
device_idx=device_idx)
self.simulator_loop = int(self.action_timestep / self.simulator.timestep)
self.load()
def reload(self, config_file):
@ -77,6 +88,8 @@ class BaseEnv(gym.Env):
robot = Freight(self.config)
elif self.config['robot'] == 'Fetch':
robot = Fetch(self.config)
elif self.config['robot'] == 'Locobot':
robot = Locobot(self.config)
else:
raise Exception('unknown robot type: {}'.format(self.config['robot']))
@ -99,10 +112,16 @@ class BaseEnv(gym.Env):
self.simulator.step()
def step(self, action):
return NotImplementedError
"""
Overwritten by subclasses
"""
return NotImplementedError()
def reset(self):
return NotImplementedError
"""
Overwritten by subclasses
"""
return NotImplementedError()
def set_mode(self, mode):
self.simulator.mode = mode

1478
gibson2/envs/locomotor_env.py
View File

File diff suppressed because it is too large Load Diff

26
gibson2/utils/utils.py
View File

@ -1,6 +1,6 @@
import numpy as np
import yaml
from scipy.spatial.transform import Rotation as R
# File I/O related
def parse_config(config):
@ -8,19 +8,29 @@ def parse_config(config):
config_data = yaml.load(f, Loader=yaml.FullLoader)
return config_data
# Geometry related
def rotate_vector_3d(v, r, p, y):
"""Rotates vector by roll, pitch and yaw counterclockwise"""
rot_x = np.array([[1, 0, 0], [0, np.cos(-r), -np.sin(-r)], [0, np.sin(-r), np.cos(-r)]])
rot_y = np.array([[np.cos(-p), 0, np.sin(-p)], [0, 1, 0], [-np.sin(-p), 0, np.cos(-p)]])
rot_z = np.array([[np.cos(-y), -np.sin(-y), 0], [np.sin(-y), np.cos(-y), 0], [0, 0, 1]])
return np.dot(rot_x, np.dot(rot_y, np.dot(rot_z, v)))
"""Rotates 3d vector by roll, pitch and yaw counterclockwise"""
local_to_global = R.from_euler('xyz', [r, p, y]).as_dcm()
global_to_local = local_to_global.T
return np.dot(global_to_local, v)
def rotate_vector_2d(v, yaw):
"""Rotates 2d vector by yaw counterclockwise"""
local_to_global = R.from_euler('z', yaw).as_dcm()
global_to_local = local_to_global.T
global_to_local = global_to_local[:2, :2]
if len(v.shape) == 1:
return np.dot(global_to_local, v)
elif len(v.shape) == 2:
return np.dot(global_to_local, v.T).T
else:
print('Incorrect input shape for rotate_vector_2d', v.shape)
return v
def l2_distance(v1, v2):
"""Returns the L2 distance between vector v1 and v2."""
return np.linalg.norm(v1 - v2)
return np.linalg.norm(np.array(v1) - np.array(v2))
def quatFromXYZW(xyzw, seq):

2
setup.py
View File

@ -110,7 +110,7 @@ setup(
'gym==0.12',
'Pillow==5.4.1',
'PyYAML==5.3',
'pybullet==2.4.1',
'pybullet==2.4.9',
'transforms3d==0.3.1',
'tqdm==4.19.9',
'Pillow==5.4.1',