Merge pull request #2116 from erwincoumans/master

various fixes
2024-12-14 05:40:05 +00:00 · 2019-02-18 19:18:21 -08:00 · 2019-02-18 19:18:21 -08:00 · fd161fa061
commit fd161fa061
parent 7e5ac33cf1 127cde6ec7
31 changed files with 258 additions and 96 deletions
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@ -5722,7 +5722,7 @@ bool PhysicsServerCommandProcessor::processCollisionFilterCommand(const struct S
 			btCollisionObject* colObj = 0;
 			if (body->m_multiBody)
 			{
-				if (clientCmd.m_collisionFilterArgs.m_linkIndexA)
+				if (clientCmd.m_collisionFilterArgs.m_linkIndexA==-1)
 				{
 					colObj = body->m_multiBody->getBaseCollider();
 				}
@ -7653,6 +7653,15 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 			{
 				mb->setCanSleep(false);
 			}
+			if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableWakeup)
+			{
+				mb->setCanWakeup(true);
+			}
+			if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableWakeup)
+			{
+				mb->setCanWakeup(false);
+			}
+			
 		}

 		if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@ -326,6 +326,8 @@ enum DynamicsActivationState
 	eActivationStateDisableSleeping = 2,
 	eActivationStateWakeUp = 4,
 	eActivationStateSleep = 8,
+	eActivationStateEnableWakeup = 16,
+	eActivationStateDisableWakeup = 32,
 };

 struct b3DynamicsInfo
@ -844,6 +846,7 @@ enum eURDF_Flags
 	URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
 	URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536,
 	URDF_MAINTAIN_LINK_ORDER = 131072,
+	URDF_ENABLE_WAKEUP = 262144,
 };

 enum eUrdfGeomTypes  //sync with UrdfParser UrdfGeomTypes
