Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge branch 'master' into master

Browse Source
This commit is contained in:
YunfeiBai
2018-01-08 18:13:03 -08:00
committed by GitHub
parent 0e0d3da2b5 6a74f63604
commit de3b8bf3c9
41 changed files with 2968 additions and 1413 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
examples/pybullet/examples/humanoid_manual_control.py Normal file
View File

@@ -0,0 +1,32 @@
import pybullet as p
import time
p.connect(p.GUI)
obUids = p.loadMJCF("mjcf/humanoid.xml")
humanoid = obUids[1]
gravId = p.addUserDebugParameter("gravity",-10,10,-10)
jointIds=[]
paramIds=[]
p.setPhysicsEngineParameter(numSolverIterations=10)
p.changeDynamics(humanoid,-1,linearDamping=0, angularDamping=0)
for j in range (p.getNumJoints(humanoid)):
p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
info = p.getJointInfo(humanoid,j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
p.setRealTimeSimulation(1)
while(1):
p.setGravity(0,0,p.readUserDebugParameter(gravId))
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(humanoid,jointIds[i],p.POSITION_CONTROL,targetPos, force=5*240.)
time.sleep(0.01)

26
examples/pybullet/examples/jointFrictionAndMotor.py Normal file
View File

@@ -0,0 +1,26 @@
import pybullet as p
import time
p.connect(p.GUI)
door=p.loadURDF("door.urdf")
#linear/angular damping for base and all children=0
p.changeDynamics(door,-1,linearDamping=0, angularDamping=0)
for j in range (p.getNumJoints(door)):
p.changeDynamics(door,j,linearDamping=0, angularDamping=0)
print(p.getJointInfo(door,j))
frictionId = p.addUserDebugParameter("jointFriction",0,20,10)
torqueId = p.addUserDebugParameter("joint torque",0,20,5)
while (1):
frictionForce = p.readUserDebugParameter(frictionId)
jointTorque = p.readUserDebugParameter(torqueId)
#set the joint friction
p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL,targetVelocity=0,force=frictionForce)
#apply a joint torque
p.setJointMotorControl2(door,1,p.TORQUE_CONTROL,force=jointTorque)
p.stepSimulation()
time.sleep(0.01)

567
examples/pybullet/examples/urdfEditor.py Normal file
View File

