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):