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)
scene = StadiumScene()
scene.load()
robots = []
#config = parse_config('../configs/fetch_p2p_nav.yaml')
#fetch = Fetch(config)
#robots.append(fetch)
config = parse_config('../configs/jr_p2p_nav.yaml')
jr = JR2_Kinova(config)
robots.append(jr)
#config = parse_config('../configs/locobot_p2p_nav.yaml')
#locobot = Locobot(config)
#robots.append(locobot)
#config = parse_config('../configs/turtlebot_p2p_nav.yaml')
#turtlebot = Turtlebot(config)
#robots.append(turtlebot)
positions = [[0, 0, 0]
#[1, 0, 0],
#[0, 1, 0],
#[1, 1, 0]
]
for robot, position in zip(robots, positions):
robot.load()
robot.set_position(position)
robot.robot_specific_reset()
robot.keep_still()
for _ in range(2400): # keep still for 10 seconds
p.stepSimulation()
time.sleep(1. / 240.)
for _ in range(2400): # move with small random actions for 10 seconds
for robot, position in zip(robots, positions):
action = np.random.uniform(-1, 1, robot.action_dim)
#robot.apply_action(action)
robot.apply_action([0, 0, 0, 0, 0, 0, 0])
p.stepSimulation()
time.sleep(1. / 240.0)
p.disconnect()
def main():
p.connect(p.GUI)
p.setGravity(0,0,-9.8)
p.setTimeStep(1./240.)
scene = StadiumScene()
scene.load()
for _ in range(24000): # at least 100 seconds
p.stepSimulation()
time.sleep(1./240.)
p.disconnect()
def main():
config = parse_config('../configs/jr2_reaching.yaml')
s = Simulator(mode='gui', image_width=512, image_height=512)
scene = StadiumScene()
s.import_scene(scene)
jr = JR2_Kinova(config)
s.import_robot(jr)
marker = VisualMarker(visual_shape=p.GEOM_SPHERE,
rgba_color=[0, 0, 1, 0.3],
radius=0.5)
s.import_object(marker)
marker.set_position([0, 4, 1])
for i in range(10000):
with Profiler('Simulator step'):
jr.apply_action([0, 0, 0, 0, 0, 0, 0])
s.step()
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
s.disconnect()
def main():
config = parse_config('../configs/jr2_reaching.yaml')
s = Simulator(mode='gui', image_width=512, image_height=512)
#scene = BuildingScene('Rs',
# build_graph=True,
# pybullet_load_texture=True)
scene = StadiumScene()
s.import_scene(scene)
jr = JR2_Kinova_Head(config)
#turtlebot = Turtlebot(config)
s.import_robot(jr)
for _ in range(10):
obj = YCBObject('003_cracker_box')
s.import_object(obj)
obj.set_position_orientation(np.random.uniform(low=0, high=2, size=3),
[0, 0, 0, 1])
for i in range(10000):
with Profiler('Simulator step'):
jr.apply_action([0, 0, 0.1, 0.1, 0, 0, 0, 0, 0])
s.step()
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
s.disconnect()
def test_multiagent():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot1 = Turtlebot(config)
turtlebot2 = Turtlebot(config)
turtlebot3 = Turtlebot(config)
s.import_robot(turtlebot1)
s.import_robot(turtlebot2)
s.import_robot(turtlebot3)
turtlebot1.set_position([1, 0, 0.5])
turtlebot2.set_position([0, 0, 0.5])
turtlebot3.set_position([-1, 0, 0.5])
nbody = p.getNumBodies()
for i in range(100):
#turtlebot1.apply_action(1)
#turtlebot2.apply_action(1)
#turtlebot3.apply_action(1)
s.step()
s.disconnect()
assert nbody == 7
def test_import_stadium():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
print(s.objects)
assert s.objects == list(range(4))
s.disconnect()
def test_import_box():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
print(s.objects)
# wall = [pos, dim]
wall = [[[0, 7, 1.01], [10, 0.2, 1]], [[0, -7, 1.01], [6.89, 0.1, 1]],
[[7, -1.5, 1.01], [0.1, 5.5, 1]], [[-7, -1, 1.01], [0.1, 6, 1]],
[[-8.55, 5, 1.01], [1.44, 0.1, 1]], [[8.55, 4, 1.01], [1.44, 0.1, 1]]]
obstacles = [[[-0.5, 2, 1.01], [3.5, 0.1, 1]], [[4.5, -1, 1.01], [1.5, 0.1, 1]],
[[-4, -2, 1.01], [0.1, 2, 1]], [[2.5, -4, 1.01], [1.5, 0.1, 1]]]
for i in range(len(wall)):
curr = wall[i]
obj = BoxShape(curr[0], curr[1])
s.import_object(obj)
for i in range(len(obstacles)):
curr = obstacles[i]
obj = BoxShape(curr[0], curr[1])
s.import_object(obj)
config = parse_config(os.path.join(gibson2.root_path, '../test/test.yaml'))
turtlebot1 = Turtlebot(config)
turtlebot2 = Turtlebot(config)
s.import_robot(turtlebot1)
s.import_robot(turtlebot2)
turtlebot1.set_position([6., -6., 0.])
turtlebot2.set_position([-3., 4., 0.])
for i in range(100):
s.step()
s.disconnect()
def load(self):
if self.config['scene'] == 'stadium':
scene = StadiumScene()
elif self.config['scene'] == 'building':
scene = BuildingScene(self.config['model_id'])
self.simulator.import_scene(scene)
if self.config['robot'] == 'Turtlebot':
robot = Turtlebot(self.config)
elif self.config['robot'] == 'Husky':
robot = Husky(self.config)
elif self.config['robot'] == 'Ant':
robot = Ant(self.config)
elif self.config['robot'] == 'Humanoid':
robot = Humanoid(self.config)
elif self.config['robot'] == 'JR2':
robot = JR2(self.config)
elif self.config['robot'] == 'JR2_Kinova':
robot = JR2_Kinova(self.config)
else:
raise Exception('unknown robot type: {}'.format(
self.config['robot']))
self.scene = scene
self.robots = [robot]
for robot in self.robots:
self.simulator.import_robot(robot)
def test_quadrotor():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
quadrotor = Quadrotor(config)
s.import_robot(quadrotor)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_jr2():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
jr2 = JR2(config)
s.import_robot(jr2)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_humanoid():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
humanoid = Humanoid(config)
s.import_robot(humanoid)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_turtlebot():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_fetch():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
fetch = Fetch(config)
s.import_robot(fetch)
for i in range(100):
fetch.calc_state()
s.step()
s.disconnect()
def test_import_object():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
obj = YCBObject('003_cracker_box')
s.import_object(obj)
objs = s.objects
s.disconnect()
assert objs == list(range(5))
def load(self):
"""
Load the scene and robot
"""
if self.config['scene'] == 'empty':
scene = EmptyScene()
elif self.config['scene'] == 'stadium':
scene = StadiumScene()
elif self.config['scene'] == 'building':
scene = BuildingScene(
self.config['model_id'],
waypoint_resolution=self.config.get('waypoint_resolution',
0.2),
num_waypoints=self.config.get('num_waypoints', 10),
build_graph=self.config.get('build_graph', False),
trav_map_resolution=self.config.get('trav_map_resolution',
0.1),
trav_map_erosion=self.config.get('trav_map_erosion', 2),
is_interactive=self.config.get('is_interactive', False),
pybullet_load_texture=self.config.get('pybullet_load_texture',
False),
)
self.simulator.import_scene(scene,
load_texture=self.config.get(
'load_texture', True))
if self.config['robot'] == 'Turtlebot':
robot = Turtlebot(self.config)
elif self.config['robot'] == 'Husky':
robot = Husky(self.config)
elif self.config['robot'] == 'Ant':
robot = Ant(self.config)
elif self.config['robot'] == 'Humanoid':
robot = Humanoid(self.config)
elif self.config['robot'] == 'JR2':
robot = JR2(self.config)
elif self.config['robot'] == 'JR2_Kinova':
robot = JR2_Kinova(self.config)
elif self.config['robot'] == 'Freight':
robot = Freight(self.config)
elif self.config['robot'] == 'Fetch':
robot = Fetch(self.config)
elif self.config['robot'] == 'Locobot':
robot = Locobot(self.config)
elif self.config['robot'] == 'DexHandRobot':
robot = DexHandRobot(self.config)
else:
raise Exception('unknown robot type: {}'.format(
self.config['robot']))
self.scene = scene
self.robots = [robot]
for robot in self.robots:
self.simulator.import_robot(robot)
def test_humanoid_position():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
humanoid = Humanoid(config)
s.import_robot(humanoid)
humanoid.set_position([0, 0, 5])
nbody = p.getNumBodies()
pos = humanoid.get_position()
s.disconnect()
assert nbody == 5
assert np.allclose(pos, np.array([0, 0, 5]))
def test_fetch():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
config = parse_config(
os.path.join(gibson2.root_path, '../test/test_continuous.yaml'))
fetch = Fetch(config)
s.import_robot(fetch)
for i in range(100):
fetch.apply_action(np.array([0] * 2))
fetch.calc_state()
s.step()
s.disconnect()
def test_ant():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
ant = Ant(config)
s.import_robot(ant)
ant2 = Ant(config)
s.import_robot(ant2)
ant2.set_position([0, 2, 2])
nbody = p.getNumBodies()
for i in range(100):
s.step()
s.disconnect()
assert nbody == 6
def test_turtlebot_position():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
turtlebot.set_position([0, 0, 5])
nbody = p.getNumBodies()
pos = turtlebot.get_position()
s.disconnect()
assert nbody == 5
assert np.allclose(pos, np.array([0, 0, 5]))
def test_import_many_object():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
for i in range(30):
obj = YCBObject('003_cracker_box')
s.import_object(obj)
for j in range(1000):
s.step()
last_obj = s.objects[-1]
s.disconnect()
assert (last_obj == 33)
def test_import_human():
s = Simulator(mode='gui')
scene = StadiumScene()
s.import_scene(scene)
obj = Pedestrian()
s.import_object(obj)
for j in range(100):
s.step()
obj.reset_position_orientation([j * 0.001, 0, 0], [0, 0, 0, 1])
last_obj = s.objects[-1]
s.disconnect()
assert (last_obj == 4)
def test_simulator():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
obj = YCBObject('006_mustard_bottle')
for i in range(10):
s.import_object(obj)
obj = YCBObject('002_master_chef_can')
for i in range(10):
s.import_object(obj)
for i in range(1000):
s.step()
s.disconnect()
def test_ant():
s = Simulator(mode='headless', timestep=1 / 40.0)
scene = StadiumScene()
s.import_scene(scene)
ant = Ant(config)
s.import_robot(ant)
ant2 = Ant(config)
s.import_robot(ant2)
ant2.set_position([0, 2, 2])
nbody = p.getNumBodies()
for i in range(100):
s.step()
#ant.apply_action(np.random.randint(17))
#ant2.apply_action(np.random.randint(17))
s.disconnect()
assert nbody == 6
def load(self):
"""
Load the scene and robot
"""
if self.config['scene'] == 'stadium':
scene = StadiumScene()
elif self.config['scene'] == 'building':
scene = BuildingScene(
self.config['model_id'],
build_graph=self.config.get('build_graph', False),
trav_map_erosion=self.config.get('trav_map_erosion', 2),
should_load_replaced_objects=self.config.get(
'should_load_replaced_objects', False))
# scene: class_id = 0
# robot: class_id = 1
# objects: class_id > 1
self.simulator.import_scene(scene,
load_texture=self.config.get(
'load_texture', True),
class_id=0)
if self.config['robot'] == 'Turtlebot':
robot = Turtlebot(self.config)
elif self.config['robot'] == 'Husky':
robot = Husky(self.config)
elif self.config['robot'] == 'Ant':
robot = Ant(self.config)
elif self.config['robot'] == 'Humanoid':
robot = Humanoid(self.config)
elif self.config['robot'] == 'JR2':
robot = JR2(self.config)
elif self.config['robot'] == 'JR2_Kinova':
robot = JR2_Kinova(self.config)
elif self.config['robot'] == 'Freight':
robot = Freight(self.config)
elif self.config['robot'] == 'Fetch':
robot = Fetch(self.config)
else:
raise Exception('unknown robot type: {}'.format(
self.config['robot']))
self.scene = scene
self.robots = [robot]
for robot in self.robots:
self.simulator.import_robot(robot, class_id=1)
def show_action_sensor_space():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
turtlebot.set_position([0, 1, 0.5])
ant = Ant(config)
s.import_robot(ant)
ant.set_position([0, 2, 0.5])
h = Humanoid(config)
s.import_robot(h)
h.set_position([0, 3, 2])
jr = JR2(config)
s.import_robot(jr)
jr.set_position([0, 4, 0.5])
jr2 = JR2_Kinova(config)
s.import_robot(jr2)
jr2.set_position([0, 5, 0.5])
husky = Husky(config)
s.import_robot(husky)
husky.set_position([0, 6, 0.5])
quad = Quadrotor(config)
s.import_robot(quad)
quad.set_position([0, 7, 0.5])
for robot in s.robots:
print(type(robot), len(robot.ordered_joints), robot.calc_state().shape)
for i in range(100):
s.step()
s.disconnect()
def test_import_rbo_object():
s = Simulator(mode='headless')
try:
scene = StadiumScene()
s.import_scene(scene)
obj = RBOObject('book')
s.import_articulated_object(obj)
obj2 = RBOObject('microwave')
s.import_articulated_object(obj2)
obj.set_position([0, 0, 2])
obj2.set_position([0, 1, 2])
obj3 = InteractiveObj(
os.path.join(gibson2.assets_path, 'models', 'scene_components', 'door.urdf'))
s.import_articulated_object(obj3)
for i in range(100):
s.step()
finally:
s.disconnect()
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
#print(instance.pybullet_uuid, link_id, pos, orn)
instance.poses_rot = poses_rot
instance.poses_trans = poses_trans
def isconnected(self):
return p.getConnectionInfo(self.cid)['isConnected']
def disconnect(self):
if self.isconnected():
p.disconnect(self.cid)
self.renderer.release()
if __name__ == '__main__':
s = Simulator()
scene = StadiumScene()
s.import_scene(scene)
obj = YCBObject('006_mustard_bottle')
for i in range(10):
s.import_object(obj)
obj = YCBObject('002_master_chef_can')
for i in range(10):
s.import_object(obj)
while s.isconnected():
s.step()