--- a/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp
+++ b/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp
@ -1839,9 +1839,12 @@ bool PhysXServerCommandProcessor::processForwardDynamicsCommand(const struct Sha
 			float* depthBuffer = 0;
 			int* segmentationMaskBuffer = 0;
 			int startPixelIndex = 0;
+			
 			int width = 1024;
 			int height = 768;
+			m_data->m_pluginManager.getRenderInterface()->getWidthAndHeight(width, height);
 			int numPixelsCopied = 0;
+			

 			m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels,
 				depthBuffer, numRequestedPixels,
--- a/examples/SharedMemory/physx/URDF2PhysX.cpp
+++ b/examples/SharedMemory/physx/URDF2PhysX.cpp
@ -25,7 +25,7 @@
 #include "../../CommonInterfaces/CommonFileIOInterface.h"
 #include "../../Importers/ImportObjDemo/LoadMeshFromObj.h"
 #include "../../Importers/ImportSTLDemo/LoadMeshFromSTL.h"
-#include "Importers/ImportURDFDemo/UrdfParser.h"
+#include "../../Importers/ImportURDFDemo/UrdfParser.h"



--- a/examples/SharedMemory/physx/URDF2PhysX.h
+++ b/examples/SharedMemory/physx/URDF2PhysX.h
@ -2,7 +2,7 @@
 #define URDF2PHYSX_H

 #include "Bullet3Common/b3AlignedObjectArray.h"
-#include "Importers/ImportURDFDemo/URDFJointTypes.h"
+#include "../../Importers/ImportURDFDemo/URDFJointTypes.h"

 namespace physx
 {
--- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp
+++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp
@ -241,7 +241,9 @@ static void SimpleResizeCallback(float widthf, float heightf)
 	if (gWindow && gWindow->m_data->m_instancingRenderer)
 	{
 		gWindow->m_data->m_instancingRenderer->resize(width, height);
+		gWindow->setWidthAndHeight(width, height);
 	}
+	

 	//if (gApp && gApp->m_instancingRenderer)
 	//	gApp->m_instancingRenderer->resize(width, height);
@ -1281,7 +1283,7 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
 		{
 			m_data->m_window->endRendering();
 			m_data->m_window->startRendering();
-			glViewport(0,0, sourceWidth, sourceHeight); 
+			glViewport(0,0, sourceWidth*m_data->m_window->getRetinaScale(), sourceHeight*m_data->m_window->getRetinaScale());
 			B3_PROFILE("m_instancingRenderer render");
 			m_data->m_instancingRenderer->writeTransforms();
 			if (m_data->m_hasLightDirection)
@ -1373,7 +1375,7 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
 			{
 				{
 					m_data->m_window->startRendering();
-					glViewport(0,0, sourceWidth, sourceHeight); 
+					glViewport(0,0, sourceWidth*m_data->m_window->getRetinaScale(), sourceHeight*m_data->m_window->getRetinaScale());
 					BT_PROFILE("renderScene");
 					m_data->m_instancingRenderer->renderSceneInternal(B3_SEGMENTATION_MASK_RENDERMODE);
 				}
--- a/examples/pybullet/examples/createMultiBodyBatch.py
+++ b/examples/pybullet/examples/createMultiBodyBatch.py
@ -9,7 +9,7 @@ if (cid<0):
 	
 p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
 p.setTimeStep(1./120.)
-logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
 #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
 p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
 #disable rendering during creation.
@ -112,9 +112,6 @@ indices=[	0, 1, 2, 0, 2, 3,  #//ground face
 #p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
 #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
 visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals)
-#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices)
-
-#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj")
 collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=meshScale)#MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)

 texUid = p.loadTexture("tex256.png")
@ -124,16 +121,16 @@ batchPositions=[]



-for x in range (33):
-  for y in range (34):
-    for z in range (4):
+for x in range (32):
+  for y in range (32):
+    for z in range (10):
      batchPositions.append([x*meshScale[0]*5.5,y*meshScale[1]*5.5,(0.5+z)*meshScale[2]*2.5])

-bodyUid = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True)
+bodyUid = p.createMultiBody(baseMass=0,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True)
 p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid)		

-#p.syncBodyInfo()
-#print("numBodies=",p.getNumBodies())
+p.syncBodyInfo()
+print("numBodies=",p.getNumBodies())
 p.stopStateLogging(logId)
 p.setGravity(0,0,-10)

--- a/examples/pybullet/examples/humanoidMotionCapture.py
+++ b/examples/pybullet/examples/humanoidMotionCapture.py
@ -38,9 +38,9 @@ timeStep = 1./600.

 p.setPhysicsEngineParameter(fixedTimeStep=timeStep)

-path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
-#path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt"
-#path = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
+path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt"
+#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt"
+#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"


 #p.loadURDF("plane.urdf",[0,0,-1.03])
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_backflip.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_backflip.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_cartwheel.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_cartwheel.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_crawl.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_crawl.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_a.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_a.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_b.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_b.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_facedown.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_facedown.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_faceup.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_faceup.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_jump.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_jump.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_kick.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_kick.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_punch.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_punch.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_roll.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_roll.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_run.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_run.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spin.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spin.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spinkick.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spinkick.txt
--- a/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_walk.txt
+++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_walk.txt
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py
@ -26,8 +26,15 @@ class HumanoidStablePD(object):
    self._sim_model = self._pybullet_client.loadURDF(
      "humanoid/humanoid.urdf", [0,0.889540259,0],globalScaling=0.25, useFixedBase=useFixedBase, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)

+    #self._pybullet_client.setCollisionFilterGroupMask(self._sim_model,-1,collisionFilterGroup=0,collisionFilterMask=0)
+    #for j in range (self._pybullet_client.getNumJoints(self._sim_model)):
+    #  self._pybullet_client.setCollisionFilterGroupMask(self._sim_model,j,collisionFilterGroup=0,collisionFilterMask=0)
+    
+    
+    self._end_effectors = [5,8,11,14]  #ankle and wrist, both left and right
+    
    self._kin_model = self._pybullet_client.loadURDF(
-      "humanoid/humanoid.urdf", [0,12.85,0],globalScaling=0.25, useFixedBase=True, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
+      "humanoid/humanoid.urdf", [0,0.85,0],globalScaling=0.25, useFixedBase=True, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
    
    self._pybullet_client.changeDynamics(self._sim_model, -1, lateralFriction=0.9)
    for j in range (self._pybullet_client.getNumJoints(self._sim_model)):
@ -35,6 +42,18 @@ class HumanoidStablePD(object):
      
    self._pybullet_client.changeDynamics(self._sim_model, -1, linearDamping=0, angularDamping=0)
    self._pybullet_client.changeDynamics(self._kin_model, -1, linearDamping=0, angularDamping=0)
+    
+    #todo: add feature to disable simulation for a particular object. Until then, disable all collisions
+    self._pybullet_client.setCollisionFilterGroupMask(self._kin_model,-1,collisionFilterGroup=0,collisionFilterMask=0)
+    self._pybullet_client.changeDynamics(self._kin_model,-1,activationState=self._pybullet_client.ACTIVATION_STATE_SLEEP+self._pybullet_client.ACTIVATION_STATE_ENABLE_SLEEPING+self._pybullet_client.ACTIVATION_STATE_DISABLE_WAKEUP)
+    alpha = 0.4
+    self._pybullet_client.changeVisualShape(self._kin_model,-1, rgbaColor=[1,1,1,alpha])
+    for j in range (self._pybullet_client.getNumJoints(self._kin_model)):
+      self._pybullet_client.setCollisionFilterGroupMask(self._kin_model,j,collisionFilterGroup=0,collisionFilterMask=0)
+      self._pybullet_client.changeDynamics(self._kin_model,j,activationState=self._pybullet_client.ACTIVATION_STATE_SLEEP+self._pybullet_client.ACTIVATION_STATE_ENABLE_SLEEPING+self._pybullet_client.ACTIVATION_STATE_DISABLE_WAKEUP)
+      self._pybullet_client.changeVisualShape(self._kin_model,j, rgbaColor=[1,1,1,alpha])
+    
+    
    self._poseInterpolator = humanoid_pose_interpolator.HumanoidPoseInterpolator()
    
    for i in range (self._mocap_data.NumFrames()-1):
@ -53,7 +72,7 @@ class HumanoidStablePD(object):
      self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, j, self._pybullet_client.POSITION_CONTROL,targetPosition=[0,0,0,1], targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[jointFrictionForce,jointFrictionForce,jointFrictionForce])
      self._pybullet_client.setJointMotorControl2(self._kin_model, j, self._pybullet_client.POSITION_CONTROL, targetPosition=0, positionGain=0, targetVelocity=0,force=0)
      self._pybullet_client.setJointMotorControlMultiDof(self._kin_model, j, self._pybullet_client.POSITION_CONTROL,targetPosition=[0,0,0,1], targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[jointFrictionForce,jointFrictionForce,0])
-		
+    
    self._jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
    
    #only those body parts/links are allowed to touch the ground, otherwise the episode terminates
@ -126,9 +145,9 @@ class HumanoidStablePD(object):
    keyFrameDuration = self._mocap_data.KeyFrameDuraction()
    cycleTime = self.getCycleTime()
    #print("self._motion_data.NumFrames()=",self._mocap_data.NumFrames())
-    cycles = self.calcCycleCount(t, cycleTime)
+    self._cycleCount = self.calcCycleCount(t, cycleTime)
    #print("cycles=",cycles)
-    frameTime = t - cycles*cycleTime
+    frameTime = t - self._cycleCount*cycleTime
    if (frameTime<0):
      frameTime += cycleTime
    
@ -143,12 +162,29 @@ class HumanoidStablePD(object):

    self._frameFraction = (frameTime - self._frame*keyFrameDuration)/(keyFrameDuration)
    
+    
+  def computeCycleOffset(self):
+    firstFrame=0
+    lastFrame = self._mocap_data.NumFrames()-1
+    frameData = self._mocap_data._motion_data['Frames'][0]
+    frameDataNext = self._mocap_data._motion_data['Frames'][lastFrame]
+    
+    basePosStart = [frameData[1],frameData[2],frameData[3]]
+    basePosEnd = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
+    self._cycleOffset = [basePosEnd[0]-basePosStart[0],basePosEnd[1]-basePosStart[1],basePosEnd[2]-basePosStart[2]]
+    return self._cycleOffset
+    
  def computePose(self, frameFraction):
    frameData = self._mocap_data._motion_data['Frames'][self._frame]
    frameDataNext = self._mocap_data._motion_data['Frames'][self._frameNext]
    
-    pose = self._poseInterpolator.Slerp(frameFraction, frameData, frameDataNext, self._pybullet_client)
+    self._poseInterpolator.Slerp(frameFraction, frameData, frameDataNext, self._pybullet_client)
    #print("self._poseInterpolator.Slerp(", frameFraction,")=", pose)
+    self.computeCycleOffset()
+    oldPos = self._poseInterpolator._basePos
+    self._poseInterpolator._basePos = [oldPos[0]+self._cycleCount*self._cycleOffset[0],oldPos[1]+self._cycleCount*self._cycleOffset[1],oldPos[2]+self._cycleCount*self._cycleOffset[2]]
+    pose = self._poseInterpolator.GetPose()
+    
    return pose


@ -186,7 +222,62 @@ class HumanoidStablePD(object):
         self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, jointIndex, controlMode=self._pybullet_client.TORQUE_CONTROL, force=force)
      dofIndex+=self._jointDofCounts[index]
      
