def test_jr2():
s = Simulator(mode='gui')
try:
scene = BuildingScene('Ohoopee')
s.import_scene(scene)
jr2 = JR2_Kinova(config)
s.import_robot(jr2)
jr2.set_position([-6, 0, 0.1])
obj3 = InteractiveObj(os.path.join(gibson2.assets_path, 'models',
'scene_components', 'door.urdf'),
scale=2)
s.import_interactive_object(obj3)
obj3.set_position_rotation(
[-5, -1, 0], [0, 0, np.sqrt(0.5), np.sqrt(0.5)])
jr2.apply_action([0.005, 0.005, 0, 0, 0, 0, 0, 0, 0, 0])
for _ in range(400):
s.step()
jr2.apply_action([
0, 0, 0, 0, 3.1408197119196117, -1.37402907967774,
-0.8377005721485424, -1.9804208517373096, 0.09322135043256494,
2.62937740156038
])
for _ in range(400):
s.step()
finally:
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 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 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 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 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()
from gibson2.core.physics.robot_locomotors import Turtlebot, JR2_Kinova, Fetch
from gibson2.core.simulator import Simulator
from gibson2.core.physics.scene import EmptyScene
from gibson2.core.physics.interactive_objects import InteractiveObj, BoxShape, YCBObject
from gibson2.utils.utils import parse_config
from gibson2.core.render.profiler import Profiler
import pytest
import pybullet as p
import numpy as np
config = parse_config('../configs/jr_interactive_nav.yaml')
s = Simulator(mode='headless', timestep=1 / 240.0)
scene = EmptyScene()
s.import_scene(scene)
jr = JR2_Kinova(config)
s.import_robot(jr)
jr.robot_body.reset_position([0, 0, 0])
jr.robot_body.reset_orientation([0, 0, 1, 0])
fetch = Fetch(config)
s.import_robot(fetch)
fetch.robot_body.reset_position([0, 1, 0])
fetch.robot_body.reset_orientation([0, 0, 1, 0])
obj = InteractiveObj(
filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf')
s.import_interactive_object(obj)
obj.set_position([-2, 0, 0.5])
obj = InteractiveObj(
filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf')
s.import_interactive_object(obj)
obj.set_position([-2, 2, 0.5])
