Merge pull request #2116 from erwincoumans/master

various fixes

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)

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/SharedMemory/physx/PhysXServerCommandProcessor.cpp

@@ -1839,10 +1839,13 @@ 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,
 				segmentationMaskBuffer, numRequestedPixels,

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"
 
 
 

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
 {

examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp

@@ -241,8 +241,10 @@ 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);
 				}

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 x in range (32):
-  for y in range (34):
+  for y in range (32):
-    for z in range (4):
+    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()
+p.syncBodyInfo()
-#print("numBodies=",p.getNumBodies())
+print("numBodies=",p.getNumBodies())
 p.stopStateLogging(logId)
 p.setGravity(0,0,-10)
 

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
 
 
@@ -186,8 +222,63 @@ 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()
     cycleTime = keyFrameDuration*(self._mocap_data.NumFrames()-1)
@@ -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
+    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
@@ -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)
+    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
@@ -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)
+      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
@@ -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
+    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)
@@ -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

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

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

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

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',

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;