def __init__(self, config, gpu_idx=0):
#assert(self.config["envname"] == self.__class__.__name__ or self.config["envname"] == "TestEnv")
self.config = self.parse_config(config)
SemanticRobotEnv.__init__(self,
self.config,
gpu_idx,
scene_type="building",
tracking_camera=tracking_camera)
self.robot_introduce(Husky(self.config, env=self))
self.scene_introduce()
self.total_reward = 0
self.total_frame = 0
self.flag_timeout = 1
self.visualid = -1
self.lastid = None
self.gui = self.config["mode"] == "gui"
if self.gui:
self.visualid = p.createVisualShape(
p.GEOM_MESH,
fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'),
meshScale=[0.2, 0.2, 0.2],
rgbaColor=[1, 0, 0, 0.7])
self.colisionid = p.createCollisionShape(
p.GEOM_MESH,
fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'),
meshScale=[0.2, 0.2, 0.2])
self.lastid = None
self.obstacle_dist = 100
self.semantic_flagIds = []
debug_semantic = 1
if debug_semantic and self.gui:
for i in range(self.semantic_pos.shape[0]):
pos = self.semantic_pos[i]
pos[2] += 0.2 # make flag slight above object
visualId = p.createVisualShape(p.GEOM_MESH,
fileName=os.path.join(
pybullet_data.getDataPath(),
'cube.obj'),
meshScale=[0.1, 0.1, 0.1],
rgbaColor=[1, 0, 0, 0.7])
flagId = p.createMultiBody(baseVisualShapeIndex=visualId,
baseCollisionShapeIndex=-1,
basePosition=pos)
self.semantic_flagIds.append(flagId)
def add_mesh(self,
obj_id,
obj_file,
transform,
vis_mesh_file=None,
static=False):
if static:
cid = p.createCollisionShape(p.GEOM_MESH,
fileName=obj_file,
meshScale=transform.scale,
flags=p.GEOM_FORCE_CONCAVE_TRIMESH,
physicsClientId=self._pid)
else:
cid = p.createCollisionShape(p.GEOM_MESH,
fileName=obj_file,
meshScale=transform.scale,
physicsClientId=self._pid)
vid = -1
if vis_mesh_file:
vid = p.createVisualShape(p.GEOM_MESH,
fileName=vis_mesh_file,
meshScale=transform.scale,
physicsClientId=self._pid)
rot_q = np.roll(transform.rotation.elements,
-1) # w,x,y,z -> x,y,z,w (which pybullet expects)
mass = 0 if static else 1
bid = p.createMultiBody(baseMass=mass,
baseCollisionShapeIndex=cid,
baseVisualShapeIndex=vid,
basePosition=transform.translation,
baseOrientation=rot_q,
physicsClientId=self._pid)
body = Body(id=obj_id, bid=bid, vid=vid, cid=cid, static=static)
self._obj_id_to_body[obj_id] = body
self._bid_to_body[bid] = body
return body
def __init__(self, gl, pos):
super(Quad, self).__init__(gl, pos)
vbo = gl.buffer(Quad.VB)
vbo_content = [(vbo, "4f", "in_pos")]
ibo = gl.buffer(Quad.IB)
self.program = Materials.program(gl, "node")
self.vao = gl.vertex_array(self.program, vbo_content, ibo)
self.col_id = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=[1.0, 1.0, 1.0])
self.col_debug_id = pb.createVisualShape(pb.GEOM_BOX,
halfExtents=[1.0, 1.0, 1.0])
pb.createMultiBody(0.0, self.col_id, self.col_debug_id, [*pos],
[*self.rotation])
def load_all_models():
global pixel_collision, wall_collision
orient = p.getQuaternionFromEuler([math.pi / 2, 0, 0])
extents = pixel_extents * 0.5
pixel_collision = p.createCollisionShape(shapeType=p.GEOM_BOX,
halfExtents=extents,
collisionFramePosition=extents)
wall_extents = np.array([SCENE_SIZE, WALL_THICKNESS, WALL_HEIGHT]) * 0.5
wall_offsets = wall_extents.copy()
wall_offsets[1] = 0
wall_collision = p.createCollisionShape(
shapeType=p.GEOM_BOX,
halfExtents=wall_extents,
collisionFramePosition=wall_offsets)
dictfile = open('models/objlist.txt')
for line in dictfile:
split = line[:-1].split(' ')
assert (len(split) == 3)
objname = split[0]
units = split[1]
scale = [UNIT_SCALE[units]] * 3
model_names.append(objname)
units_dict[objname] = units
pos = [float(x) for x in split[2].split(',')]
visual_shape_dict[objname] = p.createVisualShape(
shapeType=p.GEOM_MESH,
fileName='models/' + objname + '.obj',
visualFrameOrientation=orient,
visualFramePosition=pos,
meshScale=scale)
collision_shape_dict[objname] = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName='models/' + objname + '.obj',
collisionFrameOrientation=orient,
collisionFramePosition=pos,
meshScale=scale)
def reset(self, x=0, y=0, z=0.05, q1=0, q2=0, q3=0, q4=1, gravity=-9.81,
ML_R=21.5, ML_Kv=10.5, ML_Kvis=0.0005,
MR_R=21.5, MR_Kv=10.5, MR_Kvis=0.0005,
latency=0.02):
p.resetSimulation()
p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, 0)
p.setGravity(0, 0, gravity)
self.time = 0
self.robot = p.loadURDF('walker_a/urdf/walker_a_0_5.urdf', basePosition=[0, 0, 1],
flags=p.URDF_USE_INERTIA_FROM_FILE & p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
local_vel, local_rot_vel, acc = self.local_pose(0, self.sim_timestep * self.action_every_x_timestep)
jointPosition, jointVelocity = self.getJointStates(self.robot)
if self.include_joint_velocity:
state = jointPosition.extend(jointVelocity)
else:
state = jointPosition
##### Environment section
return jointPosition
def _load(self):
baseOrientation = [0, 0, 0, 1]
colBoxId = p.createCollisionShape(p.GEOM_BOX, halfExtents=self.dimension)
visualShapeId = p.createVisualShape(p.GEOM_BOX, halfExtents=self.dimension, rgbaColor=self.color)
if self.visual_only:
body_id = p.createMultiBody(baseCollisionShapeIndex=-1,
baseVisualShapeIndex=visualShapeId)
else:
body_id = p.createMultiBody(baseMass=self.mass,
baseCollisionShapeIndex=colBoxId,
baseVisualShapeIndex=visualShapeId)
p.resetBasePositionAndOrientation(body_id, self.basePos, baseOrientation)
return body_id
def camera_test(p):
extents = 0.25
colcubeId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[0.01, 0.01, 0.01])
object = [
p.createMultiBody(0.1, colcubeId, 2, [extents, extents, 0.05],
[0.000000, 0.000000, 0.0, 1.0])
]
#object = [p.createMultiBody(0.1, colcubeId, 2, [-extents, extents, 0.05], [0.000000, 0.000000, 0.0, 1.0])]
#object = [p.createMultiBody(0.1, colcubeId, 2, [extents, -extents, 0.05], [0.000000, 0.000000, 0.0, 1.0])]
object = [
p.createMultiBody(0.1, colcubeId, 2, [-extents, -extents, 0.05],
[0.000000, 0.000000, 0.0, 1.0])
]
def load(self):
mass = 200
# basePosition = [1,2,2]
baseOrientation = [0, 0, 0, 1]
colBoxId = p.createCollisionShape(p.GEOM_BOX, halfExtents=self.dimension)
visualShapeId = p.createVisualShape(p.GEOM_BOX, halfExtents=self.dimension)
self.body_id = p.createMultiBody(baseMass=mass,
baseCollisionShapeIndex=colBoxId,
baseVisualShapeIndex=visualShapeId,
basePosition=self.basePos,
baseOrientation=baseOrientation)
return self.body_id
def createStrut(radius,
length,
mass,
basePosition,
baseOrientation=[0, 0, 0, 1]):
visualShapeId = -1
colCylinderId = p.createCollisionShape(p.GEOM_CYLINDER,
radius=radius,
height=length)
cylinderUid = p.createMultiBody(mass, colCylinderId, visualShapeId,
basePosition, baseOrientation)
return cylinderUid
def make_heightfield(self, height_map=None):
if self.config["terrain_name"] == "flat":
return
if hasattr(self, 'terrain'):
p.removeBody(self.terrain, physicsClientId=self.client_ID)
if height_map is None:
heightfieldData = np.zeros(self.config["env_width"] * self.config["max_steps"])
terrainShape = p.createCollisionShape(shapeType=p.GEOM_HEIGHTFIELD, meshScale=[self.config["mesh_scale_lat"] , self.config["mesh_scale_lat"] , self.config["mesh_scale_vert"]],
heightfieldTextureScaling=(self.config["env_width"] - 1) / 2,
heightfieldData=heightfieldData,
numHeightfieldRows=self.config["max_steps"],
numHeightfieldColumns=self.config["env_width"], physicsClientId=self.client_ID)
else:
heightfieldData = height_map.ravel(order='F')
terrainShape = p.createCollisionShape(shapeType=p.GEOM_HEIGHTFIELD, meshScale=[self.config["mesh_scale_lat"], self.config["mesh_scale_lat"], self.config["mesh_scale_vert"]],
heightfieldTextureScaling=(self.config["env_width"] - 1) / 2,
heightfieldData=heightfieldData,
numHeightfieldRows=height_map.shape[0],
numHeightfieldColumns=height_map.shape[1],
physicsClientId=self.client_ID)
terrain = p.createMultiBody(0, terrainShape, physicsClientId=self.client_ID)
p.resetBasePositionAndOrientation(terrain, [0, 0, 0], [0, 0, 0, 1], physicsClientId=self.client_ID)
return terrain
def load(self):
collision_id = p.createCollisionShape(p.GEOM_MESH,
fileName=self.collision_filename,
meshScale=self.scale)
visual_id = p.createVisualShape(p.GEOM_MESH,
fileName=self.visual_filename,
meshScale=self.scale)
body_id = p.createMultiBody(baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id,
basePosition=[0, 0, 0],
baseMass=0.1)
self.body_id = body_id
return body_id
def create_bullet_body(self):
self._shape_id = pybullet.createCollisionShape(
shapeType=pybullet.GEOM_BOX,
#radius=.5,
halfExtents=[.5, .5, .5],
physicsClientId=self._physics_client_id,
)
self._body_id = pybullet.createMultiBody(
baseMass=1.,
baseCollisionShapeIndex=self._shape_id,
basePosition=self._location + glm.vec3(0, 0, .5),
baseOrientation=pybullet.getQuaternionFromAxisAngle([1, 0, 0],
-math.pi / 2),
physicsClientId=self._physics_client_id,
)
def add_box(self, sides, base_position, mass=0.):
collision_box = p.createCollisionShape(p.GEOM_BOX,
halfExtents=sides,
physicsClientId=self.my_id)
visual_box = p.createVisualShape(p.GEOM_BOX,
halfExtents=sides,
rgbaColor=self._get_color(),
physicsClientId=self.my_id)
box = p.createMultiBody(baseMass=mass,
baseCollisionShapeIndex=collision_box,
baseVisualShapeIndex=visual_box,
basePosition=base_position,
physicsClientId=self.my_id)
self.objects.add(box)
return box
def add_sphere(self, radius, base_position, mass=0.):
collision_sphere = p.createCollisionShape(p.GEOM_SPHERE,
radius=radius,
physicsClientId=self.my_id)
visual_sphere = p.createVisualShape(p.GEOM_SPHERE,
radius=radius,
rgbaColor=self._get_color(),
physicsClientId=self.my_id)
sphere = p.createMultiBody(baseMass=mass,
baseCollisionShapeIndex=collision_sphere,
baseVisualShapeIndex=visual_sphere,
basePosition=base_position,
physicsClientId=self.my_id)
self.objects.add(sphere)
return sphere
def _randomly_place_objects(self, urdfList, pos_range):
objectUids = []
objectClasses = []
shift = [0, -0.02, 0]
meshScale = [0.3, 0.3, 0.3]
list_x_pos = []
list_y_pos = []
random.seed(self.img_save_cnt)
for idx, (_, _) in enumerate(urdfList):
list_x_pos.append(random.uniform(pos_range[0], pos_range[1]))
list_y_pos.append(random.uniform(pos_range[2], pos_range[3]))
for idx, (urdf_path, class_name) in enumerate(urdfList):
# print(urdf_path)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName=urdf_path,
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale)
try:
collisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=urdf_path,
collisionFramePosition=shift,
meshScale=meshScale)
except:
print(urdf_path)
continue
xpos = list_x_pos[idx]
ypos = list_y_pos[idx]
uid = p.createMultiBody(baseMass=1,
baseInertialFramePosition=[-0.2, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[xpos, ypos, .15],
useMaximalCoordinates=True)
objectUids.append(uid)
objectClasses.append(class_name)
for _ in range(150):
p.stepSimulation()
return objectUids, objectClasses
def reset(self):
'''環境を初期化する'''
if RENDER:
print('Environment.reset\n')
#bulletの世界をリセット
p.resetSimulation()
#フィールドを表示
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
self.planeId = p.loadURDF("plane100.urdf")
#オブジェクトモデルを表示
p.setAdditionalSearchPath(pybullet_data.getDataPath())
self.car = racecar.Racecar(p, pybullet_data.getDataPath())
# 摩擦係数を変更
p.changeDynamics(self.car.racecarUniqueId, 1,
lateralFriction=10) # 前輪左
p.changeDynamics(self.car.racecarUniqueId, 3,
lateralFriction=10) # 前輪右
p.changeDynamics(self.car.racecarUniqueId, 12,
lateralFriction=10) # 後輪左
p.changeDynamics(self.car.racecarUniqueId, 14,
lateralFriction=10) # 後輪右
# ターゲットを表示
targetX, targetY = np.random.permutation(np.arange(10))[0:2]
self.targetPos = [targetX, targetY, 0]
self.target = p.createCollisionShape(
p.GEOM_CYLINDER,
radius=0.5,
height=2,
collisionFramePosition=self.targetPos)
vis = p.createVisualShape(p.GEOM_CYLINDER,
radius=0.5,
length=2,
visualFramePosition=self.targetPos,
rgbaColor=[0, 255, 0, 1])
p.createMultiBody(0, self.target, vis)
# 目標の面積, 重心の位置を取得する
frame = self.renderPicture(self.car.racecarUniqueId)
_, area_sum = self.calc_area(frame)
area_v = 0
observation = (area_sum, area_v)
return observation, frame
def __init__(self, shape, location=nulltrans(), mass=None, modelfile=None, non_convex_collision=False, alpha=0.05, resolution=100000): self.shape = shape self.alpha = alpha self.resolution = resolution self.global_damping = 0 if mass is None: mass = shape.mass() inertia_axes = shape.principal_inertia_axes() inertia_frame = shape.inertia_frame() self.masscenter = evalcache.unlazy_if_need(shape.center()) self.inertia_orientation = inertia_frame.rotation() self.inertia_orientation = self.inertia_orientation.x.unlazy(), self.inertia_orientation.y.unlazy(), self.inertia_orientation.z.unlazy(), self.inertia_orientation.w.unlazy() nodes, triangles = zencad.triangulate(shape, 0.01) if modelfile is None: self.meshpath = shape.__lazyhexhash__[12:] + ".obj" else: self.meshpath = modelfile write_as_obj_wavefront(self.meshpath, nodes, triangles) if non_convex_collision: self.meshpath2 = self.meshpath + ".convex.obj" volumed_colision(self.meshpath, self.meshpath2, self.meshpath + ".log.txt", alpha, resolution) else: self.meshpath2 = self.meshpath self.visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=self.meshpath2) self.collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=self.meshpath2) #self.kkk = nulltrans() #self.kkk = rotateX(deg(60)) * rotateZ(deg(180)) #self.kkk = inertia_frame.unlazy() #self.tquat = (rotateX(deg(60)) * rotateZ(deg(180))).rotation() self.boxId = p.createMultiBody(baseMass=mass, baseCollisionShapeIndex=self.collisionShapeId, baseVisualShapeIndex=self.visualShapeId, basePosition = location.translation(), baseOrientation = location.rotation(), baseInertialFramePosition = self.masscenter #,baseInertialFrameOrientation=self.kkk.rotation() ) p.changeDynamics(self.boxId, -1, linearDamping=self.global_damping, angularDamping=self.global_damping) p.changeDynamics(self.boxId, -1, maxJointVelocity=100000)
def reset(self, sim_id: int):
self.sim_id_ = sim_id
# Create geometry
col_idx = pb.createCollisionShape(pb.GEOM_SPHERE,
radius=1.0,
physicsClientId=self.sim_id)
vis_idx = pb.createVisualShape(pb.GEOM_SPHERE,
radius=1.0,
physicsClientId=self.sim_id)
model_args = dict(baseCollisionShapeIndex=col_idx,
baseVisualShapeIndex=vis_idx)
self.robot_id_ = pb.createMultiBody(**model_args,
physicsClientId=self.sim_id)
def load(self):
mass = 1000
baseOrientation = [0, 0, 0, 1]
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=self.dimension)
visualShapeId = p.createVisualShape(p.GEOM_BOX,
halfExtents=self.dimension)
self.body_id = p.createMultiBody(baseMass=mass,
baseCollisionShapeIndex=colBoxId,
baseVisualShapeIndex=visualShapeId)
p.resetBasePositionAndOrientation(self.body_id, self.basePos,
baseOrientation)
return self.body_id
def _load(self):
if self.collision:
collision_id = p.createCollisionShape(
p.GEOM_MESH,
fileName=self.collision_filename,
meshScale=self.scale)
else:
collision_id = -1
visual_id = p.createVisualShape(p.GEOM_MESH,
fileName=self.visual_filename,
meshScale=self.scale)
body_id = p.createMultiBody(baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id,
basePosition=[0.2, 0.2, 1.5],
baseMass=0.1)
return body_id
def create_object(attributes=(FRICTION, MASS, ORI, POS, WLH),
color=(0., 1., 0., 1.)):
friction, mass, ori, pos, wlh = attributes
half_wlh = np.divide(wlh, 2.)
collision = p.createCollisionShape(p.GEOM_BOX, halfExtents=half_wlh)
visual = p.createVisualShape(p.GEOM_BOX,
halfExtents=half_wlh,
rgbaColor=color)
obj_id = p.createMultiBody(baseMass=mass,
baseCollisionShapeIndex=collision,
baseVisualShapeIndex=visual,
basePosition=pos,
baseOrientation=ori)
obj = Obj(obj_id, attributes, color)
set_friction(obj, friction)
return obj
def add_box(self, obj_id, half_extents, transform, static=False):
cid = p.createCollisionShape(p.GEOM_BOX,
halfExtents=half_extents,
physicsClientId=self._pid)
rot_q = np.roll(transform.rotation.elements,
-1) # w,x,y,z -> x,y,z,w (which pybullet expects)
mass = 0 if static else 1
bid = p.createMultiBody(baseMass=mass,
baseCollisionShapeIndex=cid,
basePosition=transform.translation,
baseOrientation=rot_q,
physicsClientId=self._pid)
body = Body(id=obj_id, bid=bid, vid=-1, cid=cid, static=static)
self._obj_id_to_body[obj_id] = body
self._bid_to_body[bid] = body
return body
def load(self):
if self.loaded:
return
self.coll_id = p.createCollisionShape(p.GEOM_HEIGHTFIELD,
heightfieldData=np.zeros(
self.length * 2),
numHeightfieldRows=self.length,
numHeightfieldColumns=2)
self.body_id = p.createMultiBody(0, self.coll_id)
p.changeVisualShape(self.body_id, -1, rgbaColor=[0.4, 0.8, 0.8, 1])
p.resetBasePositionAndOrientation(self.body_id,
posObj=[0, self.y_shift, 0],
ornObj=[0, 0, 0, 1])
p.resetBaseVelocity(self.coll_id, [self.base_vel, 0, 0])
self.loaded = True
def add_ground_plane(self, size):
if self.ground_plane_bid is not None:
bullet.removeBody(self.ground_plane_bid)
bullet.removeCollisionShape(self.ground_coll_shape)
size_height = 1.0
pos = [0, 0, 0]
pos = np.add(pos,
np.multiply(self.coord.height_3d(), -size_height * 0.5))
coll_shape = bullet.createCollisionShape(
bullet.GEOM_BOX,
halfExtents=[0.5 * x for x in [size, size, size_height]])
bid = bullet.createMultiBody(baseMass=0,
baseCollisionShapeIndex=coll_shape,
basePosition=pos)
self.ground_coll_shape = coll_shape
self.ground_plane_bid = bid
def add_object(self,
type,
location,
rotation,
scale,
velocity,
angular_velocity,
color,
mass=1,
lat_fric=0.,
restitution=.3,
lin_damp=0,
angular_damp=0,
**kwargs):
"""create an pybullet base object from a wavefront .obj file
set up initial parameters and physical properties"""
# Occluders are not put into physical simulation
mass = scale[0] * scale[1] * scale[
2] * 100 if type != "occluder" else scale[0] * scale[1] * scale[
2] * 1000
self.types.append(type)
self.scales.append(scale)
self.colors.append(color)
type = TYPES2SHAPES[type]
obj_path = os.path.join(self.obj_dir, '%s.obj' % type)
orn_quat = p.getQuaternionFromEuler(rotation)
col_id = p.createCollisionShape(p.GEOM_MESH,
fileName=obj_path,
meshScale=scale)
obj_id = p.createMultiBody(mass,
col_id,
basePosition=location,
baseOrientation=orn_quat)
p.changeDynamics(obj_id,
-1,
lateralFriction=lat_fric,
restitution=restitution,
linearDamping=lin_damp,
angularDamping=angular_damp)
if self.forward:
omega_quat = p.getQuaternionFromEuler(angular_velocity)
p.resetBaseVelocity(obj_id, velocity, omega_quat)
else:
omega_quat = p.getQuaternionFromEuler(angular_velocity)
p.resetBaseVelocity(obj_id, reverse_xyz(velocity),
reverse_euler(omega_quat))
return obj_id
def reset(self):
p.resetSimulation(physicsClientId=self.client)
bodies = construct_3d_space(min_obj=30, physicsClientId=self.client)
self.bodies = bodies
p.setGravity(0, 0, -10, physicsClientId=self.client)
self.iter = 0
# Construct the player sprite
player_vid = p.createVisualShape(p.GEOM_SPHERE,
radius=0.5,
rgbaColor=(0.3, 0.3, 0.3, 1),
physicsClientId=self.client)
player_cid = p.createCollisionShape(p.GEOM_SPHERE,
radius=0.5,
physicsClientId=self.client)
player_body = p.createMultiBody(1,
baseVisualShapeIndex=player_vid,
baseCollisionShapeIndex=player_cid,
basePosition=[5, 5, 0.5],
baseOrientation=[0, 0, 0, 1],
physicsClientId=self.client)
goal_body = spawn_rand_goal_3d(first=True, physicsClientId=self.client)
self.player_body = player_body
self.goal_body = goal_body
# g = space.add_collision_handler(collision_types['player'], collision_types['object'])
# g.begin = remove_player
# self.space = space
# self.screen = screen
# self.draw_options = draw_options
# self.player_body = body
self.total_reward = 0
self.total_length = 0
self.frame_buffer = []
for i in range(self.frame_stack):
step_3d_simulation(self.player_body, physicsClientId=self.client)
self.frame_buffer.append(get_3d_image(physicsClientId=self.client))
obs = self.construct_obs()
return obs
def load_floor_planes(self):
if self.is_interactive:
for f in range(len(self.floors)):
# load the floor plane with the original floor texture for each floor
plane_name = os.path.join(get_model_path(self.model_id),
"plane_z_up_{}.obj".format(f))
collision_id = p.createCollisionShape(p.GEOM_MESH,
fileName=plane_name)
visual_id = p.createVisualShape(p.GEOM_MESH,
fileName=plane_name)
texture_filename = get_texture_file(plane_name)
texture_id = p.loadTexture(texture_filename)
floor_body_id = p.createMultiBody(
baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id)
p.changeVisualShape(floor_body_id,
-1,
textureUniqueId=texture_id)
floor_height = self.floors[f]
p.resetBasePositionAndOrientation(floor_body_id,
posObj=[0, 0, floor_height],
ornObj=[0, 0, 0, 1])
# Since both the floor plane and the scene mesh have mass 0 (static),
# PyBullet seems to have already disabled collision between them.
# Just to be safe, explicit disable collision between them.
p.setCollisionFilterPair(self.mesh_body_id,
floor_body_id,
-1,
-1,
enableCollision=0)
self.floor_body_ids.append(floor_body_id)
else:
# load the default floor plane (only once) and later reset it to different floor heiights
plane_name = os.path.join(pybullet_data.getDataPath(),
"mjcf/ground_plane.xml")
floor_body_id = p.loadMJCF(plane_name)[0]
p.resetBasePositionAndOrientation(floor_body_id,
posObj=[0, 0, 0],
ornObj=[0, 0, 0, 1])
p.setCollisionFilterPair(self.mesh_body_id,
floor_body_id,
-1,
-1,
enableCollision=0)
self.floor_body_ids.append(floor_body_id)
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 load_scale(self, fn, pos=None, ori=None, scale=1):
collision_fn = fn.replace('.obj', '_vhacd.obj')
if not os.path.exists(collision_fn):
print('''No collision model for {} was found. Falling back to
visual model as collision geometry. This might impact
performance of your simulation''').format(fn)
collision_fn = fn
visual_fn = fn if self.visual_model else collision_fn
obj_id = fn.split('/')[-1].replace('.obj', '')
print scale
obj = Wavefront(visual_fn)
size = obj.size
max_size = size.max()
#scale = np.clip(max_size, 0.08, 0.9)/max_size * scale
center = obj.center * scale
if pos is None:
pos = np.array([0, 0, max_size * scale])
if ori is None:
ori = [0, 0, 0]
ori = p.getQuaternionFromEuler(ori)
vid = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName=visual_fn,
rgbaColor=[0.1, 0.1, 1, 1],
specularColor=[0.4, .4, 0],
meshScale=[scale] * 3,
visualFramePosition=-center)
cid = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName=collision_fn,
meshScale=[scale] * 3,
collisionFramePosition=-center)
bid = p.createMultiBody(baseMass=max_size * scale,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=cid,
baseVisualShapeIndex=vid,
basePosition=pos,
baseOrientation=ori)
if self.debug:
self.drawFrame([0, 0, 0], bid)
return bid
def setup_world(world, player_chunk_position):
"""Sets up the basic debug world."""
plane = pybullet.createCollisionShape(pybullet.GEOM_PLANE)
pybullet.createMultiBody(0, plane, -1, [0, 0, -9])
# Later on my plan is to just generate a world. For now, we need some debug cubes.
cubes.append(cubeRender.createCube([0, 12, 0], 1, [45, 45, 45]))
cubes.append(cubeRender.createCube([4, -4, 6], 1, [0, 0, 0]))
cubes.append(cubeRender.createCube([4, 5.9, 9], 2, [45, 30, 10]))
addSTO([18, 3], 1, [0.6, 0.2, 0.1])
boxestodelete = worldGen.resetWorldBoxes(
chunksize, -9, player_chunk_position,
world) # We run this once to initiate the first collision boxes.
return boxestodelete
import pybullet as p
import time
p.connect(p.GUI)
p.setPhysicsEngineParameter(allowedCcdPenetration=0.0)
terrain_mass=0
terrain_visual_shape_id=-1
terrain_position=[0,0,0]
terrain_orientation=[0,0,0,1]
terrain_collision_shape_id = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName="terrain.obj",
flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE,
meshScale=[0.5, 0.5, 0.5])
p.createMultiBody(
terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
terrain_position, terrain_orientation)
useMaximalCoordinates = True
sphereRadius = 0.005
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1
visualShapeId = -1
for i in range (5):
import pybullet as p
import time
import pybullet_data
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
visualShift = [0,0,0]
collisionShift = [0,0,0]
inertiaShift = [0,0,-0.5]
meshScale=[1,1,1]
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="cube.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=visualShift, meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="cube.obj", collisionFramePosition=collisionShift,meshScale=meshScale)
p.createMultiBody(baseMass=1,baseInertialFramePosition=inertiaShift,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,0,1], useMaximalCoordinates=False)
while (1):
p.stepSimulation()
time.sleep(1./240.)
import pybullet as p
import time
import math
# This simple snake logic is from some 15 year old Havok C++ demo
# Thanks to Michael Ewert!
p.connect(p.GUI)
plane = p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, plane)
useMaximalCoordinates = True
sphereRadius = 0.25
#colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]])
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1
visualShapeId = -1
link_Masses = []
linkCollisionShapeIndices = []
linkVisualShapeIndices = []
linkPositions = []
linkOrientations = []
linkInertialFramePositions = []
linkInertialFrameOrientations = []
indices = []
jointTypes = []
axis = []
import pybullet as p
import time
p.connect(p.GUI)
p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, 0)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1
visualShapeId = -1
link_Masses = [1]
linkCollisionShapeIndices = [colBoxId]
linkVisualShapeIndices = [-1]
linkPositions = [[0, 0, 0.11]]
linkOrientations = [[0, 0, 0, 1]]
linkInertialFramePositions = [[0, 0, 0]]
linkInertialFrameOrientations = [[0, 0, 0, 1]]
indices = [0]
jointTypes = [p.JOINT_REVOLUTE]
axis = [[0, 0, 1]]
for i in range(3):
for j in range(3):
for k in range(3):
basePosition = [
1 + i * 5 * sphereRadius, 1 + j * 5 * sphereRadius, 1 + k * 5 * sphereRadius + 1
23
]
#p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale,
vertices=vertices,
indices=indices,
uvs=uvs,
normals=normals)
collisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX, halfExtents=meshScale
) #MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
texUid = p.loadTexture("tex256.png")
batchPositions = []
for x in range(32):
for y in range(32):
for z in range(10):
batchPositions.append(
[x * meshScale[0] * 5.5, y * meshScale[1] * 5.5, (0.5 + z) * meshScale[2] * 2.5])
bodyUid = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
import pybullet as p
import time
p.connect(p.GUI)
useCollisionShapeQuery = True
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1)
geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.2, 0.2])
baseOrientationB = p.getQuaternionFromEuler([0, 0.3, 0]) #[0,0.5,0.5,0]
basePositionB = [1.5, 0, 1]
obA = -1
obB = -1
obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1])
obB = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=geomBox,
basePosition=basePositionB,
baseOrientation=baseOrientationB)
lineWidth = 3
colorRGB = [1, 0, 0]
lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
lineToXYZ=[0, 0, 0],
lineColorRGB=colorRGB,
lineWidth=lineWidth,
lifeTime=0)
pitch = 0
yaw = 0
while (p.isConnected()):
pitch += 0.01
if (pitch >= 3.1415 * 2.):
import pybullet as p import time p.connect(p.GUI) if (1): box_collision_shape_id = p.createCollisionShape( shapeType=p.GEOM_BOX, halfExtents=[0.01,0.01,0.055]) box_mass=0.1 box_visual_shape_id = -1 box_position=[0,0.1,1] box_orientation=[0,0,0,1] p.createMultiBody( box_mass, box_collision_shape_id, box_visual_shape_id, box_position, box_orientation, useMaximalCoordinates=True) terrain_mass=0 terrain_visual_shape_id=-1 terrain_position=[0,0,0] terrain_orientation=[0,0,0,1] terrain_collision_shape_id = p.createCollisionShape( shapeType=p.GEOM_MESH, fileName="terrain.obj", flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE, meshScale=[0.5, 0.5, 0.5]) p.createMultiBody( terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id, terrain_position, terrain_orientation)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0,-0.02,0]
meshScale=[0.1,0.1,0.1]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="duck.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
rangex = 3
rangey = 3
for i in range (rangex):
for j in range (rangey ):
p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.stopStateLogging(logId)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]]
currentColor = 0
while (1):
import pybullet as p
import time
useMaximalCoordinates = 0
p.connect(p.GUI)
#p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
monastryId = concaveEnv =p.createCollisionShape(p.GEOM_MESH,fileName="samurai_monastry.obj",flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
orn = p.getQuaternionFromEuler([1.5707963,0,0])
p.createMultiBody (0,monastryId, baseOrientation=orn)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1
visualShapeId = -1
for i in range (5):
for j in range (5):
for k in range (5):
if (k&2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates)
else:
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
p.setGravity(0,0,-10)
import pybullet as p
import time
import math
p.connect(p.GUI)
#don't create a ground plane, to allow for gaps etc
p.resetSimulation()
#p.createCollisionShape(p.GEOM_PLANE)
#p.createMultiBody(0,0)
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
#a few different ways to create a mesh:
vertices = [[-0.246350, -0.246483, -0.000624], [-0.151407, -0.176325, 0.172867],
[-0.246350, 0.249205, -0.000624], [-0.151407, 0.129477, 0.172867],
[0.249338, -0.246483, -0.000624], [0.154395, -0.176325, 0.172867],
[0.249338, 0.249205, -0.000624], [0.154395, 0.129477, 0.172867]]
indices = [
0, 3, 2, 3, 6, 2, 7, 4, 6, 5, 0, 4, 6, 0, 2, 3, 5, 7, 0, 1, 3, 3, 7, 6, 7, 5, 4, 5, 1, 0, 6, 4,
0, 3, 1, 5
]
#convex mesh from obj
stoneId = p.createCollisionShape(p.GEOM_MESH, vertices=vertices, indices=indices)
boxHalfLength = 0.5
import pybullet as p import time import math p.connect(p.GUI) #don't create a ground plane, to allow for gaps etc p.resetSimulation() #p.createCollisionShape(p.GEOM_PLANE) #p.createMultiBody(0,0) #p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]); p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]); p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) sphereRadius = 0.05 colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj") boxHalfLength = 0.5 boxHalfWidth = 2.5 boxHalfHeight = 0.1 segmentLength = 5 colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight]) mass = 1 visualShapeId = -1 segmentStart = 0 for i in range (segmentLength):