@@ -0,0 +1,567 @@
import pybullet as p
import time
class UrdfInertial(object):
def __init__(self):
self.mass = 1
self.inertia_xxyyzz=[7,8,9]
self.origin_rpy=[1,2,3]
self.origin_xyz=[4,5,6]
class UrdfContact(object):
def __init__(self):
self.lateral_friction = 1
self.rolling_friction = 0
self.spinning_friction = 0
class UrdfLink(object):
def __init__(self):
self.link_name = "dummy"
self.urdf_inertial = UrdfInertial()
self.urdf_visual_shapes=[]
self.urdf_collision_shapes=[]
class UrdfVisual(object):
def __init__(self):
self.origin_rpy = [1,2,3]
self.origin_xyz = [4,5,6]
self.geom_type = p.GEOM_BOX
self.geom_radius = 1
self.geom_extents = [7,8,9]
self.geom_length=[10]
self.geom_meshfilename = "meshfile"
self.geom_meshscale=[1,1,1]
self.material_rgba = [1,0,0,1]
self.material_name = ""
class UrdfCollision(object):
def __init__(self):
self.origin_rpy = [1,2,3]
self.origin_xyz = [4,5,6]
self.geom_type = p.GEOM_BOX
self.geom_radius = 1
self.geom_length = 2
self.geom_extents = [7,8,9]
self.geom_meshfilename = "meshfile"
self.geom_meshscale = [1,1,1]
class UrdfJoint(object):
def __init__(self):
self.link = UrdfLink()
self.joint_name = "joint_dummy"
self.joint_type = p.JOINT_REVOLUTE
self.joint_lower_limit = 0
self.joint_upper_limit = -1
self.parent_name = "parentName"
self.child_name = "childName"
self.joint_origin_xyz = [1,2,3]
self.joint_origin_rpy = [1,2,3]
self.joint_axis_xyz = [1,2,3]
class UrdfEditor(object):
def __init__(self):
self.initialize()
def initialize(self):
self.urdfLinks=[]
self.urdfJoints=[]
self.robotName = ""
self.linkNameToIndex={}
self.jointTypeToName={p.JOINT_FIXED:"JOINT_FIXED" ,\
p.JOINT_REVOLUTE:"JOINT_REVOLUTE",\
p.JOINT_PRISMATIC:"JOINT_PRISMATIC" }
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId):
dyn = p.getDynamicsInfo(bodyUid,linkIndex,physicsClientId=physicsClientId)
urdfLink.urdf_inertial.mass = dyn[0]
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
#todo
urdfLink.urdf_inertial.origin_xyz = dyn[3]
rpy = p.getEulerFromQuaternion(dyn[4])
urdfLink.urdf_inertial.origin_rpy = rpy
visualShapes = p.getVisualShapeData(bodyUid,physicsClientId=physicsClientId)
matIndex = 0
for v in visualShapes:
if (v[1]==linkIndex):
#print("visualShape base:",v)
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
if (v[2]==p.GEOM_BOX):
urdfVisual.geom_extents = v[3]
if (v[2]==p.GEOM_SPHERE):
urdfVisual.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
urdfVisual.geom_meshfilename = v[4].decode("utf-8")
urdfVisual.geom_meshscale = v[3]
if (v[2]==p.GEOM_CYLINDER):
urdfVisual.geom_length=v[3][0]
urdfVisual.geom_radius=v[3][1]
if (v[2]==p.GEOM_CAPSULE):
urdfVisual.geom_length=v[3][0]
urdfVisual.geom_radius=v[3][1]
#print("Capsule length=",urdfVisual.geom_length)
#print("Capsule radius=",urdfVisual.geom_radius)
urdfVisual.origin_xyz = v[5]
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
urdfVisual.material_rgba = v[7]
name = 'mat_{}_{}'.format(linkIndex,matIndex)
urdfVisual.material_name = name
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex=matIndex+1
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex,physicsClientId=physicsClientId)
for v in collisionShapes:
#print("collisionShape base:",v)
urdfCollision = UrdfCollision()
print("geom type=",v[0])
urdfCollision.geom_type = v[2]
if (v[2]==p.GEOM_BOX):
urdfCollision.geom_extents = v[3]
if (v[2]==p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
urdfCollision.geom_meshfilename = v[4].decode("utf-8")
urdfCollision.geom_meshscale = v[3]
print("p.GEOM_MESH filename=",urdfCollision.geom_meshfilename)
if (v[2]==p.GEOM_CYLINDER):
urdfCollision.geom_length=v[3][0]
urdfCollision.geom_radius=v[3][1]
if (v[2]==p.GEOM_CAPSULE):
urdfCollision.geom_length=v[3][0]
urdfCollision.geom_radius=v[3][1]
print("Capsule length=",urdfCollision.geom_length)
print("Capsule radius=",urdfCollision.geom_radius)
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
def initializeFromBulletBody(self, bodyUid, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.linkNameToIndex[baseLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(baseLink)
#print(visualShapes)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid,physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink,physicsClientId)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.linkNameToIndex[urdfLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.link = urdfLink
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_axis_xyz = jointInfo[13]
orgParentIndex = jointInfo[16]
if (orgParentIndex<0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dynChild = p.getDynamicsInfo(bodyUid,j,physicsClientId=physicsClientId)
childInertiaPos = dynChild[3]
childInertiaOrn = dynChild[4]
parentCom2JointPos=jointInfo[14]
parentCom2JointOrn=jointInfo[15]
tmpPos,tmpOrn = p.multiplyTransforms(childInertiaPos,childInertiaOrn,parentCom2JointPos,parentCom2JointOrn)
tmpPosInv,tmpOrnInv = p.invertTransform(tmpPos,tmpOrn)
dynParent = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
parentInertiaPos = dynParent[3]
parentInertiaOrn = dynParent[4]
pos,orn = p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, tmpPosInv, tmpOrnInv)
pos,orn_unused=p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, parentCom2JointPos,[0,0,0,1])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
def writeInertial(self,file,urdfInertial, precision=5):
file.write("\t\t<inertial>\n")
str = '\t\t\t<origin rpy=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\" xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(\
urdfInertial.origin_rpy[0],urdfInertial.origin_rpy[1],urdfInertial.origin_rpy[2],\
urdfInertial.origin_xyz[0],urdfInertial.origin_xyz[1],urdfInertial.origin_xyz[2], prec=precision)
file.write(str)
str = '\t\t\t<mass value=\"{:.{prec}f}\"/>\n'.format(urdfInertial.mass,prec=precision)
file.write(str)
str = '\t\t\t<inertia ixx=\"{:.{prec}f}\" ixy=\"0\" ixz=\"0\" iyy=\"{:.{prec}f}\" iyz=\"0\" izz=\"{:.{prec}f}\"/>\n'.format(\
urdfInertial.inertia_xxyyzz[0],\
urdfInertial.inertia_xxyyzz[1],\
urdfInertial.inertia_xxyyzz[2],prec=precision)
file.write(str)
file.write("\t\t</inertial>\n")
def writeVisualShape(self,file,urdfVisual, precision=5):
file.write("\t\t<visual>\n")
str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\
urdfVisual.origin_rpy[0],urdfVisual.origin_rpy[1],urdfVisual.origin_rpy[2],
urdfVisual.origin_xyz[0],urdfVisual.origin_xyz[1],urdfVisual.origin_xyz[2], prec=precision)
file.write(str)
file.write("\t\t\t<geometry>\n")
if urdfVisual.geom_type == p.GEOM_BOX:
str = '\t\t\t\t<box size=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfVisual.geom_extents[0],\
urdfVisual.geom_extents[1],urdfVisual.geom_extents[2], prec=precision)
file.write(str)
if urdfVisual.geom_type == p.GEOM_SPHERE:
str = '\t\t\t\t<sphere radius=\"{:.{prec}f}\"/>\n'.format(urdfVisual.geom_radius,\
prec=precision)
file.write(str)
if urdfVisual.geom_type == p.GEOM_MESH:
str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfVisual.geom_meshfilename,\
prec=precision)
file.write(str)
if urdfVisual.geom_type == p.GEOM_CYLINDER:
str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
urdfVisual.geom_length, urdfVisual.geom_radius, prec=precision)
file.write(str)
if urdfVisual.geom_type == p.GEOM_CAPSULE:
str = '\t\t\t\t<capsule length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
urdfVisual.geom_length, urdfVisual.geom_radius, prec=precision)
file.write(str)
file.write("\t\t\t</geometry>\n")
str = '\t\t\t<material name=\"{}\">\n'.format(urdfVisual.material_name)
file.write(str)
str = '\t\t\t\t<color rgba="{:.{prec}f} {:.{prec}f} {:.{prec}f} {:.{prec}f}" />\n'.format(urdfVisual.material_rgba[0],\
urdfVisual.material_rgba[1],urdfVisual.material_rgba[2],urdfVisual.material_rgba[3],prec=precision)
file.write(str)
file.write("\t\t\t</material>\n")
file.write("\t\t</visual>\n")
def writeCollisionShape(self,file,urdfCollision, precision=5):
file.write("\t\t<collision>\n")
str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\
urdfCollision.origin_rpy[0],urdfCollision.origin_rpy[1],urdfCollision.origin_rpy[2],
urdfCollision.origin_xyz[0],urdfCollision.origin_xyz[1],urdfCollision.origin_xyz[2], prec=precision)
file.write(str)
file.write("\t\t\t<geometry>\n")
if urdfCollision.geom_type == p.GEOM_BOX:
str = '\t\t\t\t<box size=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfCollision.geom_extents[0],\
urdfCollision.geom_extents[1],urdfCollision.geom_extents[2], prec=precision)
file.write(str)
if urdfCollision.geom_type == p.GEOM_SPHERE:
str = '\t\t\t\t<sphere radius=\"{:.{prec}f}\"/>\n'.format(urdfCollision.geom_radius,\
prec=precision)
file.write(str)
if urdfCollision.geom_type == p.GEOM_MESH:
str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfCollision.geom_meshfilename,\
prec=precision)
file.write(str)
if urdfCollision.geom_type == p.GEOM_CYLINDER:
str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
urdfCollision.geom_length, urdfCollision.geom_radius, prec=precision)
file.write(str)
if urdfCollision.geom_type == p.GEOM_CAPSULE:
str = '\t\t\t\t<capsule length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
urdfCollision.geom_length, urdfCollision.geom_radius, prec=precision)
file.write(str)
file.write("\t\t\t</geometry>\n")
file.write("\t\t</collision>\n")
def writeLink(self, file, urdfLink):
file.write("\t<link name=\"")
file.write(urdfLink.link_name)
file.write("\">\n")
self.writeInertial(file,urdfLink.urdf_inertial)
for v in urdfLink.urdf_visual_shapes:
self.writeVisualShape(file,v)
for c in urdfLink.urdf_collision_shapes:
self.writeCollisionShape(file,c)
file.write("\t</link>\n")
def writeJoint(self, file, urdfJoint, precision=5):
jointTypeStr = "invalid"
if urdfJoint.joint_type == p.JOINT_REVOLUTE:
if urdfJoint.joint_upper_limit < urdfJoint.joint_lower_limit:
jointTypeStr = "continuous"
else:
jointTypeStr = "revolute"
if urdfJoint.joint_type == p.JOINT_FIXED:
jointTypeStr = "fixed"
if urdfJoint.joint_type == p.JOINT_PRISMATIC:
jointTypeStr = "prismatic"
str = '\t<joint name=\"{}\" type=\"{}\">\n'.format(urdfJoint.joint_name, jointTypeStr)
file.write(str)
str = '\t\t<parent link=\"{}\"/>\n'.format(urdfJoint.parent_name)
file.write(str)
str = '\t\t<child link=\"{}\"/>\n'.format(urdfJoint.child_name)
file.write(str)
if urdfJoint.joint_type == p.JOINT_PRISMATIC:
#todo: handle limits
lowerLimit=-0.5
upperLimit=0.5
str='<limit effort="1000.0" lower="{:.{prec}f}" upper="{:.{prec}f}" velocity="0.5"/>'.format(lowerLimit,upperLimit,prec=precision)
file.write(str)
file.write("\t\t<dynamics damping=\"1.0\" friction=\"0.0001\"/>\n")
str = '\t\t<origin xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_xyz[0],\
urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
file.write(str)
str = '\t\t<axis xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_axis_xyz[0],\
urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision)
file.write(str)
file.write("\t</joint>\n")
def saveUrdf(self, fileName):
file = open(fileName,"w")
file.write("<?xml version=\"0.0\" ?>\n")
file.write("<robot name=\"")
file.write(self.robotName)
file.write("\">\n")
for link in self.urdfLinks:
self.writeLink(file,link)
for joint in self.urdfJoints:
self.writeJoint(file,joint)
file.write("</robot>\n")
file.close()
#def addLink(...)
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
#create visual and collision shapes
#for link in self.urdfLinks:
# for v in link.urdf_visual_shapes:
# v.tmp_visual_shape_ids=[]
# shapeType = v[0]
# if shapeType==p.GEOM_BOX:
# p.createVisualShape(physicsClientId=physId)
#for joint in self.urdfJoints:
# self.writeJoint(file,joint)
def __del__(self):
pass
##########################################
org2 = p.connect(p.DIRECT)
org = p.connect(p.SHARED_MEMORY)
if (org<0):
org = p.connect(p.DIRECT)
gui = p.connect(p.GUI)
p.resetSimulation(physicsClientId=org)
#door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf
mb = p.loadURDF("r2d2.urdf", physicsClientId=org)
for i in range(p.getNumJoints(mb,physicsClientId=org)):
p.setJointMotorControl2(mb,i,p.VELOCITY_CONTROL,force=0,physicsClientId=org)
#print("numJoints:",p.getNumJoints(mb,physicsClientId=org))
#print("base name:",p.getBodyInfo(mb,physicsClientId=org))
#for i in range(p.getNumJoints(mb,physicsClientId=org)):
# print("jointInfo(",i,"):",p.getJointInfo(mb,i,physicsClientId=org))
# print("linkState(",i,"):",p.getLinkState(mb,i,physicsClientId=org))
parser = UrdfEditor()
parser.initializeFromBulletBody(mb,physicsClientId=org)
parser.saveUrdf("test.urdf")
if (1):
print("\ncreatingMultiBody...................n")
obUid = parser.createMultiBody(physicsClientId=gui)
parser2 = UrdfEditor()
print("\n###########################################\n")
parser2.initializeFromBulletBody(obUid,physicsClientId=gui)
parser2.saveUrdf("test2.urdf")
for i in range (p.getNumJoints(obUid, physicsClientId=gui)):
p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0,force=1,physicsClientId=gui)
print(p.getJointInfo(obUid,i,physicsClientId=gui))
parser=0
p.setRealTimeSimulation(1,physicsClientId=gui)
while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]):
p.stepSimulation(physicsClientId=gui)
time.sleep(0.01)

