Implement btMultiBodySphericalJointMotor, able to track a quaternion position target.

Expose this btMultiBodySphericalJointMotor through PyBullet.setJointMotorControlMultiDof Expose PyBullet.getQuaternionSlerp Improve PyBullet.setJointMotorControlMultiDof Improve humanoidMotionCapture.py with slerp and using setJointMotorControlMultiDof Expose btMultiBody::spatialTransform Fix btMultiBody::setupPlanar from DeepMimic codebase Add support for multidof joints in btMultiBody::compTreeLinkVelocities, thanks to DeepMimic codebase @xbpeng
2018-11-13 14:32:18 -08:00
parent a06b5de7b6
commit 7dd524075c
13 changed files with 778 additions and 68 deletions
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -818,6 +818,22 @@ B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle c
 	return 0;
 }

+B3_SHARED_API int b3JointControlSetDesiredPositionMultiDof(b3SharedMemoryCommandHandle commandHandle, int qIndex, const double* position, int dofCount)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	if ((qIndex >= 0) && ((qIndex + dofCount) < MAX_DEGREE_OF_FREEDOM) && dofCount > 0 && dofCount <= 4)
+	{
+		for (int dof = 0; dof < dofCount; dof++)
+		{
+			command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex + dof] = position[dof];
+			command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
+			command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex + dof] |= SIM_DESIRED_STATE_HAS_Q;
+		}
+	}
+	return 0;
+}
+
 B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -870,6 +886,22 @@ B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle c
 	return 0;
 }

+B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, const double* velocity, int dofCount)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	if ((dofIndex >= 0) && ((dofIndex+ dofCount) < MAX_DEGREE_OF_FREEDOM) && dofCount>=0 && dofCount<=4)
+	{
+		for (int dof = 0; dof < dofCount; dof++)
+		{
+			command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex+dof] = velocity[dof];
+			command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
+			command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex+dof] |= SIM_DESIRED_STATE_HAS_QDOT;
+		}
+	}
+	return 0;
+}
+
 B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -5221,3 +5253,14 @@ B3_SHARED_API void b3InvertTransform(const double pos[3], const double orn[4], d
 	outOrn[2] = invOrn[2];
 	outOrn[3] = invOrn[3];
 }
+
+B3_SHARED_API void b3QuaternionSlerp(const double startQuat[/*4*/], const double endQuat[/*4*/], double interpolationFraction, double outOrn[/*4*/])
+{
+	b3Quaternion start(startQuat[0], startQuat[1], startQuat[2], startQuat[3]);
+	b3Quaternion end(endQuat[0], endQuat[1], endQuat[2], endQuat[3]);
+	b3Quaternion result = start.slerp(end, interpolationFraction);
+	outOrn[0] = result[0];
+	outOrn[1] = result[1];
+	outOrn[2] = result[2];
+	outOrn[3] = result[3];
+}
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -440,12 +440,15 @@ extern "C"

 	///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
 	B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
+	B3_SHARED_API int b3JointControlSetDesiredPositionMultiDof(b3SharedMemoryCommandHandle commandHandle, int qIndex, const double* position, int dofCount);
+
 	B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
 	B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
 	B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity);

 	///Only use when controlMode is CONTROL_MODE_VELOCITY
 	B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
+	B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, const double* velocity, int dofCount);
 	B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
 	///Only use if when controlMode is CONTROL_MODE_TORQUE,
 	B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
@@ -631,6 +634,7 @@ extern "C"

 	B3_SHARED_API void b3MultiplyTransforms(const double posA[/*3*/], const double ornA[/*4*/], const double posB[/*3*/], const double ornB[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]);
 	B3_SHARED_API void b3InvertTransform(const double pos[/*3*/], const double orn[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]);
+	B3_SHARED_API void b3QuaternionSlerp(const double startQuat[/*4*/], const double endQuat[/*4*/], double interpolationFraction, double outOrn[/*4*/]);

 #ifdef __cplusplus
 }
--- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
@@ -149,6 +149,8 @@ bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b
 		if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size()))
 		{
 			info = bodyJoints->m_jointInfo[jointIndex];
+			info.m_qSize = 0;
+			info.m_uSize = 0;
 			switch (info.m_jointType)
 			{
 				case eSphericalType:
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -13,6 +13,8 @@
 #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
 #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
 #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h"
+

 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
@@ -2711,7 +2713,8 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()

 bool PhysicsServerCommandProcessor::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
 {
-	bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
+	bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute 
+		|| mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
 	return canHaveMotor;
 }

@@ -2722,10 +2725,10 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
 	for (int i = 0; i < numLinks; i++)
 	{
 		int mbLinkIndex = i;
+		float maxMotorImpulse = 1.f;

 		if (supportsJointMotor(mb, mbLinkIndex))
 		{
-			float maxMotorImpulse = 1.f;
 			int dof = 0;
 			btScalar desiredVelocity = 0.f;
 			btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse);
@@ -2737,6 +2740,13 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
 			m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
 			motor->finalizeMultiDof();
 		}
+		if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eSpherical)
+		{
+			btMultiBodySphericalJointMotor* motor = new btMultiBodySphericalJointMotor(mb, mbLinkIndex, 1000*maxMotorImpulse);
+			mb->getLink(mbLinkIndex).m_userPtr = motor;
+			m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
+			motor->finalizeMultiDof();
+		}
 	}
 }

@@ -2916,6 +2926,18 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
 			std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
 			m_data->m_strings.push_back(baseName);
 			mb->setBaseName(baseName->c_str());
+			
+#if 0
+			btAlignedObjectArray<char> urdf;
+			mb->dumpUrdf(urdf);
+			FILE* f = fopen("e:/pybullet.urdf", "w");
+			if (f)
+			{
+				fwrite(&urdf[0], urdf.size(), 1, f);
+				fclose(f);
+			}
+#endif
+			
 		}
 		else
 		{
@@ -5811,6 +5833,74 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
 								numMotors++;
 							}
 						}
+
+						if (mb->getLink(link).m_jointType == btMultibodyLink::eSpherical)
+						{
+							btMultiBodySphericalJointMotor* motor = (btMultiBodySphericalJointMotor*)mb->getLink(link).m_userPtr;
+							if (motor)
+							{
+								if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_RHS_CLAMP) != 0)
+								{
+									motor->setRhsClamp(clientCmd.m_sendDesiredStateCommandArgument.m_rhsClamp[velIndex]);
+								}
+								bool hasDesiredPosOrVel = false;
+								btScalar kp = 0.f;
+								btScalar kd = 0.f;
+								btVector3 desiredVelocity(0, 0, 0);
+								if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
+								{
+									hasDesiredPosOrVel = true;
+									desiredVelocity.setValue(
+										clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex+0],
+										clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex+1],
+										clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex+2]);
+									kd = 0.1;
+								}
+								btQuaternion desiredPosition(0, 0, 0, 1);
+								if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0)
+								{
+									hasDesiredPosOrVel = true;
+									desiredPosition.setValue(
+										clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 0],
+										clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 1],
+										clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 2],
+										clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 3]);
+									kp = 0.1;
+								}
+
+								if (hasDesiredPosOrVel)
+								{
+									if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP) != 0)
+									{
+										kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
+									}
+
+									if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
+									{
+										kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
+									}
+
+									motor->setVelocityTarget(desiredVelocity, kd);
+									//todo: instead of clamping, combine the motor and limit
+									//and combine handling of limit force and motor force.
+
+									//clamp position
+									//if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
+									//{
+									//	btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
+									//}
+									motor->setPositionTarget(desiredPosition, kp);
+
+									btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime;
+
+									if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
+										maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->m_physicsDeltaTime;
+
+									motor->setMaxAppliedImpulse(maxImp);
+								}
+								numMotors++;
+							}
+						}
 						velIndex += mb->getLink(link).m_dofCount;
 						posIndex += mb->getLink(link).m_posVarCount;
 					}
@@ -8038,8 +8128,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
 				{
 					mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]);
 				}
-				
-				
+								
 				posVarCountIndex += mb->getLink(i).m_posVarCount;
 				uDofIndex += mb->getLink(i).m_dofCount;
 			}
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -445,7 +445,7 @@ enum EnumSimDesiredStateUpdateFlags
 	SIM_DESIRED_STATE_HAS_KD = 4,
 	SIM_DESIRED_STATE_HAS_KP = 8,
 	SIM_DESIRED_STATE_HAS_MAX_FORCE = 16,
-	SIM_DESIRED_STATE_HAS_RHS_CLAMP = 32
+	SIM_DESIRED_STATE_HAS_RHS_CLAMP = 32,
 };

 enum EnumSimParamUpdateFlags
--- a/examples/pybullet/examples/humanoidMotionCapture.py
+++ b/examples/pybullet/examples/humanoidMotionCapture.py
@@ -8,11 +8,15 @@ p.connect(p.GUI)
 import pybullet_data

 useMotionCapture=True
+useMotionCaptureReset=not useMotionCapture
+

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

