deep_mimic: add option for spherical joint drive motor, next to stable PD control

(existing policies won't work with those motors, needs tuning and re-training)

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)
 

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

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

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

@@ -17,6 +17,7 @@ class PyBulletDeepMimicEnv(Env):
       self._num_agents = 1
       self._pybullet_client = pybullet_client
       self._isInitialized = False
+      self._useStablePD = True
       self.reset()
     
     def reset(self):
@@ -260,8 +261,13 @@ class PyBulletDeepMimicEnv(Env):
             #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/testrl.py

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