Add preliminary PhysX 4.0 backend for PyBullet

Add inverse dynamics / mass matrix code from DeepMimic, thanks to Xue Bin (Jason) Peng Add example how to use stable PD control for humanoid with spherical joints (see humanoidMotionCapture.py) Fix related to TinyRenderer object transforms not updating when using collision filtering
2019-01-22 21:08:37 -08:00
parent 80684f44ea
commit ae8e83988b
366 changed files with 131855 additions and 359 deletions
--- a/examples/pybullet/examples/humanoidMotionCapture.py
+++ b/examples/pybullet/examples/humanoidMotionCapture.py
@@ -1,22 +1,45 @@
 import pybullet as p
 import json
+import time

 p.connect(p.GUI)
-#p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1)

+useZUp = False
+useYUp = not useZUp
+
+if useYUp:
+	p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1)
+
+from pdControllerExplicit import PDControllerExplicitMultiDof
+from pdControllerStable import PDControllerStableMultiDof
+
+explicitPD = PDControllerExplicitMultiDof(p)
+stablePD = PDControllerStableMultiDof(p)
+
+p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16])

 import pybullet_data
-
-useMotionCapture=True
-useMotionCaptureReset=not useMotionCapture
-
+p.setTimeOut(10000)
+useMotionCapture=False
+useMotionCaptureReset=False#not useMotionCapture
+useExplicitPD = True

 p.setAdditionalSearchPath(pybullet_data.getDataPath())
-p.setPhysicsEngineParameter(numSolverIterations=200)
-#path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
-path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt"
+p.setPhysicsEngineParameter(numSolverIterations=30)
+#p.setPhysicsEngineParameter(solverResidualThreshold=1e-30)

-p.loadURDF("plane.urdf",[0,0,-0.03])
+#explicit PD control requires small timestep
+timeStep = 1./600.
+#timeStep = 1./240.
+
+p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
+
+path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
+#path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt"
+#path = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
+
+
+#p.loadURDF("plane.urdf",[0,0,-1.03])
 print("path = ", path)
 with open(path, 'r') as f:
    motion_dict = json.load(f)
@@ -26,29 +49,105 @@ print(motion_dict['Loop'])
 numFrames = len(motion_dict['Frames'])
 print("#frames = ", numFrames)

+
 frameId= p.addUserDebugParameter("frame",0,numFrames-1,0)

+
+erpId = p.addUserDebugParameter("erp",0,1,0.2)
+
+kpMotorId = p.addUserDebugParameter("kpMotor",0,1,.2)
+forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000)
+
+
+
+
 jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
 							"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
 							
-humanoid = p.loadURDF("humanoid/humanoid.urdf", globalScaling=0.25)
+startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]]
+
+p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0])
+p.addUserDebugText("Spherical Drive", [startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]], [0,0,0])
+p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]], [0,0,0])
+p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0])
+
+humanoid =  p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
+humanoid2 = p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
+humanoid3 = p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
+humanoid4 = p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
+
+humanoid_fix = p.createConstraint(humanoid,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[0],[0,0,0,1])
+humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1])
+humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1])
+humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1])
+
+
+startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447,
+1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153,
+0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495,
+0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348]
+
+startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-0.1050802848,0,0.935706195,0.08277326387,0.3002461862,0,0,0,0,0,1.114409628,0.3618553952,
+-0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0,
+-4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973]
+
+p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1])
+p.resetBasePositionAndOrientation(humanoid2, startLocations[1],[0,0,0,1])
+p.resetBasePositionAndOrientation(humanoid3, startLocations[2],[0,0,0,1])
+p.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1])
+
+
+
+index0=7
+for j in range (p.getNumJoints(humanoid)):
+	ji = p.getJointInfo(humanoid,j)
+	targetPosition=[0]
+	jointType = ji[2]
+	if (jointType == p.JOINT_SPHERICAL):
+		targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]]
+		targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]]
+		index0+=4
+		print("spherical position: ",targetPosition)
+		print("spherical velocity: ",targetVel)
+		p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
+		p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel)
+	if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
+		targetPosition=[startPose[index0]]
+		targetVel = [startVel[index0]]
+		index0+=1
+		print("revolute:", targetPosition)
+		print("revolute velocity:", targetVel)
+		p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
+		p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel)
+	
+
+

 for j in range (p.getNumJoints(humanoid)):
 	ji = p.getJointInfo(humanoid,j)
 	targetPosition=[0]
-	if (ji[2] == p.JOINT_SPHERICAL):
+	jointType = ji[2]
+	if (jointType == p.JOINT_SPHERICAL):
 		targetPosition=[0,0,0,1]
