def make_cargo(self): orientation = pybullet.getQuaternionFromEuler([np.pi / 2, 0, 0]) cargo_shift = [0.0, 0.0, 0.325] mesh_scale = [0.1, 0.1, 0.1] visual_id = pybullet.createVisualShape(shapeType=pybullet.GEOM_SPHERE, radius=0.1,\ rgbaColor=[1, 0, 1, 1], specularColor=[0.8, .0, 0], visualFrameOrientation=orientation, meshScale=mesh_scale) collision_id = pybullet.createCollisionShape(shapeType=pybullet.GEOM_SPHERE, radius=0.1,\ halfExtents=[0.1, 0.1, 0.1], collisionFrameOrientation=orientation, meshScale=mesh_scale) self.cargo_id = pybullet.createMultiBody(baseMass=0.1,\ baseCollisionShapeIndex=collision_id,\ baseVisualShapeIndex=visual_id,\ basePosition=cargo_shift) pybullet.changeDynamics(self.cargo_id, -1, lateralFriction=self.k_friction) pybullet.changeDynamics(self.cargo_id, -1, angularDamping=0.1) pybullet.changeDynamics(self.cargo_id, -1, linearDamping=0.1)
def create_agent_from_obj(self, visual_filename, collision_filename, scale=1.0, mass=1.0, pos=[0, 0, 0], orient=[0, 0, 0, 1], rgba=[1, 1, 1, 1], maximal=False): visual_shape = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=visual_filename, meshScale=scale, rgbaColor=rgba, physicsClientId=self.id) collision_shape = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=collision_filename, meshScale=scale, physicsClientId=self.id) body = p.createMultiBody(baseMass=mass, baseCollisionShapeIndex=collision_shape, baseVisualShapeIndex=visual_shape, basePosition=pos, baseOrientation=orient, useMaximalCoordinates=maximal, physicsClientId=self.id) agent = Agent() agent.init(body, self.id, self.np_random, indices=-1) return agent
def create_target(self, human): # Set target on mouth mouth_pos = [0, -0.11, 0.03] head_pos, head_orient = p.getLinkState(human, 23, computeForwardKinematics=True, physicsClientId=self.id)[:2] target_pos, target_orient = p.multiplyTransforms( head_pos, head_orient, mouth_pos, [0, 0, 0, 1], physicsClientId=self.id) # 将mouth位置转换到相对头的位置 target_pos = np.array(target_pos) sphere_visual = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.01, rgbaColor=[0, 1, 0, 1], physicsClientId=self.id) # target target = p.createMultiBody(baseMass=0.0, baseCollisionShapeIndex=-1, baseVisualShapeIndex=sphere_visual, basePosition=target_pos, useMaximalCoordinates=False, physicsClientId=self.id) p.changeVisualShape(target, -1, rgbaColor=[0.9, 0.3, 0.3, 1], physicsClientId=self.id) return target, target_pos
def wall(x=0., y=0., z=0., width=1., height=1., thickness=0.01, orientation=DEFAULT_ORI): half_extents = [width / 2, thickness, height / 2] position = [x, y, z + height / 2] pos = [0, 0., 0.] visual_shape_id = p.createVisualShape(shapeType=p.GEOM_BOX, halfExtents=half_extents, visualFramePosition=pos) collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=half_extents, collisionFramePosition=pos) mb = p.createMultiBody(baseMass=0., baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id, basePosition=position, baseOrientation=orientation, useMaximalCoordinates=False) p.changeVisualShape(mb, -1, rgbaColor=[1, 1, 1, 0.5], specularColor=[0.4, 0.4, 0]) return mb
def create_body( self, shape=p.GEOM_CAPSULE, radius=0, length=0, position_offset=[0, 0, 0], orientation=[0, 0, 0, 1], specular_color=[0.1, 0.1, 0.1], rgba_Color=[0.8, 0.6, 0.4, 1], ): # 给身体不同部分创建不同颜色 visual_shape = p.createVisualShape(shape, radius=radius, length=length, rgbaColor=rgba_Color, specularColor=specular_color, visualFramePosition=position_offset, visualFrameOrientation=orientation, physicsClientId=self.id) # 创建反射 collision_shape = p.createCollisionShape( shape, radius=radius, height=length, collisionFramePosition=position_offset, collisionFrameOrientation=orientation, physicsClientId=self.id) # return uniqel id return collision_shape, visual_shape
def create_bowl(self): bowl_scale = 0.75 visual_filename = os.path.join( self.directory, 'dinnerware', 'bowl_reduced_compressed.obj') # 重排列文件设置使导入更容易 collision_filename = os.path.join(self.directory, 'dinnerware', 'bowl_vhacd.obj') bowl_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=visual_filename, meshScale=[bowl_scale] * 3, physicsClientId=self.id) bowl_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=collision_filename, meshScale=[bowl_scale] * 3, physicsClientId=self.id) bowl_pos = np.array([-0.15, -0.55, 0.75]) + np.array([ self.np_random.uniform(-0.05, 0.05), self.np_random.uniform(-0.05, 0.05), 0 ]) bowl = p.createMultiBody( baseMass=0.1, baseCollisionShapeIndex=bowl_collision, baseVisualShapeIndex=bowl_visual, basePosition=bowl_pos, baseOrientation=p.getQuaternionFromEuler([np.pi / 2.0, 0, 0], physicsClientId=self.id), baseInertialFramePosition=[0, 0.04 * bowl_scale, 0], useMaximalCoordinates=False, physicsClientId=self.id) return bowl
def create_primitives(shapeType=2, rgbaColor=[1, 1, 0, 1], pos=[0, 0, 0], radius=1, length=2, halfExtents=[0.5, 0.5, 0.5], baseMass=1, basePosition=[0, 0, 0]): visualShapeId = p.createVisualShape(shapeType=shapeType, rgbaColor=rgbaColor, visualFramePosition=pos, radius=radius, length=length, halfExtents=halfExtents) collisionShapeId = p.createCollisionShape(shapeType=shapeType, collisionFramePosition=pos, radius=radius, height=length, halfExtents=halfExtents) bodyId = p.createMultiBody(baseMass=baseMass, baseInertialFramePosition=[0, 0, 0], baseVisualShapeIndex=visualShapeId, baseCollisionShapeIndex=collisionShapeId, basePosition=basePosition, useMaximalCoordinates=True) return visualShapeId, collisionShapeId, bodyId
def create_targets(self): self.target = None self.target_visualshape = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.1, rgbaColor=[1, 0, 0, 1], physicsClientId=self.client_ID) self.update_targets()
def move_arm_test(env): print(f"Running {__file__}::{move_arm_test.__name__}()") con = SimConnection(env) arm_ctrl = BasicController(con) cmd = Command() state = State() arm_ctrl.execute(cmd, state) print(state.tool_pose()) marker_visual_id = pb.createVisualShape(pb.GEOM_BOX, halfExtents=[0.005, 0.005, 0.005], rgbaColor=[1, 0, 0, 1]) #pb.createMultiBody(baseVisualShapeIndex=marker_visual_id, basePosition=state.tool_pose()[:3]) cmd.make(kind=UR_CMD_KIND_MOVE_TOOL_POSE, target_position=Tool(-0.4, -0.4, 0.3, 0, math.pi, 0)) pb.createMultiBody(baseVisualShapeIndex=marker_visual_id, basePosition=cmd.target_position()[:3]) arm_ctrl.execute(cmd, state) while not state.is_goal_reached(): # st = time.time() arm_ctrl.execute(cmd, state) env.update() # latency = time.time()-st # leftover = 1/240. - latency # if leftover > 0: # time.sleep(leftover) print("Passed.")
def _flag_reposition(self): walk_target_x, walk_target_y, _ = self.robot.get_target_position() self.flag = None if self.gui and not self.config["display_ui"]: self.visual_flagId = p.createVisualShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.5, 0.5, 0.5], rgbaColor=[1, 0, 0, 0]) self.last_flagId = p.createMultiBody(baseVisualShapeIndex=self.visual_flagId, baseCollisionShapeIndex=-1, basePosition=[walk_target_x, walk_target_y, 0.5])
def _lookup_model(ref): """ Lookup collision shape from 3D-FUTURE models. """ if ref not in model_map: return -1, -1 mid = model_map[ref] model_file = (model_dir / mid / 'raw_model.obj') # NOTE(ycho): Temporary workaround for degenerate model? # if '39057a21-0a68-3494-8522-2e473dd6a38f' in str(model_file): # return -1, -1 if not model_file.exists(): logging.warn('No such model file : {}'.format(model_file)) return -1, -1 # TODO(ycho): remove this flag ... only for visualization # or leave it in? idk... col_id = pb.createCollisionShape( pb.GEOM_MESH, fileName=str(model_file), meshScale=c['scale'], # flags=pb.GEOM_FORCE_CONCAVE_TRIMESH, physicsClientId=sim_id ) vis_id = pb.createVisualShape( pb.GEOM_MESH, fileName=str(model_file), meshScale=c['scale'], flags=pb.GEOM_FORCE_CONCAVE_TRIMESH, physicsClientId=sim_id ) return (col_id, vis_id)
def __init__(self, config, gpu_idx=0): self.config = self.parse_config(config) print(self.config["envname"]) assert (self.config["envname"] == self.__class__.__name__ or self.config["envname"] == "TestEnv") CameraRobotEnv.__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
def __init__( self, width, position, orientation, color=(0, 1, 0, 0.5), **kwargs, ): """ Create a cube marker for visualization Args: width (float): Length of one side of the cube. position: Position (x, y, z) orientation: Orientation as quaternion (x, y, z, w) color: Color of the cube as a tuple (r, b, g, q) """ self._kwargs = kwargs self.shape_id = pybullet.createVisualShape( shapeType=pybullet.GEOM_BOX, halfExtents=[width / 2] * 3, rgbaColor=color, **self._kwargs, ) self.body_id = pybullet.createMultiBody( baseVisualShapeIndex=self.shape_id, basePosition=position, baseOrientation=orientation, **self._kwargs, )
def __init__(self, position, rotEuler, rgb=(0, 0, 0), collision=False, size=1.0): real_size = size * 0.125 position = (position[0], position[1], position[2] + real_size) self.visual_id = p.createVisualShape( p.GEOM_BOX, rgbaColor=list(rgb) + [1], halfExtents=[real_size, real_size, real_size]) if collision: self.collision_id = p.createCollisionShape( p.GEOM_BOX, halfExtents=[real_size, real_size, real_size]) self.body = p.createMultiBody( baseMass=0, baseCollisionShapeIndex=self.collision_id, baseVisualShapeIndex=self.visual_id, basePosition=position, baseOrientation=euler_to_quaternion(*rotEuler)) else: self.body = p.createMultiBody( baseMass=0, baseVisualShapeIndex=self.visual_id, basePosition=position, baseOrientation=euler_to_quaternion(*rotEuler))
def _randomly_place_objects(self, objList): """Randomly places the objects in the bin. """ objectUids = [] shift = [0, -0.02, 0] meshScale = [0.2, 0.2, 0.2] list_x_pos = [0.55, 0.6, 0.65] list_y_pos = [0.2, 0.05, 0.2] for idx, obj_path in enumerate(objList): visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=obj_path, rgbaColor=[1, 1, 1, 1], specularColor=[0.4, .4, 0], visualFramePosition=shift, meshScale=meshScale) collisionShapeId = p.createCollisionShape( shapeType=p.GEOM_MESH, fileName=obj_path, collisionFramePosition=shift, meshScale=meshScale) 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) for _ in range(1000): p.stepSimulation() return objectUids
def build_the_wall(self): colBoxId = p.createCollisionShape( p.GEOM_BOX, halfExtents=[0.01, self.world_size / 2, 3]) visualShapeId = p.createVisualShape( p.GEOM_BOX, halfExtents=[0.1, self.world_size / 2, 3]) bodyUid = p.createMultiBody(baseMass=0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=colBoxId, baseVisualShapeIndex=visualShapeId, basePosition=[ self.world_size - 0.5, self.world_size * 0.5 - 0.5, 0 ], baseOrientation=[0, 0, 0, 1], useMaximalCoordinates=True) bodyUid = p.createMultiBody( baseMass=0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=colBoxId, baseVisualShapeIndex=visualShapeId, basePosition=[-0.5, self.world_size * 0.5 - 0.5, 0], baseOrientation=[0, 0, 0, 1], useMaximalCoordinates=True) bodyUid = p.createMultiBody( baseMass=0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=colBoxId, baseVisualShapeIndex=visualShapeId, basePosition=[ self.world_size * 0.5 - 0.5, self.world_size - 0.5, 0 ], baseOrientation=[0, 0, 0.7071068, 0.7071068], useMaximalCoordinates=True)
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 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 __init__(self, pos, rot, scale, z_scale=1): self.z_scale = z_scale obj_filepath = found_object_directories[np.random.choice( np.arange(total_num_objects), 1)[0]] mesh_scale = [0.01 * scale, 0.01 * scale, 0.01 * scale * z_scale] visualShapeId = pb.createVisualShape(shapeType=pb.GEOM_MESH, fileName=obj_filepath, rgbaColor=[ np.random.random(), np.random.random(), np.random.random(), 1 ], meshScale=mesh_scale) collisionShapeId = pb.createCollisionShape(shapeType=pb.GEOM_MESH, fileName=obj_filepath, meshScale=mesh_scale) # collisionShapeId = pb.createCollisionShape(shapeType=pb.GEOM_BOX, # halfExtents=[0.024*scale, 0.024*scale, 0.024*scale]) object_id = pb.createMultiBody( baseMass=0.1, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=pos, baseOrientation=rot) # pb.changeDynamics(object_id, -1, mass=0.1, lateralFriction=1.0, spinningFriction=0.0, rollingFriction=0.0) super(RandomObject, self).__init__(constants.RANDOM, object_id)
def show_spheres(self, jp, scolors='rainbow'): spheres = [] for s in self.sphere_configs: loc = get_sphere_loc(jp, s[:-1]) spheres.append(Sphere(loc, s[-1])) if scolors is None: scolors = self.gen_sphere_colors() ids = [] for sphere, color in zip(spheres, scolors): obstacle_visual_id = p.createVisualShape( shapeType=p.GEOM_SPHERE, radius=sphere.radius, rgbaColor=color, physicsClientId=self.clid, ) #obstacle_collision_id = p.createCollisionShape( # shapeType=p.GEOM_SPHERE, # radius=sphere.radius, # physicsClientId=self.clid, #) obstacle_id = p.createMultiBody( basePosition=sphere.center, baseVisualShapeIndex=obstacle_visual_id, #baseCollisionShapeIndex=obstacle_collision_id, physicsClientId=self.clid, ) ids.append(obstacle_id) self.obstacle_ids.extend(ids) return ids
def build_markers(num_markers): marker_radius = 0.02 markers = [] for i in range(num_markers): if (i == REF_NECK_JOINT_ID) or (i == REF_PELVIS_JOINT_ID)\ or (i in REF_HIP_JOINT_IDS): col = [0, 0, 1, 1] elif (i in REF_TOE_JOINT_IDS): col = [1, 0, 0, 1] else: col = [0, 1, 0, 1] virtual_shape_id = pybullet.createVisualShape( shapeType=pybullet.GEOM_SPHERE, radius=marker_radius, rgbaColor=col) body_id = pybullet.createMultiBody( baseMass=0, baseCollisionShapeIndex=-1, baseVisualShapeIndex=virtual_shape_id, basePosition=[0, 0, 0], useMaximalCoordinates=True) markers.append(body_id) return markers
def __init__( self, shape_type, position, orientation, color=(0, 1, 0, 0.5), pybullet_client_id=0, **kwargs, ): """ Create a cube marker for visualization Args: shape_type: Shape type of the object (e.g. pybullet.GEOM_BOX). position: Position (x, y, z) orientation: Orientation as quaternion (x, y, z, w) kwargs: Keyword arguments that are passed to pybullet.createVisualShape. Use this to specify shape-type-specify parameters like the object size. """ self._pybullet_client_id = pybullet_client_id self.shape_id = pybullet.createVisualShape( shapeType=shape_type, rgbaColor=color, physicsClientId=self._pybullet_client_id, **kwargs, ) self.body_id = pybullet.createMultiBody( baseVisualShapeIndex=self.shape_id, basePosition=position, baseOrientation=orientation, physicsClientId=self._pybullet_client_id, )
def _setup_experiment(self): # add plane to push on (slightly below the base of the robot) self.planeId = p.loadURDF("plane.urdf", [0, 0, 0], useFixedBase=True) self._setup_gripper() self.walls = [] if self.level == 0: pass elif self.level == 1: half_extents = [0.2, 0.05, 0.3] colId = p.createCollisionShape(p.GEOM_BOX, halfExtents=half_extents) visId = p.createVisualShape(p.GEOM_BOX, halfExtents=half_extents, rgbaColor=[0.2, 0.2, 0.2, 0.8]) wallId = p.createMultiBody( 0, colId, visId, basePosition=[0.6, 0.30, 0.2], baseOrientation=p.getQuaternionFromEuler([0, 0, 1.1])) p.changeDynamics(wallId, -1, lateralFriction=1) self.walls.append(wallId) for wallId in self.walls: p.changeVisualShape(wallId, -1, rgbaColor=[0.2, 0.2, 0.2, 0.8]) self.set_camera_position([0, 0], yaw=113, pitch=-40) self.state = self._obs() self._draw_state() # set gravity p.setGravity(0, 0, -10)
def create_spheres(self, radius=0.01, mass=0.0, batch_positions=[[0, 0, 0]], visual=True, collision=True, rgba=[0, 1, 1, 1]): sphere_collision = p.createCollisionShape( shapeType=p.GEOM_SPHERE, radius=radius, physicsClientId=self.id) if collision else -1 sphere_visual = p.createVisualShape( shapeType=p.GEOM_SPHERE, radius=radius, rgbaColor=rgba, physicsClientId=self.id) if visual else -1 last_sphere_id = p.createMultiBody( baseMass=mass, baseCollisionShapeIndex=sphere_collision, baseVisualShapeIndex=sphere_visual, basePosition=[0, 0, 0], useMaximalCoordinates=False, batchPositions=batch_positions, physicsClientId=self.id) spheres = [] for body in list( range(last_sphere_id - len(batch_positions) + 1, last_sphere_id + 1)): sphere = Agent() sphere.init(body, self.id, self.np_random, indices=-1) spheres.append(sphere) return spheres
def cap(width, wall_width=0.05, position=[ 0, 0, 0, ], orientation=[0, 0, 0]): # create a cap that can be used to close corners or tunnels. # By default it is a wall in the y-z-plane that can be rotated and translated to the desired position # create a corner (cube open at neg. x and pos. z) as a building block of an obstacle parcour half_extents = [wall_width / 2, width / 2 + wall_width, width / 2] pos = [0, 0., 0.] visual_shape_id = p.createVisualShape(shapeType=p.GEOM_BOX, halfExtents=half_extents, visualFramePosition=pos) collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=half_extents, collisionFramePosition=pos) mb = p.createMultiBody(baseMass=0., baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id, basePosition=position, baseOrientation=orientation, useMaximalCoordinates=False) p.changeVisualShape(mb, -1, rgbaColor=[1, 1, 1, 0.5], specularColor=[0.4, 0.4, 0]) return mb
def create_sphere(self, radius=0.01, mass=0.0, pos=[0, 0, 0], visual=True, collision=True, rgba=[0, 1, 1, 1], maximal_coordinates=False, return_collision_visual=False): sphere_collision = p.createCollisionShape( shapeType=p.GEOM_SPHERE, radius=radius, physicsClientId=self.id) if collision else -1 sphere_visual = p.createVisualShape( shapeType=p.GEOM_SPHERE, radius=radius, rgbaColor=rgba, physicsClientId=self.id) if visual else -1 if return_collision_visual: return sphere_collision, sphere_visual body = p.createMultiBody(baseMass=mass, baseCollisionShapeIndex=sphere_collision, baseVisualShapeIndex=sphere_visual, basePosition=pos, useMaximalCoordinates=maximal_coordinates, physicsClientId=self.id) sphere = Agent() sphere.init(body, self.id, self.np_random, indices=-1) return sphere
def create_elastic_ball(ball_x, ball_y, ball_z): ''' Create an elastic ball in the simulation. Returns the created ball's id. ''' ball_initial_position = [ball_x, ball_y, ball_z] ball_visual_id = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.1, rgbaColor=[0.75, 0.3, 0, 1], specularColor=[0.4, .4, 0]) ball_collision_id = p.createCollisionShape(shapeType=p.GEOM_SPHERE, radius=0.15) # create the ball with visual, collision, and initial position ball_id = p.createMultiBody(baseMass=1, baseCollisionShapeIndex=ball_collision_id, baseVisualShapeIndex=ball_visual_id, basePosition=ball_initial_position) p.changeDynamics(ball_id, -1, lateralFriction=0, spinningFriction=0, rollingFriction=0, restitution=1.1) return ball_id
def add_shape(self, track): if track.has_shape is True: if track.shape.type == PrimitiveShape.CYLINDER: position = [] orientation = [] visual_shape_id = p.createVisualShape( p.GEOM_CYLINDER, radius=track.shape.dimensions[0], length=track.shape.dimensions[1]) collision_shape_id = p.createCollisionShape( p.GEOM_CYLINDER, radius=track.shape.dimensions[0], length=track.shape.dimensions[1]) entity_id = p.createMultiBody( baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id, basePosition=position, baseOrientation=orientation, flags=p.URDF_ENABLE_SLEEPING or p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES) if entity_id >= 0: self.entity_id_map[track.id] = entity_id self.reverse_entity_id_map[entity_id] = track.id elif track.shape.type == PrimitiveShape.MESH: pass else: raise NotImplementedError( "Only cylinder shapes for unknown objects supported at the moment." )
def putBox(self, x, y, length, width, height, mass, xPix, yPix): # xPix and yPix are the pixel location to be placed heightOffset = self.state[yPix, xPix] statetest = np.array(self.state) colBoxId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[ float(length) / 2, float(width) / 2, float(height) / 2 ]) randomColor = boxColorCollection[self.boxcount] visualBoxId = p.createVisualShape(p.GEOM_BOX, rgbaColor=randomColor, halfExtents=[ float(length) / 2, float(width) / 2, float(height) / 2 ]) p.createMultiBody(mass, colBoxId, visualBoxId, [ x + float(length) / 2, y - float(width) / 2, heightOffset + float(height) / 2 ], [0, 0, 0, 1])
16, 20, 21, 22, 20, 22, 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):
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.)
corners3D=[] imgW = int(width/10) imgH = int(height/10) img = p.getCameraImage(imgW,imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL) rgbBuffer=img[2] depthBuffer = img[3] print("rgbBuffer.shape=",rgbBuffer.shape) print("depthBuffer.shape=",depthBuffer.shape) #disable rendering temporary makes adding objects faster p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE, rgbaColor=[1,1,1,1], radius=0.03 ) collisionShapeId = -1 #p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale) for i in range (4): w = cornersX[i] h = cornersY[i] rayFrom,rayTo, _= getRayFromTo(w,h) rf = np.array(rayFrom) rt = np.array(rayTo) vec = rt-rf l = np.sqrt(np.dot(vec,vec)) newTo = (0.01/l)*vec+rf #print("len vec=",np.sqrt(np.dot(vec,vec))) p.addUserDebugLine(rayFrom,newTo,[1,0,0]) corners3D.append(newTo)