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

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];
+}

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
 }

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:

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;
 					}
@@ -8039,7 +8129,6 @@ 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;
 			}

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

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):
+
+once=True
 p.getCameraImage(320,200)
-	frame = int(p.readUserDebugParameter(frameId))
+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]]
+	#	once=False
-	neckRot = [frameData[13],frameData[14],frameData[15],frameData[12]]
+	chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
-	rightHipRot = [frameData[17],frameData[18],frameData[19],frameData[16]]
+	chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
-	rightKneeRot = [frameData[20]]
+	chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
-	rightAnkleRot = [frameData[22],frameData[23],frameData[24],frameData[21]]
+	
-	rightShoulderRot = [frameData[26],frameData[27],frameData[28],frameData[25]]
+	neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
-	rightElbowRot = [frameData[29]]
+	neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
-	leftHipRot = [frameData[31],frameData[32],frameData[33],frameData[30]]
+	neckRot =  p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
-	leftKneeRot = [frameData[34]]
+	
-	leftAnkleRot = [frameData[36],frameData[37],frameData[38],frameData[35]]
+	rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
-	leftShoulderRot = [frameData[40],frameData[41],frameData[42],frameData[39]]
+	rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
-	leftElbowRot = [frameData[43]]
+	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:
+	p.stepSimulation()
-		for i in range(len(paramIds)):
+	#time.sleep(1./240.)
-			c = paramIds[i]
-			targetPos = p.readUserDebugParameter(c)
-			
-			p.setJointMotorControl2(humanoid,jointIds[i],
-				p.POSITION_CONTROL,targetPos, force=5*240.)
-	time.sleep(0.1)

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"},

setup.py

@@ -317,6 +317,7 @@ sources = ["examples/pybullet/pybullet.c"]\
 +["src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp"]\
 +["src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp"]\
 +["src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp"]\
++["src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp"]\
 +["src/BulletDynamics/Vehicle/btRaycastVehicle.cpp"]\
 +["src/BulletDynamics/Vehicle/btWheelInfo.cpp"]\
 +["src/BulletDynamics/Character/btKinematicCharacterController.cpp"]\

src/BulletDynamics/CMakeLists.txt

@@ -41,6 +41,7 @@ SET(BulletDynamics_SRCS
 	Featherstone/btMultiBodyMLCPConstraintSolver.cpp
 	Featherstone/btMultiBodyPoint2Point.cpp
 	Featherstone/btMultiBodySliderConstraint.cpp
+	Featherstone/btMultiBodySphericalJointMotor.cpp
 	MLCPSolvers/btDantzigLCP.cpp
 	MLCPSolvers/btMLCPSolver.cpp
 	MLCPSolvers/btLemkeAlgorithm.cpp

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -37,9 +37,7 @@ const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m
 const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
 }  // namespace
 
-namespace
+void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
-{
-void SpatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
 	const btVector3 &displacement,       // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
 	const btVector3 &top_in,             // top part of input vector
 	const btVector3 &bottom_in,          // bottom part of input vector
@@ -50,6 +48,10 @@ void SpatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in
 	bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
 }
 
+namespace
+{
+
+
 #if 0
     void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
                                  const btVector3 &displacement,
@@ -330,6 +332,9 @@ void btMultiBody::setupPlanar(int i,
 	m_links[i].updateCacheMultiDof();
 	//
 	updateLinksDofOffsets();
+
+	m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized());
+	m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized());
 }
 
 void btMultiBody::finalizeMultiDof()
@@ -540,35 +545,26 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
 {
 	int num_links = getNumLinks();
 	// Calculates the velocities of each link (and the base) in its local frame
-	omega[0] = quatRotate(m_baseQuat, getBaseOmega());
+	const btQuaternion& base_rot = getWorldToBaseRot();
-	vel[0] = quatRotate(m_baseQuat, getBaseVel());
+	omega[0] = quatRotate(base_rot, getBaseOmega());
+	vel[0] = quatRotate(base_rot, getBaseVel());
 
 	for (int i = 0; i < num_links; ++i)
 	{
-		const int parent = m_links[i].m_parent;
+		const btMultibodyLink& link = getLink(i);
+		const int parent = link.m_parent;
 
 		// transform parent vel into this frame, store in omega[i+1], vel[i+1]
-		SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
+		spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.m_cachedRVector,
 			omega[parent + 1], vel[parent + 1],
 			omega[i + 1], vel[i + 1]);
 
 		// now add qidot * shat_i
-		//only supported for revolute/prismatic joints, todo: spherical and planar joints
+		const btScalar* jointVel = getJointVelMultiDof(i);
-		switch (m_links[i].m_jointType)
+		for (int dof = 0; dof < link.m_dofCount; ++dof)
 		{
-			case btMultibodyLink::ePrismatic:
+			omega[i + 1] += jointVel[dof] * link.getAxisTop(dof);
-			case btMultibodyLink::eRevolute:
+			vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof);
-			{
-				btVector3 axisTop = m_links[i].getAxisTop(0);
-				btVector3 axisBottom = m_links[i].getAxisBottom(0);
-				btScalar jointVel = getJointVel(i);
-				omega[i + 1] += jointVel * axisTop;
-				vel[i + 1] += jointVel * axisBottom;
-				break;
-			}
-			default:
-			{
-			}
 		}
 	}
 }

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -600,6 +600,15 @@ public:
 		m_userIndex2 = index;
 	}
 
+	static void spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
+		const btVector3 &displacement,     // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
+		const btVector3 &top_in,       // top part of input vector
+		const btVector3 &bottom_in,    // bottom part of input vector
+		btVector3 &top_out,         // top part of output vector
+		btVector3 &bottom_out);      // bottom part of output vector
+
+
+
 private:
 	btMultiBody(const btMultiBody &);     // not implemented
 	void operator=(const btMultiBody &);  // not implemented

src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp

@@ -0,0 +1,172 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySphericalJointMotor.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "LinearMath/btTransformUtil.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+
+btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true),
+	m_desiredVelocity(0, 0, 0),
+	m_desiredPosition(0,0,0,1),
+	m_kd(1.),
+	m_kp(0.2),
+	m_erp(1),
+	m_rhsClamp(SIMD_INFINITY)
+{
+
+	m_maxAppliedImpulse = maxMotorImpulse;
+}
+
+
+void btMultiBodySphericalJointMotor::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+
+btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor()
+{
+}
+
+int btMultiBodySphericalJointMotor::getIslandIdA() const
+{
+	if (this->m_linkA < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodySphericalJointMotor::getIslandIdB() const
+{
+	if (m_linkB < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodySphericalJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+												 btMultiBodyJacobianData& data,
+												 const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+	
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	const btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+
+	
+	btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
+	
+	btQuaternion desiredQuat = m_desiredPosition;
+	btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
+		m_bodyA->getJointPosMultiDof(m_linkA)[1],
+		m_bodyA->getJointPosMultiDof(m_linkA)[2],
+		m_bodyA->getJointPosMultiDof(m_linkA)[3]);
+
+btQuaternion relRot = currentQuat.inverse() * desiredQuat;
+	btVector3 angleDiff;
+	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
+
+
+
+	for (int row = 0; row < getNumRows(); row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+		int dof = row;
+		
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar desiredVelocity = this->m_desiredVelocity[row];
+		
+		btScalar velocityError = desiredVelocity - currentVelocity;
+
+		btMatrix3x3 frameAworld;
+		frameAworld.setIdentity();
+		frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+		btScalar posError = 0;
+		{
+			btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eSpherical:
+				{
+					btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
+					posError = m_kp*angleDiff[row % 3];
+					fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+						btVector3(0,0,0), dummy, dummy,
+						posError,
+						infoGlobal,
+						-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+					constraintRow.m_orgConstraint = this;
+					constraintRow.m_orgDofIndex = row;
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+	}
+}

src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h

@@ -0,0 +1,77 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
+#define BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
+
+#include "btMultiBodyConstraint.h"
+struct btSolverInfo;
+
+class btMultiBodySphericalJointMotor : public btMultiBodyConstraint
+{
+protected:
+	btVector3 m_desiredVelocity;
+	btQuaternion m_desiredPosition;
+	btScalar m_kd;
+	btScalar m_kp;
+	btScalar m_erp;
+	btScalar m_rhsClamp;  //maximum error
+
+public:
+	btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse);
+	
+	virtual ~btMultiBodySphericalJointMotor();
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = kd;
+	}
+
+	virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp = 1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = kp;
+	}
+
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+	virtual btScalar getErp() const
+	{
+		return m_erp;
+	}
+	virtual void setRhsClamp(btScalar rhsClamp)
+	{
+		m_rhsClamp = rhsClamp;
+	}
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+};
+
+#endif  //BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H