-	#p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, force=0)
+		p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[0,0,0])
+		p.setJointMotorControlMultiDof(humanoid3,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[31,31,31])
+		p.setJointMotorControlMultiDof(humanoid4,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[1,1,1])
+
+	if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
+		p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0, force=0)
+		p.setJointMotorControl2(humanoid3,j,p.VELOCITY_CONTROL,targetVelocity=0, force=31)
+		p.setJointMotorControl2(humanoid4,j,p.VELOCITY_CONTROL,targetVelocity=0, force=10)
 	
 	#print(ji)
 	print("joint[",j,"].type=",jointTypes[ji[2]])
 	print("joint[",j,"].name=",ji[1])


+	
 jointIds=[]
 paramIds=[]
 for j in range (p.getNumJoints(humanoid)):
-	p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
+	#p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
 	p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1])
 	info = p.getJointInfo(humanoid,j)
 	#print(info)
@@ -57,32 +156,56 @@ for j in range (p.getNumJoints(humanoid)):
 		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))
-			print("jointName=",jointName, "at ", j)
+			#paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
+			#print("jointName=",jointName, "at ", j)
 		
 p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1])
 chest=1
 neck=2
-rightShoulder=3
-rightElbow=4
-leftShoulder=6
-leftElbow = 7
-rightHip = 9
-rightKnee=10
-rightAnkle=11
-leftHip = 12
-leftKnee=13
-leftAnkle=14
+rightHip=3
+rightKnee=4
+rightAnkle=5
+rightShoulder=6
+rightElbow=7
+leftHip=9
+leftKnee=10
+leftAnkle=11
+leftShoulder=12
+leftElbow=13
+
+#rightShoulder=3
+#rightElbow=4
+#leftShoulder=6
+#leftElbow = 7
+#rightHip = 9
+#rightKnee=10
+#rightAnkle=11
+#leftHip = 12
+#leftKnee=13
+#leftAnkle=14

 import time


+kpOrg = [0,0,0,0,0,0,0,1000,1000,1000,1000,100,100,100,100,500,500,500,500,500,400,400,400,400,400,400,400,400,300,500,500,500,500,500,400,400,400,400,400,400,400,400,300]
+kdOrg = [0,0,0,0,0,0,0,100,100,100,100,10,10,10,10,50,50,50,50,50,40,40,40,40,40,40,40,40,30,50,50,50,50,50,40,40,40,40,40,40,40,40,30]
+
 once=True
 p.getCameraImage(320,200)
-maxForce=1000
+
+


 while (p.isConnected()):
+
+	
+	erp = p.readUserDebugParameter(erpId)
+	
+	kpMotor = p.readUserDebugParameter(kpMotorId)
+	maxForce=p.readUserDebugParameter(forceMotorId)
+	kp=kpMotor
+	
+	
 	frameReal = p.readUserDebugParameter(frameId)
 	frame = int(frameReal)
 	frameNext = frame+1
@@ -111,12 +234,18 @@ while (p.isConnected()):
 	baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
 	baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
 	#pre-rotate to make z-up
-	y2zPos=[0,0,0.0]
-	y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
-	basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
-
-	p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
-	#	once=False
+	if (useZUp):
+		y2zPos=[0,0,0.0]
+		y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
+		basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
+		p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
+		
+		y2zPos=[0,2,0.0]
+		y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
+		basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
+		p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn)
+	
+	
 	chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
 	chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
 	chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
@@ -164,26 +293,7 @@ while (p.isConnected()):
 	leftElbowRotEnd = [frameDataNext[43]]
 	leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])]
 	
-	#print("chestRot=",chestRot)
-	p.setGravity(0,0,0)
-	
-	
-	kp=1
-	if (useMotionCapture):
-		
-		p.setJointMotorControlMultiDof(humanoid,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=maxForce)
-		p.setJointMotorControlMultiDof(humanoid,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=maxForce)
-		p.setJointMotorControlMultiDof(humanoid,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=maxForce)
-		p.setJointMotorControlMultiDof(humanoid,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=maxForce)
-		p.setJointMotorControlMultiDof(humanoid,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=maxForce)
-		p.setJointMotorControlMultiDof(humanoid,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=maxForce)
-		p.setJointMotorControlMultiDof(humanoid,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=maxForce)
-		p.setJointMotorControlMultiDof(humanoid,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=maxForce)
-		p.setJointMotorControlMultiDof(humanoid,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=maxForce)
-		p.setJointMotorControlMultiDof(humanoid,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=maxForce)
-		p.setJointMotorControlMultiDof(humanoid,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=maxForce)
-		p.setJointMotorControlMultiDof(humanoid,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=maxForce)
-	if (useMotionCaptureReset):
+	if (0):#if (once):
 		p.resetJointStateMultiDof(humanoid,chest,chestRot)
 		p.resetJointStateMultiDof(humanoid,neck,neckRot)
 		p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot)
