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