-      
+  def setJointMotors(self, desiredPositions, maxForces):
+    controlMode = self._pybullet_client.POSITION_CONTROL
+    startIndex=7
+    chest=1
+    neck=2
+    rightHip=3
+    rightKnee=4
+    rightAnkle=5
+    rightShoulder=6
+    rightElbow=7
+    leftHip=9
+    leftKnee=10
+    leftAnkle=11
+    leftShoulder=12
+    leftElbow=13
+    kp = 0.2
+    
+    
+    #self._jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
+    maxForce = maxForces[startIndex]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,chest,controlMode, targetPosition=self._poseInterpolator._chestRot,positionGain=kp, force=[maxForce])
+    maxForce = maxForces[startIndex]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,neck,controlMode,targetPosition=self._poseInterpolator._neckRot,positionGain=kp, force=[maxForce])
+    maxForce = maxForces[startIndex]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightHip,controlMode,targetPosition=self._poseInterpolator._rightHipRot,positionGain=kp, force=[maxForce])
+    maxForce = maxForces[startIndex]
+    startIndex+=1
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightKnee,controlMode,targetPosition=self._poseInterpolator._rightKneeRot,positionGain=kp, force=[maxForce])
+    maxForce = maxForces[startIndex]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightAnkle,controlMode,targetPosition=self._poseInterpolator._rightAnkleRot,positionGain=kp, force=[maxForce])
+    maxForce = [maxForces[startIndex],maxForces[startIndex+1],maxForces[startIndex+2],maxForces[startIndex+3]]
+    startIndex+=4
+    maxForce = maxForces[startIndex]
+    maxForce = maxForces[startIndex]
+    startIndex+=1
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightElbow, controlMode,targetPosition=self._poseInterpolator._rightElbowRot,positionGain=kp, force=[maxForce])
+    maxForce = maxForces[startIndex]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftHip, controlMode,targetPosition=self._poseInterpolator._leftHipRot,positionGain=kp, force=[maxForce])
+    maxForce = maxForces[startIndex]
+    startIndex+=1
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftKnee, controlMode,targetPosition=self._poseInterpolator._leftKneeRot,positionGain=kp, force=[maxForce])
+    maxForce = maxForces[startIndex]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftAnkle, controlMode,targetPosition=self._poseInterpolator._leftAnkleRot,positionGain=kp, force=[maxForce])
+    maxForce = maxForces[startIndex]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftShoulder, controlMode,targetPosition=self._poseInterpolator._leftShoulderRot,positionGain=kp, force=[maxForce])
+    maxForce = maxForces[startIndex]
+    startIndex+=1
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftElbow, controlMode,targetPosition=self._poseInterpolator._leftElbowRot,positionGain=kp, force=[maxForce])
+    #print("startIndex=",startIndex)
      
  def getPhase(self):
    keyFrameDuration = self._mocap_data.KeyFrameDuraction()