@@ -196,5 +306,118 @@ while (p.isConnected()):
 		p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot)
 		p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot)
 		p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot)
+	once=False
+	#print("chestRot=",chestRot)
+	p.setGravity(0,0,-10)
+	
+	kp=kpMotor
+	if (useExplicitPD):
+		jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
+		#[x,y,z] base position and [x,y,z,w] base orientation!
+		totalDofs =7 
+		for dof in jointDofCounts:
+			totalDofs += dof
+		
+		jointIndicesAll = [
+								chest,
+								neck, 
+								rightHip,
+												rightKnee,
+								rightAnkle,
+								rightShoulder,
+												rightElbow,
+								leftHip,
+												leftKnee,
+								leftAnkle,
+								leftShoulder,
+												leftElbow
+		]
+		basePos,baseOrn = p.getBasePositionAndOrientation(humanoid)
+		pose = [ basePos[0],basePos[1],basePos[2],
+													baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3],
+													chestRot[0],chestRot[1],chestRot[2],chestRot[3],
+													neckRot[0],neckRot[1],neckRot[2],neckRot[3],
+													rightHipRot[0],rightHipRot[1],rightHipRot[2],rightHipRot[3],
+													rightKneeRot[0],
+													rightAnkleRot[0],rightAnkleRot[1],rightAnkleRot[2],rightAnkleRot[3],
+													rightShoulderRot[0],rightShoulderRot[1],rightShoulderRot[2],rightShoulderRot[3],
+													rightElbowRot[0],
+													leftHipRot[0],leftHipRot[1],leftHipRot[2],leftHipRot[3],
+													leftKneeRot[0],
+													leftAnkleRot[0],leftAnkleRot[1],leftAnkleRot[2],leftAnkleRot[3],
+													leftShoulderRot[0],leftShoulderRot[1],leftShoulderRot[2],leftShoulderRot[3],
+													leftElbowRot[0]	]
+		
+				
+		#print("pose=")
+		#for po in pose:
+		#	print(po)
+		
+		
+		taus = stablePD.computePD(bodyUniqueId=humanoid, 
+			jointIndices = jointIndicesAll,
+			desiredPositions = pose,
+			desiredVelocities = [0]*totalDofs,
+			kps = kpOrg,
+			kds = kdOrg,
+			maxForces = [maxForce]*totalDofs, 
+			timeStep=timeStep)
+			
+		taus3 = explicitPD.computePD(bodyUniqueId=humanoid3, 
+			jointIndices = jointIndicesAll,
+			desiredPositions = pose,
+			desiredVelocities = [0]*totalDofs,
+			kps = kpOrg,
+			kds = kdOrg,
+			maxForces = [maxForce*0.05]*totalDofs, 
+			timeStep=timeStep)
+
+		#taus=[0]*43
+		dofIndex=7
+		for index in range (len(jointIndicesAll)):
+			jointIndex = jointIndicesAll[index]
+			if jointDofCounts[index]==4:
+				
+				p.setJointMotorControlMultiDof(humanoid,jointIndex,p.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]])
+				p.setJointMotorControlMultiDof(humanoid3,jointIndex,p.TORQUE_CONTROL,force=[taus3[dofIndex+0],taus3[dofIndex+1],taus3[dofIndex+2]])
+	
+			if jointDofCounts[index]==1:
+				
+				p.setJointMotorControlMultiDof(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]])
+				p.setJointMotorControlMultiDof(humanoid3, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus3[dofIndex]])
+
+			dofIndex+=jointDofCounts[index]
+		
+		#print("len(taus)=",len(taus))
+		#print("taus=",taus)
+		
+		
+	p.setJointMotorControlMultiDof(humanoid2,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=[maxForce])
+	
+	kinematicHumanoid4 = True
+	if (kinematicHumanoid4):
+		p.resetJointStateMultiDof(humanoid4,chest,chestRot)
+		p.resetJointStateMultiDof(humanoid4,neck,neckRot)
+		p.resetJointStateMultiDof(humanoid4,rightHip,rightHipRot)
+		p.resetJointStateMultiDof(humanoid4,rightKnee,rightKneeRot)
+		p.resetJointStateMultiDof(humanoid4,rightAnkle,rightAnkleRot)
+		p.resetJointStateMultiDof(humanoid4,rightShoulder,rightShoulderRot)
+		p.resetJointStateMultiDof(humanoid4,rightElbow, rightElbowRot)
+		p.resetJointStateMultiDof(humanoid4,leftHip, leftHipRot)
+		p.resetJointStateMultiDof(humanoid4,leftKnee, leftKneeRot)
+		p.resetJointStateMultiDof(humanoid4,leftAnkle, leftAnkleRot)
+		p.resetJointStateMultiDof(humanoid4,leftShoulder, leftShoulderRot)
+		p.resetJointStateMultiDof(humanoid4,leftElbow, leftElbowRot)
 	p.stepSimulation()
