def load_blocks(self, thing_below, num_bases=None): self.num_blocks = len(thing_below) self.num_bases = self.num_blocks if num_bases is None else num_bases self.blocks = ["b%d" % b for b in range(self.num_blocks)] self.bases = ["t%d" % t for t in range(self.num_bases)] self.thing_below = dict(thing_below) # copy self.block_above = self.invert(thing_below) # # cube urdf path # fpath = os.path.dirname(os.path.abspath(__file__)) + '/../../urdfs/objects' # pb.setAdditionalSearchPath(fpath) # re-usable collision shape cube_mass = 2 cube_side = .02 half_exts = (cube_side * .5, ) * 3 if not hasattr(self, "cube_collision"): self.cube_collision = pb.createCollisionShape( pb.GEOM_BOX, halfExtents=half_exts) colors = list(it.product([0, 1], repeat=3)) self.block_id = {} for b, block in enumerate(self.blocks): base, level = self.base_and_level_of(block) height = .01 + level * .0201 pos, quat = self.placement_of(base) pos = (pos[0], pos[1], height) rgba = colors[b % len(colors)] + (1, ) # self.block_id[block] = pb.loadURDF( # 'cube.urdf', basePosition=pos, baseOrientation=quat, useFixedBase=False) # pb.changeVisualShape(self.block_id[block], linkIndex=-1, rgbaColor=rgba) cube_visual = pb.createVisualShape(pb.GEOM_BOX, halfExtents=half_exts, rgbaColor=rgba) self.block_id[block] = pb.createMultiBody(cube_mass, self.cube_collision, cube_visual, basePosition=pos, baseOrientation=quat) self.step() # let blocks settle
def single_attack(self): V = 3 robot_base_position = p.getBasePositionAndOrientation(self.biped_robot) robot_base_position = robot_base_position[0] robot_base_position = [ robot_base_position[0] + self.init_pos[0], robot_base_position[1] + self.init_pos[1], robot_base_position[2] + self.init_pos[2] - 0.3 ] Velocity = [ random.random() * 5 - 2.5, random.random() * 5 - 2.5, random.random() * 5 - 2.5 ] mass = self.attack_strength / sqrt(Velocity[0]**2 + Velocity[1]**2 + Velocity[2]**2) mass = mass / V sphereRadius = 0.02 * random.randint(2, 4) colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius) visualShapeId = -1 # basePosition = robot_base_position + Velocity basePosition = [ robot_base_position[0] + Velocity[0], robot_base_position[1] + Velocity[1], robot_base_position[2] + Velocity[2] ] baseOrientation = [0, 0, 0, 1] sphereUid = p.createMultiBody(mass, colSphereId, visualShapeId, basePosition, baseOrientation) p.changeDynamics(sphereUid, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0, contactStiffness=10000, contactDamping=0) p.resetBaseVelocity(sphereUid, [ -V * Velocity[0] * random.uniform(0.8, 1.2), -V * Velocity[1] * random.uniform(0.8, 1.2), -V * Velocity[2] * random.uniform(0.8, 1.2) ])
def __init__(self, robot_init_range: float, ball_init_x: float, ball_init_y: float, ball_x_vel_range: float, ball_y_vel: float, ball_radius: float, camera_height: float, camera_angle: float, dt: float=(1/30.0), brick_texture='brick2.jpg', ball_color=(0, 1, 0), mode=pb.DIRECT, noise=0.0): self._robot_init_range = robot_init_range self._ball_init_x = ball_init_x self._ball_x_vel_range = ball_x_vel_range self._ball_init_y = ball_init_y self._ball_y_vel = ball_y_vel self._ball_radius = ball_radius self._camera_height = camera_height self._camera_angle = camera_angle self._dt = dt self._gravity = 9.8 self.device = pt.device('cpu') self._noise = noise pb.connect(mode) pb.setGravity(0, 0, 0) pb.setAdditionalSearchPath(pybullet_data.getDataPath()) self._text_id = pb.loadTexture(brick_texture) self._back_id = pb.loadURDF('plane.urdf', globalScaling=4) self._floor_id = pb.loadURDF('plane.urdf') pb.changeVisualShape(self._back_id, -1, textureUniqueId=self._text_id) self._ball_vis_id = pb.createVisualShape(shapeType=pb.GEOM_SPHERE, radius=self._ball_radius, rgbaColor=[ball_color[0], ball_color[1], ball_color[2], 1], specularColor=[0.4, .4, 0], visualFramePosition=[0, 0, 0]) self._ball_col_id = pb.createCollisionShape(shapeType=pb.GEOM_SPHERE, radius=self._ball_radius, collisionFramePosition=[0, 0, 0]) self._ball_id = pb.createMultiBody(baseMass=1, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=self._ball_col_id, baseVisualShapeIndex=self._ball_vis_id, basePosition=[0, 0, 0], useMaximalCoordinates=True)
def load_agent(self): xyz = [1, -1, 0] orientation = [0, 1, 0, 1] self.agent = pb.createCollisionShape(pb.GEOM_CYLINDER, height=0.7, radius=0.05) self.agent_mb = pb.createMultiBody(1, self.agent, -1, basePosition=xyz, baseOrientation=orientation) pb.changeVisualShape(self.agent, -1, rgbaColor=[1, 1, 0, 1]) self.agent_cid = pb.createConstraint(self.agent, -1, -1, -1, pb.JOINT_FIXED, [0, 0, 0], [0, 0, 0], xyz, childFrameOrientation=orientation)
def __init__(self, width, position, orientation, color=(0, 1, 0, 0.5)): """ 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.shape_id = pybullet.createVisualShape( shapeType=pybullet.GEOM_BOX, halfExtents=[width / 2] * 3, rgbaColor=color, ) self.body_id = pybullet.createMultiBody( baseVisualShapeIndex=self.shape_id, basePosition=position, baseOrientation=orientation, )
def load_agent(self): obj = pb.getBasePositionAndOrientation(self.obj_to_classify) target = obj[0] xyz=[target[0] - 1.25, target[1], target[2]] #xyz=[0,-1.25,0] orientation = [0,1,0,1] self.agent = pb.createCollisionShape(pb.GEOM_BOX, halfExtents=[self.wandSide, self.wandSide, self.wandLength]) self.agent_mb= pb.createMultiBody(1,self.agent,-1,basePosition=xyz, baseOrientation=orientation) pb.changeVisualShape(self.agent_mb,-1,rgbaColor=[1,1,0,1]) self.agent_cid = pb.createConstraint(self.agent_mb,-1,-1,-1,pb.JOINT_FIXED, [0,0,0],[0,0,0],xyz, childFrameOrientation=orientation) self.fov = 90
def _load(self): if self.visual_shape == p.GEOM_BOX: shape = p.createVisualShape(self.visual_shape, rgbaColor=self.rgba_color, halfExtents=self.half_extents, visualFramePosition=self.initial_offset) elif self.visual_shape in [p.GEOM_CYLINDER, p.GEOM_CAPSULE]: shape = p.createVisualShape(self.visual_shape, rgbaColor=self.rgba_color, radius=self.radius, length=self.length, visualFramePosition=self.initial_offset) else: shape = p.createVisualShape(self.visual_shape, rgbaColor=self.rgba_color, radius=self.radius, visualFramePosition=self.initial_offset) body_id = p.createMultiBody(baseVisualShapeIndex=shape, baseCollisionShapeIndex=-1) return body_id
def __createCollider(self, mass): if self.model is None: return col_pos = [ self.model.position.x + self.col_local_pos[0], self.model.position.y + self.col_local_pos[1], self.model.position.z + self.col_local_pos[2] ] self.colId = p.createCollisionShape(p.GEOM_CYLINDER, radius=self.col_radius, height=self.col_height) boxId = p.createMultiBody(baseMass=mass, baseCollisionShapeIndex=self.colId, basePosition=col_pos) p.changeDynamics(self.colId, -1, linearDamping=5.0, lateralFriction=1, restitution=0.0)
def _load_geometry(self): shiftX = np.random.uniform(-0.5, 0.5) shiftY = np.random.uniform(-0.5, 0.5) shift = [shiftX, shiftY, -0.15] meshScale = [0.15, 0.15, 0.1] path = os.path.abspath(os.path.dirname(__file__)) self.groundColId = p.createCollisionShape( shapeType=p.GEOM_MESH, fileName=os.path.join(path, "ground.obj"), collisionFramePosition=shift, meshScale=meshScale, flags=p.GEOM_FORCE_CONCAVE_TRIMESH) self.groundId = p.createMultiBody( baseMass=0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=self.groundColId, basePosition=[0, 0, 0], useMaximalCoordinates=True)
def __init__(self, radius, position, color=(0, 1, 0, 0.5)): """ Create a sphere 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.shape_id = p.createVisualShape( shapeType=p.GEOM_SPHERE, radius=radius, rgbaColor=color, ) self.body_id = p.createMultiBody( baseVisualShapeIndex=self.shape_id, basePosition=position, baseOrientation=[0, 0, 0, 1], )
def load(self): collision_id = p.createCollisionShape(p.GEOM_MESH, fileName=self.collision_filename) visual_id = p.createVisualShape(p.GEOM_MESH, fileName=self.visual_filename) body_id = p.createMultiBody(basePosition=[0, 0, 0], baseMass=60, baseCollisionShapeIndex=collision_id, baseVisualShapeIndex=visual_id) self.body_id = body_id p.resetBasePositionAndOrientation(self.body_id, self.pos, [-0.5, -0.5, -0.5, 0.5]) self.cid = p.createConstraint(self.body_id, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], self.pos, parentFrameOrientation=[-0.5, -0.5, -0.5, 0.5]) # facing x axis return body_id
def _createHeightmap(self, **kwargs): loadKeys = { "shapeType": pybullet.GEOM_MESH, "fileName": self.mesh_file, "flags": pybullet.GEOM_FORCE_CONCAVE_TRIMESH | pybullet.GEOM_CONCAVE_INTERNAL_EDGE, } if "scale" in kwargs: loadKeys.update({"meshScale": kwargs["scale"]}) # Create the terrain via the obj data terrain_collision_shape = pybullet.createCollisionShape(**loadKeys) # Create the heightmap loadKeys = { "baseMass": 0, "baseCollisionShapeIndex": terrain_collision_shape, "baseVisualShapeIndex": -1, "basePosition": [0, 0, 0], "baseOrientation": [0, 0, 0, 1], } if "basePosition" in kwargs: loadKeys.update({"basePosition": kwargs["basePosition"]}) if "baseOrientation" in kwargs: loadKeys.update({ "baseOrientation": pybullet.getQuaternionFromEuler(kwargs["baseOrientation"]) }) self.id = pybullet.createMultiBody(**loadKeys) if "texture" in kwargs: self.texture_id = pybullet.loadTexture(kwargs["texture"]) print(self.texture_id) pybullet.changeVisualShape(self.id, -1, -1, self.texture_id)
def _add_capsule_cylinder(self, obj_type, radius, length, color, position, orientation=None): if orientation is None: orientation = [0, 0, 0] # Check for color and orientation self.check_color_orientation(color, position, orientation) orientation = p.getQuaternionFromEuler(orientation) if not (radius > 0 and radius * 2 < MAX_OBJ_DIM): raise AttributeError( "Ivalid radius passed to _add_capsule_cylinder") if not (length > 0 and length + radius <= MAX_OBJ_DIM): raise AttributeError( "Ivalid length passed to _add_capsule_cylinder") valid_pos = lambda p: abs(p) + radius <= self._env_dim / 2 if not all(map(valid_pos, position)): raise AttributeError( "Ivalid position passed to _add_capsule_cylinder") visual_shp = p.createVisualShape(obj_type, radius=radius, length=length, rgbaColor=color) collision_shp = p.createCollisionShape(obj_type, radius=radius, height=length) obj_id = p.createMultiBody(MASS, collision_shp, visual_shp, basePosition=position, baseOrientation=orientation) self.objects_id.append(obj_id) return obj_id
def _place_single_object(self, urdfList): objectUids = [] objectClasses = [] shift = [0, -0.02, 0] # meshScale = [0.3, 0.3, 0.3] meshScale = [0.6, 0.6, 0.6] list_x_pos = [] list_y_pos = [] xpos = 0.5 ypos = 0.15 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 uid = p.createMultiBody(baseMass=1, baseInertialFramePosition=[-0.2, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[xpos, ypos, .11], useMaximalCoordinates=True) objectUids.append(uid) objectClasses.append(class_name) for _ in range(150): p.stepSimulation() return objectUids, objectClasses
def init_tool(self, robot, mesh_scale=[1]*3, pos_offset=[0]*3, orient_offset=[0, 0, 0, 1], left=True, maximal=False, alpha=1.0): if left: gripper_pos, gripper_orient = p.getLinkState(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2] else: gripper_pos, gripper_orient = p.getLinkState(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2] transform_pos, transform_orient = p.multiplyTransforms(positionA=gripper_pos, orientationA=gripper_orient, positionB=pos_offset, orientationB=orient_offset, physicsClientId=self.id) if self.task == 'scratch_itch': tool = p.loadURDF(os.path.join(self.directory, 'scratcher', 'tool_scratch.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id) elif self.task == 'bed_bathing': tool = p.loadURDF(os.path.join(self.directory, 'bed_bathing', 'wiper.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id) elif self.task == 'bite_transfer' and self.robot_type == "panda": tool = p.loadURDF(os.path.join(self.directory, 'dinnerware', 'drop_fork.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id) elif self.task in ['drinking', 'scooping', 'feeding', 'arm_manipulation']: if self.task == 'drinking': visual_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup.obj') collision_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup_vhacd.obj') elif self.task in ['scooping', 'feeding']: visual_filename = os.path.join(self.directory, 'dinnerware', 'spoon_reduced_compressed.obj') collision_filename = os.path.join(self.directory, 'dinnerware', 'spoon_vhacd.obj') elif self.task == 'arm_manipulation': visual_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper.obj') collision_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper_vhacd.obj') tool_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=visual_filename, meshScale=mesh_scale, rgbaColor=[1, 1, 1, alpha], physicsClientId=self.id) tool_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=collision_filename, meshScale=mesh_scale, physicsClientId=self.id) tool = p.createMultiBody(baseMass=0.01, baseCollisionShapeIndex=tool_collision, baseVisualShapeIndex=tool_visual, basePosition=transform_pos, baseOrientation=transform_orient, useMaximalCoordinates=maximal, physicsClientId=self.id) if left: # Disable collisions between the tool and robot for j in (range(71, 86) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [47, 49, 50, 51, 52] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]): for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]: p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id) # Create constraint that keeps the tool in the gripper constraint = p.createConstraint(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id) else: # Disable collisions between the tool and robot for j in (range(49, 64) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [25, 27, 28, 29, 30] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]): for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]: p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id) # Create constraint that keeps the tool in the gripper constraint = p.createConstraint(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id) p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id) return tool
def initSim(nbobj, rad, pos1): # créé le sol p.createCollisionShape(p.GEOM_PLANE) basePlane = p.createMultiBody(0, 0) # met une seed au rng random.seed(time.time()) # random.seed("Thibault") # créé la sphere qui sera le point centrale de la "Shape" nbPop = 100 pop = [] for i in range(0, nbPop - 1): body = p.createCollisionShape(p.GEOM_SPHERE, radius=rad * 4) temp = s.Shape(body, pos1) temp.createRandom(nbobj, rad) idtmp = temp.createBody() pop.append(temp) pop[i].setId(idtmp) for i in range(-1, nbobj): for j in range(-1, nbobj): for elem1 in range(0, nbPop - 1): for elem2 in range(elem1 + 1, nbPop - 1): p.setCollisionFilterPair(pop[elem1].getId(), pop[elem2].getId(), i, j, 0) print((i + 2) / (nbobj + 1)) # ========================================================== # Truc lié a la simulation # ========================================================== for elem in pop: for i in range(0, nbobj): p.setJointMotorControl2(bodyUniqueId=elem.getId(), jointIndex=i, controlMode=p.VELOCITY_CONTROL, targetVelocity=random.randint(0, 3)) # met la gravité p.setGravity(0, 0, -9.81) return pop
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 ObjectSpawner(): print("Objectspawner") p.connect(p.DIRECT) cube_visual = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.125, 0.125, 0.125]) cube_collision = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.25, 0.25, 0.25]) cube_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=cube_collision, baseVisualShapeIndex=cube_visual, basePosition=[5, 1, 0.725]) p.loadURDF("./urdf/table/table.urdf", basePosition=[5, 1, 0], globalScaling=1) p.loadURDF("./urdf/chair/chair.urdf", basePosition=[6, 1, 0], globalScaling=1) p.loadURDF("./urdf/chair/chair.urdf", basePosition=[7, 1, 0], globalScaling=1)
def __init__(self, config, obj_dir, forward=True): p.resetSimulation() self.obj_dir = obj_dir self.objects = config self.forward = forward self.object_ids = [] self.types = [] self.scales = [] self.colors = [] p.setAdditionalSearchPath(pybullet_data.getDataPath()) for obj_params in self.objects: self.object_ids.append(self.add_object(**obj_params)) plane_id = p.createCollisionShape(p.GEOM_MESH, fileName="plane.obj", meshScale=[10, 10, 10]) self.ground_id = p.createMultiBody(0., baseCollisionShapeIndex=plane_id, basePosition=[0, 0, 0])
def _create_object(self, pybullet_client_id, **kwargs): """ :param pybullet_client_id: (list) specifies the pybullet client ids. :param kwargs: (params) parameters for the goal shape creation. :return: (tuple) the first position specifies the shape_id and the second specifies the block id for pybullet. """ position = np.array(self._position) position[-1] += WorldConstants.FLOOR_HEIGHT shape_id = pybullet.createVisualShape( shapeType=pybullet.GEOM_BOX, halfExtents=self._size / 2, rgbaColor=np.append(self._color, self._alpha), physicsClientId=pybullet_client_id) block_id = pybullet.createMultiBody(baseVisualShapeIndex=shape_id, basePosition=position, baseOrientation=self._orientation, physicsClientId=pybullet_client_id) return shape_id, block_id
def _initialize_dataset(self): for i, (obj_id, model_path, obj_com) in enumerate( zip(self.dataset.obj_ids, self.dataset.collider_paths, self.dataset.obj_coms)): try: collision_shape = pybullet.createCollisionShape( shapeType=pybullet.GEOM_MESH, fileName=model_path, meshScale=[self.dataset.obj_scale] * 3) model = pybullet.createMultiBody( baseMass=1.0, baseVisualShapeIndex=-1, baseCollisionShapeIndex=collision_shape, basePosition=[i * 0.5, 0, 0], baseOrientation=[1, 0, 0, 0], baseInertialFramePosition=obj_com) self.models[obj_id] = model except Exception as _: self.models.clear() raise ValueError("Could not load models for simulation.")
def __init__(self, n_spheres, r_min, r_max, duration, physics_client_id): self.duration = duration colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=0.001, physicsClientId=physics_client_id) self.list_of_spheres = [] for i in range(n_spheres): r = r_min + (r_max - r_min) * i / float(n_spheres - 1) visSphereId = p.createVisualShape( p.GEOM_SPHERE, radius=r, physicsClientId=physics_client_id) sphere_id = p.createMultiBody(0, colSphereId, visSphereId, [0, 0, 100], physicsClientId=physics_client_id) sdl = p.getVisualShapeData(sphere_id, physicsClientId=physics_client_id) p.changeVisualShape(sphere_id, sdl[0][1], rgbaColor=[1, 1, 0.8, 0.5], physicsClientId=physics_client_id) self.list_of_spheres.append(sphere_id)
def update_targets(self): if self.target is None: self.target = np.array( self.config["target_spawn_mu"]) + np.random.rand(2) * np.array( self.config["target_spawn_sigma"]) - np.array( self.config["target_spawn_sigma"]) / 2 self.target_body = p.createMultiBody( baseMass=0, baseVisualShapeIndex=self.target_visualshape, basePosition=[self.target[0], self.target[1], 0], physicsClientId=self.client_ID) else: self.target = np.array( self.config["target_spawn_mu"]) + np.random.rand(2) * np.array( self.config["target_spawn_sigma"]) - np.array( self.config["target_spawn_sigma"]) / 2 p.resetBasePositionAndOrientation( self.target_body, [self.target[0], self.target[1], 0], [0, 0, 0, 1], physicsClientId=self.client_ID)
def __init__(self, pos, rot, x_scale, y_scale, z_scale): visualShapeId = pb.createVisualShape(shapeType=pb.GEOM_BOX, halfExtents=[ 0.05 / 2 * x_scale, 0.15 / 2 * y_scale, 0.05 / 2 * z_scale ]) collisionShapeId = pb.createCollisionShape(shapeType=pb.GEOM_BOX, halfExtents=[ 0.05 / 2 * x_scale, 0.15 / 2 * y_scale, 0.05 / 2 * z_scale ]) object_id = pb.createMultiBody( baseMass=0.1, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=pos, baseOrientation=rot) super(RandomBrick, self).__init__(constants.BRICK, object_id)
def __init__( self, number_of_goals, goal_size=0.015, initial_position=[0.18, 0.18, 0.08], ): """ Import a marker for visualization Args: number_of_goals (int): the desired number of goals to display goal_size (float): how big should this goal be initial_position (list of floats): where in xyz space should the goal first be displayed """ color_cycle = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1]] goal_shape_ids = [None] * number_of_goals self.goal_ids = [None] * number_of_goals self.goal_orientations = [None] * number_of_goals # Can use both a block, or a sphere: uncomment accordingly for i in range(number_of_goals): color = color_cycle[i % len(color_cycle)] goal_shape_ids[i] = pybullet.createVisualShape( # shapeType=pybullet.GEOM_BOX, # halfExtents=[goal_size] * number_of_goals, shapeType=pybullet.GEOM_SPHERE, radius=goal_size, rgbaColor=color, ) self.goal_ids[i] = pybullet.createMultiBody( baseVisualShapeIndex=goal_shape_ids[i], basePosition=initial_position, baseOrientation=[0, 0, 0, 1], ) ( _, self.goal_orientations[i], ) = pybullet.getBasePositionAndOrientation(self.goal_ids[i])
def add_object(self, shape, mass=1, init_pos=(0, 0, 1), init_orn=(0, 0, 0), scale=(1, 1, 1), init_v=(0, 0, 0), lat_fric=0., restitution=.9, lin_damp=0, angular_damp=0, disappear_time=100000, appear_time=0, **kwargs): """ create an pybullet base object from a wavefront .obj file set up initial parameters and physical properties """ scale = [x * y for x, y in zip(scale, SHAPE_DIMENSIONS[shape])] shape = "cube" obj_path = os.path.join(self.obj_dir, "shapes", '%s.obj' % shape) init_orn_quat = p.getQuaternionFromEuler(deg2rad(init_orn)) col_id = p.createCollisionShape(p.GEOM_MESH, fileName=obj_path, meshScale=scale) obj_id = p.createMultiBody(mass, col_id, basePosition=init_pos, baseOrientation=init_orn_quat) p.resetBaseVelocity(obj_id, linearVelocity=init_v) p.changeDynamics(obj_id, -1, lateralFriction=lat_fric, restitution=restitution, linearDamping=lin_damp, angularDamping=angular_damp) self.init_positions.append(init_pos) self.disappear_time.append(disappear_time) self.appear_time.append(appear_time) return obj_id
def _add_crate_floor(self): """Add a crate floor.""" x_length = self.crate_wall_width / 2 y_length = self.crate_wall_width / 2 z_length = self.crate_wall_thickness / 2 collision_shape_id = pb.createCollisionShape( pb.GEOM_BOX, halfExtents=[x_length, y_length, z_length], collisionFramePosition=[0, 0, 0], physicsClientId=self.client_id ) visual_shape_id = -1 if self.client_mode == pb.GUI: visual_shape_id = pb.createVisualShape( pb.GEOM_BOX, halfExtents=[x_length, y_length, z_length], visualFramePosition=[0, 0, 0], physicsClientId=self.client_id ) multibody_id = pb.createMultiBody( baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id, physicsClientId=self.client_id ) self.crate_side_ids.append(multibody_id)
def create_flying_body(group, collision_id=NULL_ID, visual_id=NULL_ID, mass=STATIC_MASS): # TODO: more generally clone the body indices = list(range(len(group) + 1)) masses = len(group) * [STATIC_MASS] + [mass] visuals = len(group) * [NULL_ID] + [visual_id] collisions = len(group) * [NULL_ID] + [collision_id] types = [CARTESIAN_TYPES[joint][0] for joint in group] + [p.JOINT_FIXED] #parents = [BASE_LINK] + indices[:-1] parents = indices assert len(indices) == len(visuals) == len(collisions) == len(types) == len(parents) link_positions = len(indices) * [unit_point()] link_orientations = len(indices) * [unit_quat()] inertial_positions = len(indices) * [unit_point()] inertial_orientations = len(indices) * [unit_quat()] axes = len(indices) * [unit_point()] axes = [CARTESIAN_TYPES[joint][1] for joint in group] + [unit_point()] # TODO: no way of specifying joint limits return p.createMultiBody( baseMass=STATIC_MASS, baseCollisionShapeIndex=NULL_ID, baseVisualShapeIndex=NULL_ID, basePosition=unit_point(), baseOrientation=unit_quat(), baseInertialFramePosition=unit_point(), baseInertialFrameOrientation=unit_quat(), linkMasses=masses, linkCollisionShapeIndices=collisions, linkVisualShapeIndices=visuals, linkPositions=link_positions, linkOrientations=link_orientations, linkInertialFramePositions=inertial_positions, linkInertialFrameOrientations=inertial_orientations, linkParentIndices=parents, linkJointTypes=types, linkJointAxis=axes, physicsClientId=CLIENT, )
def build_scene(): map_label = np.zeros((MAP_SIZE, MAP_SIZE), dtype=np.uint8) bodies = [] walls = np.random.sample((4, )) * MAX_WALL_INNESS wall_pixels = np.ceil(walls / PIXEL_FRACTION).astype(np.int) map_label[wall_pixels[0]:MAP_SIZE - wall_pixels[2], wall_pixels[1]:MAP_SIZE - wall_pixels[3]] = 1 orients = [ p.getQuaternionFromEuler([0, 0, math.pi / 2]), p.getQuaternionFromEuler([0, 0, 0]) ] for i in range(4): offset = walls[i] - WALL_THICKNESS / 2 if i > 1: offset = SCENE_SIZE - offset pos = np.zeros((3, )) pos[i % 2] = offset obj = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=wall_collision, baseOrientation=orients[i % 2], basePosition=pos) bodies.append(obj) colorize(obj) for i in range(np.random.randint(6, 10)): obj = insert_model(np.random.choice(model_names), pos=random_pos(walls), rot=np.random.random() * 2 * math.pi) colorize(obj) bodies.append(obj) # for i in range(8): # p.addUserDebugLine([0, 0, i / 4], [SCENE_SIZE, 0, i / 4], [0, 0, 1]) # p.addUserDebugLine([SCENE_SIZE, 0, i / 4], [SCENE_SIZE, SCENE_SIZE, i / 4], [0, 0, 1]) # p.addUserDebugLine([SCENE_SIZE, SCENE_SIZE, i / 4], [0, SCENE_SIZE, i / 4], [0, 0, 1]) # p.addUserDebugLine([0, SCENE_SIZE, i / 4], [0, 0, i / 4], [0, 0, 1]) return walls, map_label, bodies
def addCylinder(pos: list, raidus: float, length: float, mass: float = 10000., rgba: list = [1., 1., 1., 1.], physicsClientId: int = 0): visual_shape = p.createVisualShape(p.GEOM_CYLINDER, radius=raidus, length=length, rgbaColor=rgba, physicsClientId=physicsClientId) collision_shape = p.createCollisionShape(p.GEOM_CYLINDER, radius=raidus, height=length, physicsClientId=physicsClientId) entity_id = p.createMultiBody( baseMass=mass, baseCollisionShapeIndex=collision_shape, baseVisualShapeIndex=visual_shape, basePosition=[pos[0], pos[1], pos[2] + length / 2.], physicsClientId=physicsClientId) return entity_id
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)
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)
) #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, baseVisualShapeIndex=visualShapeId, basePosition=[0, 0, 2], batchPositions=batchPositions, useMaximalCoordinates=True) p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid) p.syncBodyInfo() print("numBodies=", p.getNumBodies()) p.stopStateLogging(logId) p.setGravity(0, 0, -10) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]] currentColor = 0
useMaximalCoordinates = 0 p.connect(p.GUI) p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates) p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(fixedTimeStep=1./120.) 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) p.setRealTimeSimulation(1) while (1): time.sleep(0.01)
def createMultiBody(self, basePosition=[0,0,0],baseOrientation=[0,0,0,1], physicsClientId=0): #assume link[0] is base if (len(self.urdfLinks)==0): return -1 #for i in range (len(self.urdfLinks)): # print("link", i, "=",self.urdfLinks[i].link_name) base = self.urdfLinks[0] #v.tmp_collision_shape_ids=[] baseMass = base.urdf_inertial.mass baseCollisionShapeIndex = -1 baseShapeTypeArray=[] baseRadiusArray=[] baseHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] meshScaleArray=[] basePositionsArray=[] baseOrientationsArray=[] for v in base.urdf_collision_shapes: shapeType = v.geom_type baseShapeTypeArray.append(shapeType) baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) baseRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) meshScaleArray.append(v.geom_meshscale) basePositionsArray.append(v.origin_xyz) orn=p.getQuaternionFromEuler(v.origin_rpy) baseOrientationsArray.append(orn) if (len(baseShapeTypeArray)): #print("fileNameArray=",fileNameArray) baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray, radii=baseRadiusArray, halfExtents=baseHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=meshScaleArray, collisionFramePositions=basePositionsArray, collisionFrameOrientations=baseOrientationsArray, physicsClientId=physicsClientId) urdfVisuals = base.urdf_visual_shapes shapeTypes=[v.geom_type for v in urdfVisuals] halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals] radii=[v.geom_radius for v in urdfVisuals] lengths=[v.geom_length for v in urdfVisuals] fileNames=[v.geom_meshfilename for v in urdfVisuals] meshScales=[v.geom_meshscale for v in urdfVisuals] rgbaColors=[v.material_rgba for v in urdfVisuals] visualFramePositions=[v.origin_xyz for v in urdfVisuals] visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals] baseVisualShapeIndex = -1 if (len(shapeTypes)): #print("len(shapeTypes)=",len(shapeTypes)) #print("len(halfExtents)=",len(halfExtents)) #print("len(radii)=",len(radii)) #print("len(lengths)=",len(lengths)) #print("len(fileNames)=",len(fileNames)) #print("len(meshScales)=",len(meshScales)) #print("len(rgbaColors)=",len(rgbaColors)) #print("len(visualFramePositions)=",len(visualFramePositions)) #print("len(visualFrameOrientations)=",len(visualFrameOrientations)) baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes, halfExtents=halfExtents,radii=radii,lengths=lengths,fileNames=fileNames, meshScales=meshScales,rgbaColors=rgbaColors,visualFramePositions=visualFramePositions, visualFrameOrientations=visualFrameOrientations,physicsClientId=physicsClientId) linkMasses=[] linkCollisionShapeIndices=[] linkVisualShapeIndices=[] linkPositions=[] linkOrientations=[] linkInertialFramePositions=[] linkInertialFrameOrientations=[] linkParentIndices=[] linkJointTypes=[] linkJointAxis=[] for joint in self.urdfJoints: link = joint.link linkMass = link.urdf_inertial.mass linkCollisionShapeIndex=-1 linkVisualShapeIndex=-1 linkPosition=[0,0,0] linkOrientation=[0,0,0] linkInertialFramePosition=[0,0,0] linkInertialFrameOrientation=[0,0,0] linkParentIndex=self.linkNameToIndex[joint.parent_name] linkJointType=joint.joint_type linkJointAx = joint.joint_axis_xyz linkShapeTypeArray=[] linkRadiusArray=[] linkHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] linkMeshScaleArray=[] linkPositionsArray=[] linkOrientationsArray=[] for v in link.urdf_collision_shapes: shapeType = v.geom_type linkShapeTypeArray.append(shapeType) linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) linkRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) linkMeshScaleArray.append(v.geom_meshscale) linkPositionsArray.append(v.origin_xyz) linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy)) if (len(linkShapeTypeArray)): linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray, radii=linkRadiusArray, halfExtents=linkHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=linkMeshScaleArray, collisionFramePositions=linkPositionsArray, collisionFrameOrientations=linkOrientationsArray, physicsClientId=physicsClientId) urdfVisuals = link.urdf_visual_shapes linkVisualShapeIndex = -1 shapeTypes=[v.geom_type for v in urdfVisuals] halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals] radii=[v.geom_radius for v in urdfVisuals] lengths=[v.geom_length for v in urdfVisuals] fileNames=[v.geom_meshfilename for v in urdfVisuals] meshScales=[v.geom_meshscale for v in urdfVisuals] rgbaColors=[v.material_rgba for v in urdfVisuals] visualFramePositions=[v.origin_xyz for v in urdfVisuals] visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals] if (len(shapeTypes)): print("fileNames=",fileNames) linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes, halfExtents=halfExtents,radii=radii,lengths=lengths, fileNames=fileNames,meshScales=meshScales,rgbaColors=rgbaColors, visualFramePositions=visualFramePositions, visualFrameOrientations=visualFrameOrientations, physicsClientId=physicsClientId) linkMasses.append(linkMass) linkCollisionShapeIndices.append(linkCollisionShapeIndex) linkVisualShapeIndices.append(linkVisualShapeIndex) linkPositions.append(joint.joint_origin_xyz) linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy)) linkInertialFramePositions.append(link.urdf_inertial.origin_xyz) linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy)) linkParentIndices.append(linkParentIndex) linkJointTypes.append(joint.joint_type) linkJointAxis.append(joint.joint_axis_xyz) obUid = p.createMultiBody(baseMass,\ baseCollisionShapeIndex=baseCollisionShapeIndex, baseVisualShapeIndex=baseVisualShapeIndex, basePosition=basePosition, baseOrientation=baseOrientation, baseInertialFramePosition=base.urdf_inertial.origin_xyz, baseInertialFrameOrientation=p.getQuaternionFromEuler(base.urdf_inertial.origin_rpy), linkMasses=linkMasses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, linkPositions=linkPositions, linkOrientations=linkOrientations, linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=linkParentIndices, linkJointTypes=linkJointTypes, linkJointAxis=linkJointAxis, physicsClientId=physicsClientId) return obUid
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): for j in range (5): for k in range (5):
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): p.createMultiBody(baseMass=0, baseCollisionShapeIndex=colBoxId, basePosition=[segmentStart, 0, -0.1]) segmentStart = segmentStart - 1 for i in range(segmentLength): height = 0 if (i % 2): height = 1 p.createMultiBody(baseMass=0, baseCollisionShapeIndex=colBoxId, basePosition=[segmentStart, 0, -0.1 + height]) segmentStart = segmentStart - 1 baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.]) for i in range(segmentLength):
def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0): #assume link[0] is base if (len(self.urdfLinks)==0): return -1 base = self.urdfLinks[0] #v.tmp_collision_shape_ids=[] baseMass = base.urdf_inertial.mass print("baseMass=",baseMass) baseCollisionShapeIndex = -1 baseShapeTypeArray=[] baseRadiusArray=[] baseHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] meshScaleArray=[] basePositionsArray=[] baseOrientationsArray=[] for v in base.urdf_collision_shapes: print("base v.origin_xyz=",v.origin_xyz) print("base v.origin_rpy=",v.origin_rpy) shapeType = v.geom_type baseShapeTypeArray.append(shapeType) baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) baseRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) meshScaleArray.append(v.geom_meshscale) basePositionsArray.append(v.origin_xyz) print("v.origin_rpy=",v.origin_rpy) orn=p.getQuaternionFromEuler(v.origin_rpy) baseOrientationsArray.append(orn) print("baseHalfExtentsArray=",baseHalfExtentsArray) if (len(baseShapeTypeArray)): baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray, radii=baseRadiusArray, halfExtents=baseHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=meshScaleArray, collisionFramePositions=basePositionsArray, collisionFrameOrientations=baseOrientationsArray, physicsClientId=physicsClientId) print("baseCollisionShapeIndex=",baseCollisionShapeIndex) linkMasses=[] linkCollisionShapeIndices=[] linkVisualShapeIndices=[] linkPositions=[] linkOrientations=[] linkMeshScaleArray=[] linkInertialFramePositions=[] linkInertialFrameOrientations=[] linkParentIndices=[] linkJointTypes=[] linkJointAxis=[] for joint in self.urdfJoints: link = joint.link linkMass = link.urdf_inertial.mass print("linkMass=",linkMass) linkCollisionShapeIndex=-1 linkVisualShapeIndex=-1 linkPosition=[0,0,0] linkOrientation=[0,0,0] linkInertialFramePosition=[0,0,0] linkInertialFrameOrientation=[0,0,0] linkParentIndex=self.linkNameToIndex[joint.parent_name] print("parentId=",linkParentIndex) linkJointType=joint.joint_type print("linkJointType=",linkJointType, self.jointTypeToName[linkJointType] ) linkJointAx = joint.joint_axis_xyz linkShapeTypeArray=[] linkRadiusArray=[] linkHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] linkPositionsArray=[] linkOrientationsArray=[] for v in link.urdf_collision_shapes: print("link v.origin_xyz=",v.origin_xyz) print("link v.origin_rpy=",v.origin_rpy) shapeType = v.geom_type linkShapeTypeArray.append(shapeType) linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) linkRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) linkMeshScaleArray.append(v.geom_meshscale) linkPositionsArray.append(v.origin_xyz) linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy)) print("linkHalfExtentsArray=",linkHalfExtentsArray) if (len(linkShapeTypeArray)): linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray, radii=linkRadiusArray, halfExtents=linkHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=linkMeshScaleArray, collisionFramePositions=linkPositionsArray, collisionFrameOrientations=linkOrientationsArray, physicsClientId=physicsClientId) linkMasses.append(linkMass) linkCollisionShapeIndices.append(linkCollisionShapeIndex) linkVisualShapeIndices.append(linkVisualShapeIndex) linkPositions.append(joint.joint_origin_xyz) linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy)) linkInertialFramePositions.append(link.urdf_inertial.origin_xyz) linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy)) linkParentIndices.append(linkParentIndex) linkJointTypes.append(joint.joint_type) linkJointAxis.append(joint.joint_axis_xyz) print("\n\n\nlinkMasses=",linkMasses) obUid = p.createMultiBody(baseMass,\ baseCollisionShapeIndex= baseCollisionShapeIndex, basePosition=basePosition, baseInertialFramePosition=base.urdf_inertial.origin_xyz, baseInertialFrameOrientation=base.urdf_inertial.origin_rpy, linkMasses=linkMasses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, linkPositions=linkPositions, linkOrientations=linkOrientations, linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=linkParentIndices, linkJointTypes=linkJointTypes, linkJointAxis=linkJointAxis, physicsClientId=physicsClientId) return obUid
count+=1 if ((count % 100)==0): print(count,"out of ", imgW*imgH/(stepX*stepY)) rayFrom,rayTo, alpha = getRayFromTo(w*(width/imgW),h*(height/imgH)) rf = np.array(rayFrom) rt = np.array(rayTo) vec = rt-rf l = np.sqrt(np.dot(vec,vec)) depthImg = float(depthBuffer[h,w]) far=1000. near=0.01 depth = far * near / (far - (far - near) * depthImg) depth/=math.cos(alpha) newTo = (depth/l)*vec+rf p.addUserDebugLine(rayFrom,newTo,[1,0,0]) mb = p.createMultiBody(baseMass=0,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = newTo, useMaximalCoordinates=True) color = rgbBuffer[h,w] color=[color[0]/255.,color[1]/255.,color[2]/255.,1] p.changeVisualShape(mb,-1,rgbaColor=color) p.addUserDebugLine(corners3D[0],corners3D[1],[1,0,0]) p.addUserDebugLine(corners3D[1],corners3D[2],[1,0,0]) p.addUserDebugLine(corners3D[2],corners3D[3],[1,0,0]) p.addUserDebugLine(corners3D[3],corners3D[0],[1,0,0]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) print("ready\n") #p.removeBody(plane) #p.removeBody(cube) while (1): p.setGravity(0,0,-10)
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 = []
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj") collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, vertices=vertices, collisionFramePosition=shift, meshScale=meshScale) texUid = p.loadTexture("tex256.png") rangex = 1 rangey = 1 for i in range(rangex): for j in range(rangey): bodyUid = 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.changeVisualShape(bodyUid, -1, textureUniqueId=texUid) 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): p.getCameraImage(320, 200) mouseEvents = p.getMouseEvents()
def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0): #assume link[0] is base if (len(self.urdfLinks)==0): return -1 base = self.urdfLinks[0] #v.tmp_collision_shape_ids=[] baseMass = base.urdf_inertial.mass baseCollisionShapeIndex = -1 baseShapeTypeArray=[] baseRadiusArray=[] baseHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] meshScaleArray=[] basePositionsArray=[] baseOrientationsArray=[] for v in base.urdf_collision_shapes: shapeType = v.geom_type baseShapeTypeArray.append(shapeType) baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) baseRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) meshScaleArray.append(v.geom_meshscale) basePositionsArray.append(v.origin_xyz) orn=p.getQuaternionFromEuler(v.origin_rpy) baseOrientationsArray.append(orn) if (len(baseShapeTypeArray)): baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray, radii=baseRadiusArray, halfExtents=baseHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=meshScaleArray, collisionFramePositions=basePositionsArray, collisionFrameOrientations=baseOrientationsArray, physicsClientId=physicsClientId) urdfVisuals = base.urdf_visual_shapes baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals], halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals], radii=[v.geom_radius for v in urdfVisuals], lengths=[v.geom_length[0] for v in urdfVisuals], fileNames=[v.geom_meshfilename for v in urdfVisuals], meshScales=[v.geom_meshscale for v in urdfVisuals], rgbaColors=[v.material_rgba for v in urdfVisuals], visualFramePositions=[v.origin_xyz for v in urdfVisuals], visualFrameOrientations=[v.origin_rpy for v in urdfVisuals], physicsClientId=physicsClientId) # urdfVisual = base.urdf_visual_shapes[0] # baseVisualShapeIndex = p.createVisualShape(shapeType=urdfVisual.geom_type, # halfExtents=[ext * 0.5 for ext in urdfVisual.geom_extents], # radius=urdfVisual.geom_radius, # length=urdfVisual.geom_length[0], # fileName=urdfVisual.geom_meshfilename, # meshScale=urdfVisual.geom_meshscale, # rgbaColor=urdfVisual.material_rgba, # visualFramePosition=urdfVisual.origin_xyz, # visualFrameOrientation=urdfVisual.origin_rpy, # physicsClientId=physicsClientId) linkMasses=[] linkCollisionShapeIndices=[] linkVisualShapeIndices=[] linkPositions=[] linkOrientations=[] linkMeshScaleArray=[] linkInertialFramePositions=[] linkInertialFrameOrientations=[] linkParentIndices=[] linkJointTypes=[] linkJointAxis=[] for joint in self.urdfJoints: link = joint.link linkMass = link.urdf_inertial.mass linkCollisionShapeIndex=-1 linkVisualShapeIndex=-1 linkPosition=[0,0,0] linkOrientation=[0,0,0] linkInertialFramePosition=[0,0,0] linkInertialFrameOrientation=[0,0,0] linkParentIndex=self.linkNameToIndex[joint.parent_name] linkJointType=joint.joint_type linkJointAx = joint.joint_axis_xyz linkShapeTypeArray=[] linkRadiusArray=[] linkHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] linkPositionsArray=[] linkOrientationsArray=[] for v in link.urdf_collision_shapes: shapeType = v.geom_type linkShapeTypeArray.append(shapeType) linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) linkRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) linkMeshScaleArray.append(v.geom_meshscale) linkPositionsArray.append(v.origin_xyz) linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy)) if (len(linkShapeTypeArray)): linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray, radii=linkRadiusArray, halfExtents=linkHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=linkMeshScaleArray, collisionFramePositions=linkPositionsArray, collisionFrameOrientations=linkOrientationsArray, physicsClientId=physicsClientId) urdfVisuals = link.urdf_visual_shapes linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals], halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals], radii=[v.geom_radius for v in urdfVisuals], lengths=[v.geom_length[0] for v in urdfVisuals], fileNames=[v.geom_meshfilename for v in urdfVisuals], meshScales=[v.geom_meshscale for v in urdfVisuals], rgbaColors=[v.material_rgba for v in urdfVisuals], visualFramePositions=[v.origin_xyz for v in urdfVisuals], visualFrameOrientations=[v.origin_rpy for v in urdfVisuals], physicsClientId=physicsClientId) linkMasses.append(linkMass) linkCollisionShapeIndices.append(linkCollisionShapeIndex) linkVisualShapeIndices.append(linkVisualShapeIndex) linkPositions.append(joint.joint_origin_xyz) linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy)) linkInertialFramePositions.append(link.urdf_inertial.origin_xyz) linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy)) linkParentIndices.append(linkParentIndex) linkJointTypes.append(joint.joint_type) linkJointAxis.append(joint.joint_axis_xyz) obUid = p.createMultiBody(baseMass,\ baseCollisionShapeIndex=baseCollisionShapeIndex, baseVisualShapeIndex=baseVisualShapeIndex, basePosition=basePosition, baseInertialFramePosition=base.urdf_inertial.origin_xyz, baseInertialFrameOrientation=base.urdf_inertial.origin_rpy, linkMasses=linkMasses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, linkPositions=linkPositions, linkOrientations=linkOrientations, linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=linkParentIndices, linkJointTypes=linkJointTypes, linkJointAxis=linkJointAxis, physicsClientId=physicsClientId) return obUid
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 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.)
p.resetSimulation() meshScale = [.1, .1, .01] shift = [0, 0, 0] visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH, fileName="marble_cube.obj", rgbaColor=[1, 1, 1, 1], specularColor=[1, 1, 1], visualFramePosition=shift, meshScale=meshScale) #collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="textures/marble_cube.obj", collisionFramePosition=shift,meshScale=meshScale) collisionShapeId = -1 uiCube = p.createMultiBody(baseMass=0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[0, 1, 0], useMaximalCoordinates=True) textOrn = p.getQuaternionFromEuler([0, 0, -1.5707963]) numLines = 1 lines = [-1] * numLines p.stepSimulation() #disable rendering during loading makes it much faster p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) #objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] kitchenObj = p.loadSDF("kitchens/1.sdf") #objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] objects = [ p.loadURDF("pr2_gripper.urdf", 0.500000, 0.300006, 0.700000, -0.000000, -0.000000, -0.000031,