diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 30bca5b4c..9b5f34e14 100644
--- 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)
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index df2ec8875..86e218372 100644
--- 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
diff --git a/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp b/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp
index 1d18b3143..c3a287f0d 100644
--- 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,
diff --git a/examples/SharedMemory/physx/URDF2PhysX.cpp b/examples/SharedMemory/physx/URDF2PhysX.cpp
index 02713f029..b810fb7d5 100644
--- 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"
 
 
 
diff --git a/examples/SharedMemory/physx/URDF2PhysX.h b/examples/SharedMemory/physx/URDF2PhysX.h
index 9c0c653e8..8cae67c41 100644
--- 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
 {
diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp
index 05e24945b..9f4943e4d 100644
--- 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);
 				}
diff --git a/examples/pybullet/examples/createMultiBodyBatch.py b/examples/pybullet/examples/createMultiBodyBatch.py
index 3e8d33cab..dd6fdc38b 100644
--- 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)
 
diff --git a/examples/pybullet/examples/humanoidMotionCapture.py b/examples/pybullet/examples/humanoidMotionCapture.py
index 765a88498..3300e121c 100644
--- 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])
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_backflip.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_backflip.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_backflip.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_backflip.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_cartwheel.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_cartwheel.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_cartwheel.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_cartwheel.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_crawl.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_crawl.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_crawl.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_crawl.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_dance_a.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_a.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_dance_a.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_a.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_dance_b.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_b.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_dance_b.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_b.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_getup_facedown.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_facedown.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_getup_facedown.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_facedown.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_getup_faceup.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_faceup.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_getup_faceup.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_faceup.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_jump.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_jump.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_jump.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_jump.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_kick.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_kick.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_kick.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_kick.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_punch.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_punch.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_punch.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_punch.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_roll.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_roll.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_roll.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_roll.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_run.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_run.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_run.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_run.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_spin.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spin.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_spin.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spin.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_spinkick.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spinkick.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_spinkick.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spinkick.txt
diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_walk.txt
similarity index 100%
rename from examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt
rename to examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_walk.txt
diff --git 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
index c3e7ca54e..fb123a265 100644
--- 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
diff --git 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
index 35f618cc8..68e4d613e 100644
--- 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()
             
 
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py
index 5cf9a5bcc..2057dde5a 100644
--- 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)
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py
index 39c015f6b..38a9469e9 100644
--- 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()
       
         
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index f3e01e5dc..b0d72691e 100644
--- 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);
diff --git a/setup.py b/setup.py
index 760d3c452..4bb958fb0 100644
--- 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',
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 5b009ac1c..44a7ecaf9 100644
--- 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();
+		}
 	}
 }
 
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index 8b953b899..4aa083741 100644
--- 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;