-	#time.sleep(1./240.)
+	time.sleep(timeStep)
--- a/examples/pybullet/examples/pdControl.py
+++ b/examples/pybullet/examples/pdControl.py
@@ -1,7 +1,8 @@

 import pybullet as p
+from pdControllerExplicit import PDControllerExplicitMultiDof
 from pdControllerExplicit import PDControllerExplicit
-from pdControllerExplicit import PDControllerStable
+from pdControllerStable import PDControllerStable

 import time

@@ -13,7 +14,7 @@ pole2 = p.loadURDF("cartpole.urdf", [0,1,0], useMaximalCoordinates=useMaximalCoo
 pole3 = p.loadURDF("cartpole.urdf", [0,2,0], useMaximalCoordinates=useMaximalCoordinates)
 pole4 = p.loadURDF("cartpole.urdf", [0,3,0], useMaximalCoordinates=useMaximalCoordinates)

-exPD = PDControllerExplicit(p)
+exPD = PDControllerExplicitMultiDof(p)
 sPD = PDControllerStable(p)


@@ -60,7 +61,7 @@ timeStep = 0.001
 	
 	
 while p.isConnected():
-	p.getCameraImage(320,200)
+	#p.getCameraImage(320,200)
 	timeStep = p.readUserDebugParameter(timeStepId)
 	p.setTimeStep(timeStep)

@@ -76,8 +77,15 @@ while p.isConnected():
 	kdPole = p.readUserDebugParameter(kdPoleId)
 	maxForcePole = p.readUserDebugParameter(maxForcePoleId)
 	
-	taus = exPD.computePD(pole, [0,1], [desiredPosCart,desiredPosPole],[desiredVelCart,desiredVelPole], [kpCart,kpPole], [kdCart,kdPole],[maxForceCart,maxForcePole], timeStep)
-	p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
+	basePos,baseOrn = p.getBasePositionAndOrientation(pole)
+	
+	baseDof=7
+	taus = exPD.computePD(pole, [0,1], [basePos[0],basePos[1],basePos[2],baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3],desiredPosCart,desiredPosPole],
+			[0,0,0,0,0,0,0,desiredVelCart,desiredVelPole], [0,0,0,0,0,0,0,kpCart,kpPole], [0,0,0,0,0,0,0,kdCart,kdPole],[0,0,0,0,0,0,0,maxForceCart,maxForcePole], timeStep)
+	
+	for j in [0,1]:
+		p.setJointMotorControlMultiDof(pole, j, controlMode=p.TORQUE_CONTROL, force=[taus[j+baseDof]])
+	#p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
 		
 	if (pd>=0):
 		link = 0
@@ -89,7 +97,10 @@ while p.isConnected():
 	
 	
 	taus = sPD.computePD(pole4, [0,1], [desiredPosCart,desiredPosPole],[desiredVelCart,desiredVelPole], [kpCart,kpPole], [kdCart,kdPole],[maxForceCart,maxForcePole], timeStep)
-	p.setJointMotorControlArray(pole4, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
+	#p.setJointMotorControlArray(pole4, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
+	for j in [0,1]:
+		p.setJointMotorControlMultiDof(pole4, j, controlMode=p.TORQUE_CONTROL, force=[taus[j]])
+
 		
 	p.setJointMotorControl2(pole3,0, p.POSITION_CONTROL, targetPosition=desiredPosCart, targetVelocity=desiredVelCart, positionGain=timeStep*(kpCart/150.), velocityGain=0.5, force=maxForceCart)
 	p.setJointMotorControl2(pole3,1, p.POSITION_CONTROL, targetPosition=desiredPosPole, targetVelocity=desiredVelPole, positionGain=timeStep*(kpPole/150.), velocityGain=0.5, force=maxForcePole)
--- a/examples/pybullet/examples/pdControllerExplicit.py
+++ b/examples/pybullet/examples/pdControllerExplicit.py
@@ -1,5 +1,66 @@
 import numpy as np

+
+class PDControllerExplicitMultiDof(object):
+		def __init__(self, pb):
+			self._pb = pb
+
+		def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
+			
+			numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
+			curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
+			q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
+			baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
+			qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0]
+			qError = [0,0,0, 0,0,0,0]
+			qIndex=7
+			qdotIndex=7
+			zeroAccelerations=[0,0,0,  0,0,0,0]
+			for i in range (numJoints):
+				js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
+				
+				jointPos=js[0]
+				jointVel = js[1]
+				q1+=jointPos
+				
+				if len(js[0])==1:
+					desiredPos=desiredPositions[qIndex]
+					
+					qdiff=desiredPos - jointPos[0]
+					qError.append(qdiff)
+					zeroAccelerations.append(0.)
+					qdot1+=jointVel
+					qIndex+=1
+					qdotIndex+=1
+				if len(js[0])==4:
+					desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]]
+					axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos)
+					jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0]
+					qdot1+=jointVelNew
+					qError.append(axis[0])
+					qError.append(axis[1])
+					qError.append(axis[2])
+					qError.append(0)
+					desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]]
+					zeroAccelerations+=[0.,0.,0.,0.]
+					qIndex+=4
+					qdotIndex+=4
+			
+			q = np.array(q1)
+			qdot=np.array(qdot1)
+			qdotdesired = np.array(desiredVelocities)
+			qdoterr = qdotdesired-qdot
+			Kp = np.diagflat(kps)
+			Kd = np.diagflat(kds)
+			p =  Kp.dot(qError)
+			d = Kd.dot(qdoterr)
+			forces = p + d
+			maxF = np.array(maxForces)
+			forces = np.clip(forces, -maxF , maxF )
+			return forces
+
+
+			
 class PDControllerExplicit(object):
 		def __init__(self, pb):
 			self._pb = pb