@ -317,13 +408,32 @@ class HumanoidStablePD(object):
      
    return terminates
    
+  def quatMul(self, q1, q2):
+      return [ q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1],
+               q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2],
+              q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0],
+              q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2]]
+              
+  def calcRootAngVelErr(self, vel0, vel1):
+    diff = [vel0[0]-vel1[0],vel0[1]-vel1[1], vel0[2]-vel1[2]]
+    return diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2]
+
+  
+    
+  def calcRootRotDiff(self,orn0, orn1):
+    orn0Conj = [-orn0[0],-orn0[1],-orn0[2],orn0[3]]
+    q_diff = self.quatMul(orn1, orn0Conj)
+    axis,angle = self._pybullet_client.getAxisAngleFromQuaternion(q_diff)
+    return angle*angle
+  
  def getReward(self, pose):
    #from DeepMimic double cSceneImitate::CalcRewardImitate
+    #todo: compensate for ground height in some parts, once we move to non-flat terrain
    pose_w = 0.5
    vel_w = 0.05
-    end_eff_w = 0 #0.15
-    root_w = 0#0.2
-    com_w = 0#0.1
+    end_eff_w = 0.15
+    root_w = 0.2
+    com_w = 0 #0.1

    total_w = pose_w + vel_w + end_eff_w + root_w + com_w
    pose_w /= total_w
