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