@@ -25,43 +86,4 @@ class PDControllerExplicit(object):
 			forces = np.clip(forces, -maxF , maxF )
 			return forces
 			
-			
-class PDControllerStable(object):
-		def __init__(self, pb):
-			self._pb = pb
-
-		def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
-			numJoints = self._pb.getNumJoints(bodyUniqueId)
-			jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
-			q1 = []
-			qdot1 = []
-			zeroAccelerations = []
-			for i in range (numJoints):
-				q1.append(jointStates[i][0])
-				qdot1.append(jointStates[i][1])
-				zeroAccelerations.append(0)
-			q = np.array(q1)
-			qdot=np.array(qdot1)
-			qdes = np.array(desiredPositions)
-			qdotdes = np.array(desiredVelocities)
-			qError = qdes - q
-			qdotError = qdotdes - qdot
-			Kp = np.diagflat(kps)
-			Kd = np.diagflat(kds)
-			p =  Kp.dot(qError)
-			d = Kd.dot(qdotError)
-			forces = p + d
-			
-			M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
-			M2 = np.array(M1)
-			M = (M2 + Kd * timeStep)
-			c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
-			c = np.array(c1)
-			A = M
-			b = -c + p + d
-			qddot = np.linalg.solve(A, b)
-			tau = p + d - Kd.dot(qddot) * timeStep
-			maxF = np.array(maxForces)
-			forces = np.clip(tau, -maxF , maxF )
-			#print("c=",c)
-			return tau
+			
--- a/examples/pybullet/examples/pdControllerStable.py
+++ b/examples/pybullet/examples/pdControllerStable.py
@@ -0,0 +1,144 @@
+import numpy as np
+
+
+
+class PDControllerStableMultiDof(object):
+		def __init__(self, pb):
+			self._pb = pb
+
+		def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
+			
+			numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
+			curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
+			#q1 = [desiredPositions[0],desiredPositions[1],desiredPositions[2],desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]]
+			q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
+			
+			#qdot1 = [0,0,0, 0,0,0,0]
+			baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
+			
+			qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0]
+			qError = [0,0,0, 0,0,0,0]
+			
+			qIndex=7
+			qdotIndex=7
+			zeroAccelerations=[0,0,0,  0,0,0,0]
+			for i in range (numJoints):
+				js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
+				
+				jointPos=js[0]
+				jointVel = js[1]
+				q1+=jointPos
+				
+				if len(js[0])==1:
+					desiredPos=desiredPositions[qIndex]
+					
+					qdiff=desiredPos - jointPos[0]
+					qError.append(qdiff)
+					zeroAccelerations.append(0.)
+					qdot1+=jointVel
+					qIndex+=1
+					qdotIndex+=1
+				if len(js[0])==4:
+					desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]]
+					axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos)
+					jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0]
+					qdot1+=jointVelNew
+					qError.append(axis[0])
+					qError.append(axis[1])
+					qError.append(axis[2])
+					qError.append(0)
+					desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]]
+					zeroAccelerations+=[0.,0.,0.,0.]
+					qIndex+=4
+					qdotIndex+=4
+			
+			q = np.array(q1)
+			qdot=np.array(qdot1)
+			
+			qdotdesired = np.array(desiredVelocities)
+			qdoterr = qdotdesired-qdot
+			
+			
+			Kp = np.diagflat(kps)
+			Kd = np.diagflat(kds)
+			
+			p =  Kp.dot(qError)
+			
+			#np.savetxt("pb_qError.csv", qError, delimiter=",")
+			#np.savetxt("pb_p.csv", p, delimiter=",")
+			
+			d = Kd.dot(qdoterr)
+
+			#np.savetxt("pb_d.csv", d, delimiter=",")
+			#np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",")
+			
+			
+			M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1, flags=1)
+			
+			
+			M2 = np.array(M1)
+			#np.savetxt("M2.csv", M2, delimiter=",")
+			
+			M = (M2 + Kd * timeStep)
+			
+			#np.savetxt("pbM_tKd.csv",M, delimiter=",")
+			
+		
+			
+			c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
+			
+			
+			c = np.array(c1)
+			#np.savetxt("pbC.csv",c, delimiter=",")
+			A = M
+			#p = [0]*43
+			b =  p + d -c
+			#np.savetxt("pb_acc.csv",b, delimiter=",")
+			qddot = np.linalg.solve(A, b)
+			tau = p + d - Kd.dot(qddot) * timeStep
+			#print("len(tau)=",len(tau))
+			maxF = np.array(maxForces)
+			forces = np.clip(tau, -maxF , maxF )
+			return forces
+			
+		
+			
+class PDControllerStable(object):
+		def __init__(self, pb):
+			self._pb = pb
+
+		def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
+			numJoints = self._pb.getNumJoints(bodyUniqueId)
+			jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
+			q1 = []
+			qdot1 = []
+			zeroAccelerations = []
+			for i in range (numJoints):
+				q1.append(jointStates[i][0])
+				qdot1.append(jointStates[i][1])
+				zeroAccelerations.append(0)
+			q = np.array(q1)
+			qdot=np.array(qdot1)
+			qdes = np.array(desiredPositions)
+			qdotdes = np.array(desiredVelocities)
+			qError = qdes - q
+			qdotError = qdotdes - qdot
+			Kp = np.diagflat(kps)
+			Kd = np.diagflat(kds)
+			p =  Kp.dot(qError)
+			d = Kd.dot(qdotError)
+			forces = p + d
+			
+			M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
+			M2 = np.array(M1)
+			M = (M2 + Kd * timeStep)
+			c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
+			c = np.array(c1)
+			A = M
+			b = -c + p + d
+			qddot = np.linalg.solve(A, b)
+			tau = p + d - Kd.dot(qddot) * timeStep
+			maxF = np.array(maxForces)
+			forces = np.clip(tau, -maxF , maxF )
+			#print("c=",c)
+			return tau
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -10,6 +10,11 @@
 #include "../SharedMemory/dart/DARTPhysicsC_API.h"
 #endif