-p.loadURDF("plane.urdf")
+p.loadURDF("plane.urdf",[0,0,-1])
 print("path = ", path)
 with open(path, 'r') as f:
    motion_dict = json.load(f)
@@ -31,6 +35,11 @@ humanoid = p.loadURDF("humanoid/humanoid.urdf", globalScaling=0.25)

 for j in range (p.getNumJoints(humanoid)):
 	ji = p.getJointInfo(humanoid,j)
+	targetPosition=[0]
+	if (ji[2] == p.JOINT_SPHERICAL):
+		targetPosition=[0,0,0,1]
+	#p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, force=0)
+	
 	#print(ji)
 	print("joint[",j,"].type=",jointTypes[ji[2]])
 	print("joint[",j,"].name=",ji[1])
@@ -51,7 +60,7 @@ for j in range (p.getNumJoints(humanoid)):
 			paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
 			print("jointName=",jointName, "at ", j)
 		
-p.changeVisualShape(humanoid,14,rgbaColor=[1,0,0,1])
+p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1])
 chest=1
 neck=2
 rightShoulder=3
@@ -66,34 +75,115 @@ leftKnee=13
 leftAnkle=14

 import time
-p.setRealTimeSimulation(0)
-while (1):
-	p.getCameraImage(320,200)
-	frame = int(p.readUserDebugParameter(frameId))
+
+
+once=True
+p.getCameraImage(320,200)
+maxForce=1000
+
+
+while (p.isConnected()):
+	frameReal = p.readUserDebugParameter(frameId)
+	frame = int(frameReal)
+	frameNext = frame+1
+	if (frameNext >=  numFrames):
+		frameNext = frame
+	
+	frameFraction = frameReal - frame
+	#print("frameFraction=",frameFraction)
+	#print("frame=",frame)
+	#print("frameNext=", frameNext)
+	
+	#getQuaternionSlerp
+	
 	frameData = motion_dict['Frames'][frame]
+	frameDataNext = motion_dict['Frames'][frameNext]
+	
 	#print("duration=",frameData[0])
 	#print(pos=[frameData])
-	basePos1 = [frameData[1],frameData[2],frameData[3]]
-	baseOrn1 = [frameData[5],frameData[6], frameData[7],frameData[4]]
+	
+	basePos1Start = [frameData[1],frameData[2],frameData[3]]
+	basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
+	basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), 
+		basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), 
+		basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
+	baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
+	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]
+	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)
-	chestRot = [frameData[9],frameData[10],frameData[11],frameData[8]]
-	neckRot = [frameData[13],frameData[14],frameData[15],frameData[12]]
-	rightHipRot = [frameData[17],frameData[18],frameData[19],frameData[16]]
-	rightKneeRot = [frameData[20]]
-	rightAnkleRot = [frameData[22],frameData[23],frameData[24],frameData[21]]
-	rightShoulderRot = [frameData[26],frameData[27],frameData[28],frameData[25]]
-	rightElbowRot = [frameData[29]]
-	leftHipRot = [frameData[31],frameData[32],frameData[33],frameData[30]]
-	leftKneeRot = [frameData[34]]
-	leftAnkleRot = [frameData[36],frameData[37],frameData[38],frameData[35]]
-	leftShoulderRot = [frameData[40],frameData[41],frameData[42],frameData[39]]
-	leftElbowRot = [frameData[43]]
+	#	once=False
+	chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
+	chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
+	chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
+	
+	neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
+	neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
+	neckRot =  p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
+	
+	rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
+	rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
+	rightHipRot = p.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction)
+	
+	rightKneeRotStart = [frameData[20]]
+	rightKneeRotEnd = [frameDataNext[20]]
+	rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])]
+	
+	rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
+	rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]]
+	rightAnkleRot =  p.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction)
+		
+	rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]]
+	rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
+	rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
+	
+	rightElbowRotStart = [frameData[29]]
+	rightElbowRotEnd = [frameDataNext[29]]
+	rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])]
+	
+	leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
+	leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]]
+	leftHipRot = p.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
+	
+	leftKneeRotStart = [frameData[34]]
+	leftKneeRotEnd = [frameDataNext[34]]
+	leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ]
+	
+	leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
+	leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
+	leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
+	
+	leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
+	leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]]
+	leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction)
+	leftElbowRotStart = [frameData[43]]
+	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):
 		p.resetJointStateMultiDof(humanoid,chest,chestRot)
 		p.resetJointStateMultiDof(humanoid,neck,neckRot)
 		p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot)