@ -386,8 +496,21 @@ class HumanoidStablePD(object):
    num_joints = 15
    
    root_rot_w = mJointWeights[root_id]
-    #pose_err += root_rot_w * cKinTree::CalcRootRotErr(joint_mat, pose0, pose1)
-    #vel_err += root_rot_w * cKinTree::CalcRootAngVelErr(joint_mat, vel0, vel1)
+    rootPosSim,rootOrnSim = self._pybullet_client.getBasePositionAndOrientation(self._sim_model)
+    rootPosKin ,rootOrnKin = self._pybullet_client.getBasePositionAndOrientation(self._kin_model)
+    linVelSim, angVelSim = self._pybullet_client.getBaseVelocity(self._sim_model)
+    linVelKin, angVelKin = self._pybullet_client.getBaseVelocity(self._kin_model)
+    
+    
+    root_rot_err = self.calcRootRotDiff(rootOrnSim,rootOrnKin)
+    pose_err += root_rot_w * root_rot_err
+    
+    
+    root_vel_diff  = [linVelSim[0]-linVelKin[0],linVelSim[1]-linVelKin[1],linVelSim[2]-linVelKin[2]]
+    root_vel_err = root_vel_diff[0]*root_vel_diff[0]+root_vel_diff[1]*root_vel_diff[1]+root_vel_diff[2]*root_vel_diff[2]
+    
+    root_ang_vel_err = self.calcRootAngVelErr( angVelSim, angVelKin)
+    vel_err += root_rot_w * root_ang_vel_err

    for j in range (num_joints):
      curr_pose_err = 0
@ -418,35 +541,27 @@ class HumanoidStablePD(object):
      pose_err += w * curr_pose_err
      vel_err += w * curr_vel_err
    
-    #  bool is_end_eff = sim_char.IsEndEffector(j)
-    #  if (is_end_eff)
-    #  {
-    #    tVector pos0 = sim_char.CalcJointPos(j)
-    #    tVector pos1 = cKinTree::CalcJointWorldPos(joint_mat, pose1, j)
-    #    double ground_h0 = mGround->SampleHeight(pos0)
-    #    double ground_h1 = kin_char.GetOriginPos()[1]
-    #
-    #    tVector pos_rel0 = pos0 - root_pos0
-    #    tVector pos_rel1 = pos1 - root_pos1
-    #    pos_rel0[1] = pos0[1] - ground_h0
-    #    pos_rel1[1] = pos1[1] - ground_h1
-    #
-    #    pos_rel0 = origin_trans * pos_rel0
-    #    pos_rel1 = kin_origin_trans * pos_rel1
-    #
-    #    curr_end_err = (pos_rel1 - pos_rel0).squaredNorm()
-    #    end_eff_err += curr_end_err;
-    #    ++num_end_effs;
-    #  }
-    #}
-    #if (num_end_effs > 0):
-    #  end_eff_err /= num_end_effs
-    #
+      is_end_eff = j in self._end_effectors
+      if is_end_eff:
+        
+        linkStateSim = self._pybullet_client.getLinkState(self._sim_model, j)
+        linkStateKin = self._pybullet_client.getLinkState(self._kin_model, j)
+        linkPosSim = linkStateSim[0]
+        linkPosKin = linkStateKin[0]
+        linkPosDiff = [linkPosSim[0]-linkPosKin[0],linkPosSim[1]-linkPosKin[1],linkPosSim[2]-linkPosKin[2]]
+        curr_end_err = linkPosDiff[0]*linkPosDiff[0]+linkPosDiff[1]*linkPosDiff[1]+linkPosDiff[2]*linkPosDiff[2]
+        end_eff_err += curr_end_err
+        num_end_effs+=1
+    
+    if (num_end_effs > 0):
+      end_eff_err /= num_end_effs
+    
    #double root_ground_h0 = mGround->SampleHeight(sim_char.GetRootPos())
    #double root_ground_h1 = kin_char.GetOriginPos()[1]
    #root_pos0[1] -= root_ground_h0
    #root_pos1[1] -= root_ground_h1