+#ifdef BT_ENABLE_PHYSX
+#include "../SharedMemory/physx/PhysXC_API.h"
+#endif
+
+
 #ifdef BT_ENABLE_MUJOCO
 #include "../SharedMemory/mujoco/MuJoCoPhysicsC_API.h"
 #endif
@@ -420,6 +425,13 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 				break;
 			}
 #endif
+#ifdef BT_ENABLE_PHYSX
+			case eCONNECT_PHYSX:
+			{
+				sm = b3ConnectPhysX();
+				break;
+			}
+#endif

 #ifdef BT_ENABLE_MUJOCO
 			case eCONNECT_MUJOCO:
@@ -2395,12 +2407,14 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*

 	double targetPositionArray[4] = { 0, 0, 0, 1 };
 	double targetVelocityArray[3] = { 0, 0, 0 };
+	double targetForceArray[3] = { 100000.0, 100000.0, 100000.0 };
 	int targetPositionSize = 0;
 	int targetVelocitySize = 0;
+	int targetForceSize = 0;
 	PyObject* targetPositionObj = 0;
 	PyObject* targetVelocityObj = 0;
-
-	double force = 100000.0;
+	PyObject* targetForceObj = 0;
+		
 	double kp = 0.1;
 	double kd = 1.0;
 	double maxVelocity = -1;
@@ -2408,8 +2422,8 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*

 	int physicsClientId = 0;
 	static char* kwlist[] = { "bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "maxVelocity", "physicsClientId", NULL };
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|OOddddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode,
-		&targetPositionObj, &targetVelocityObj, &force, &kp, &kd, &maxVelocity, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|OOOdddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode,
+		&targetPositionObj, &targetVelocityObj, &targetForceObj, &kp, &kd, &maxVelocity, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -2463,6 +2477,29 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*

 	}

+	if (targetForceObj)
+	{
+		int i = 0;
+		PyObject* targetForceSeq = 0;
+		targetForceSeq = PySequence_Fast(targetForceObj, "expected a force sequence");
+		targetForceSize = PySequence_Size(targetForceObj);
+
+		if (targetForceSize < 0)
+		{
+			targetForceSize = 0;
+		}
+		if (targetForceSize >3)
+		{
+			targetForceSize = 3;
+		}
+		for (i = 0; i < targetForceSize; i++)
+		{
+			targetForceArray[i] = pybullet_internalGetFloatFromSequence(targetForceSeq, i);
+		}
+
+	}
+
+
 	//if (targetPositionSize == 0 && targetVelocitySize == 0)
 	//{

