further work on urdfEditor.py, fix some serialization issues

2018-01-08 12:25:56 -08:00
parent a85a4f387b
commit e97b751781
23 changed files with 1926 additions and 1455 deletions
--- a/examples/pybullet/examples/urdfEditor.py
+++ b/examples/pybullet/examples/urdfEditor.py
@@ -31,7 +31,8 @@ class UrdfVisual(object):
 		self.geom_radius = 1
 		self.geom_extents = [7,8,9]
 		self.geom_length=[10]
-		self.geom_meshfile = "meshfile"
+		self.geom_meshfilename = "meshfile"
+		self.geom_meshscale=[1,1,1]
 		self.material_rgba = [1,0,0,1]
 		self.material_name = ""
 		
@@ -41,9 +42,10 @@ class UrdfCollision(object):
 		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_meshfile = "meshfile"
-	
+		self.geom_meshfilename = "meshfile"
+		self.geom_meshscale = [1,1,1]
 class UrdfJoint(object):
 	def __init__(self):
 		self.link = UrdfLink()
@@ -54,6 +56,7 @@ class UrdfJoint(object):
 		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):
@@ -83,7 +86,7 @@ class UrdfEditor(object):
 		matIndex = 0
 		for v in visualShapes:
 			if (v[1]==linkIndex):
-				print("visualShape base:",v)
+				#print("visualShape base:",v)
 				urdfVisual = UrdfVisual()
 				urdfVisual.geom_type = v[2]
 				if (v[2]==p.GEOM_BOX):
@@ -91,10 +94,16 @@ class UrdfEditor(object):
 				if (v[2]==p.GEOM_SPHERE):
 					urdfVisual.geom_radius = v[3][0]	
 				if (v[2]==p.GEOM_MESH):
-					urdfVisual.geom_meshfile = v[4].decode("utf-8")
+					urdfVisual.geom_meshfilename = v[4].decode("utf-8")
+					urdfVisual.geom_meshscale = v[3]
 				if (v[2]==p.GEOM_CYLINDER):
-					urdfVisual.geom_radius=v[3][1]
 					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])
@@ -107,7 +116,7 @@ class UrdfEditor(object):
 				
 		collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex,physicsClientId=physicsClientId)
 		for v in collisionShapes:
-			print("collisionShape base:",v)
+			#print("collisionShape base:",v)
 			urdfCollision = UrdfCollision()
 			print("geom type=",v[0])
 			urdfCollision.geom_type = v[2]
@@ -116,12 +125,17 @@ class UrdfEditor(object):
 			if (v[2]==p.GEOM_SPHERE):
 				urdfCollision.geom_radius = v[3][0]
 			if (v[2]==p.GEOM_MESH):
-					urdfCollision.geom_meshfile = v[4].decode("utf-8")
-			#localInertiaFrame*childTrans
+					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_radius=v[3][1]
 				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
@@ -164,16 +178,24 @@ class UrdfEditor(object):
 			urdfJoint.child_name = urdfLink.link_name

 			#todo, compensate for inertia/link frame offset
-			dyn = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
-			parentInertiaPos = dyn[3]
-			parentInertiaOrn = dyn[4]
 			
-			pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
-				jointInfo[14], jointInfo[15])
+			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):
@@ -207,13 +229,17 @@ class UrdfEditor(object):
 				prec=precision)
 			file.write(str)
 		if urdfVisual.geom_type == p.GEOM_MESH:
-			str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfVisual.geom_meshfile,\
+			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)
@@ -240,14 +266,17 @@ class UrdfEditor(object):
 				prec=precision)
 			file.write(str)
 		if urdfCollision.geom_type == p.GEOM_MESH:
-			str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfCollision.geom_meshfile,\
+			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")
 		
@@ -281,6 +310,14 @@ class UrdfEditor(object):
 		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)
@@ -319,24 +356,41 @@ class UrdfEditor(object):
 		baseMass = base.urdf_inertial.mass
 		print("baseMass=",baseMass)
 		baseCollisionShapeIndex = -1
+
+		baseShapeTypeArray=[]
+		baseRadiusArray=[]
+		baseHalfExtentsArray=[]
+		lengthsArray=[]
+		fileNameArray=[]
+		meshScaleArray=[]
+		basePositionsArray=[]
+		baseOrientationsArray=[]
 		
-		if (len(base.urdf_collision_shapes)):
-			v = base.urdf_collision_shapes[0]
+		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
-			if shapeType==p.GEOM_BOX:
-				baseCollisionShapeIndex = p.createCollisionShape(shapeType, \
-					halfExtents = [0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]],\
-					collisionFramePosition=v.origin_xyz,\
-					collisionFrameOrientation=p.getQuaternionFromEuler(v.origin_rpy),\
-					physicsClientId=physicsClientId)
-					
-			if shapeType==p.GEOM_SPHERE:
-				baseCollisionShapeIndex = p.createCollisionShape(shapeType, \
-					radius=v.geom_radius,\
-					collisionFramePosition=v.origin_xyz,\
-					collisionFrameOrientation=p.getQuaternionFromEuler(v.origin_rpy),\
+			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)