-    #root_pos_err = (root_pos0 - root_pos1).squaredNorm()
+    root_pos_diff = [rootPosSim[0]-rootPosKin[0],rootPosSim[1]-rootPosKin[1],rootPosSim[2]-rootPosKin[2]]
+    root_pos_err = root_pos_diff[0]*root_pos_diff[0]+root_pos_diff[1]*root_pos_diff[1]+root_pos_diff[2]*root_pos_diff[2]
    #  
    #root_rot_err = cMathUtil::QuatDiffTheta(root_rot0, root_rot1)
    #root_rot_err *= root_rot_err
@ -454,10 +569,8 @@ class HumanoidStablePD(object):
    #root_vel_err = (root_vel1 - root_vel0).squaredNorm()
    #root_ang_vel_err = (root_ang_vel1 - root_ang_vel0).squaredNorm()

-    #root_err = root_pos_err
-    #    + 0.1 * root_rot_err
-    #    + 0.01 * root_vel_err
-    #    + 0.001 * root_ang_vel_err
+    root_err = root_pos_err + 0.1 * root_rot_err+ 0.01 * root_vel_err+ 0.001 * root_ang_vel_err
+    
    #com_err = 0.1 * (com_vel1_world - com_vel0_world).squaredNorm()
    
    #print("pose_err=",pose_err)
@ -470,7 +583,13 @@ class HumanoidStablePD(object):

    reward = pose_w * pose_reward + vel_w * vel_reward + end_eff_w * end_eff_reward + root_w * root_reward + com_w * com_reward

-    #print("reward = %f (pose_reward=%f, vel_reward=%f, end_eff_reward=%f, root_reward=%f, com_reward=%f)\n", reward,
+    
    # pose_reward,vel_reward,end_eff_reward, root_reward, com_reward);
+    #print("reward=",reward)
+    #print("pose_reward=",pose_reward)
+    #print("vel_reward=",vel_reward)
+    #print("end_eff_reward=",end_eff_reward)
+    #print("root_reward=",root_reward)
+    #print("com_reward=",com_reward)
    
    return reward
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py
@ -12,18 +12,18 @@ import random
      
      
 class PyBulletDeepMimicEnv(Env):
-    def __init__(self, args=None, enable_draw=False, pybullet_client=None):
-      super().__init__(args, enable_draw)
+    def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None):
+      super().__init__(arg_parser, enable_draw)
      self._num_agents = 1
      self._pybullet_client = pybullet_client
      self._isInitialized = False
+      self._useStablePD = True
+      self._arg_parser = arg_parser
      self.reset()
    
    def reset(self):
      
      
-      startTime = 0. #float(rn)/rnrange * self._humanoid.getCycleTime()
-      self.t = startTime
      if not self._isInitialized:
        if self.enable_draw:
          self._pybullet_client =  bullet_client.BulletClient(connection_mode=p1.GUI)
@ -43,16 +43,20 @@ class PyBulletDeepMimicEnv(Env):
        self._pybullet_client.changeDynamics(self._planeId, linkIndex=-1, lateralFriction=0.9)
        
        self._mocapData = motion_capture_data.MotionCaptureData()
-        #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
-        motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
+        
+        motion_file = self._arg_parser.parse_strings('motion_file')
+        print("motion_file=",motion_file[0])
+                
+        motionPath = pybullet_data.getDataPath()+"/"+motion_file[0]
+        #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
        self._mocapData.Load(motionPath)
-        timeStep = 1./600
+        timeStep = 1./600.
        useFixedBase=False
        self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData, timeStep, useFixedBase)
        self._isInitialized = True
        
        self._pybullet_client.setTimeStep(timeStep)
-        self._pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
+        self._pybullet_client.setPhysicsEngineParameter(numSubSteps=1)
       
        
        selfCheck = False
@ -73,6 +77,8 @@ class PyBulletDeepMimicEnv(Env):
      #startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
      rnrange = 1000
      rn = random.randint(0,rnrange)
+      startTime = float(rn)/rnrange * self._humanoid.getCycleTime()
+      self.t = startTime
      
      self._humanoid.setSimTime(startTime)
      
@ -247,21 +253,30 @@ class PyBulletDeepMimicEnv(Env):
    def log_val(self, agent_id, val):
        pass
        
+        
    def update(self, timeStep):
      #print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t)