@@ -2480,7 +2517,7 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 		}

 		if (//(controlMode != CONTROL_MODE_VELOCITY)&&
-			//(controlMode != CONTROL_MODE_TORQUE) &&
+			(controlMode != CONTROL_MODE_TORQUE) &&
 			(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)//&&
 			//(controlMode != CONTROL_MODE_PD)
 			)
@@ -2505,13 +2542,17 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 			break;
 		}

+		
+#endif
 		case CONTROL_MODE_TORQUE:
 		{
-			b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex,
-				force);
+			if (info.m_uSize == targetForceSize)
+			{
+				b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
+					targetForceArray, targetForceSize);
+			}
 			break;
 		}
-#endif
 		case CONTROL_MODE_POSITION_VELOCITY_PD:
 		case CONTROL_MODE_PD:
 		{
@@ -2544,7 +2585,11 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 				//printf("Warning: targetVelocity array size doesn't match joint dimentions (got %d, expected %d).", targetVelocitySize, info.m_uSize);
 			}
 			b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
-			b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
+			if (info.m_uSize == targetForceSize || targetForceSize==1)
+			{
+				b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
+					targetForceArray, targetForceSize);
+			}
 			break;
 		}
 		default:
@@ -9051,6 +9096,54 @@ static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* a
 }


+static PyObject* pybullet_getAxisDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	PyObject* quatStartObj;
+	PyObject* quatEndObj;
+	double quatStart[4];
+	double quatEnd[4];
+	int physicsClientId = 0;
+	int hasQuatStart = 0;
+	int hasQuatEnd = 0;
+
+	static char* kwlist[] = { "quaternionStart", "quaternionEnd", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatStartObj, &quatEndObj, &physicsClientId))
+	{
+		return NULL;
+	}
+
+	if (quatStartObj)
+	{
+		hasQuatStart = pybullet_internalSetVector4d(quatStartObj, quatStart);
+	}
+
+	if (quatEndObj)
+	{
+		hasQuatEnd = pybullet_internalSetVector4d(quatEndObj, quatEnd);
+	}
+	if (hasQuatStart && hasQuatEnd)
+	{
+		double axisOut[3];
+		b3GetAxisDifferenceQuaternion(quatStart, quatEnd, axisOut);
+		{
+			PyObject* pylist;
+			int i;
+			pylist = PyTuple_New(3);
+			for (i = 0; i < 3; i++)
+				PyTuple_SetItem(pylist, i, PyFloat_FromDouble(axisOut[i]));
+			return pylist;
+		}
+	}
+	else
+	{
+		PyErr_SetString(SpamError, "Require start and end quaternion, each with 4 components [x,y,z,w].");
+		return NULL;
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+
 static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	PyObject* quatStartObj;
@@ -9565,13 +9658,16 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 		PyObject* objVelocitiesQdot;
 		PyObject* objAccelerations;
 		int physicsClientId = 0;
+		int flags = 0;
 		b3PhysicsClientHandle sm = 0;
 		static char* kwlist[] = {"bodyUniqueId", "objPositions",
 								 "objVelocities", "objAccelerations",
+								"flags",
 								 "physicsClientId", NULL};
-		if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist,
+		if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|ii", kwlist,
 										 &bodyUniqueId, &objPositionsQ,
 										 &objVelocitiesQdot, &objAccelerations,
+										&flags,
 										 &physicsClientId))
 		{
 			static char* kwlist2[] = {"bodyIndex", "objPositions",
@@ -9607,7 +9703,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 				double* jointPositionsQ = (double*)malloc(szInBytesQ);
 				double* jointVelocitiesQdot = (double*)malloc(szInBytesQdot);
 				double* jointAccelerations = (double*)malloc(szInBytesQdot);
-				double* jointForcesOutput = (double*)malloc(szInBytesQdot);
+				

 				for (i = 0; i < szObPos; i++)
 				{
@@ -9625,10 +9721,12 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 				{
 					b3SharedMemoryStatusHandle statusHandle;
 					int statusType;
+
 					b3SharedMemoryCommandHandle commandHandle =
 						b3CalculateInverseDynamicsCommandInit2(
 							sm, bodyUniqueId, jointPositionsQ, szObPos, jointVelocitiesQdot,
 							jointAccelerations, szObVel);
+					b3CalculateInverseDynamicsSetFlags(commandHandle, flags);
 					statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);

 					statusType = b3GetStatusType(statusHandle);
@@ -9637,14 +9735,13 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 					{
 						int bodyUniqueId;
 						int dofCount;
-
-						b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,
-															  &dofCount, 0);
+						b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,&dofCount, 0);

 						if (dofCount)
 						{
-							b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,
-																  jointForcesOutput);
+							double* jointForcesOutput = (double*)malloc(sizeof(double) * dofCount);
+							
+							b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,jointForcesOutput);
 							{
 								{
 									int i;
@@ -9654,6 +9751,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 														PyFloat_FromDouble(jointForcesOutput[i]));
 								}
 							}