@@ -106,11 +196,5 @@ while (1):
 		p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot)
 		p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot)
 		p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot)
-	else:
-		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.1)
+	p.stepSimulation()
+	#time.sleep(1./240.)
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -208,7 +208,7 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3])
 	return 0;
 }

-//  vector - double[3] which will be set by values from obVec
+//  vector - double[4] which will be set by values from obVec
 static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4])
 {
 	int i, len;
@@ -1879,6 +1879,7 @@ static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObje
 	Py_INCREF(Py_None);
 	return Py_None;
 }
+
 //this method is obsolete, use pybullet_setJointMotorControl2 instead
 static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
 {
@@ -2387,6 +2388,180 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
 	//  return NULL;
 }

+
+static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int bodyUniqueId, jointIndex, controlMode;
+
+	double targetPositionArray[4] = { 0, 0, 0, 1 };
+	double targetVelocityArray[3] = { 0, 0, 0 };
+	int targetPositionSize = 0;
+	int targetVelocitySize = 0;
+	PyObject* targetPositionObj = 0;
+	PyObject* targetVelocityObj = 0;
+
+	double force = 100000.0;
+	double kp = 0.1;
+	double kd = 1.0;
+	double maxVelocity = -1;
+	b3PhysicsClientHandle sm = 0;
+
+	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))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	if (targetPositionObj)
+	{
+		PyObject* targetPositionSeq = 0;
+		int i = 0;
+		targetPositionSeq = PySequence_Fast(targetPositionObj, "expected a targetPosition sequence");
+		targetPositionSize = PySequence_Size(targetPositionObj);
+
+		if (targetPositionSize < 0)
+		{
+			targetPositionSize = 0;
+		}
+		if (targetPositionSize >4)
+		{
+			targetPositionSize = 4;
+		}
+		for (i = 0; i < targetPositionSize; i++)
+		{
+			targetPositionArray[i] = pybullet_internalGetFloatFromSequence(targetPositionSeq, i);
+		}
+	}
+
+	if (targetVelocityObj)
+	{
+		int i = 0;
+		PyObject* targetVelocitySeq = 0;
+		targetVelocitySeq = PySequence_Fast(targetVelocityObj, "expected a targetVelocity sequence");
+		targetVelocitySize = PySequence_Size(targetVelocityObj);
+
+		if (targetVelocitySize < 0)
+		{
+			targetVelocitySize = 0;
+		}
+		if (targetVelocitySize >3)
+		{
+			targetVelocitySize = 3;
+		}
+		for (i = 0; i < targetVelocitySize; i++)
+		{
+			targetVelocityArray[i] = pybullet_internalGetFloatFromSequence(targetVelocitySeq, i);
+		}
+
+	}
+
+	//if (targetPositionSize == 0 && targetVelocitySize == 0)
+	//{
+
+	{
+		int numJoints;
+		b3SharedMemoryCommandHandle commandHandle;
+		b3SharedMemoryStatusHandle statusHandle;
+		struct b3JointInfo info;
+
+		numJoints = b3GetNumJoints(sm, bodyUniqueId);
+		if ((jointIndex >= numJoints) || (jointIndex < 0))
+		{
+			PyErr_SetString(SpamError, "Joint index out-of-range.");
+			return NULL;
+		}
+
+		if (//(controlMode != CONTROL_MODE_VELOCITY)&&
+			//(controlMode != CONTROL_MODE_TORQUE) &&
+			(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)//&&
+			//(controlMode != CONTROL_MODE_PD)
+			)
+		{
+			PyErr_SetString(SpamError, "Illegal control mode.");
+			return NULL;
+		}
+
+		commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, controlMode);
+
+		b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info);
+				
+		switch (controlMode)
+		{
+#if 0
+		case CONTROL_MODE_VELOCITY:
+		{
+			b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,
+				targetVelocity);
+			b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+			b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
+			break;
+		}
+
+		case CONTROL_MODE_TORQUE:
+		{
+			b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex,
+				force);
+			break;
+		}
+#endif
+		case CONTROL_MODE_POSITION_VELOCITY_PD:
+		case CONTROL_MODE_PD:
+		{
+
+			//make sure size == info.m_qSize
+			
+			if (maxVelocity > 0)
+			{
+				b3JointControlSetMaximumVelocity(commandHandle, info.m_uIndex, maxVelocity);
+			}
+
+			if (info.m_qSize == targetPositionSize)
+			{
+				b3JointControlSetDesiredPositionMultiDof(commandHandle, info.m_qIndex,
+					targetPositionArray, targetPositionSize);
+			}
+			else
+			{
+				//printf("Warning: targetPosition array size doesn't match joint position size  (got %d, expected %d).",targetPositionSize, info.m_qSize);
+			}
+			
+			b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
+			if (info.m_uSize == targetVelocitySize)
+			{
+				b3JointControlSetDesiredVelocityMultiDof(commandHandle, info.m_uIndex,
+					targetVelocityArray, targetVelocitySize);
+			}
+			else
+			{
+				//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);
+			break;
+		}
+		default:
+		{
+		}
+		};
+
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+
+		Py_INCREF(Py_None);
+		return Py_None;
+	}
+	//  PyErr_SetString(SpamError, "Error parsing arguments in setJointControl.");
+	//  return NULL;
+}
+
+
 static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	int bodyUniqueId, jointIndex, controlMode;
@@ -3454,14 +3629,13 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObje
 static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	{
+		b3PhysicsClientHandle sm = 0;
 		int bodyUniqueId;
 		int jointIndex;
-		double targetPositionArray[4] = { 0, 0, 0, 0 };
+		double targetPositionArray[4] = { 0, 0, 0, 1 };
 		double targetVelocityArray[3] = { 0, 0, 0 };
 		int targetPositionSize = 0;
 		int targetVelocitySize = 0;
-
-		b3PhysicsClientHandle sm = 0;
 		PyObject* targetPositionObj = 0;
 		PyObject* targetVelocityObj = 0;
 			
@@ -8634,6 +8808,56 @@ static PyObject* pybullet_invertTransform(PyObject* self,
 	return NULL;
 }

+
+static PyObject* pybullet_getQuaternionSlerp(PyObject* self,	PyObject* args, PyObject* keywds)
+{
+	PyObject* quatStartObj;
+	PyObject* quatEndObj;
+	double quatStart[4];
+	double quatEnd[4];
+	double interpolationFraction;
+	int physicsClientId = 0;
+	int hasQuatStart = 0;
+	int hasQuatEnd = 0;
+
+	static char* kwlist[] = { "quaternionStart", "quaternionEnd", "interpolationFraction", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOd|i", kwlist, &quatStartObj, &quatEndObj, &interpolationFraction, &physicsClientId))
+	{
+		return NULL;
+	}
+
+	if (quatStartObj)
+	{
+		hasQuatStart = pybullet_internalSetVector4d(quatStartObj, quatStart);
+	}
+
+	if (quatEndObj)
+	{
+		hasQuatEnd = pybullet_internalSetVector4d(quatEndObj, quatEnd);
+	}
+	if (hasQuatStart && hasQuatEnd)
+	{
+		double quatOut[4];
+		b3QuaternionSlerp(quatStart, quatEnd, interpolationFraction, quatOut);
+		{
+			PyObject* pylist;
+			int i;
+			pylist = PyTuple_New(4);
+			for (i = 0; i < 4; i++)
+				PyTuple_SetItem(pylist, i, PyFloat_FromDouble(quatOut[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;
+}
+
+
 /// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
 /// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
 static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
@@ -9750,6 +9974,11 @@ static PyMethodDef SpamMethods[] = {
 	 "Set a single joint motor control mode and desired target value. There is "
 	 "no immediate state change, stepSimulation will process the motors."},

+	 { "setJointMotorControlMultiDof", (PyCFunction)pybullet_setJointMotorControlMultiDof, METH_VARARGS | METH_KEYWORDS,
+		"Set a single joint motor control mode and desired target value. There is "
+		"no immediate state change, stepSimulation will process the motors."
+		"This method sets multi-degree-of-freedom motor such as the spherical joint motor." },
+
 	{"setJointMotorControlArray", (PyCFunction)pybullet_setJointMotorControlArray, METH_VARARGS | METH_KEYWORDS,
 	 "Set an array of motors control mode and desired target value. There is "
 	 "no immediate state change, stepSimulation will process the motors."
@@ -9883,6 +10112,9 @@ static PyMethodDef SpamMethods[] = {
 	{"getMatrixFromQuaternion", (PyCFunction)pybullet_getMatrixFromQuaternion, METH_VARARGS | METH_KEYWORDS,
 	 "Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"},

+	 { "getQuaternionSlerp", (PyCFunction)pybullet_getQuaternionSlerp, METH_VARARGS | METH_KEYWORDS,
+	 "Compute the spherical interpolation given a start and end quaternion and an interpolation value in range [0..1]" },
+
 	{"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS | METH_KEYWORDS,
 	 "Given an object id, joint positions, joint velocities and joint "
 	 "accelerations, compute the joint forces using Inverse Dynamics"},