+      self._pybullet_client.setTimeStep(timeStep)
+      self._humanoid._timeStep = timeStep
+        
      for i in range(1):
          self.t += timeStep
          self._humanoid.setSimTime(self.t)
          
          if self.desiredPose:
-            #kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
-            #self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=False)
+            kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
+            self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=True)
            #pos,orn=self._pybullet_client.getBasePositionAndOrientation(self._humanoid._sim_model)
            #self._pybullet_client.resetBasePositionAndOrientation(self._humanoid._kin_model, [pos[0]+3,pos[1],pos[2]],orn)
            #print("desiredPositions=",self.desiredPose)
            maxForces = [0,0,0,0,0,0,0,200,200,200,200, 50,50,50,50, 200,200,200,200, 150, 90,90,90,90, 100,100,100,100, 60, 200,200,200,200,  150, 90, 90, 90, 90, 100,100,100,100, 60]
-            taus = self._humanoid.computePDForces(self.desiredPose, desiredVelocities=None, maxForces=maxForces)
-            self._humanoid.applyPDForces(taus)
+            
+            if self._useStablePD:
+              taus = self._humanoid.computePDForces(self.desiredPose, desiredVelocities=None, maxForces=maxForces)
+              self._humanoid.applyPDForces(taus)
+            else:
+              self._humanoid.setJointMotors(self.desiredPose, maxForces=maxForces)
+            
            self._pybullet_client.stepSimulation()
            

--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py
@ -14,17 +14,18 @@ pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
 z2y = pybullet_client.getQuaternionFromEuler([-math.pi*0.5,0,0])
 #planeId = pybullet_client.loadURDF("plane.urdf",[0,0,0],z2y)
 planeId= pybullet_client.loadURDF("plane_implicit.urdf",[0,0,0],z2y, useMaximalCoordinates=True)
-        
+pybullet_client.changeDynamics(planeId, linkIndex=-1, lateralFriction=0.9)        
 #print("planeId=",planeId)
+
 pybullet_client.configureDebugVisualizer(pybullet_client.COV_ENABLE_Y_AXIS_UP,1)
 pybullet_client.setGravity(0,-9.8,0)

 pybullet_client.setPhysicsEngineParameter(numSolverIterations=10)
-pybullet_client.changeDynamics(planeId, linkIndex=-1, lateralFriction=0.9)
+

 mocapData = motion_capture_data.MotionCaptureData()
-#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
-motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
+#motionPath = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
+motionPath = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt"
 mocapData.Load(motionPath)
 timeStep = 1./600
 useFixedBase=False
@ -94,23 +95,15 @@ while (1):
      #humanoid.initializePose(pose=pose, phys_model = humanoid._sim_model, initBase=True, initializeVelocity=True)
      #humanoid.resetPose()
      
-      action =   [-6.541649997234344482e-02,1.138873845338821411e-01,-1.215099543333053589e-01,4.610761404037475586e-01,-4.278013408184051514e-01,
-        4.064738750457763672e-02,7.801693677902221680e-02,4.934634566307067871e-01,1.321935355663299561e-01,-1.393979601562023163e-02,-6.699572503566741943e-02,
-        4.778462052345275879e-01,3.740053176879882812e-01,-3.230125308036804199e-01,-3.549785539507865906e-02,-3.283375874161720276e-03,5.070925354957580566e-01,
-        1.033667206764221191e+00,-3.644275963306427002e-01,-3.374500572681427002e-02,1.294951438903808594e-01,-5.880850553512573242e-01,
-        1.185980588197708130e-01,6.445263326168060303e-02,1.625719368457794189e-01,4.615224599838256836e-01,3.817881345748901367e-01,-4.382217228412628174e-01,
-        1.626710966229438782e-02,-4.743926972150802612e-02,3.833046853542327881e-01,1.067031383514404297e+00,3.039606213569641113e-01,
-        -1.891726106405258179e-01,3.595829010009765625e-02,-7.283059358596801758e-01]
      
-      pos_tar2 = humanoid.convertActionToPose(action)
-      desiredPose = np.array(pos_tar2)
-      #desiredPose = humanoid.computePose(humanoid._frameFraction)
-      #desiredPose = targetPose.GetPose() 
+      desiredPose = humanoid.computePose(humanoid._frameFraction)
+      #desiredPose = desiredPose.GetPose() 
      #curPose = HumanoidPoseInterpolator()
      #curPose.reset()
      s = humanoid.getState()
      #np.savetxt("pb_record_state_s.csv", s, delimiter=",")
