move scene and simulator
This commit is contained in:
fxia22
parent
87838a1120
commit
e4f3ec9a0e
|
|
@ -0,0 +1,59 @@
|
||||||
|
import pybullet as p
|
||||||
|
import os, inspect
|
||||||
|
|
||||||
|
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
|
||||||
|
parentdir = os.path.dirname(currentdir)
|
||||||
|
os.sys.path.insert(0, parentdir)
|
||||||
|
import pybullet_data
|
||||||
|
from gibson2.data.datasets import get_model_path
|
||||||
|
|
||||||
|
|
||||||
|
class Scene:
|
||||||
|
def load(self):
|
||||||
|
raise (NotImplementedError())
|
||||||
|
|
||||||
|
|
||||||
|
class StadiumScene(Scene):
|
||||||
|
zero_at_running_strip_start_line = True # if False, center of coordinates (0,0,0) will be at the middle of the stadium
|
||||||
|
stadium_halflen = 105 * 0.25 # FOOBALL_FIELD_HALFLEN
|
||||||
|
stadium_halfwidth = 50 * 0.25 # FOOBALL_FIELD_HALFWID
|
||||||
|
|
||||||
|
def load(self):
|
||||||
|
filename = os.path.join(pybullet_data.getDataPath(), "stadium_no_collision.sdf")
|
||||||
|
self.stadium = p.loadSDF(filename)
|
||||||
|
planeName = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
|
||||||
|
self.ground_plane_mjcf = p.loadMJCF(planeName)
|
||||||
|
for i in self.ground_plane_mjcf:
|
||||||
|
pos, orn = p.getBasePositionAndOrientation(i)
|
||||||
|
p.resetBasePositionAndOrientation(i, [pos[0], pos[1], pos[2] - 0.005], orn)
|
||||||
|
|
||||||
|
for i in self.ground_plane_mjcf:
|
||||||
|
p.changeVisualShape(i, -1, rgbaColor=[1, 1, 1, 0.5])
|
||||||
|
|
||||||
|
self.scene_obj_list = self.stadium
|
||||||
|
|
||||||
|
return [self.stadium, self.ground_plane_mjcf]
|
||||||
|
|
||||||
|
|
||||||
|
class BuildingScene(Scene):
|
||||||
|
def __init__(self, model_id):
|
||||||
|
self.model_id = model_id
|
||||||
|
|
||||||
|
def load(self):
|
||||||
|
filename = os.path.join(get_model_path(self.model_id), "mesh_z_up.obj")
|
||||||
|
scaling = [1, 1, 1]
|
||||||
|
collisionId = p.createCollisionShape(p.GEOM_MESH, fileName=filename, meshScale=scaling,
|
||||||
|
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
|
||||||
|
visualId = -1
|
||||||
|
boundaryUid = p.createMultiBody(baseCollisionShapeIndex=collisionId, baseVisualShapeIndex=visualId)
|
||||||
|
p.changeDynamics(boundaryUid, -1, lateralFriction=1)
|
||||||
|
self.scene_obj_list = [(boundaryUid, -1)] # baselink index -1
|
||||||
|
|
||||||
|
planeName = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
|
||||||
|
self.ground_plane_mjcf = p.loadMJCF(planeName)
|
||||||
|
|
||||||
|
p.resetBasePositionAndOrientation(self.ground_plane_mjcf[0], posObj=[0, 0, 0], ornObj=[0, 0, 0, 1])
|
||||||
|
p.changeVisualShape(boundaryUid, -1, rgbaColor=[168 / 255.0, 164 / 255.0, 92 / 255.0, 1.0],
|
||||||
|
specularColor=[0.5, 0.5, 0.5])
|
||||||
|
|
||||||
|
return [(boundaryUid,), self.ground_plane_mjcf]
|
||||||
|
|
@ -1,84 +0,0 @@
|
||||||
import sys, os
|
|
||||||
sys.path.append(os.path.dirname(__file__))
|
|
||||||
import pybullet as p
|
|
||||||
import time
|
|
||||||
import gym
|
|
||||||
|
|
||||||
|
|
||||||
class Scene:
|
|
||||||
"A base class for single- and multiplayer scenes"
|
|
||||||
|
|
||||||
def __init__(self, gravity, timestep, frame_skip, env):
|
|
||||||
#self.np_random, seed = gym.utils.seeding.np_random(None)
|
|
||||||
self.timestep = timestep
|
|
||||||
self.frame_skip = frame_skip
|
|
||||||
self.env = env
|
|
||||||
|
|
||||||
self.dt = self.timestep * self.frame_skip
|
|
||||||
self.cpp_world = World(gravity, timestep, frame_skip)
|
|
||||||
|
|
||||||
self.test_window_still_open = True # or never opened
|
|
||||||
self.human_render_detected = False # if user wants render("human"), we open test window
|
|
||||||
|
|
||||||
self.multiplayer_robots = {}
|
|
||||||
|
|
||||||
def test_window(self):
|
|
||||||
"Call this function every frame, to see what's going on. Not necessary in learning."
|
|
||||||
self.human_render_detected = True
|
|
||||||
return self.test_window_still_open
|
|
||||||
|
|
||||||
def actor_introduce(self, robot):
|
|
||||||
"Usually after scene reset"
|
|
||||||
if not self.multiplayer: return
|
|
||||||
self.multiplayer_robots[robot.player_n] = robot
|
|
||||||
|
|
||||||
def actor_is_active(self, robot):
|
|
||||||
"""
|
|
||||||
Used by robots to see if they are free to exclusiveley put their HUD on the test window.
|
|
||||||
Later can be used for click-focus robots.
|
|
||||||
"""
|
|
||||||
return not self.multiplayer
|
|
||||||
|
|
||||||
def episode_restart(self):
|
|
||||||
"This function gets overridden by specific scene, to reset specific objects into their start positions"
|
|
||||||
self.cpp_world.clean_everything()
|
|
||||||
#self.cpp_world.test_window_history_reset()
|
|
||||||
|
|
||||||
def global_step(self):
|
|
||||||
"""
|
|
||||||
The idea is: apply motor torques for all robots, then call global_step(), then collect
|
|
||||||
observations from robots using step() with the same action.
|
|
||||||
"""
|
|
||||||
self.cpp_world.step(self.frame_skip)
|
|
||||||
|
|
||||||
class SingleRobotEmptyScene(Scene):
|
|
||||||
multiplayer = False # this class is used "as is" for InvertedPendulum, Reacher
|
|
||||||
|
|
||||||
class World:
|
|
||||||
|
|
||||||
def __init__(self, gravity, timestep, frame_skip):
|
|
||||||
self.gravity = gravity
|
|
||||||
self.timestep = timestep
|
|
||||||
self.frame_skip = frame_skip
|
|
||||||
self.clean_everything()
|
|
||||||
self._is_render = False
|
|
||||||
self._last_frame_time = 0.0
|
|
||||||
|
|
||||||
def clean_everything(self):
|
|
||||||
#p.resetSimulation()
|
|
||||||
p.setGravity(0, 0, -self.gravity)
|
|
||||||
#p.setDefaultContactERP(0.9)
|
|
||||||
p.setPhysicsEngineParameter(fixedTimeStep=self.timestep*self.frame_skip, numSolverIterations=50, numSubSteps=(self.frame_skip-1))
|
|
||||||
|
|
||||||
def step(self, frame_skip):
|
|
||||||
if self._is_render:
|
|
||||||
# Sleep, otherwise the computation takes less time than real time,
|
|
||||||
# which will make the visualization like a fast-forward video.
|
|
||||||
time_spent = time.time() - self._last_frame_time
|
|
||||||
self._last_frame_time = time.time()
|
|
||||||
#time_to_sleep = self.timestep * self.frame_skip - time_spent
|
|
||||||
#if time_to_sleep > 0:
|
|
||||||
# time.sleep(time_to_sleep)
|
|
||||||
p.stepSimulation()
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,71 +0,0 @@
|
||||||
import os, inspect
|
|
||||||
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
|
|
||||||
parentdir = os.path.dirname(currentdir)
|
|
||||||
os.sys.path.insert(0,parentdir)
|
|
||||||
import pybullet_data
|
|
||||||
|
|
||||||
from gibson2.data.datasets import get_model_path
|
|
||||||
from gibson2.core.physics.scene_abstract import Scene
|
|
||||||
import pybullet as p
|
|
||||||
|
|
||||||
|
|
||||||
class BuildingScene(Scene):
|
|
||||||
def __init__(self, robot, model_id, gravity, timestep, frame_skip, env = None):
|
|
||||||
Scene.__init__(self, gravity, timestep, frame_skip, env)
|
|
||||||
|
|
||||||
# contains cpp_world.clean_everything()
|
|
||||||
# stadium_pose = cpp_household.Pose()
|
|
||||||
# if self.zero_at_running_strip_start_line:
|
|
||||||
# stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants
|
|
||||||
|
|
||||||
filename = os.path.join(get_model_path(model_id), "mesh_z_up.obj")
|
|
||||||
#filename = os.path.join(get_model_path(model_id), "3d", "blender.obj")
|
|
||||||
#textureID = p.loadTexture(os.path.join(get_model_path(model_id), "3d", "rgb.mtl"))
|
|
||||||
|
|
||||||
if robot.model_type == "MJCF":
|
|
||||||
MJCF_SCALING = robot.mjcf_scaling
|
|
||||||
scaling = [1.0/MJCF_SCALING, 1.0/MJCF_SCALING, 0.6/MJCF_SCALING]
|
|
||||||
else:
|
|
||||||
scaling = [1, 1, 1]
|
|
||||||
magnified = [2, 2, 2]
|
|
||||||
|
|
||||||
collisionId = p.createCollisionShape(p.GEOM_MESH, fileName=filename, meshScale=scaling, flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
|
|
||||||
|
|
||||||
|
|
||||||
view_only_mesh = os.path.join(get_model_path(model_id), "mesh_view_only_z_up.obj")
|
|
||||||
if os.path.exists(view_only_mesh):
|
|
||||||
visualId = p.createVisualShape(p.GEOM_MESH,
|
|
||||||
fileName=view_only_mesh,
|
|
||||||
meshScale=scaling, flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
|
|
||||||
else:
|
|
||||||
visualId = -1
|
|
||||||
|
|
||||||
boundaryUid = p.createMultiBody(baseCollisionShapeIndex = collisionId, baseVisualShapeIndex = visualId)
|
|
||||||
p.changeDynamics(boundaryUid, -1, lateralFriction=1)
|
|
||||||
#print(p.getDynamicsInfo(boundaryUid, -1))
|
|
||||||
self.scene_obj_list = [(boundaryUid, -1)] # baselink index -1
|
|
||||||
|
|
||||||
|
|
||||||
planeName = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
|
|
||||||
self.ground_plane_mjcf = p.loadMJCF(planeName)
|
|
||||||
|
|
||||||
if "z_offset" in self.env.config:
|
|
||||||
z_offset = self.env.config["z_offset"]
|
|
||||||
else:
|
|
||||||
z_offset = -10 #with hole filling, we don't need ground plane to be the same height as actual floors
|
|
||||||
|
|
||||||
p.resetBasePositionAndOrientation(self.ground_plane_mjcf[0], posObj = [0,0,z_offset], ornObj = [0,0,0,1])
|
|
||||||
p.changeVisualShape(boundaryUid, -1, rgbaColor=[168 / 255.0, 164 / 255.0, 92 / 255.0, 1.0],
|
|
||||||
specularColor=[0.5, 0.5, 0.5])
|
|
||||||
|
|
||||||
def episode_restart(self):
|
|
||||||
Scene.episode_restart(self)
|
|
||||||
|
|
||||||
|
|
||||||
class SinglePlayerBuildingScene(BuildingScene):
|
|
||||||
multiplayer = False
|
|
||||||
def __init__(self, robot, model_id, gravity, timestep, frame_skip, env = None):
|
|
||||||
BuildingScene.__init__(self, robot, model_id, gravity, timestep, frame_skip, env)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,53 +0,0 @@
|
||||||
from gibson2.core.physics.scene_abstract import Scene
|
|
||||||
import pybullet as p
|
|
||||||
import os
|
|
||||||
import pybullet_data
|
|
||||||
|
|
||||||
class StadiumScene(Scene):
|
|
||||||
zero_at_running_strip_start_line = True # if False, center of coordinates (0,0,0) will be at the middle of the stadium
|
|
||||||
stadium_halflen = 105*0.25 # FOOBALL_FIELD_HALFLEN
|
|
||||||
stadium_halfwidth = 50*0.25 # FOOBALL_FIELD_HALFWID
|
|
||||||
|
|
||||||
def __init__(self, robot, gravity, timestep, frame_skip, env=None):
|
|
||||||
Scene.__init__(self, gravity, timestep, frame_skip, env)
|
|
||||||
|
|
||||||
filename = os.path.join(pybullet_data.getDataPath(), "stadium_no_collision.sdf")
|
|
||||||
self.stadium = p.loadSDF(filename)
|
|
||||||
planeName = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
|
|
||||||
self.ground_plane_mjcf = p.loadMJCF(planeName)
|
|
||||||
for i in self.ground_plane_mjcf:
|
|
||||||
pos, orn = p.getBasePositionAndOrientation(i)
|
|
||||||
p.resetBasePositionAndOrientation(i, [pos[0], pos[1], pos[2] - 0.005], orn)
|
|
||||||
|
|
||||||
for i in self.ground_plane_mjcf:
|
|
||||||
p.changeVisualShape(i, -1, rgbaColor=[1, 1, 1, 0.5])
|
|
||||||
|
|
||||||
self.scene_obj_list = self.stadium
|
|
||||||
|
|
||||||
|
|
||||||
def episode_restart(self):
|
|
||||||
Scene.episode_restart(self) # contains cpp_world.clean_everything()
|
|
||||||
|
|
||||||
|
|
||||||
class SinglePlayerStadiumScene(StadiumScene):
|
|
||||||
"This scene created by environment, to work in a way as if there was no concept of scene visible to user."
|
|
||||||
multiplayer = False
|
|
||||||
|
|
||||||
class MultiplayerStadiumScene(StadiumScene):
|
|
||||||
multiplayer = True
|
|
||||||
players_count = 3
|
|
||||||
def actor_introduce(self, robot):
|
|
||||||
StadiumScene.actor_introduce(self, robot)
|
|
||||||
i = robot.player_n - 1 # 0 1 2 => -1 0 +1
|
|
||||||
robot.move_robot(0, i, 0)
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
|
|
||||||
cid = p.connect(p.SHARED_MEMORY)
|
|
||||||
if (cid < 0):
|
|
||||||
p.connect(p.GUI)
|
|
||||||
|
|
||||||
scene = StadiumScene(gravity=9.8, timestep=0.0165, frame_skip=1)
|
|
||||||
scene.episode_restart()
|
|
||||||
while True:
|
|
||||||
pass
|
|
||||||
|
|
@ -0,0 +1,32 @@
|
||||||
|
import pybullet as p
|
||||||
|
from gibson2.core.physics.scene import StadiumScene, BuildingScene
|
||||||
|
|
||||||
|
|
||||||
|
class Simulator:
|
||||||
|
def __init__(self, gravity=9.8, timestep=1 / 240.0):
|
||||||
|
self.gravity = gravity
|
||||||
|
self.timestep = timestep
|
||||||
|
self.cid = p.connect(p.GUI)
|
||||||
|
p.setTimeStep(self.timestep)
|
||||||
|
p.setGravity(0, 0, -self.gravity)
|
||||||
|
self.objects = []
|
||||||
|
|
||||||
|
def import_scene(self, scene):
|
||||||
|
new_objects = scene.load()
|
||||||
|
for item in new_objects:
|
||||||
|
self.objects.append(item)
|
||||||
|
|
||||||
|
def step(self):
|
||||||
|
p.stepSimulation()
|
||||||
|
|
||||||
|
def isconnected(self):
|
||||||
|
return p.getConnectionInfo(self.cid)['isConnected']
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
s = Simulator()
|
||||||
|
scene = BuildingScene('space7')
|
||||||
|
s.import_scene(scene)
|
||||||
|
|
||||||
|
while s.isconnected():
|
||||||
|
s.step()
|
||||||
Loading…
Reference in New Issue