def __init__(self, headless=False): if headless: self._client = p.connect(p.DIRECT) else: self._client = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, 0) self.dt = 1./240. # Load and texture plane with features planeId = p.loadURDF('plane.urdf') textureId = p.loadTexture('sim_files/blank-texture.png') p.changeVisualShape(planeId, -1, textureUniqueId=textureId) cubeId = p.loadURDF('cube.urdf', [0, 0, 1], p.getQuaternionFromEuler( [0, 0, 0]), globalScaling=1.5) textureId = p.loadTexture('sim_files/dot-texture.png') p.changeVisualShape(cubeId, -1, textureUniqueId=textureId) # Setup camera params self.width = 450 self.height = 450 self.fov = 45.0 self.aspect = 1 self.nearVal = 0.1 self.farVal = 5.0 self._projMatrix = p.computeProjectionMatrixFOV(fov=self.fov, aspect=self.aspect, nearVal=self.nearVal, farVal=self.farVal) self.camPosition = np.array([-0.5, 0.5, 4.]) self.targetPosition = np.array([0., 0., 0.]) self.upVector = np.array([0., 1., 0.])
def loadModels(self): p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) p.setGravity(0, 0, -9.81) # p.setPhysicsEngineParameter(numSolverIterations=self.numSolverIterations) #p.setTimeStep(self.fixedTimeStep) orn = p.getQuaternionFromEuler([math.pi / 30 * 0, 0 * math.pi / 50, 0]) p.setAdditionalSearchPath(pybullet_data.getDataPath()) planeUid = p.loadURDF("plane_transparent.urdf", [0, 0, 0], orn) p.changeDynamics(planeUid, -1, lateralFriction=1) texUid = p.loadTexture("concrete.png") p.changeVisualShape(planeUid, -1, textureUniqueId=texUid) if self.useStairs: stairsUid = p.loadURDF("../urdf/stairs_gen.urdf.xml", [0, -1, 0], orn) flags = p.URDF_USE_SELF_COLLISION quadruped = p.loadURDF( "../urdf/spotmicroai_gen.urdf.xml", self.init_position, self.init_oritentation, useFixedBase=self.useFixedBase, useMaximalCoordinates=self.useMaximalCoordinates, flags=flags) #p.URDF_USE_IMPLICIT_CYLINDER) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.changeDynamics(quadruped, -1, lateralFriction=0.8) return quadruped
def add_aruco_tag( self, tag_id=0, rgba=(0, 1, 0, 1), aruco_dict_id=cv2.aruco.DICT_4X4_50, ): aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_id) tag = cv2.aruco.drawMarker(aruco_dict, tag_id, aruco_dict.markerSize + 2, borderBits=1) tag = np.pad(tag, (1, 1), mode='constant', constant_values=255) tag_size = tag.shape[0] texture_img = np.zeros((4 * tag_size, 4 * tag_size, 3), dtype=np.uint8) texture_img[..., :] = [255 * c for c in rgba[:3]] # flip image so aruco tag appears properly texture_img[-tag_size:, -tag_size:, :] = tag[::-1, :, np.newaxis] img = Image.fromarray(texture_img, mode="RGB") tmp_fname = f'tmp_cube_tex{tag_id}.png' img.save(tmp_fname) texture_id = pb.loadTexture(tmp_fname) pb.changeVisualShape(self.id, -1, rgbaColor=(1, 1, 1, rgba[-1]), textureUniqueId=texture_id, physicsClientId=self.client) os.remove(tmp_fname)
def make_box(self, size, position=[0, 0, 0], orientation=[0, 0, 0, 1], mass=0, tex=None, color=None, tag=None, **kwargs): cid = pb.createCollisionShape(pb.GEOM_BOX, halfExtents=np.array(size) * 0.5) vid = pb.createVisualShape(pb.GEOM_BOX, halfExtents=np.array(size) * 0.5) id = pb.createMultiBody(mass, baseCollisionShapeIndex=cid, baseVisualShapeIndex=vid, basePosition=position, baseOrientation=orientation) if color is not None: pb.changeVisualShape(id, -1, rgbaColor=color) if tex is not None: if type(tex) is str: tex = pb.loadTexture(tex) pb.changeVisualShape(id, -1, textureUniqueId=tex) if len(kwargs): pb.changeDynamics(id, -1, **kwargs) if tag is not None: self.__tag_map[id] = tag return id
def create_object(self): random_id = random.randint(0, 999) visual_id = pb.createVisualShape( shapeType=pb.GEOM_MESH, fileName=f'random_urdfs/{random_id:03}/{random_id:03}.obj', rgbaColor=None, meshScale=[0.1, 0.1, 0.1]) collision_id = pb.createCollisionShape( shapeType=pb.GEOM_MESH, fileName=f'random_urdfs/{random_id:03}/{random_id:03}_coll.obj', meshScale=[0.1, 0.1, 0.1]) link_id = pb.createMultiBody( baseMass=1.0, baseCollisionShapeIndex=visual_id, baseVisualShapeIndex=collision_id, basePosition=[0, 0, 3], baseOrientation=pb.getQuaternionFromEuler([0, 0, 0])) texture_paths = glob.glob(os.path.join('dtd', '**', '*.jpg'), recursive=True) random_texture_path = texture_paths[random.randint(0, len(texture_paths) - 1)] texture_id = pb.loadTexture(random_texture_path) pb.changeVisualShape(link_id, -1, textureUniqueId=texture_id) self._loaded_objects.append(link_id)
def add_mesh(self, path="./data/chair/", mesh_name="chair.obj", tex_name="diffuse.tga", position=[0, 0, 1], orientation=[0.7071068, 0, 0, 0.7071068], scale=0.1): shift = [0, -0.02, 0] meshScale = [scale, scale, scale] # the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=path + mesh_name, rgbaColor=[1, 1, 1, 1], specularColor=[0.4, .4, 0], visualFramePosition=shift, meshScale=meshScale) collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=path + mesh_name, collisionFramePosition=shift, meshScale=meshScale) rangex = 5 rangey = 5 bodyUid = p.createMultiBody(baseMass=0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=position, baseOrientation=orientation, useMaximalCoordinates=True) if tex_name != None: texUid = p.loadTexture(path + tex_name) p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid) return bodyUid
def _reset(self): #print("KukaGymEnv _reset") self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) texUid = p.loadTexture("./cube_new/aaa.png") cube_objects = p.loadSDF("./cube_new/model.sdf") self.cubeId = cube_objects[0] p.changeVisualShape(cube_objects[0], -1,rgbaColor =[1,1,1,1]) p.changeVisualShape(cube_objects[0], -1, textureUniqueId = texUid) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) self.tray = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000) self.table = p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-0.820000,0.000000,0.000000,0.0,1.0) #print("table:::",self.table) #print("tray:::",self.tray) #This is where the object spawn at random i should adjust this so it spawn at random position with specific area bound if reward increased xpos = 0.58 ypos = 0.04 ang = 3.14*0.5 orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.15,orn[0],orn[1],orn[2],orn[3]) #This where robot is reset i should modefy this to adjust height of end-effector p.setGravity(0,0,-10) self._kuka_hand = Kuka_Handlit(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() self._observation = self.getExtendedObservation() return np.array([self._observation])
def load_urdf(self): """Load the URDF of the field into the environment The field URDF comes with its own dimensions and textures, collidables. Note that this also calls the method responsible for setting button joint IDs. """ self.model_id = p.loadURDF(Utilities.gen_urdf_path("field/field.urdf"), useFixedBase=True) # Load the texture as well # Not done automatically for some reason p.changeVisualShape(self.model_id, -1, textureUniqueId=p.loadTexture( Utilities.gen_urdf_path("field/field.png"))) self.buttons.populate_joint_ids([1, 3, 5, 7, 9, 11, 13, 15, 17, 19]) # TODO: replace with setJointMotorControlArray for b in self.buttons: p.setJointMotorControl2(self.model_id, b.joint_id, controlMode=p.POSITION_CONTROL, targetPosition=0.0, force=0.2, positionGain=0.8)
def make_ball(self, radius, position=[0, 0, 0], mass=0, tex=None, color=None, tag=None, **kwargs): cid = pb.createCollisionShape(pb.GEOM_SPHERE, radius=radius) vid = pb.createVisualShape(pb.GEOM_SPHERE, radius=radius) id = pb.createMultiBody(mass, baseCollisionShapeIndex=cid, baseVisualShapeIndex=vid, basePosition=position) if color is not None: pb.changeVisualShape(id, -1, rgbaColor=color) if tex is not None: if type(tex) is str: tex = pb.loadTexture(tex) pb.changeVisualShape(id, -1, textureUniqueId=tex) if len(kwargs): pb.changeDynamics(id, -1, **kwargs) if tag is not None: self.__tag_map[id] = tag return id
def load_rope_geometry_from_urdf(modelUrdfFile, texturePath=""): print("Loading urdf '{}'".format(modelUrdfFile)) if (not os.path.exists(modelUrdfFile)): print("Failed to find model paths '{}'".format(modelUrdfFile)) return RopeObj.INVALID_UID, np.array([]) multiBodyId = "" if (modelUrdfFile.endswith(ModelFormats.Urdf)): multiBodyId = p.loadURDF(modelUrdfFile) print("(INFO) Loaded new model '{}'".format(multiBodyId)) else: print("Unknown model format '{}'".format(modelUrdfFile)) return RopeObj.INVALID_UID, np.array([]) # Override textures if (texturePath != ""): p.changeVisualShape(multiBodyId, -1, textureUniqueId=p.loadTexture(texturePath)) link_poses, _, _ = link_states(multiBodyId, p.getNumJoints(multiBodyId)) linkPositions = link_poses[:, 0:3] if len(linkPositions) > 0: print("(INFO) Found {} links and {} joints".format(len(linkPositions), p.getNumJoints(multiBodyId))) else: print("Failed to get link states.") return RopeObj.INVALID_UID, np.array([]) return multiBodyId, np.array(linkPositions)
def LoadModel(modelGeometryFile, modelCollisionFile="", texturePath=""): print("Loading urdf") if (not os.path.exists(modelGeometryFile) and not os.path.exists(modelCollisionFile)): print("Failed to find model paths") return meshScale = [0.1, 0.1, 0.1] visId = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=modelGeometryFile, rgbaColor=None, meshScale=meshScale) colId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=modelCollisionFile, meshScale=meshScale) multiBodyId = p.createMultiBody(baseMass=1.0, baseCollisionShapeIndex=colId, baseVisualShapeIndex=visId, basePosition=MODEL_INITIAL_POS, baseOrientation=p.getQuaternionFromEuler( [0, 0, 0])) # Override textures p.changeVisualShape(multiBodyId, -1, textureUniqueId=p.loadTexture(textPath)) return
def apply_texture(self, obj_id, patternPath="envs/dtd/images"): """ Apply texture to pybullet object Parameters: :param obj_id: (int) ID obtained from `p.loadURDF/SDF/..()` :param path: (str) Relative path to *.jpg (recursive) with textures """ if patternPath is None: return if patternPath == self.textures_path: # no change, can use cached value texture_paths = self.all_textures_ else: pp = os.path.abspath(os.path.join(repodir, str(patternPath))) texture_paths = glob.glob(os.path.join(pp, '**', '*.jpg'), recursive=True) random_texture_path = texture_paths[random.randint( 0, len(texture_paths) - 1)] textureId = p.loadTexture(random_texture_path) try: p.changeVisualShape(obj_id, -1, rgbaColor=[1, 1, 1, 1], textureUniqueId=textureId) except: print("Failed to apply texture to obj ID:" + str(obj_id) + " from path=" + str(pp))
def load_scene_mesh(self): """ Load scene mesh """ filename = os.path.join(get_scene_path(self.scene_id), "mesh_z_up_downsampled.obj") if not os.path.isfile(filename): filename = os.path.join(get_scene_path(self.scene_id), "mesh_z_up.obj") collision_id = p.createCollisionShape( p.GEOM_MESH, fileName=filename, flags=p.GEOM_FORCE_CONCAVE_TRIMESH) if self.pybullet_load_texture: visual_id = p.createVisualShape(p.GEOM_MESH, fileName=filename) else: visual_id = -1 self.mesh_body_id = p.createMultiBody( baseCollisionShapeIndex=collision_id, baseVisualShapeIndex=visual_id) p.changeDynamics(self.mesh_body_id, -1, lateralFriction=1) if self.pybullet_load_texture: texture_filename = get_texture_file(filename) if texture_filename is not None: texture_id = p.loadTexture(texture_filename) p.changeVisualShape(self.mesh_body_id, -1, textureUniqueId=texture_id)
def random_texture(fig): texture_paths = glob.glob(os.path.join('dtd', '**', '*.jpg'), recursive=True) random_texture_path = texture_paths[random.randint(0, len(texture_paths) - 1)] textureId = p.loadTexture(random_texture_path) p.changeVisualShape(fig, -1, textureUniqueId=textureId)
def get_texture(self): if len(VisualRandomizer.loaded_textures ) < VisualRandomizer.max_textures: random_texture = random.choice(os.listdir(self.texture_dir)) VisualRandomizer.loaded_textures.append( p.loadTexture(os.path.join(self.texture_dir, random_texture))) return VisualRandomizer.loaded_textures[-1] else: return random.choice(VisualRandomizer.loaded_textures)
def init_table(self): table_path = os.path.join(self.env_root, 'urdf/table/table.urdf') self.table_id = p.loadURDF( table_path, [0.42, 0, 0], [0, 0, math.pi * 0.32, 1], globalScaling=0.6) #,physicsClientId=self.physical_id) texture_path = os.path.join( self.env_root, 'texture/table_textures/table_texture.jpg') table_textid = p.loadTexture(texture_path) p.changeVisualShape(self.table_id, -1, textureUniqueId=table_textid)
def draw_bullet(shape, color, size, r, c, data): p.resetSimulation() #p.setRealTimeSimulation(True) p.setGravity(0, 9.8, GRAVITY) p.setTimeStep(dt) planeId = p.loadURDF("plane.urdf") sizes = ['small', 'big'] colors = ['red', 'green', 'blue'] shapes = ['sphere', 'cube', 'capsule'] sizename = sizes[size] shapename = shapes[shape] colorname = colors[color] body_name = 'shapes/bullet_data/my_{}_{}_{}.urdf'.format( shapename, colorname, sizename) if size == 0: factor = 0.5 else: factor = 1 if shape == 2: body_name = 'shapes/bullet_data/my_{}_{}.urdf'.format( shapename, sizename) z_centre = 0.5 * factor cubeStartPos = [(r - 1), (c - 1), z_centre] cubeStartOrientation = p.getQuaternionFromEuler([3.14 / 2, 0, 0]) botId = p.loadURDF(body_name, cubeStartPos, cubeStartOrientation) if shape == 2: textureId = p.loadTexture( 'shapes/bullet_data/{}.png'.format(colorname)) p.changeVisualShape(botId, -1, textureUniqueId=textureId) viewMatrix = p.computeViewMatrix(cameraEyePosition=[1.5, 1.5, 2], cameraTargetPosition=[0, 0, 0], cameraUpVector=[-1, -1, 1]) projectionMatrix = p.computeProjectionMatrixFOV(fov=90.0, aspect=1.0, nearVal=0.1, farVal=30.1) import time p.setRealTimeSimulation(1) st = 1500 p.setGravity(0, 0, GRAVITY) width, height, rgbImg, depthImg, segImg = p.getCameraImage( width=30, height=30, viewMatrix=viewMatrix, projectionMatrix=projectionMatrix) data = np.array(rgbImg) return data
def add_obj(path_to_obj, position, orientation, fix_base, scale, base_mass, base_inertialframe_position, texture_file=None, rgb=None): ''' path_to_obj: str orientation: np.ndarray, (3,3) or (4,) scale: float fix_base: bool texture_file: str or None rgb: (float, float, float) or (3,) np.ndarray ''' R = orientation assert isinstance(R, np.ndarray) if R.shape == (3,3): quat = R2quat(R) else: assert len(R) == 4 quat = R collision_shape_id = pb.createCollisionShape(shapeType=pb.GEOM_MESH, meshScale=np.full(3, scale), fileName=path_to_obj) visual_shape_id = pb.createVisualShape(shapeType=pb.GEOM_MESH, meshScale=np.full(3, scale), fileName=path_to_obj) if fix_base: # simply omitting baseMass makes it fixed object. bullet_body_id = pb.createMultiBody( basePosition=position, baseOrientation=quat, baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id, ) else: bullet_body_id = pb.createMultiBody( baseMass=base_mass, baseInertialFramePosition=base_inertialframe_position, basePosition=position, baseOrientation=quat, baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id, ) if texture_file is not None: texture_id = pb.loadTexture(texture_file) else: texture_id = None if rgb is not None: rgba = [c for c in rgb] + [1.] else: rgba = None if texture_id is not None and rgba is not None: pb.changeVisualShape(bullet_body_id, -1, textureUniqueId=texture_id, rgbaColor=rgba) elif texture_id is not None and rgba is None: pb.changeVisualShape(bullet_body_id, -1, textureUniqueId=texture_id) elif texture_id is None and rgba is not None: pb.changeVisualShape(bullet_body_id, -1, rgbaColor=rgba) return bullet_body_id
def from_array(bitmap, client_id): """ Load a texture from memory and return a non-negative texture unique id if the loading succeeds. """ # PyBullet can only load textures from file, so ... with tempfile.NamedTemporaryFile() as tmp_file: image = Image.fromarray(bitmap, 'RGB') image.save(tmp_file, 'PNG') tmp_file.flush() return pb.loadTexture(tmp_file.name, physicsClientId=client_id)
def set_marker(self, dir_path, file, pic_file): tmp = os.getcwd() os.chdir(dir_path) # c = p.loadURDF(file, (0, 0, 1), useFixedBase=True) c = p.loadURDF(file, (0, 1, 0), useFixedBase=True, globalScaling=0.1) # for i in range(1, 6): # p.loadURDF(file, (0.2*i, 1, 0), useFixedBase=True, globalScaling=0.1) os.chdir(tmp) x = p.loadTexture(pic_file) p.changeVisualShape(c, -1, textureUniqueId=x)
def floor(clientId, texture=False, wall=False): floorId = p.loadURDF('env/model/floor.urdf', physicsClientId=clientId) if texture: textureId = p.loadTexture( 'env/model/texture/wood1.jpg', physicsClientId=clientId) p.changeVisualShape( floorId, -1, textureUniqueId=textureId, physicsClientId=clientId) if wall: p.loadURDF('samurai.urdf', physicsClientId=clientId) return floorId
def add_drone(self, RobotController, name, image): if self.get_drone_by_name(name) is not None: raise Exception(f'drone with name "{name}" already exists') try: # create instance of controller controller = RobotController(self.enforce_motor_limits) # get color color = controller.get_color() assert (len(color) == 3) color.append(1.) # get label if image is not None: texture_id = pybullet.loadTexture(image) # load urdf id = pybullet.loadURDF( os.path.join('.', 'urdf', 'drone.urdf'), basePosition=np.array([0., 0., 0.3]), baseOrientation=pybullet.getQuaternionFromEuler([0., 0., 0.]), useFixedBase=0, flags=(pybullet.URDF_USE_IMPLICIT_CYLINDER | pybullet.URDF_USE_INERTIA_FROM_FILE)) # apply color and label pybullet.changeVisualShape(id, -1, rgbaColor=color) if image is None: pybullet.changeVisualShape(id, 1, rgbaColor=[1., 1., 1., 0.]) else: pybullet.changeVisualShape(id, 1, rgbaColor=[1., 1., 1., 0.75], textureUniqueId=texture_id) # set contact parameters pybullet.changeDynamics(id, -1, lateralFriction=1.0, spinningFriction=0.0, rollingFriction=0.0, restitution=0.5, contactDamping=-1, contactStiffness=-1) self.drones.append({ 'id': id, 'module': None, 'RobotController': RobotController, 'name': name, 'controller': controller, }) except Exception as err: print(f'Failed to add {name} because of the following error:') print(f'\n==========\n{traceback.format_exc()}==========\n')
def load(self, parent): self.parent = parent num_planes = p.getNumJoints(parent) self.joints = [-1] + list(range(num_planes)) self.textures = [] for j in self.joints: p.changeVisualShape(parent, j, rgbaColor=[1, 1, 1, 1]) t = p.loadTexture(DATA_DIR + "tex256.png") self.textures.append(t) p.changeTexture(t, self.random_pixels(), self.w, self.h) p.changeVisualShape(parent, j, textureUniqueId=t)
def main(): if len(sys.argv) > 1: model_path = sys.argv[1] else: model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj') p.connect(p.GUI) p.setGravity(0, 0, -9.8) p.setTimeStep(1. / 240.) # Load scenes collision_id = p.createCollisionShape(p.GEOM_MESH, fileName=model_path, meshScale=1.0, flags=p.GEOM_FORCE_CONCAVE_TRIMESH) visual_id = p.createVisualShape(p.GEOM_MESH, fileName=model_path, meshScale=1.0) texture_filename = get_texture_file(model_path) texture_id = p.loadTexture(texture_filename) mesh_id = p.createMultiBody(baseCollisionShapeIndex=collision_id, baseVisualShapeIndex=visual_id) # Load robots turtlebot_urdf = os.path.join(gibson2.assets_path, 'models/turtlebot/turtlebot.urdf') robot_id = p.loadURDF(turtlebot_urdf, flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL) # Load objects obj_visual_filename = os.path.join( gibson2.assets_path, 'models/ycb/002_master_chef_can/textured_simple.obj') obj_collision_filename = os.path.join( gibson2.assets_path, 'models/ycb/002_master_chef_can/textured_simple_vhacd.obj') collision_id = p.createCollisionShape(p.GEOM_MESH, fileName=obj_collision_filename, meshScale=1.0) visual_id = p.createVisualShape(p.GEOM_MESH, fileName=obj_visual_filename, meshScale=1.0) object_id = p.createMultiBody(baseCollisionShapeIndex=collision_id, baseVisualShapeIndex=visual_id, basePosition=[1.0, 0.0, 1.0], baseMass=0.1) for _ in range(10000): p.stepSimulation() p.disconnect()
def set_texture(body, image_path): #if color is not None: # set_color(body, color) assert image_path.endswith('.png') texture = p.loadTexture(image_path) assert 0 <= texture p.changeVisualShape( body, linkIndex=NULL_ID, shapeIndex=NULL_ID, #rgbaColor=WHITE, textureUniqueId=texture, #specularColor=(0, 0, 0), physicsClientId=get_client()) return texture
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) if texture_filename is not None: texture_id = p.loadTexture(texture_filename) else: texture_id = -1 floor_body_id = p.createMultiBody( baseCollisionShapeIndex=collision_id, baseVisualShapeIndex=visual_id) if texture_id != -1: 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_plane(self): self.plane_id = self.p.loadURDF(os.path.join(self.env_root, 'urdf/table/plane.urdf'), [0.7, 0, 0], [0, 0, -math.pi * 0.02, 1], globalScaling=0.7) texture_path = os.path.join(self.opt.project_root, 'scripts', 'Envs', 'texture/real_textures') texture_file = os.path.join( texture_path, random.sample(os.listdir(texture_path), 1)[0]) self.textid = p.loadTexture(texture_file) self.p.changeVisualShape(self.plane_id, -1, rgbaColor=[1, 1, 1, 0.9]) self.p.changeVisualShape(self.plane_id, -1, textureUniqueId=self.textid)
def __init__(self): physicsClient = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -9.81) planeId = p.loadURDF("plane.urdf") # load models from URDF lowerBallSpawnPos = [0, 0, 3] robotSpawnPos = [0, 0, 8] defaultOrientation = p.getQuaternionFromEuler([0, 0, 0]) self.lower_ball_id = p.loadURDF( "../model_3d/urdf/lower_ball.urdf", lowerBallSpawnPos, defaultOrientation, flags=p.URDF_USE_INERTIA_FROM_FILE) self.robot_id = p.loadURDF( "../model_3d/urdf/robot.urdf", robotSpawnPos, defaultOrientation, flags=p.URDF_USE_INERTIA_FROM_FILE) # extract joint indices if p.getNumJoints(self.robot_id) != 2: print("robot does not have 2 joints!") exit() name_to_joint_idx = {} for i in range(p.getNumJoints(self.robot_id)): name_to_joint_idx.update({p.getJointInfo(self.robot_id, i)[1]: i}) self.motor_x_idx = name_to_joint_idx['lateral_rotation_axis'] self.motor_y_idx = name_to_joint_idx['primary_rotation_axis'] # add texture for better visualization texUid = p.loadTexture("../model_3d/urdf/media/circles.png") p.changeVisualShape(self.lower_ball_id, -1, textureUniqueId=texUid) p.changeVisualShape(self.robot_id, -1, textureUniqueId=texUid) # load controller param = ModelParam() param.l = 1.0 param.r1 = 3.0 param.r2 = 2.0 self.controller = Controller(param)
def create_textured_square(size, color=None, width=MAX_TEXTURE_WIDTH, height=MAX_TEXTURE_WIDTH): body = load_model('models/square.urdf', scale=size) if color is not None: set_color(body, color) path = os.path.join(TEMP_DIR, 'texture.png') image = MAX_PIXEL_VALUE * np.ones((width, height, 3), dtype=np.uint8) import scipy.misc scipy.misc.imsave(path, image) texture = p.loadTexture(path) p.changeVisualShape(body, NULL_ID, textureUniqueId=texture, physicsClientId=CLIENT) return body, texture
def readTexture(self, texture): '''Convert the texture into the color format taken by pybullet''' if (texture[0] == '#'): r = float( self.hexToInt(texture[1]) * 16 + self.hexToInt(texture[2])) / 255.0 g = float( self.hexToInt(texture[3]) * 16 + self.hexToInt(texture[4])) / 255.0 b = float( self.hexToInt(texture[5]) * 16 + self.hexToInt(texture[5])) / 255.0 color = [r, g, b, 1] return color else: textId = p.loadTexture(texture) return textId
import pybullet as p from time import sleep import matplotlib.pyplot as plt import numpy as np physicsClient = p.connect(p.GUI) p.setGravity(0,0,0) bearStartPos1 = [-3.3,0,0] bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0]) bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1) bearStartPos2 = [0,0,0] bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0]) bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2) textureId = p.loadTexture("checker_grid.jpg") #p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId) #p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId) useRealTimeSimulation = 1 if (useRealTimeSimulation): p.setRealTimeSimulation(1) while 1: if (useRealTimeSimulation): camera = p.getDebugVisualizerCamera() viewMat = camera[2] projMat = camera[3] #An example of setting the view matrix for the projective texture. #viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
import pybullet as p p.connect(p.GUI) plane = p.loadURDF("plane.urdf") visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS) print(visualData) curTexUid = visualData[0][8] print(curTexUid) texUid = p.loadTexture("tex256.png") print("texUid=", texUid) p.changeVisualShape(plane, -1, textureUniqueId=texUid) for i in range(100): p.getCameraImage(320, 200) p.changeVisualShape(plane, -1, textureUniqueId=curTexUid) for i in range(100): p.getCameraImage(320, 200)