Merge pull request #1994 from erwincoumans/master

Preparation to replicate parts of DeepMimic in plain PyBullet
2018-11-17 17:53:50 -08:00
parent 70f94ddd81 8c0b4d9e12
commit f2a98777e7
17 changed files with 695 additions and 11 deletions
--- a/examples/CommonInterfaces/CommonFileIOInterface.h
+++ b/examples/CommonInterfaces/CommonFileIOInterface.h
@@ -22,7 +22,7 @@ struct CommonFileIOInterface
 	virtual bool findResourcePath(const char* fileName,  char* resourcePathOut, int resourcePathMaxNumBytes)=0;
 	virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)=0;
 	virtual int getFileSize(int fileHandle)=0;
-
+	virtual void enableFileCaching(bool enable) = 0;
 };

 #endif //COMMON_FILE_IO_INTERFACE_H
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/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];
+}
+
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/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
 }
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -8004,6 +8004,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 	if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_FILE_CACHING)
 	{
 		b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching);
+		m_data->m_pluginManager.getFileIOInterface()->enableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching!=0);
 	}

 	SharedMemoryStatus& serverCmd = serverStatusOut;
--- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp
+++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp
@@ -283,6 +283,11 @@ struct InMemoryFileIO : public CommonFileIOInterface
 		}
 		return 0;
 	}
+
+	virtual void enableFileCaching(bool enable)
+	{
+		(void)enable;
+	}
 };

 struct WrapperFileIO : public CommonFileIOInterface
@@ -292,10 +297,12 @@ struct WrapperFileIO : public CommonFileIOInterface

 	WrapperFileHandle m_wrapperFileHandles[B3_MAX_FILEIO_INTERFACES];
 	InMemoryFileIO m_cachedFiles;
+	bool m_enableFileCaching;

 	WrapperFileIO()
 		:CommonFileIOInterface(0,0),
-		m_numWrapperInterfaces(0)
+		m_numWrapperInterfaces(0),
+		m_enableFileCaching(true)
 	{
 		for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
 		{
@@ -406,7 +413,10 @@ struct WrapperFileIO : public CommonFileIOInterface
 							}

 							//potentially register a zero byte file, or files that only can be read partially
+							if (m_enableFileCaching)
+							{
 								m_cachedFiles.registerFile(fileName, buffer, fileSize);
+							}
 							childFileIO->fileClose(childHandle);
 							break;
 						}
@@ -525,6 +535,15 @@ struct WrapperFileIO : public CommonFileIOInterface
 		return numBytes;
 	}

+	virtual void enableFileCaching(bool enable)
+	{
+		m_enableFileCaching = enable;
+		if (!enable)
+		{
+			m_cachedFiles.clearCache();
+		}
+	}
+
 };


--- a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h
+++ b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h
@@ -253,4 +253,9 @@ struct ZipFileIO : public CommonFileIOInterface
 		return size;
 	}

+	virtual void enableFileCaching(bool enable)
+	{
+		(void)enable;
+	}
+
 };
--- a/examples/Utils/b3BulletDefaultFileIO.h
+++ b/examples/Utils/b3BulletDefaultFileIO.h
@@ -186,6 +186,11 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface
 		}
 		return size;
 	}
+
+	virtual void enableFileCaching(bool enable)
+	{
+		(void) enable;
+	}
 };

 #endif //B3_BULLET_DEFAULT_FILE_IO_H
--- a/examples/pybullet/examples/fileIOPlugin.py
+++ b/examples/pybullet/examples/fileIOPlugin.py
@@ -0,0 +1,20 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+fileIO = p.loadPlugin("fileIOPlugin")
+if (fileIO>=0):
+	p.executePluginCommand(fileIO, "pickup.zip", [p.AddFileIOAction, p.ZipFileIO])
+	objs= p.loadSDF("pickup/model.sdf")
+	dobot =objs[0]
+	p.changeVisualShape(dobot,-1,rgbaColor=[1,1,1,1])
+		
+else:
+	print("fileIOPlugin is disabled.")
+
+
+p.setPhysicsEngineParameter(enableFileCaching=False)
+
+while (1):
+	p.stepSimulation()
+	time.sleep(1./240.)
--- a/examples/pybullet/examples/humanoidMotionCapture.py
+++ b/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)
--- a/examples/pybullet/examples/pickup.zip
+++ b/examples/pybullet/examples/pickup.zip
--- a/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf
+++ b/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>
--- a/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py
+++ b/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)
+    
--- a/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py
+++ b/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.)
+
--- a/examples/pybullet/gym/pybullet_envs/mimic/motion_capture_data.py
+++ b/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]
+ 
--- a/examples/pybullet/pybullet.c
+++ b/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"},
--- a/setup.py
+++ b/setup.py
@@ -585,7 +585,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:

 setup(
 	name = 'pybullet',
-	version='2.3.7',
+	version='2.3.9',
 	description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
 	long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
 	url='https://github.com/bulletphysics/bullet3',
--- a/src/LinearMath/btThreads.cpp
+++ b/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;