Expose PyBullet.calculateVelocityQuaternion, getAxisAngleFromQuaternion, getQuaternionFromAxisAngle, getDifferenceQuaternion

Add preparation for DeepMimic humanoid environment, replicating parts of https://github.com/xbpeng/DeepMimic
Loading humanoid.urdf and applying motion action: examples/pybullet/gym/pybullet_envs/mimic/humanoid.py
Loading MotionCapture data: examples/pybullet/gym/pybullet_envs/mimic/motion_capture_data.py
Little test: examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -4,6 +4,7 @@
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Common/b3Matrix3x3.h"
 #include "Bullet3Common/b3Transform.h"
+#include "Bullet3Common/b3TransformUtil.h"
 
 #include <string.h>
 #include "SharedMemoryCommands.h"
@@ -5264,3 +5265,46 @@ B3_SHARED_API void b3QuaternionSlerp(const double startQuat[/*4*/], const double
 	outOrn[2] = result[2];
 	outOrn[3] = result[3];
 }
+
+B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/])
+{
+	b3Quaternion start(startQuat[0], startQuat[1], startQuat[2], startQuat[3]);
+	b3Quaternion end(endQuat[0], endQuat[1], endQuat[2], endQuat[3]);
+	b3Vector3 pos=b3MakeVector3(0, 0, 0);
+	b3Vector3 linVel, angVel;
+	b3TransformUtil::calculateVelocityQuaternion(pos, pos, start, end, deltaTime, linVel, angVel);
+	angVelOut[0] = angVel[0];
+	angVelOut[1] = angVel[1];
+	angVelOut[2] = angVel[2];
+}
+
+B3_SHARED_API void b3GetQuaternionFromAxisAngle(const double axis[/*3*/], double angle, double outQuat[/*4*/])
+{
+	b3Quaternion quat(b3MakeVector3(axis[0], axis[1], axis[2]), angle);
+	outQuat[0] = quat[0];
+	outQuat[1] = quat[1];
+	outQuat[2] = quat[2];
+	outQuat[3] = quat[3];
+}
+B3_SHARED_API void b3GetAxisAngleFromQuaternion(const double quat[/*4*/], double axis[/*3*/], double* angle)
+{
+	b3Quaternion q(quat[0], quat[1], quat[2], quat[3]);
+	b3Vector3 ax = q.getAxis();
+	axis[0] = ax[0];
+	axis[1] = ax[1];
+	axis[2] = ax[2];
+	*angle = q.getAngle();
+}
+
+B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], const double endQuat[/*4*/], double outOrn[/*4*/])
+{
+	b3Quaternion orn0(startQuat[0], startQuat[1], startQuat[2], startQuat[3]);
+	b3Quaternion orn1a(endQuat[0], endQuat[1], endQuat[2], endQuat[3]);
+	b3Quaternion orn1 = orn0.nearest(orn1a);
+	b3Quaternion dorn = orn1 * orn0.inverse();
+	outOrn[0] = dorn[0];
+	outOrn[1] = dorn[1];
+	outOrn[2] = dorn[2];
+	outOrn[3] = dorn[3];
+}
+

examples/SharedMemory/PhysicsClientC_API.h

@@ -571,7 +571,7 @@ extern "C"
 	// Sets the number of threads to use to compute the ray intersections for the batch. Specify 0 to let Bullet decide, 1 (default) for single core execution, 2 or more to select the number of threads to use.
 	B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle commandHandle, int numThreads);
 	//max num rays for b3RaycastBatchAddRay is MAX_RAY_INTERSECTION_BATCH_SIZE
-	B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]);
+	B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[/*3*/], const double rayToWorld[/*3*/]);
 	//max num rays for b3RaycastBatchAddRays is MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING
 	B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorld, const double* rayToWorld, int numRays);
 	B3_SHARED_API void b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle commandHandle, int parentObjectUniqueId, int parentLinkIndex);
@@ -635,6 +635,12 @@ 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*/]);
+	B3_SHARED_API void b3GetQuaternionFromAxisAngle(const double axis[/*3*/], double angle, double outQuat[/*4*/]);
+	B3_SHARED_API void b3GetAxisAngleFromQuaternion(const double quat[/*4*/], double axis[/*3*/], double* angle);
+	B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], const double endQuat[/*4*/], double outOrn[/*4*/]);
+	B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/]);
+
+	
 
 #ifdef __cplusplus
 }