+							free(jointForcesOutput);
 						}
 					}
 					else
@@ -9665,7 +9763,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 				free(jointPositionsQ);
 				free(jointVelocitiesQdot);
 				free(jointAccelerations);
-				free(jointForcesOutput);
+				
 				if (pylist) return pylist;
 			}
 			else
@@ -9879,9 +9977,10 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
 		PyObject* objPositions;
 		int physicsClientId = 0;
 		b3PhysicsClientHandle sm = 0;
-		static char* kwlist[] = {"bodyUniqueId", "objPositions", "physicsClientId", NULL};
-		if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|i", kwlist,
-										 &bodyUniqueId, &objPositions, &physicsClientId))
+		int flags = 0;
+		static char* kwlist[] = {"bodyUniqueId", "objPositions", "flags", "physicsClientId", NULL};
+		if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|ii", kwlist,
+										 &bodyUniqueId, &objPositions, &flags, &physicsClientId))
 		{
 			return NULL;
 		}
@@ -9894,15 +9993,16 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py

 		{
 			int szObPos = PySequence_Size(objPositions);
-			int numJoints = b3GetNumJoints(sm, bodyUniqueId);
-			if (numJoints && (szObPos == numJoints))
+			///int dofCountQ = b3GetNumJoints(sm, bodyUniqueId);
+			
+			if (szObPos>=0)//(szObPos == dofCountQ))
 			{
-				int byteSizeJoints = sizeof(double) * numJoints;
-				PyObject* pyResultList;
+				int byteSizeJoints = sizeof(double) * szObPos;
+				PyObject* pyResultList=NULL;
 				double* jointPositions = (double*)malloc(byteSizeJoints);
 				double* massMatrix = NULL;
 				int i;
-				for (i = 0; i < numJoints; i++)
+				for (i = 0; i < szObPos; i++)
 				{
 					jointPositions[i] =
 						pybullet_internalGetFloatFromSequence(objPositions, i);
@@ -9911,17 +10011,19 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
 					b3SharedMemoryStatusHandle statusHandle;
 					int statusType;
 					b3SharedMemoryCommandHandle commandHandle =
-						b3CalculateMassMatrixCommandInit(sm, bodyUniqueId, jointPositions);
+						b3CalculateMassMatrixCommandInit(sm, bodyUniqueId, jointPositions, szObPos);
+					b3CalculateMassMatrixSetFlags(commandHandle, flags);
 					statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
 					statusType = b3GetStatusType(statusHandle);
 					if (statusType == CMD_CALCULATED_MASS_MATRIX_COMPLETED)
 					{
 						int dofCount;
 						b3GetStatusMassMatrix(sm, statusHandle, &dofCount, NULL);
+						pyResultList = PyTuple_New(dofCount);
 						if (dofCount)
 						{
 							int byteSizeDofCount = sizeof(double) * dofCount;
-							pyResultList = PyTuple_New(dofCount);
+							

 							massMatrix = (double*)malloc(dofCount * byteSizeDofCount);
 							b3GetStatusMassMatrix(sm, statusHandle, NULL, massMatrix);
@@ -10368,6 +10470,11 @@ static PyMethodDef SpamMethods[] = {
 	 { "getDifferenceQuaternion", (PyCFunction)pybullet_getDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
 		 "Compute the quaternion difference from two quaternions." },

+		 { "getAxisDifferenceQuaternion", (PyCFunction)pybullet_getAxisDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
+		 "Compute the velocity axis difference from two quaternions." },
+
+		 
+
 	 { "calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS,
 		 "Compute the angular velocity given start and end quaternion and delta time." },

@@ -10555,6 +10662,10 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "DART", eCONNECT_DART);  // user read
 #endif

+#ifdef BT_ENABLE_PHYSX
+	PyModule_AddIntConstant(m, "PhysX", eCONNECT_PHYSX);
+#endif
+
 #ifdef BT_ENABLE_MUJOCO
 	PyModule_AddIntConstant(m, "MuJoCo", eCONNECT_MUJOCO);  // user read
 #endif
@@ -10667,6 +10778,8 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "URDF_INITIALIZE_SAT_FEATURES", URDF_INITIALIZE_SAT_FEATURES);
 	PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_COLORS_FROM_MTL", URDF_USE_MATERIAL_COLORS_FROM_MTL);
 	PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL", URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL);
+	PyModule_AddIntConstant(m, "URDF_MAINTAIN_LINK_ORDER", URDF_MAINTAIN_LINK_ORDER);
+	
 	
 	PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping);
 	PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping);