def create_shape_array(geoms, poses, colors=None): # https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/pybullet.c # createCollisionShape: height # createVisualShape: length # createCollisionShapeArray: lengths # createVisualShapeArray: lengths mega_geom = defaultdict(list) for geom in geoms: extended_geom = get_default_geometry() extended_geom.update(geom) #extended_geom = geom.copy() for key, value in extended_geom.items(): mega_geom[plural(key)].append(value) collision_args = mega_geom.copy() for (point, quat) in poses: collision_args['collisionFramePositions'].append(point) collision_args['collisionFrameOrientations'].append(quat) collision_id = p.createCollisionShapeArray(physicsClientId=CLIENT, **collision_args) if (colors is None): # or not has_gui(): return collision_id, NULL_ID visual_args = mega_geom.copy() for (point, quat), color in zip(poses, colors): # TODO: color doesn't seem to work correctly here visual_args['rgbaColors'].append(color) visual_args['visualFramePositions'].append(point) visual_args['visualFrameOrientations'].append(quat) visual_id = p.createVisualShapeArray(physicsClientId=CLIENT, **visual_args) return collision_id, visual_id
def corner(width, wall_width=0.05, position=[0., 0., 0.], orientation=[0., 0., 0., 1.]): # create a corner (cube open at neg. x and pos. z) as a building block of an obstacle parcour half_extents = [[width / 2, width / 2 + wall_width, wall_width / 2], [width / 2, wall_width / 2, width / 2], [width / 2, wall_width / 2, width / 2], [wall_width / 2, width / 2 + wall_width, width / 2]] positions = [[0, 0., -width / 2 - wall_width / 2], [0, -width / 2 - wall_width / 2, 0.], [0, width / 2 + wall_width / 2, 0.], [width / 2 + wall_width / 2, 0., 0.]] visual_shape_id = p.createVisualShapeArray(shapeTypes=[p.GEOM_BOX] * 4, halfExtents=half_extents, visualFramePositions=positions) collision_shape_id = p.createCollisionShapeArray( shapeTypes=[p.GEOM_BOX] * 4, halfExtents=half_extents, collisionFramePositions=positions) 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 tunnel(length, width, wall_width=0.05, position=DEFAULT_POS, orientation=DEFAULT_ORI): # create a rectangular tunnel as a building block of an obstacle parcour half_extents = [[length / 2, width / 2 + wall_width, wall_width / 2], [length / 2, width / 2 + wall_width, wall_width / 2], [length / 2, wall_width / 2, width / 2], [length / 2, wall_width / 2, width / 2]] positions = [[0, 0., -width / 2 - wall_width / 2], [0, 0., width / 2 + wall_width / 2], [0, -width / 2 - wall_width / 2, 0.], [0, width / 2 + wall_width / 2, 0.]] visual_shape_id = p.createVisualShapeArray(shapeTypes=[p.GEOM_BOX] * 4, halfExtents=half_extents, visualFramePositions=positions ) collision_shape_id = p.createCollisionShapeArray(shapeTypes=[p.GEOM_BOX] * 4, halfExtents=half_extents, collisionFramePositions=positions ) 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 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
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 = False 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]]) mass = 1 visualShapeId = -1 link_Masses = [] linkCollisionShapeIndices = [] linkVisualShapeIndices = [] linkPositions = [] linkOrientations = [] linkInertialFramePositions = [] linkInertialFrameOrientations = [] indices = [] jointTypes = []
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
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
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
shift2 = [0, 0, 0] meshScale = [0.1, 0.1, 0.1] #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) visualShapeId = p.createVisualShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX], halfExtents=[[0, 0, 0], [0.1, 0.1, 0.1]], fileNames=["duck.obj", ""], visualFramePositions=[ shift1, shift2, ], meshScales=[meshScale, meshScale]) collisionShapeId = p.createCollisionShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX], halfExtents=[[0, 0, 0], [0.1, 0.1, 0.1]], fileNames=["duck_vhacd.obj", ""], collisionFramePositions=[ shift1, shift2, ], meshScales=[meshScale, meshScale]) rangex = 2 rangey = 2 for i in range(rangex): for j in range(rangey): mb = p.createMultiBody(baseMass=1, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[((-rangex / 2) + i * 2) * meshScale[0] * 2, (-rangey / 2 + j) * meshScale[1] * 4, 1], useMaximalCoordinates=False)
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
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