examples/pybullet/examples/humanoidMotionCapture.py

@@ -16,7 +16,7 @@ p.setPhysicsEngineParameter(numSolverIterations=200)
 #path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
 path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt"
 
-p.loadURDF("plane.urdf",[0,0,-1])
+p.loadURDF("plane.urdf",[0,0,-0.03])
 print("path = ", path)
 with open(path, 'r') as f:
     motion_dict = json.load(f)

examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf

@@ -1,8 +1,8 @@
 <robot name="dumpUrdf">
-	<link name="base" >
+	<link name="root" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0 0 0" />
-			 <mass value = "0.000000" />
+			 <mass value = "0.00100" />
 			<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
 		</inertial>
 	</link>
@@ -20,7 +20,7 @@
 		</collision>
 	</link>
 	<joint name="joint0" type="fixed" >
-		<parent link = "base" />
+		<parent link = "root" />
 		<child link="link0" />
 				<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
 	</joint>

examples/pybullet/gym/pybullet_envs/mimic/humanoid.py

@@ -0,0 +1,289 @@
+import os,  inspect
+import math
+currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
+parentdir = os.path.dirname(os.path.dirname(currentdir))
+os.sys.path.insert(0,parentdir)
+
+class HumanoidPose(object):
+  def __init__(self):
+    pass
+  
+  def Reset(self):
+    
+    self._basePos = [0,0,0]
+    self._baseLinVel = [0,0,0]
+    self._baseOrn = [0,0,0,1]
+    self._baseAngVel = [0,0,0]
+    
+    self._chestRot = [0,0,0,1]
+    self._chestVel = [0,0,0]
+    self._neckRot = [0,0,0,1]
+    self._neckVel = [0,0,0]
+    
+    self._rightHipRot = [0,0,0,1]
+    self._rightHipVel = [0,0,0]
+    self._rightKneeRot = [0]
+    self._rightKneeVel = [0]
+    self._rightAnkleRot = [0,0,0,1]
+    self._rightAnkleVel = [0,0,0]
+    
+    self._rightShoulderRot = [0,0,0,1]
+    self._rightShoulderVel = [0,0,0]
+    self._rightElbowRot = [0]
+    self._rightElbowVel = [0]
+
+    self._leftHipRot = [0,0,0,1]
+    self._leftHipVel = [0,0,0]
+    self._leftKneeRot = [0]
+    self._leftKneeVel = [0]
+    self._leftAnkleRot = [0,0,0,1]
+    self._leftAnkleVel = [0,0,0]
+    
+    self._leftShoulderRot = [0,0,0,1]
+    self._leftShoulderVel = [0,0,0]
+    self._leftElbowRot = [0]
+    self._leftElbowVel = [0]
+
+  def ComputeLinVel(self,posStart, posEnd, deltaTime):
+    vel = [(posEnd[0]-posStart[0])/deltaTime,(posEnd[1]-posStart[1])/deltaTime,(posEnd[2]-posStart[2])/deltaTime]
+    return vel
+  
+  def ComputeAngVel(self,ornStart, ornEnd, deltaTime, bullet_client):
+    dorn = bullet_client.getDifferenceQuaternion(ornStart,ornEnd)
+    axis,angle = bullet_client.getAxisAngleFromQuaternion(dorn)
+    angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
+    return angVel
+    
+  def NormalizeQuaternion(self, orn):
+    length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2]+orn[3]*orn[3]
+    if (length2>0):
+    	length = math.sqrt(length2)
+    print("Normalize? length=",length)
+    
+    
+  def PostProcessMotionData(self, frameData):
+    baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
+    self.NormalizeQuaternion(baseOrn1Start)
+    chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
+    
+    neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
+    rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
+    rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
+    rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]]
+    leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
+    leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
+    leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
+    
+  	
+  def Slerp(self, frameFraction, frameData, frameDataNext,bullet_client ):
+    keyFrameDuration = frameData[0]
+    basePos1Start = [frameData[1],frameData[2],frameData[3]]
+    basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
+    self._basePos = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), 
+      basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), 
+      basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
+    self._baseLinVel = self.ComputeLinVel(basePos1Start,basePos1End, keyFrameDuration)
+    baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
+    baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
+    self._baseOrn = bullet_client.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
+    self._baseAngVel = self.ComputeAngVel(baseOrn1Start,baseOrn1Next, keyFrameDuration, bullet_client)
+    
+    ##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)
+
+    chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
+    chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
+    self._chestRot = bullet_client.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
+    self._chestVel = self.ComputeAngVel(chestRotStart,chestRotEnd,keyFrameDuration,bullet_client)
+    
+    neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
+    neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
+    self._neckRot =  bullet_client.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
+    self._neckVel = self.ComputeAngVel(neckRotStart,neckRotEnd,keyFrameDuration,bullet_client)
+    
+    rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
+    rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
+    self._rightHipRot = bullet_client.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction)
+    self._rightHipVel = self.ComputeAngVel(rightHipRotStart,rightHipRotEnd,keyFrameDuration,bullet_client)
+    
+    rightKneeRotStart = [frameData[20]]
+    rightKneeRotEnd = [frameDataNext[20]]
+    self._rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])]
+    self._rightKneeVel = (rightKneeRotEnd[0]-rightKneeRotStart[0])/keyFrameDuration
+    
+    rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
+    rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]]
+    self._rightAnkleRot =  bullet_client.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction)
+    self._rightAnkleVel = self.ComputeAngVel(rightAnkleRotStart,rightAnkleRotEnd,keyFrameDuration,bullet_client)
+      
+    rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]]
+    rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
+    self._rightShoulderRot = bullet_client.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
+    self._rightShoulderVel = self.ComputeAngVel(rightShoulderRotStart,rightShoulderRotEnd, keyFrameDuration,bullet_client)
+    
+    rightElbowRotStart = [frameData[29]]
+    rightElbowRotEnd = [frameDataNext[29]]
+    self._rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])]
+    self._rightElbowVel = (rightElbowRotEnd[0]-rightElbowRotStart[0])/keyFrameDuration
+    
+    leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
+    leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]]
+    self._leftHipRot = bullet_client.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
+    self._leftHipVel = self.ComputeAngVel(leftHipRotStart, leftHipRotEnd,keyFrameDuration,bullet_client)
+    
+    leftKneeRotStart = [frameData[34]]
+    leftKneeRotEnd = [frameDataNext[34]]
+    self._leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ]
+    self._leftKneeVel = (leftKneeRotEnd[0]-leftKneeRotStart[0])/keyFrameDuration
+    
+    leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
+    leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
+    self._leftAnkleRot = bullet_client.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
+    self._leftAnkleVel = self.ComputeAngVel(leftAnkleRotStart,leftAnkleRotEnd,keyFrameDuration,bullet_client)
+
+    leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
+    leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]]
+    self._leftShoulderRot = bullet_client.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction)
+    self._leftShoulderVel = self.ComputeAngVel(leftShoulderRotStart,leftShoulderRotEnd,keyFrameDuration,bullet_client)
+
+    leftElbowRotStart = [frameData[43]]
+    leftElbowRotEnd = [frameDataNext[43]]
+    self._leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])]
+    self._leftElbowVel = (leftElbowRotEnd[0]-leftElbowRotStart[0])/keyFrameDuration
+        
+
+class Humanoid(object):
+  def __init__(self, pybullet_client, motion_data, baseShift):
+    """Constructs a humanoid and reset it to the initial states.
+    Args:
+      pybullet_client: The instance of BulletClient to manage different
+        simulations.
+    """
+    self._baseShift = baseShift
+    self._pybullet_client = pybullet_client
+    self._motion_data = motion_data
+    self._humanoid = self._pybullet_client.loadURDF(
+      "humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25)
+    
+    pose = HumanoidPose()
+    
+    for i in range (self._motion_data.NumFrames()-1):
+      frameData = self._motion_data._motion_data['Frames'][i]
+      pose.PostProcessMotionData(frameData)
+    
+    self._pybullet_client.resetBasePositionAndOrientation(self._humanoid,self._baseShift,[0,0,0,1])
+    self._pybullet_client.changeDynamics(self._humanoid, -1, linearDamping=0, angularDamping=0)
+    for j in range (self._pybullet_client.getNumJoints(self._humanoid)):
+      self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0)
+      self._pybullet_client.changeVisualShape(self._humanoid, j , rgbaColor=[1,1,1,1])
+    
+    self._initial_state = self._pybullet_client.saveState()
+    self._allowed_body_parts=[11,14]
+    self.Reset()
+    
+  def Reset(self):
+    self._pybullet_client.restoreState(self._initial_state)
+    self.SetFrameTime(0)
+    pose = self.InitializePoseFromMotionData()
+    self.ApplyPose(pose, True)
+
+
+  def SetFrameTime(self, t):
+    self._frameTime = t
+    self._frame = int(self._frameTime)
+    self._frameNext = self._frame+1
+    if (self._frameNext >=  self._motion_data.NumFrames()):
+      self._frameNext = self._frame
+    self._frameFraction = self._frameTime - self._frame
+
+  def Terminates(self):
+    #check if any non-allowed body part hits the ground
+    terminates=False
+    pts = self._pybullet_client.getContactPoints()
+    for p in pts:
+      part = -1
+      if (p[1]==self._humanoid):
+        part=p[3]
+      if (p[2]==self._humanoid):
+        part=p[4]
+      if (part not in self._allowed_body_parts):
+        terminates=True
+      
+    return terminates
+        
+        
+      
+    
+    
+  def InitializePoseFromMotionData(self):
+    frameData = self._motion_data._motion_data['Frames'][self._frame]
+    frameDataNext = self._motion_data._motion_data['Frames'][self._frameNext]
+    pose = HumanoidPose()
+    pose.Slerp(self._frameFraction, frameData, frameDataNext, self._pybullet_client)
+    return pose
+    
+
+  def ApplyAction(self, action):
+    #turn action into pose
+    pose = HumanoidPose()
+    #todo: convert action vector into pose
+    #convert from angle-axis to quaternion for spherical joints
+    
+    initializeBase = False
+    self.ApplyPose(pose, initializeBase)
+    
+  def ApplyPose(self, pose, initializeBase):
+    if (initializeBase):
+      self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,0,0,1])
+      basePos=[pose._basePos[0]+self._baseShift[0],pose._basePos[1]+self._baseShift[1],pose._basePos[2]+self._baseShift[2]]
+      self._pybullet_client.resetBasePositionAndOrientation(self._humanoid,
+        basePos, pose._baseOrn)
+      self._pybullet_client.resetBaseVelocity(self._humanoid, pose._baseLinVel, pose._baseAngVel)
+      print("resetBaseVelocity=",pose._baseLinVel)
+    else:
+      self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,1,1,1])
+    maxForce=1000
+    kp=0.8
+    chest=1
+    neck=2
+    rightShoulder=3
+    rightElbow=4
+    leftShoulder=6
+    leftElbow = 7
+    rightHip = 9
+    rightKnee=10
+    rightAnkle=11
+    leftHip = 12
+    leftKnee=13
+    leftAnkle=14
+    controlMode = self._pybullet_client.POSITION_CONTROL
+    
+    if (initializeBase):
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,chest,pose._chestRot)
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,neck,pose._neckRot)
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightHip,pose._rightHipRot)
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightKnee,pose._rightKneeRot)
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightAnkle,pose._rightAnkleRot)
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightShoulder,pose._rightShoulderRot)
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightElbow, pose._rightElbowRot)
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftHip, pose._leftHipRot)
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftKnee, pose._leftKneeRot)
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftAnkle, pose._leftAnkleRot)
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftShoulder, pose._leftShoulderRot)
+      self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftElbow, pose._leftElbowRot)
+    
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=maxForce)
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=maxForce)
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=maxForce)
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=maxForce)
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=maxForce)
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=maxForce)
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=maxForce)
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=maxForce)
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=maxForce)
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=maxForce)
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=maxForce)
+    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=maxForce)
+    

examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py

@@ -0,0 +1,76 @@
+import os,  inspect
+currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
+parentdir = os.path.dirname(os.path.dirname(currentdir))
+os.sys.path.insert(0,parentdir)
+
+from pybullet_envs.deep_mimic.humanoid import Humanoid
+from pybullet_utils.bullet_client import BulletClient
+from pybullet_envs.deep_mimic.motion_capture_data import MotionCaptureData
+import pybullet_data
+import pybullet
+import time
+import random
+
+bc = BulletClient(connection_mode=pybullet.GUI)
+bc.setAdditionalSearchPath(pybullet_data.getDataPath())
+bc.configureDebugVisualizer(bc.COV_ENABLE_Y_AXIS_UP,1)
+bc.setGravity(0,-9.8,0)
+motion=MotionCaptureData()
+
+motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"#humanoid3d_spinkick.txt"#humanoid3d_backflip.txt"
+motion.Load(motionPath)
+print("numFrames = ", motion.NumFrames())
+frameTimeId= bc.addUserDebugParameter("frameTime",0,motion.NumFrames()-1.1,0)
+
+y2zOrn = bc.getQuaternionFromEuler([-1.57,0,0])
+bc.loadURDF("plane.urdf",[0,-0.08,0], y2zOrn)
+
+humanoid = Humanoid(bc, motion,[0,0,0])
+
+frameTime = 0
+keyFrameDuration = motion.KeyFrameDuraction()
+print("keyFrameDuration=",keyFrameDuration)
+#for i in range (50):
+# bc.stepSimulation()
+
+stage = 0
+
+
+
+
+
+def Reset(humanoid):
+	global frameTime
+	humanoid.Reset()
+	frameTime = 0#random.randint(0,motion.NumFrames()-2)
+	print("RESET frametime=",frameTime)
+	humanoid.SetFrameTime(frameTime)
+	pose = humanoid.InitializePoseFromMotionData()
+	humanoid.ApplyPose(pose, True)
+
+
+Reset(humanoid)
+bc.stepSimulation()
+
+while (1):
+  #frameTime = bc.readUserDebugParameter(frameTimeId)
+  #print("keyFrameDuration=",keyFrameDuration)
+  dt = (1./240.)/keyFrameDuration
+  #print("dt=",dt)
+  frameTime += dt
+  if (frameTime >= (motion.NumFrames())-1.1):
+    frameTime = motion.NumFrames()-1.1
+  #print("frameTime=", frameTime)
+  humanoid.SetFrameTime(frameTime)
+
+  pose = humanoid.InitializePoseFromMotionData()
+
+  #humanoid.ApplyPose(pose, False)#False, False)
+  if (humanoid.Terminates()):
+  	Reset(humanoid)
+  	
+  bc.stepSimulation()
+  
+  
+  time.sleep(1./240.)
+