@@ -348,6 +402,7 @@ class UrdfEditor(object):
 		linkVisualShapeIndices=[]
 		linkPositions=[]
 		linkOrientations=[]
+		linkMeshScaleArray=[]
 		linkInertialFramePositions=[]
 		linkInertialFrameOrientations=[]
 		linkParentIndices=[]
@@ -374,38 +429,39 @@ class UrdfEditor(object):
 			
 			linkJointAx = joint.joint_axis_xyz
 			
+			linkShapeTypeArray=[]
+			linkRadiusArray=[]
+			linkHalfExtentsArray=[]
+			lengthsArray=[]
+			fileNameArray=[]
+			linkPositionsArray=[]
+			linkOrientationsArray=[]
 			
-			if (len(link.urdf_collision_shapes)):
-				v = link.urdf_collision_shapes[0]
-				print("v.origin_xyz=",v.origin_xyz)
-				print("v.origin_rpy=",v.origin_rpy)
+			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
-				if shapeType==p.GEOM_BOX:
-					linkCollisionShapeIndex = p.createCollisionShape(shapeType, \
-						halfExtents = [0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]],\
-						collisionFramePosition=v.origin_xyz,\
-						collisionFrameOrientation=p.getQuaternionFromEuler(v.origin_rpy),\
-						physicsClientId=physicsClientId)
-						
-				if shapeType==p.GEOM_SPHERE:
-					linkCollisionShapeIndex = p.createCollisionShape(shapeType, \
-						radius=v.geom_radius,\
-						collisionFramePosition=v.origin_xyz,\
-						collisionFrameOrientation=p.getQuaternionFromEuler(v.origin_rpy),\
-						physicsClientId=physicsClientId)
-			#for v in link.urdf_collision_shapes:
-			#	v.tmp_collision_shape_ids=[]
-			#	shapeType = v.geom_type
-			#	if shapeType==p.GEOM_BOX:
-			#		cid = p.createCollisionShape(shapeType, \
-			#		radius=v.geom_radius,\
-			#		halfExtents = v.geom_extents,\
-			#		physicsClientId=physicsClientId)
-			#		if (cid>=0):
-			#			v.tmp_collision_shape_ids.append(cid)
-			#		else:
-			#			print("Warning: cannot convert collision shape", v)
-		
+				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)
@@ -455,23 +511,32 @@ class UrdfEditor(object):
      
 ##########################################
 org2 = p.connect(p.DIRECT)
-org = p.connect(p.DIRECT)
+org = p.connect(p.SHARED_MEMORY)
+if (org<0):
+	org = p.connect(p.DIRECT)
+	
 gui = p.connect(p.GUI)

-door = p.loadURDF("hinge.urdf", physicsClientId=org)
-for i in range(p.getNumJoints(door,physicsClientId=org)):
-	p.setJointMotorControl2(door,i,p.VELOCITY_CONTROL,force=0,physicsClientId=org)
+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(door,physicsClientId=org))
+#print("numJoints:",p.getNumJoints(mb,physicsClientId=org))

-print("base name:",p.getBodyInfo(door,physicsClientId=org))
+#print("base name:",p.getBodyInfo(mb,physicsClientId=org))

-for i in range(p.getNumJoints(door,physicsClientId=org)):
-	print("jointInfo(",i,"):",p.getJointInfo(door,i,physicsClientId=org))
-	print("linkState(",i,"):",p.getLinkState(door,i,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(door,physicsClientId=org)
+parser.initializeFromBulletBody(mb,physicsClientId=org)
 parser.saveUrdf("test.urdf")

 if (1):
@@ -480,13 +545,14 @@ if (1):
 	obUid = parser.createMultiBody(physicsClientId=gui)

 	parser2 = UrdfEditor()
+	print("\n###########################################\n")
+	
 	parser2.initializeFromBulletBody(obUid,physicsClientId=gui)
 	parser2.saveUrdf("test2.urdf")

-	print("\n###########################################\n")

 	for i in range (p.getNumJoints(obUid, physicsClientId=gui)):
-		p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0.1,force=100,physicsClientId=gui)
+		p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0.1,force=1,physicsClientId=gui)
 		print(p.getJointInfo(obUid,i,physicsClientId=gui))


--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -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++)
@@ -5639,7 +5642,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);

@@ -5657,6 +5660,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(objectsRepresentation);
+#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)
 {
@@ -5666,7 +5896,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;
@@ -5688,9 +5918,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;
 	}
@@ -5720,13 +5950,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)
 		{
@@ -5767,7 +5997,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);

@@ -8016,6 +8246,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."},