more work on pybullet_envs.deep_mimic.

allow btMultiBody to not wakeup (for some RL experiments)
move deep_mimic motion files to data/motions folder, so we can use the args files unmodified.

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -7653,6 +7653,16 @@ 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)

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

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()+"/data/motions/humanoid3d_backflip.txt"
-#path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt"
+#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt"
-#path = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
+#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
 
 
 #p.loadURDF("plane.urdf",[0,0,-1.03])

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):
@@ -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
 
 
@@ -372,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
+    end_eff_w = 0.15
-    root_w = 0#0.2
+    root_w = 0.2
-    com_w = 0#0.1
+    com_w = 0 #0.1
 
     total_w = pose_w + vel_w + end_eff_w + root_w + com_w
     pose_w /= total_w
@@ -441,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)
+    rootPosSim,rootOrnSim = self._pybullet_client.getBasePositionAndOrientation(self._sim_model)
-    #vel_err += root_rot_w * cKinTree::CalcRootAngVelErr(joint_mat, vel0, vel1)
+    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
@@ -473,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)
+      is_end_eff = j in self._end_effectors
-    #  if (is_end_eff)
+      if is_end_eff:
-    #  {
+        
-    #    tVector pos0 = sim_char.CalcJointPos(j)
+        linkStateSim = self._pybullet_client.getLinkState(self._sim_model, j)
-    #    tVector pos1 = cKinTree::CalcJointWorldPos(joint_mat, pose1, j)
+        linkStateKin = self._pybullet_client.getLinkState(self._kin_model, j)
-    #    double ground_h0 = mGround->SampleHeight(pos0)
+        linkPosSim = linkStateSim[0]
-    #    double ground_h1 = kin_char.GetOriginPos()[1]
+        linkPosKin = linkStateKin[0]
-    #
+        linkPosDiff = [linkPosSim[0]-linkPosKin[0],linkPosSim[1]-linkPosKin[1],linkPosSim[2]-linkPosKin[2]]
-    #    tVector pos_rel0 = pos0 - root_pos0
+        curr_end_err = linkPosDiff[0]*linkPosDiff[0]+linkPosDiff[1]*linkPosDiff[1]+linkPosDiff[2]*linkPosDiff[2]
-    #    tVector pos_rel1 = pos1 - root_pos1
+        end_eff_err += curr_end_err
-    #    pos_rel0[1] = pos0[1] - ground_h0
+        num_end_effs+=1
-    #    pos_rel1[1] = pos1[1] - ground_h1
+    
-    #
+    if (num_end_effs > 0):
-    #    pos_rel0 = origin_trans * pos_rel0
+      end_eff_err /= num_end_effs
-    #    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
-    #
     #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
@@ -509,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
+    root_err = root_pos_err + 0.1 * root_rot_err+ 0.01 * root_vel_err+ 0.001 * root_ang_vel_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)
@@ -525,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

examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py

@@ -12,19 +12,18 @@ import random
       
       
 class PyBulletDeepMimicEnv(Env):
-    def __init__(self, args=None, enable_draw=False, pybullet_client=None):
+    def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None):
-      super().__init__(args, enable_draw)
+      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)
@@ -44,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
@@ -74,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)
       
@@ -248,15 +253,19 @@ 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)
+            kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
-            #self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=False)
+            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)

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()+"/data/motions/humanoid3d_walk.txt"
-motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.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 = humanoid.computePose(humanoid._frameFraction)
-      desiredPose = np.array(pos_tar2)
+      #desiredPose = desiredPose.GetPose() 
-      #desiredPose = humanoid.computePose(humanoid._frameFraction)
-      #desiredPose = targetPose.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)

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)
 
@@ -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()
       
         

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);

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)
+		if (m_canWakeup)
-			wakeUp();
+		{
+			if (!m_awake)
+				wakeUp();
+		}
 	}
 }
 

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;