427
examples/pybullet/pybullet.c
View File

@@ -66,6 +66,7 @@ b3PhysicsClientHandle getPhysicsClient(int physicsClientId)
return 0;
}
static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index)
{
double v = 0.0;
@@ -153,6 +154,7 @@ static int pybullet_internalSetVector(PyObject* objVec, float vector[3])
if (seq)
{
len = PySequence_Size(objVec);
assert(len == 3);
if (len == 3)
{
for (i = 0; i < len; i++)
@@ -180,6 +182,7 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3])
if (seq)
{
len = PySequence_Size(obVec);
assert(len == 3);
if (len == 3)
{
for (i = 0; i < len; i++)
@@ -913,13 +916,12 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
{
int bodyUniqueId = -1;
int linkIndex = -2;
int flags = 0;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "flags", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex, &flags, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
{
return NULL;
}
@@ -934,11 +936,7 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
b3SharedMemoryCommandHandle cmd_handle;
b3SharedMemoryStatusHandle status_handle;
struct b3DynamicsInfo info;
int numFields = 2;
if (flags & eDYNAMICS_INFO_REPORT_INERTIA)
{
numFields++;
}
if (bodyUniqueId < 0)
{
@@ -959,25 +957,41 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
return NULL;
}
if (b3GetDynamicsInfo(status_handle, &info))
{
int numFields = 10;
PyObject* pyDynamicsInfo = PyTuple_New(numFields);
PyTuple_SetItem(pyDynamicsInfo, 0, PyFloat_FromDouble(info.m_mass));
PyTuple_SetItem(pyDynamicsInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff));
if (flags & eDYNAMICS_INFO_REPORT_INERTIA)
{
PyObject* pyInertiaDiag = PyTuple_New(3);
PyTuple_SetItem(pyInertiaDiag,0,PyFloat_FromDouble(info.m_localInertialDiagonal[0]));
PyTuple_SetItem(pyInertiaDiag,1,PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
PyTuple_SetItem(pyInertiaDiag,2,PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
PyTuple_SetItem(pyInertiaDiag, 0, PyFloat_FromDouble(info.m_localInertialDiagonal[0]));
PyTuple_SetItem(pyInertiaDiag, 1, PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
PyTuple_SetItem(pyInertiaDiag, 2, PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
PyTuple_SetItem(pyDynamicsInfo, 2, pyInertiaDiag);
}
{
PyObject* pyInertiaPos= PyTuple_New(3);
PyTuple_SetItem(pyInertiaPos, 0, PyFloat_FromDouble(info.m_localInertialFrame[0]));
PyTuple_SetItem(pyInertiaPos, 1, PyFloat_FromDouble(info.m_localInertialFrame[1]));
PyTuple_SetItem(pyInertiaPos, 2, PyFloat_FromDouble(info.m_localInertialFrame[2]));
PyTuple_SetItem(pyDynamicsInfo, 3, pyInertiaPos);
}
{
PyObject* pyInertiaOrn= PyTuple_New(4);
PyTuple_SetItem(pyInertiaOrn, 0, PyFloat_FromDouble(info.m_localInertialFrame[3]));
PyTuple_SetItem(pyInertiaOrn, 1, PyFloat_FromDouble(info.m_localInertialFrame[4]));
PyTuple_SetItem(pyInertiaOrn, 2, PyFloat_FromDouble(info.m_localInertialFrame[5]));
PyTuple_SetItem(pyInertiaOrn, 3, PyFloat_FromDouble(info.m_localInertialFrame[6]));
PyTuple_SetItem(pyDynamicsInfo, 4, pyInertiaOrn);
}
PyTuple_SetItem(pyDynamicsInfo, 5, PyFloat_FromDouble(info.m_restitution));
PyTuple_SetItem(pyDynamicsInfo, 6, PyFloat_FromDouble(info.m_rollingFrictionCoeff));
PyTuple_SetItem(pyDynamicsInfo, 7, PyFloat_FromDouble(info.m_spinningFrictionCoeff));
PyTuple_SetItem(pyDynamicsInfo, 8, PyFloat_FromDouble(info.m_contactDamping));
PyTuple_SetItem(pyDynamicsInfo, 9, PyFloat_FromDouble(info.m_contactStiffness));
return pyDynamicsInfo;
}
}
@@ -3128,7 +3142,7 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
int bodyUniqueId = -1;
int jointIndex = -1;
int jointInfoSize = 13; // size of struct b3JointInfo
int jointInfoSize = 17; // size of struct b3JointInfo
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "jointIndex", "physicsClientId", NULL};
@@ -3199,6 +3213,30 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
PyTuple_SetItem(pyListJointInfo, 12,
PyString_FromString("not available"));
}
{
PyObject* axis = PyTuple_New(3);
PyTuple_SetItem(axis, 0, PyFloat_FromDouble(info.m_jointAxis[0]));
PyTuple_SetItem(axis, 1, PyFloat_FromDouble(info.m_jointAxis[1]));
PyTuple_SetItem(axis, 2, PyFloat_FromDouble(info.m_jointAxis[2]));
PyTuple_SetItem(pyListJointInfo, 13, axis);
}
{
PyObject* pos = PyTuple_New(3);
PyTuple_SetItem(pos, 0, PyFloat_FromDouble(info.m_parentFrame[0]));
PyTuple_SetItem(pos, 1, PyFloat_FromDouble(info.m_parentFrame[1]));
PyTuple_SetItem(pos, 2, PyFloat_FromDouble(info.m_parentFrame[2]));
PyTuple_SetItem(pyListJointInfo, 14, pos);
}
{
PyObject* orn = PyTuple_New(4);
PyTuple_SetItem(orn, 0, PyFloat_FromDouble(info.m_parentFrame[3]));
PyTuple_SetItem(orn, 1, PyFloat_FromDouble(info.m_parentFrame[4]));
PyTuple_SetItem(orn, 2, PyFloat_FromDouble(info.m_parentFrame[5]));
PyTuple_SetItem(orn, 3, PyFloat_FromDouble(info.m_parentFrame[6]));
PyTuple_SetItem(pyListJointInfo, 15, orn);
}
PyTuple_SetItem(pyListJointInfo, 16, PyInt_FromLong(info.m_parentIndex));
return pyListJointInfo;
}
@@ -4834,6 +4872,107 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py
return Py_None;
}
static PyObject* pybullet_getCollisionShapeData(PyObject* self, PyObject* args, PyObject* keywds)
{
int objectUniqueId = -1;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
struct b3CollisionShapeInformation collisionShapeInfo;
int statusType;
int i;
int linkIndex;
PyObject* pyResultList = 0;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = { "objectUniqueId", "linkIndex", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &objectUniqueId, &linkIndex, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
commandHandle = b3InitRequestCollisionShapeInformation(sm, objectUniqueId, linkIndex);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED)
{
b3GetCollisionShapeInformation(sm, &collisionShapeInfo);
pyResultList = PyTuple_New(collisionShapeInfo.m_numCollisionShapes);
for (i = 0; i < collisionShapeInfo.m_numCollisionShapes; i++)
{
PyObject* collisionShapeObList = PyTuple_New(7);
PyObject* item;
item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_objectUniqueId);
PyTuple_SetItem(collisionShapeObList, 0, item);
item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_linkIndex);
PyTuple_SetItem(collisionShapeObList, 1, item);
item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_collisionGeometryType);
PyTuple_SetItem(collisionShapeObList, 2, item);
{
PyObject* vec = PyTuple_New(3);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[0]);
PyTuple_SetItem(vec, 0, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[1]);
PyTuple_SetItem(vec, 1, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[2]);
PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(collisionShapeObList, 3, vec);
}
item = PyString_FromString(collisionShapeInfo.m_collisionShapeData[i].m_meshAssetFileName);
PyTuple_SetItem(collisionShapeObList, 4, item);
{
PyObject* vec = PyTuple_New(3);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[0]);
PyTuple_SetItem(vec, 0, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[1]);
PyTuple_SetItem(vec, 1, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[2]);
PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(collisionShapeObList, 5, vec);
}
{
PyObject* vec = PyTuple_New(4);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[3]);
PyTuple_SetItem(vec, 0, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[4]);
PyTuple_SetItem(vec, 1, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[5]);
PyTuple_SetItem(vec, 2, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[6]);
PyTuple_SetItem(vec, 3, item);
PyTuple_SetItem(collisionShapeObList, 6, vec);
}
PyTuple_SetItem(pyResultList, i, collisionShapeObList);
}
return pyResultList;
}
else
{
PyErr_SetString(SpamError, "Error receiving collision shape info");
return NULL;
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds)
{
int objectUniqueId = -1;
@@ -5564,7 +5703,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
if (collisionFrameOrientationObj)
{
pybullet_internalSetVectord(collisionFrameOrientationObj,collisionFrameOrientation);
pybullet_internalSetVector4d(collisionFrameOrientationObj,collisionFrameOrientation);
}
b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition,collisionFrameOrientation);
@@ -5582,6 +5721,233 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
return NULL;
}
static PyObject* pybullet_createCollisionShapeArray(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
PyObject* shapeTypeArray = 0;
PyObject* radiusArray = 0;
PyObject* halfExtentsObjArray = 0;
PyObject* lengthArray = 0;
PyObject* fileNameArray = 0;
PyObject* meshScaleObjArray = 0;
PyObject* planeNormalObjArray = 0;
PyObject* flagsArray = 0;
PyObject* collisionFramePositionObjArray = 0;
PyObject* collisionFrameOrientationObjArray = 0;
static char* kwlist[] = { "shapeTypes", "radii", "halfExtents", "lengths", "fileNames", "meshScales", "planeNormals",
"flags", "collisionFramePositions", "collisionFrameOrientations", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "O|OOOOOOOOOi", kwlist,
&shapeTypeArray, &radiusArray, &halfExtentsObjArray, &lengthArray, &fileNameArray, &meshScaleObjArray, &planeNormalObjArray, &flagsArray, &collisionFramePositionObjArray, &collisionFrameOrientationObjArray, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
b3SharedMemoryCommandHandle commandHandle = b3CreateCollisionShapeCommandInit(sm);
{
int numShapeTypes = 0;
int numRadius = 0;
int numHalfExtents = 0;
int numLengths = 0;
int numFileNames = 0;
int numMeshScales = 0;
int numPlaneNormals = 0;
int numFlags = 0;
int numPositions = 0;
int numOrientations = 0;
int s;
PyObject* shapeTypeArraySeq = shapeTypeArray?PySequence_Fast(shapeTypeArray, "expected a sequence of shape types"):0;
PyObject* radiusArraySeq = radiusArray?PySequence_Fast(radiusArray, "expected a sequence of radii"):0;
PyObject* halfExtentsArraySeq = halfExtentsObjArray?PySequence_Fast(halfExtentsObjArray, "expected a sequence of half extents"):0;
PyObject* lengthArraySeq = lengthArray ?PySequence_Fast(lengthArray, "expected a sequence of lengths"):0;
PyObject* fileNameArraySeq = fileNameArray?PySequence_Fast(fileNameArray, "expected a sequence of filename"):0;
PyObject* meshScaleArraySeq = meshScaleObjArray?PySequence_Fast(meshScaleObjArray, "expected a sequence of mesh scale"):0;
PyObject* planeNormalArraySeq = planeNormalObjArray?PySequence_Fast(planeNormalObjArray, "expected a sequence of plane normal"):0;
PyObject* flagsArraySeq = flagsArray?PySequence_Fast(flagsArray, "expected a sequence of flags"):0;
PyObject* positionArraySeq = collisionFramePositionObjArray?PySequence_Fast(collisionFramePositionObjArray, "expected a sequence of collision frame positions"):0;
PyObject* orientationArraySeq = collisionFrameOrientationObjArray?PySequence_Fast(collisionFrameOrientationObjArray, "expected a sequence of collision frame orientations"):0;
if (shapeTypeArraySeq == 0)
{
PyErr_SetString(SpamError, "expected a sequence of shape types");
return NULL;
}
numShapeTypes = shapeTypeArray?PySequence_Size(shapeTypeArray):0;
numRadius = radiusArraySeq?PySequence_Size(radiusArraySeq):0;
numHalfExtents = halfExtentsArraySeq?PySequence_Size(halfExtentsArraySeq):0;
numLengths = lengthArraySeq?PySequence_Size(lengthArraySeq):0;
numFileNames = fileNameArraySeq?PySequence_Size(fileNameArraySeq):0;
numMeshScales = meshScaleArraySeq?PySequence_Size(meshScaleArraySeq):0;
numPlaneNormals = planeNormalArraySeq?PySequence_Size(planeNormalArraySeq):0;
for (s=0;s<numShapeTypes;s++)
{
int shapeType = pybullet_internalGetIntFromSequence(shapeTypeArraySeq, s);
if (shapeType >= GEOM_SPHERE)
{
int shapeIndex = -1;
if (shapeType == GEOM_SPHERE && s <= numRadius)
{
double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
if (radius > 0)
{
shapeIndex = b3CreateCollisionShapeAddSphere(commandHandle, radius);
}
}
if (shapeType == GEOM_BOX)
{
PyObject* halfExtentsObj = 0;
double halfExtents[3] = { 1, 1, 1 };
if (halfExtentsArraySeq && s<= numHalfExtents)
{
if (PyList_Check(halfExtentsArraySeq))
{
halfExtentsObj = PyList_GET_ITEM(halfExtentsArraySeq, s);
}
else
{
halfExtentsObj = PyTuple_GET_ITEM(halfExtentsArraySeq, s);
}
}
pybullet_internalSetVectord(halfExtentsObj, halfExtents);
shapeIndex = b3CreateCollisionShapeAddBox(commandHandle, halfExtents);
}
if (shapeType == GEOM_CAPSULE && s<=numRadius)
{
double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
double height = pybullet_internalGetFloatFromSequence(lengthArraySeq, s);
if (radius > 0 && height >= 0)
{
shapeIndex = b3CreateCollisionShapeAddCapsule(commandHandle, radius, height);
}
}
if (shapeType == GEOM_CYLINDER && s <= numRadius && s<numLengths)
{
double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
double height = pybullet_internalGetFloatFromSequence(lengthArraySeq, s);
if (radius > 0 && height >= 0)
{
shapeIndex = b3CreateCollisionShapeAddCylinder(commandHandle, radius, height);
}
}
if (shapeType == GEOM_MESH)
{
double meshScale[3] = { 1, 1, 1 };
PyObject* meshScaleObj = meshScaleArraySeq?PyList_GET_ITEM(meshScaleArraySeq, s):0;
PyObject* fileNameObj = fileNameArraySeq?PyList_GET_ITEM(fileNameArraySeq, s):0;
const char* fileName = 0;
if (fileNameObj)
{
#if PY_MAJOR_VERSION >= 3
PyObject* ob = PyUnicode_AsASCIIString(fileNameObj);
fileName = PyBytes_AS_STRING(ob);
#else
fileName = PyString_AsString(fileNameObj);
#endif
}
if (meshScaleObj)
{
pybullet_internalSetVectord(meshScaleObj, meshScale);
}
if (fileName)
{
shapeIndex = b3CreateCollisionShapeAddMesh(commandHandle, fileName, meshScale);
}
}
if (shapeType == GEOM_PLANE)
{
PyObject* planeNormalObj = planeNormalArraySeq?PyList_GET_ITEM(planeNormalArraySeq, s):0;
double planeNormal[3];
double planeConstant = 0;
pybullet_internalSetVectord(planeNormalObj, planeNormal);
shapeIndex = b3CreateCollisionShapeAddPlane(commandHandle, planeNormal, planeConstant);
}
if (flagsArraySeq)
{
int flags = pybullet_internalGetIntFromSequence(flagsArraySeq, s);
b3CreateCollisionSetFlag(commandHandle, shapeIndex, flags);
}
if (positionArraySeq || orientationArraySeq)
{
PyObject* collisionFramePositionObj = positionArraySeq?PyList_GET_ITEM(positionArraySeq, s):0;
PyObject* collisionFrameOrientationObj = orientationArraySeq?PyList_GET_ITEM(orientationArraySeq, s):0;
double collisionFramePosition[3] = { 0, 0, 0 };
double collisionFrameOrientation[4] = { 0, 0, 0, 1 };
if (collisionFramePositionObj)
{
pybullet_internalSetVectord(collisionFramePositionObj, collisionFramePosition);
}
if (collisionFrameOrientationObj)
{
pybullet_internalSetVector4d(collisionFrameOrientationObj, collisionFrameOrientation);
}
if (shapeIndex >= 0)
{
b3CreateCollisionShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition, collisionFrameOrientation);
}
}
}
}
if (shapeTypeArraySeq)
Py_DECREF(shapeTypeArraySeq);
if (radiusArraySeq)
Py_DECREF(radiusArraySeq);
if (halfExtentsArraySeq)
Py_DECREF(halfExtentsArraySeq);
if (lengthArraySeq)
Py_DECREF(lengthArraySeq);
if (fileNameArraySeq)
Py_DECREF(fileNameArraySeq);
if (meshScaleArraySeq)
Py_DECREF(meshScaleArraySeq);
if (planeNormalArraySeq)
Py_DECREF(planeNormalArraySeq);
if (flagsArraySeq)
Py_DECREF(flagsArraySeq);
if (positionArraySeq)
Py_DECREF(positionArraySeq);
if (orientationArraySeq)
Py_DECREF(orientationArraySeq);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED)
{
int uid = b3GetStatusCollisionShapeUniqueId(statusHandle);
PyObject* ob = PyLong_FromLong(uid);
return ob;
}
}
PyErr_SetString(SpamError, "createCollisionShapeArray failed.");
return NULL;
}
static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds)
{
@@ -5591,7 +5957,7 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
int shapeType=-1;
double radius=0.5;
double height = 1;
double length = 1;
PyObject* meshScaleObj=0;
double meshScale[3] = {1,1,1};
PyObject* planeNormalObj=0;
@@ -5613,9 +5979,9 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
PyObject* halfExtentsObj=0;
static char* kwlist[] = {"shapeType","radius","halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "physicsClientId", NULL};
static char* kwlist[] = {"shapeType","radius","halfExtents", "length", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOi", kwlist,
&shapeType, &radius,&halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &physicsClientId))
&shapeType, &radius,&halfExtentsObj, &length, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &physicsClientId))
{
return NULL;
}
@@ -5645,13 +6011,13 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
shapeIndex = b3CreateVisualShapeAddBox(commandHandle,halfExtents);
}
if (shapeType==GEOM_CAPSULE && radius>0 && height>=0)
if (shapeType==GEOM_CAPSULE && radius>0 && length>=0)
{
shapeIndex = b3CreateVisualShapeAddCapsule(commandHandle,radius,height);
shapeIndex = b3CreateVisualShapeAddCapsule(commandHandle,radius,length);
}
if (shapeType==GEOM_CYLINDER && radius>0 && height>=0)
if (shapeType==GEOM_CYLINDER && radius>0 && length>=0)
{
shapeIndex = b3CreateVisualShapeAddCylinder(commandHandle,radius,height);
shapeIndex = b3CreateVisualShapeAddCylinder(commandHandle,radius,length);
}
if (shapeType==GEOM_MESH && fileName)
{
@@ -5692,7 +6058,7 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
if (visualFrameOrientationObj)
{
pybullet_internalSetVectord(visualFrameOrientationObj,visualFrameOrientation);
pybullet_internalSetVector4d(visualFrameOrientationObj,visualFrameOrientation);
}
b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, visualFramePosition,visualFrameOrientation);
@@ -7944,6 +8310,9 @@ static PyMethodDef SpamMethods[] = {
{"createCollisionShape", (PyCFunction)pybullet_createCollisionShape, METH_VARARGS | METH_KEYWORDS,
"Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
{ "createCollisionShapeArray", (PyCFunction)pybullet_createCollisionShapeArray, METH_VARARGS | METH_KEYWORDS,
"Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise." },
{"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS,
"Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
@@ -8152,6 +8521,9 @@ static PyMethodDef SpamMethods[] = {
{"getVisualShapeData", (PyCFunction)pybullet_getVisualShapeData, METH_VARARGS | METH_KEYWORDS,
"Return the visual shape information for one object."},
{ "getCollisionShapeData", (PyCFunction)pybullet_getCollisionShapeData, METH_VARARGS | METH_KEYWORDS,
"Return the collision shape information for one object." },
{"changeVisualShape", (PyCFunction)pybullet_changeVisualShape, METH_VARARGS | METH_KEYWORDS,
"Change part of the visual shape information for one object."},
@@ -8370,8 +8742,7 @@ initpybullet(void)
PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read
PyModule_AddIntConstant(m, "JOINT_GEAR", eGearType); // user read
PyModule_AddIntConstant(m, "DYNAMICS_INFO_REPORT_INERTIA", eDYNAMICS_INFO_REPORT_INERTIA); // report local inertia in 'getDynamicsInfo'
PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read
PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);