examples/pybullet/gym/pybullet_envs/mimic/motion_capture_data.py

@@ -0,0 +1,19 @@
+import json
+
+class MotionCaptureData(object):
+  def __init__(self):
+    self.Reset()
+
+  def Reset(self):
+    self._motion_data = []
+
+  def Load(self, path):
+    with open(path, 'r') as f:
+      self._motion_data = json.load(f)
+
+  def NumFrames(self):
+    return  len(self._motion_data['Frames'])
+
+  def KeyFrameDuraction(self):
+  	return self._motion_data['Frames'][0][0]
+ 

examples/pybullet/pybullet.c

@@ -8809,6 +8809,55 @@ static PyObject* pybullet_invertTransform(PyObject* self,
 }
 
 
+static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	PyObject* quatStartObj;
+	PyObject* quatEndObj;
+	double quatStart[4];
+	double quatEnd[4];
+	double deltaTime;
+	int physicsClientId = 0;
+	int hasQuatStart = 0;
+	int hasQuatEnd = 0;
+
+	static char* kwlist[] = { "quaternionStart", "quaternionEnd", "deltaTime", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOd|i", kwlist, &quatStartObj, &quatEndObj, &deltaTime, &physicsClientId))
+	{
+		return NULL;
+	}
+
+	if (quatStartObj)
+	{
+		hasQuatStart = pybullet_internalSetVector4d(quatStartObj, quatStart);
+	}
+
+	if (quatEndObj)
+	{
+		hasQuatEnd = pybullet_internalSetVector4d(quatEndObj, quatEnd);
+	}
+	if (hasQuatStart && hasQuatEnd)
+	{
+		double angVelOut[3];
+		b3CalculateVelocityQuaternion(quatStart, quatEnd, deltaTime, angVelOut);
+		{
+			PyObject* pylist;
+			int i;
+			pylist = PyTuple_New(3);
+			for (i = 0; i < 3; i++)
+				PyTuple_SetItem(pylist, i, PyFloat_FromDouble(angVelOut[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_getQuaternionSlerp(PyObject* self,	PyObject* args, PyObject* keywds)
 {
 	PyObject* quatStartObj;
@@ -8858,6 +8907,145 @@ static PyObject* pybullet_getQuaternionSlerp(PyObject* self,	PyObject* args, PyO
 }
 
 
+static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int physicsClientId = 0;
+	PyObject* quatObj;
+	double quat[4];
+	int hasQuat = 0;
+
+	static char* kwlist[] = { "quaternion", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "O|i", kwlist, &quatObj, &physicsClientId))
+	{
+		return NULL;
+	}
+
+	if (quatObj)
+	{
+		hasQuat = pybullet_internalSetVector4d(quatObj, quat);
+	}
+
+	if (hasQuat)
+	{
+		double axis[3];
+		double angle;
+		b3GetAxisAngleFromQuaternion(quat, axis, &angle);
+		{
+			PyObject* pylist2 = PyTuple_New(2);
+			{
+				PyObject* axislist;
+				int i;
+				axislist = PyTuple_New(3);
+				for (i = 0; i < 3; i++)
+					PyTuple_SetItem(axislist, i, PyFloat_FromDouble(axis[i]));
+				PyTuple_SetItem(pylist2, 0, axislist);
+			}
+			PyTuple_SetItem(pylist2, 1, PyFloat_FromDouble(angle));
+			
+			return pylist2;
+		}
+	}
+	else
+	{
+		PyErr_SetString(SpamError, "Require a quaternion with 4 components [x,y,z,w].");
+		return NULL;
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+
+static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	PyObject* axisObj;
+	double axis[3];
+	double angle;
+	int physicsClientId = 0;
+	int hasAxis = 0;
+
+	static char* kwlist[] = { "axis", "angle","physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "Od|i", kwlist, &axisObj, &angle,&physicsClientId))
+	{
+		return NULL;
+	}
+
+	if (axisObj)
+	{
+		hasAxis = pybullet_internalSetVectord(axisObj, axis);
+	}
+
+	if (hasAxis)
+	{
+		double quatOut[4];
+		b3GetQuaternionFromAxisAngle(axis, angle, 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 axis [x,y,z] and angle.");
+		return NULL;
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+
+static PyObject* pybullet_getDifferenceQuaternion(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 quatOut[4];
+		b3GetQuaternionDifference(quatStart, quatEnd, 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,
@@ -10115,6 +10303,18 @@ static PyMethodDef SpamMethods[] = {
 	 { "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]" },
 
+	 { "getQuaternionFromAxisAngle", (PyCFunction)pybullet_getQuaternionFromAxisAngle, METH_VARARGS | METH_KEYWORDS,
+		 "Compute the quaternion from axis and angle representation." },
+
+	 { "getAxisAngleFromQuaternion", (PyCFunction)pybullet_getAxisAngleFromQuaternion, METH_VARARGS | METH_KEYWORDS,
+		 "Compute the quaternion from axis and angle representation." },
+
+	 { "getDifferenceQuaternion", (PyCFunction)pybullet_getDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
+		 "Compute the quaternion difference from two quaternions." },
+
+	 { "calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS,
+		 "Compute the angular velocity given start and end quaternion and delta time." },
+
 	{"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"},

src/LinearMath/btThreads.cpp

@@ -241,7 +241,7 @@ struct ThreadsafeCounter
 	}
 };
 
-static btITaskScheduler* gBtTaskScheduler;
+static btITaskScheduler* gBtTaskScheduler=0;
 static int gThreadsRunningCounter = 0;  // useful for detecting if we are trying to do nested parallel-for calls
 static btSpinMutex gThreadsRunningCounterMutex;
 static ThreadsafeCounter gThreadCounter;