-      taus = humanoid.computePDForces(desiredPose)
+      maxForces = [0,0,0,0,0,0,0,200,200,200,200, 50,50,50,50, 200,200,200,200, 150, 90,90,90,90, 100,100,100,100, 60, 200,200,200,200,  150, 90, 90, 90, 90, 100,100,100,100, 60]
+      taus = humanoid.computePDForces(desiredPose, desiredVelocities=None, maxForces=maxForces)
      
      #print("taus=",taus)
      humanoid.applyPDForces(taus)
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py
@ -15,11 +15,11 @@ from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMim
 import sys
 import random

-update_timestep = 1./600.
+update_timestep = 1./240.
 animating = True

 def update_world(world, time_elapsed):
-    timeStep = 1./600.
+    timeStep = update_timestep
    world.update(timeStep)
    reward = world.env.calc_reward(agent_id=0)
    #print("reward=",reward)
@ -48,7 +48,7 @@ args = sys.argv[1:]
 def build_world(args, enable_draw):
    arg_parser = build_arg_parser(args)
    print("enable_draw=",enable_draw)
-    env = PyBulletDeepMimicEnv(args, enable_draw)
+    env = PyBulletDeepMimicEnv(arg_parser, enable_draw)
    world = RLWorld(env, arg_parser)
    #world.env.set_playback_speed(playback_speed)

@ -71,7 +71,7 @@ def build_world(args, enable_draw):
        print("agent_type=",agent_type)
        agent = PPOAgent(world, id, json_data)
    
-        agent.set_enable_training(True)
+        agent.set_enable_training(False)
        world.reset()
    return world    

@ -83,7 +83,7 @@ if __name__ == '__main__':
    
    world = build_world(args, True)
    while (world.env._pybullet_client.isConnected()):
-      timeStep = 1./600.
+      timeStep = update_timestep
      keys = world.env.getKeyboardEvents()
      
        
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@ -10960,6 +10960,8 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping);
 	PyModule_AddIntConstant(m, "ACTIVATION_STATE_WAKE_UP", eActivationStateWakeUp);
 	PyModule_AddIntConstant(m, "ACTIVATION_STATE_SLEEP", eActivationStateSleep);
+	PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_WAKEUP", eActivationStateEnableWakeup);
+	PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_WAKEUP", eActivationStateDisableWakeup);

 	PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
 	PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);
--- a/setup.py
+++ b/setup.py
@ -454,7 +454,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:

 setup(
 	name = 'pybullet',
-	version='2.4.3',
+	version='2.4.5',
 	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/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@ -106,6 +106,7 @@ btMultiBody::btMultiBody(int n_links,
 	  m_fixedBase(fixedBase),
 	  m_awake(true),
 	  m_canSleep(canSleep),
+	  m_canWakeup(true),
 	  m_sleepTimer(0),
 	  m_userObjectPointer(0),
 	  m_userIndex2(-1),
@ -1882,6 +1883,8 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 		return;
 	}

+	
+
 	// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
 	btScalar motion = 0;
 	{
@ -1900,8 +1903,11 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 	else
 	{
 		m_sleepTimer = 0;
-		if (!m_awake)
-			wakeUp();
+		if (m_canWakeup)
+		{
+			if (!m_awake)
+				wakeUp();
+		}
 	}
 }

--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@ -454,7 +454,10 @@ public:
 	//
 	void setCanSleep(bool canSleep)
 	{
-		m_canSleep = canSleep;
+		if (m_canWakeup)
+		{
+			m_canSleep = canSleep;
+		}
 	}

 	bool getCanSleep() const
@ -462,6 +465,15 @@ public:
 		return m_canSleep;
 	}

+	bool getCanWakeup() const
+	{
+		return m_canWakeup;
+	}
+	
+	void setCanWakeup(bool canWakeup) 
+	{
+		m_canWakeup = canWakeup;
+	}
 	bool isAwake() const { return m_awake; }
 	void wakeUp();
 	void goToSleep();
@ -696,6 +708,7 @@ private:
 	// Sleep parameters.
 	bool m_awake;
 	bool m_canSleep;
+	bool m_canWakeup;
 	btScalar m_sleepTimer;

 	void *m_userObjectPointer;