Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

allow pybullet_envs.deep_mimic.testrl --arg_file run_humanoid3d_backflip_args.txt to perform a backflip. Can only backflip twice, then drops on ground.

Browse Source 
this deepmimic is still very slow, due to slow mass matrix/inverse dynamics computation. once spherical motor drive is enabled, it should be fast(er)
move pd_controller_stable to pybullet_utils for easier re-use
add plane_transparent.urdf to pybullet_data
allow spacebar in keyboardEvents (Windows for now)
This commit is contained in:
erwincoumans
2019-02-10 20:56:31 -08:00
parent 28c9ea3aad
commit 9bddca873c
17 changed files with 551 additions and 224 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/CommonInterfaces/CommonCallbacks.h
View File

@@ -11,6 +11,7 @@ typedef void (*b3RenderCallback)();
enum enum
{ {
B3G_ESCAPE = 27, B3G_ESCAPE = 27,
B3G_SPACE = 32,
B3G_F1 = 0xff00, B3G_F1 = 0xff00,
B3G_F2, B3G_F2,
B3G_F3, B3G_F3,
@@ -40,7 +41,8 @@ enum
B3G_SHIFT, B3G_SHIFT,
B3G_CONTROL, B3G_CONTROL,
B3G_ALT, B3G_ALT,
B3G_RETURN B3G_RETURN,
}; };
#endif #endif

5
examples/OpenGLWindow/Win32Window.cpp
View File

@@ -67,6 +67,11 @@ int getSpecialKeyFromVirtualKeycode(int virtualKeyCode)
switch (virtualKeyCode) switch (virtualKeyCode)
{ {
case VK_SPACE:
{
keycode = B3G_SPACE;
break;
}
case VK_RETURN: case VK_RETURN:
{ {
keycode = B3G_RETURN; keycode = B3G_RETURN;

4
examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf
View File

@@ -2,8 +2,8 @@
<link name="base" > <link name="base" >
<inertial> <inertial>
<origin rpy = "0 0 0" xyz = "0 0 0" /> <origin rpy = "0 0 0" xyz = "0 0 0" />
<mass value = "0.00100" /> <mass value = "0.0001" />
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" /> <inertia ixx = "0.0001" ixy = "0" ixz = "0" iyy = "0.0001" iyz = "0" izz = "0.0001" />
</inertial> </inertial>
</link> </link>
<link name="root" > <link name="root" >

14
examples/pybullet/gym/pybullet_data/plane_transparent.mtl Normal file
View File

@@ -0,0 +1,14 @@
newmtl Material
Ns 10.0000
Ni 1.5000
d 1.0000
Tr 0.0000
Tf 1.0000 1.0000 1.0000
illum 2
Ka 0.0000 0.0000 0.0000
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
map_Kd checker_blue.png

18
examples/pybullet/gym/pybullet_data/plane_transparent.obj Normal file
View File

@@ -0,0 +1,18 @@
# Blender v2.66 (sub 1) OBJ File: ''
# www.blender.org
mtllib plane_transparent.mtl
o Plane
v 15.000000 -15.000000 0.000000
v 15.000000 15.000000 0.000000
v -15.000000 15.000000 0.000000
v -15.000000 -15.000000 0.000000
vt 15.000000 0.000000
vt 15.000000 15.000000
vt 0.000000 15.000000
vt 0.000000 0.000000
usemtl Material
s off
f 1/1 2/2 3/3
f 1/1 3/3 4/4

29
examples/pybullet/gym/pybullet_data/plane_transparent.urdf Normal file
View File

@@ -0,0 +1,29 @@
<?xml version="1.0" ?>
<robot name="plane">
<link name="planeLink">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="plane_transparent.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 .7"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -5"/>
<geometry>
<box size="30 30 10"/>
</geometry>
</collision>
</link>
</robot>

2
examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py
View File

@@ -1 +1 @@
from pybullet_envs.deep_mimic.humanoid_deepmimic_gym_env import HumanoidDeepMimicGymEnv

90
examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py vendored
View File

@@ -1,45 +1,51 @@
from pybullet_envs.minitaur.envs import bullet_client from pybullet_utils import bullet_client
import math import math
class HumanoidPoseInterpolator(object): class HumanoidPoseInterpolator(object):
def __init__(self): def __init__(self):
pass pass
def Reset(self): def Reset(self,basePos=[0,0,0], baseOrn=[0,0,0,1],chestRot=[0,0,0,1], neckRot=[0,0,0,1],rightHipRot= [0,0,0,1], rightKneeRot=[0],rightAnkleRot = [0,0,0,1],
rightShoulderRot = [0,0,0,1],rightElbowRot = [0], leftHipRot = [0,0,0,1], leftKneeRot = [0],leftAnkleRot = [0,0,0,1],
leftShoulderRot = [0,0,0,1] ,leftElbowRot = [0],
baseLinVel = [0,0,0],baseAngVel = [0,0,0], chestVel = [0,0,0],neckVel = [0,0,0],rightHipVel = [0,0,0],rightKneeVel = [0],
rightAnkleVel = [0,0,0],rightShoulderVel = [0,0,0],rightElbowVel = [0],leftHipVel = [0,0,0],leftKneeVel = [0],leftAnkleVel = [0,0,0],leftShoulderVel = [0,0,0],leftElbowVel = [0]
):
self._basePos = [0,0,0] self._basePos = basePos
self._baseLinVel = [0,0,0] self._baseLinVel = baseLinVel
self._baseOrn = [0,0,0,1] print("HumanoidPoseInterpolator.Reset: baseLinVel = ", baseLinVel)
self._baseAngVel = [0,0,0] self._baseOrn =baseOrn
self._baseAngVel = baseAngVel
self._chestRot = [0,0,0,1] self._chestRot = chestRot
self._chestVel = [0,0,0] self._chestVel =chestVel
self._neckRot = [0,0,0,1] self._neckRot = neckRot
self._neckVel = [0,0,0] self._neckVel = neckVel
self._rightHipRot = [0,0,0,1] self._rightHipRot = rightHipRot
self._rightHipVel = [0,0,0] self._rightHipVel = rightHipVel
self._rightKneeRot = [0] self._rightKneeRot =rightKneeRot
self._rightKneeVel = [0] self._rightKneeVel = rightKneeVel
self._rightAnkleRot = [0,0,0,1] self._rightAnkleRot = rightAnkleRot
self._rightAnkleVel = [0,0,0] self._rightAnkleVel = rightAnkleVel
self._rightShoulderRot = [0,0,0,1] self._rightShoulderRot =rightShoulderRot
self._rightShoulderVel = [0,0,0] self._rightShoulderVel = rightShoulderVel
self._rightElbowRot = [0] self._rightElbowRot = rightElbowRot
self._rightElbowVel = [0] self._rightElbowVel = rightElbowVel
self._leftHipRot = [0,0,0,1] self._leftHipRot = leftHipRot
self._leftHipVel = [0,0,0] self._leftHipVel = leftHipVel
self._leftKneeRot = [0] self._leftKneeRot = leftKneeRot
self._leftKneeVel = [0] self._leftKneeVel = leftKneeVel
self._leftAnkleRot = [0,0,0,1] self._leftAnkleRot =leftAnkleRot
self._leftAnkleVel = [0,0,0] self._leftAnkleVel = leftAnkleVel
self._leftShoulderRot = [0,0,0,1] self._leftShoulderRot = leftShoulderRot
self._leftShoulderVel = [0,0,0] self._leftShoulderVel = leftShoulderVel
self._leftElbowRot = [0] self._leftElbowRot =leftElbowRot
self._leftElbowVel = [0] self._leftElbowVel = leftElbowVel
def ComputeLinVel(self,posStart, posEnd, deltaTime): def ComputeLinVel(self,posStart, posEnd, deltaTime):
vel = [(posEnd[0]-posStart[0])/deltaTime,(posEnd[1]-posStart[1])/deltaTime,(posEnd[2]-posStart[2])/deltaTime] vel = [(posEnd[0]-posStart[0])/deltaTime,(posEnd[1]-posStart[1])/deltaTime,(posEnd[2]-posStart[2])/deltaTime]
@@ -51,6 +57,14 @@ class HumanoidPoseInterpolator(object):
angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime] angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
return angVel return angVel
def ComputeAngVelRel(self,ornStart, ornEnd, deltaTime, bullet_client):
ornStartConjugate = [-ornStart[0],-ornStart[1],-ornStart[2],ornStart[3]]
pos_diff, q_diff =bullet_client.multiplyTransforms([0,0,0], ornStartConjugate, [0,0,0], ornEnd)
axis,angle = bullet_client.getAxisAngleFromQuaternion(q_diff)
angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
return angVel
def NormalizeVector(self, vec): def NormalizeVector(self, vec):
length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2] length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2]
if (length2>0): if (length2>0):
@@ -121,17 +135,17 @@ class HumanoidPoseInterpolator(object):
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]] chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]] chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
self._chestRot = bullet_client.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction) self._chestRot = bullet_client.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
self._chestVel = self.ComputeAngVel(chestRotStart,chestRotEnd,keyFrameDuration,bullet_client) self._chestVel = self.ComputeAngVelRel(chestRotStart,chestRotEnd,keyFrameDuration,bullet_client)
neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]] neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]] neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
self._neckRot = bullet_client.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction) self._neckRot = bullet_client.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
self._neckVel = self.ComputeAngVel(neckRotStart,neckRotEnd,keyFrameDuration,bullet_client) self._neckVel = self.ComputeAngVelRel(neckRotStart,neckRotEnd,keyFrameDuration,bullet_client)
rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]] rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]] rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
self._rightHipRot = bullet_client.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction) self._rightHipRot = bullet_client.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction)
self._rightHipVel = self.ComputeAngVel(rightHipRotStart,rightHipRotEnd,keyFrameDuration,bullet_client) self._rightHipVel = self.ComputeAngVelRel(rightHipRotStart,rightHipRotEnd,keyFrameDuration,bullet_client)
rightKneeRotStart = [frameData[20]] rightKneeRotStart = [frameData[20]]
rightKneeRotEnd = [frameDataNext[20]] rightKneeRotEnd = [frameDataNext[20]]
@@ -141,12 +155,12 @@ class HumanoidPoseInterpolator(object):
rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]] rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]] rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]]
self._rightAnkleRot = bullet_client.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction) self._rightAnkleRot = bullet_client.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction)
self._rightAnkleVel = self.ComputeAngVel(rightAnkleRotStart,rightAnkleRotEnd,keyFrameDuration,bullet_client) self._rightAnkleVel = self.ComputeAngVelRel(rightAnkleRotStart,rightAnkleRotEnd,keyFrameDuration,bullet_client)
rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]] rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]]
rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]] rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
self._rightShoulderRot = bullet_client.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction) self._rightShoulderRot = bullet_client.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
self._rightShoulderVel = self.ComputeAngVel(rightShoulderRotStart,rightShoulderRotEnd, keyFrameDuration,bullet_client) self._rightShoulderVel = self.ComputeAngVelRel(rightShoulderRotStart,rightShoulderRotEnd, keyFrameDuration,bullet_client)
rightElbowRotStart = [frameData[29]] rightElbowRotStart = [frameData[29]]
rightElbowRotEnd = [frameDataNext[29]] rightElbowRotEnd = [frameDataNext[29]]
@@ -156,7 +170,7 @@ class HumanoidPoseInterpolator(object):
leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]] leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]] leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]]
self._leftHipRot = bullet_client.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction) self._leftHipRot = bullet_client.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
self._leftHipVel = self.ComputeAngVel(leftHipRotStart, leftHipRotEnd,keyFrameDuration,bullet_client) self._leftHipVel = self.ComputeAngVelRel(leftHipRotStart, leftHipRotEnd,keyFrameDuration,bullet_client)
leftKneeRotStart = [frameData[34]] leftKneeRotStart = [frameData[34]]
leftKneeRotEnd = [frameDataNext[34]] leftKneeRotEnd = [frameDataNext[34]]
@@ -166,12 +180,12 @@ class HumanoidPoseInterpolator(object):
leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]] leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]] leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
self._leftAnkleRot = bullet_client.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction) self._leftAnkleRot = bullet_client.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
self._leftAnkleVel = self.ComputeAngVel(leftAnkleRotStart,leftAnkleRotEnd,keyFrameDuration,bullet_client) self._leftAnkleVel = self.ComputeAngVelRel(leftAnkleRotStart,leftAnkleRotEnd,keyFrameDuration,bullet_client)
leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]] leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]] leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]]
self._leftShoulderRot = bullet_client.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction) self._leftShoulderRot = bullet_client.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction)
self._leftShoulderVel = self.ComputeAngVel(leftShoulderRotStart,leftShoulderRotEnd,keyFrameDuration,bullet_client) self._leftShoulderVel = self.ComputeAngVelRel(leftShoulderRotStart,leftShoulderRotEnd,keyFrameDuration,bullet_client)
leftElbowRotStart = [frameData[43]] leftElbowRotStart = [frameData[43]]
leftElbowRotEnd = [frameDataNext[43]] leftElbowRotEnd = [frameDataNext[43]]

43
examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py vendored
View File

@@ -1,5 +1,5 @@
from pybullet_envs.deep_mimic.env import pd_controller_stable from pybullet_utils import pd_controller_stable
from pybullet_envs.deep_mimic.env import humanoid_pose_interpolator from pybullet_envs.deep_mimic.env import humanoid_pose_interpolator
import math import math
@@ -24,10 +24,10 @@ class HumanoidStablePD(object):
print("LOADING humanoid!") print("LOADING humanoid!")
self._sim_model = self._pybullet_client.loadURDF( self._sim_model = self._pybullet_client.loadURDF(
"humanoid/humanoid.urdf", [0,0.85,0],globalScaling=0.25, useFixedBase=useFixedBase, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER) "humanoid/humanoid.urdf", [0,0.889540259,0],globalScaling=0.25, useFixedBase=useFixedBase, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
self._kin_model = self._pybullet_client.loadURDF( self._kin_model = self._pybullet_client.loadURDF(
"humanoid/humanoid.urdf", [0,2.85,0],globalScaling=0.25, useFixedBase=True, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER) "humanoid/humanoid.urdf", [0,12.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) self._pybullet_client.changeDynamics(self._sim_model, -1, lateralFriction=0.9)
for j in range (self._pybullet_client.getNumJoints(self._sim_model)): for j in range (self._pybullet_client.getNumJoints(self._sim_model)):
@@ -64,7 +64,7 @@ class HumanoidStablePD(object):
for dof in self._jointDofCounts: for dof in self._jointDofCounts:
self._totalDofs += dof self._totalDofs += dof
self.setSimTime(0) self.setSimTime(0)
self._maxForce = 6000
self.resetPose() self.resetPose()
def resetPose(self): def resetPose(self):
@@ -146,7 +146,9 @@ class HumanoidStablePD(object):
def computePose(self, frameFraction): def computePose(self, frameFraction):
frameData = self._mocap_data._motion_data['Frames'][self._frame] frameData = self._mocap_data._motion_data['Frames'][self._frame]
frameDataNext = self._mocap_data._motion_data['Frames'][self._frameNext] frameDataNext = self._mocap_data._motion_data['Frames'][self._frameNext]
pose = self._poseInterpolator.Slerp(frameFraction, frameData, frameDataNext, self._pybullet_client) pose = self._poseInterpolator.Slerp(frameFraction, frameData, frameDataNext, self._pybullet_client)
#print("self._poseInterpolator.Slerp(", frameFraction,")=", pose)
return pose return pose
@@ -154,27 +156,34 @@ class HumanoidStablePD(object):
pose = self._poseInterpolator.ConvertFromAction(self._pybullet_client, action) pose = self._poseInterpolator.ConvertFromAction(self._pybullet_client, action)
return pose return pose
def computePDForces(self, desiredPositions, desiredVelocities = None): def computePDForces(self, desiredPositions, desiredVelocities, maxForces):
if desiredVelocities==None: if desiredVelocities==None:
desiredVelocities = [0]*self._totalDofs desiredVelocities = [0]*self._totalDofs
taus = self._stablePD.computePD(bodyUniqueId=self._sim_model, taus = self._stablePD.computePD(bodyUniqueId=self._sim_model,
jointIndices = self._jointIndicesAll, jointIndices = self._jointIndicesAll,
desiredPositions = desiredPositions, desiredPositions = desiredPositions,
desiredVelocities = desiredVelocities, desiredVelocities = desiredVelocities,
kps = self._kpOrg, kps = self._kpOrg,
kds = self._kdOrg, kds = self._kdOrg,
maxForces = [self._maxForce]*self._totalDofs, maxForces = maxForces,
timeStep=self._timeStep) timeStep=self._timeStep)
return taus return taus
def applyPDForces(self, taus): def applyPDForces(self, taus):
dofIndex=7 dofIndex=7
scaling = 1
for index in range (len(self._jointIndicesAll)): for index in range (len(self._jointIndicesAll)):
jointIndex = self._jointIndicesAll[index] jointIndex = self._jointIndicesAll[index]
if self._jointDofCounts[index]==4: if self._jointDofCounts[index]==4:
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,jointIndex,self._pybullet_client.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]]) force=[scaling*taus[dofIndex+0],scaling*taus[dofIndex+1],scaling*taus[dofIndex+2]]
#print("force[", jointIndex,"]=",force)
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,jointIndex,self._pybullet_client.TORQUE_CONTROL,force=force)
if self._jointDofCounts[index]==1: if self._jointDofCounts[index]==1:
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, jointIndex, controlMode=self._pybullet_client.TORQUE_CONTROL, force=[taus[dofIndex]]) force=[scaling*taus[dofIndex]]
#print("force[", jointIndex,"]=",force)
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, jointIndex, controlMode=self._pybullet_client.TORQUE_CONTROL, force=force)
dofIndex+=self._jointDofCounts[index] dofIndex+=self._jointDofCounts[index]
@@ -257,6 +266,13 @@ class HumanoidStablePD(object):
for l in linkPosLocal: for l in linkPosLocal:
stateVector.append(l) stateVector.append(l)
#re-order the quaternion, DeepMimic uses w,x,y,z #re-order the quaternion, DeepMimic uses w,x,y,z
if (linkOrnLocal[3]<0):
linkOrnLocal[0]*=-1
linkOrnLocal[1]*=-1
linkOrnLocal[2]*=-1
linkOrnLocal[3]*=-1
stateVector.append(linkOrnLocal[3]) stateVector.append(linkOrnLocal[3])
stateVector.append(linkOrnLocal[0]) stateVector.append(linkOrnLocal[0])
stateVector.append(linkOrnLocal[1]) stateVector.append(linkOrnLocal[1])
@@ -268,9 +284,13 @@ class HumanoidStablePD(object):
ls = self._pybullet_client.getLinkState(self._sim_model, j, computeLinkVelocity=True) ls = self._pybullet_client.getLinkState(self._sim_model, j, computeLinkVelocity=True)
linkLinVel = ls[6] linkLinVel = ls[6]
linkAngVel = ls[7] linkAngVel = ls[7]
for l in linkLinVel: linkLinVelLocal , unused = self._pybullet_client.multiplyTransforms([0,0,0], rootTransOrn, linkLinVel,[0,0,0,1])
#linkLinVelLocal=[linkLinVelLocal[0]-rootPosRel[0],linkLinVelLocal[1]-rootPosRel[1],linkLinVelLocal[2]-rootPosRel[2]]
linkAngVelLocal ,unused = self._pybullet_client.multiplyTransforms([0,0,0], rootTransOrn, linkAngVel,[0,0,0,1])
for l in linkLinVelLocal:
stateVector.append(l) stateVector.append(l)
for l in linkAngVel: for l in linkAngVelLocal:
stateVector.append(l) stateVector.append(l)
#print("stateVector len=",len(stateVector)) #print("stateVector len=",len(stateVector))
@@ -284,6 +304,9 @@ class HumanoidStablePD(object):
pts = self._pybullet_client.getContactPoints() pts = self._pybullet_client.getContactPoints()
for p in pts: for p in pts:
part = -1 part = -1
#ignore self-collision
if (p[1]==p[2]):
continue
if (p[1]==self._sim_model): if (p[1]==self._sim_model):
part=p[3] part=p[3]
if (p[2]==self._sim_model): if (p[2]==self._sim_model):

144
examples/pybullet/gym/pybullet_envs/deep_mimic/env/pd_controller_stable.py vendored
View File

@@ -1,144 +0,0 @@
import numpy as np
class PDControllerStableMultiDof(object):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
#q1 = [desiredPositions[0],desiredPositions[1],desiredPositions[2],desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]]
q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
#qdot1 = [0,0,0, 0,0,0,0]
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0]
qError = [0,0,0, 0,0,0,0]
qIndex=7
qdotIndex=7
zeroAccelerations=[0,0,0, 0,0,0,0]
for i in range (numJoints):
js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
jointPos=js[0]
jointVel = js[1]
q1+=jointPos
if len(js[0])==1:
desiredPos=desiredPositions[qIndex]
qdiff=desiredPos - jointPos[0]
qError.append(qdiff)
zeroAccelerations.append(0.)
qdot1+=jointVel
qIndex+=1
qdotIndex+=1
if len(js[0])==4:
desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]]
axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos)
jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0]
qdot1+=jointVelNew
qError.append(axis[0])
qError.append(axis[1])
qError.append(axis[2])
qError.append(0)
desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]]
zeroAccelerations+=[0.,0.,0.,0.]
qIndex+=4
qdotIndex+=4
q = np.array(q1)
qdot=np.array(qdot1)
qdotdesired = np.array(desiredVelocities)
qdoterr = qdotdesired-qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
#np.savetxt("pb_qError.csv", qError, delimiter=",")
#np.savetxt("pb_p.csv", p, delimiter=",")
d = Kd.dot(qdoterr)
#np.savetxt("pb_d.csv", d, delimiter=",")
#np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",")
M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1, flags=1)
M2 = np.array(M1)
#np.savetxt("M2.csv", M2, delimiter=",")
M = (M2 + Kd * timeStep)
#np.savetxt("pbM_tKd.csv",M, delimiter=",")
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
c = np.array(c1)
#np.savetxt("pbC.csv",c, delimiter=",")
A = M
#p = [0]*43
b = p + d -c
#np.savetxt("pb_acc.csv",b, delimiter=",")
qddot = np.linalg.solve(A, b)
tau = p + d - Kd.dot(qddot) * timeStep
#print("len(tau)=",len(tau))
maxF = np.array(maxForces)
forces = np.clip(tau, -maxF , maxF )
return forces
class PDControllerStable(object):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = self._pb.getNumJoints(bodyUniqueId)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = []
qdot1 = []
zeroAccelerations = []
for i in range (numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
zeroAccelerations.append(0)
q = np.array(q1)
qdot=np.array(qdot1)
qdes = np.array(desiredPositions)
qdotdes = np.array(desiredVelocities)
qError = qdes - q
qdotError = qdotdes - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
d = Kd.dot(qdotError)
forces = p + d
M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
M2 = np.array(M1)
M = (M2 + Kd * timeStep)
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
c = np.array(c1)
A = M
b = -c + p + d
qddot = np.linalg.solve(A, b)
tau = p + d - Kd.dot(qddot) * timeStep
maxF = np.array(maxForces)
forces = np.clip(tau, -maxF , maxF )
#print("c=",c)
return tau

73
examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py vendored
View File

@@ -2,9 +2,9 @@ import numpy as np
import math import math
from pybullet_envs.deep_mimic.env.env import Env from pybullet_envs.deep_mimic.env.env import Env
from pybullet_envs.deep_mimic.env.action_space import ActionSpace from pybullet_envs.deep_mimic.env.action_space import ActionSpace
from pybullet_envs.minitaur.envs import bullet_client from pybullet_utils import bullet_client
import time import time
import motion_capture_data from pybullet_envs.deep_mimic.env import motion_capture_data
from pybullet_envs.deep_mimic.env import humanoid_stable_pd from pybullet_envs.deep_mimic.env import humanoid_stable_pd
import pybullet_data import pybullet_data
import pybullet as p1 import pybullet as p1
@@ -20,7 +20,10 @@ class PyBulletDeepMimicEnv(Env):
self.reset() self.reset()
def reset(self): def reset(self):
self.t = 0
startTime = 0. #float(rn)/rnrange * self._humanoid.getCycleTime()
self.t = startTime
if not self._isInitialized: if not self._isInitialized:
if self.enable_draw: if self.enable_draw:
self._pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI) self._pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
@@ -29,7 +32,7 @@ class PyBulletDeepMimicEnv(Env):
self._pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath()) self._pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
z2y = self._pybullet_client.getQuaternionFromEuler([-math.pi*0.5,0,0]) z2y = self._pybullet_client.getQuaternionFromEuler([-math.pi*0.5,0,0])
self._planeId = self._pybullet_client.loadURDF("plane.urdf",[0,0,0],z2y) self._planeId = self._pybullet_client.loadURDF("plane_implicit.urdf",[0,0,0],z2y, useMaximalCoordinates=True)
#print("planeId=",self._planeId) #print("planeId=",self._planeId)
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_Y_AXIS_UP,1) self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_Y_AXIS_UP,1)
self._pybullet_client.setGravity(0,-9.8,0) self._pybullet_client.setGravity(0,-9.8,0)
@@ -38,8 +41,8 @@ class PyBulletDeepMimicEnv(Env):
self._pybullet_client.changeDynamics(self._planeId, linkIndex=-1, lateralFriction=0.9) self._pybullet_client.changeDynamics(self._planeId, linkIndex=-1, lateralFriction=0.9)
self._mocapData = motion_capture_data.MotionCaptureData() self._mocapData = motion_capture_data.MotionCaptureData()
motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt" #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt" motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
self._mocapData.Load(motionPath) self._mocapData.Load(motionPath)
timeStep = 1./600 timeStep = 1./600
useFixedBase=False useFixedBase=False
@@ -65,16 +68,17 @@ class PyBulletDeepMimicEnv(Env):
#self._pybullet_client.stepSimulation() #self._pybullet_client.stepSimulation()
time.sleep(timeStep) time.sleep(timeStep)
#print("numframes = ", self._humanoid._mocap_data.NumFrames()) #print("numframes = ", self._humanoid._mocap_data.NumFrames())
startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2) #startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
rnrange = 1000 rnrange = 1000
rn = random.randint(0,rnrange) rn = random.randint(0,rnrange)
startTime = float(rn)/rnrange * self._humanoid.getCycleTime()
self._humanoid.setSimTime(startTime) self._humanoid.setSimTime(startTime)
self._humanoid.resetPose() self._humanoid.resetPose()
#this clears the contact points. Todo: add API to explicitly clear all contact points? #this clears the contact points. Todo: add API to explicitly clear all contact points?
self._pybullet_client.stepSimulation() #self._pybullet_client.stepSimulation()
self._humanoid.resetPose()
self.needs_update_time = self.t-1#force update
def get_num_agents(self): def get_num_agents(self):
return self._num_agents return self._num_agents
@@ -198,12 +202,14 @@ class PyBulletDeepMimicEnv(Env):
self._mode = mode self._mode = mode
def need_new_action(self, agent_id): def need_new_action(self, agent_id):
return True if self.t>=self.needs_update_time:
self.needs_update_time = self.t + 1./30.
return True
return False
def record_state(self, agent_id): def record_state(self, agent_id):
state = self._humanoid.getState() state = self._humanoid.getState()
state[1]=state[1]+0.008
#print("record_state=",state)
return np.array(state) return np.array(state)
@@ -216,24 +222,43 @@ class PyBulletDeepMimicEnv(Env):
return reward return reward
def set_action(self, agent_id, action): def set_action(self, agent_id, action):
#print("action=",)
#for a in action:
# print(a)
np.savetxt("pb_action.csv", action, delimiter=",")
self.desiredPose = self._humanoid.convertActionToPose(action) self.desiredPose = self._humanoid.convertActionToPose(action)
#we need the target root positon and orientation to be zero, to be compatible with deep mimic
self.desiredPose[0] = 0
self.desiredPose[1] = 0
self.desiredPose[2] = 0
self.desiredPose[3] = 0
self.desiredPose[4] = 0
self.desiredPose[5] = 0
self.desiredPose[6] = 0
target_pose = np.array(self.desiredPose)
np.savetxt("pb_target_pose.csv", target_pose, delimiter=",")
#print("set_action: desiredPose=", self.desiredPose) #print("set_action: desiredPose=", self.desiredPose)
def log_val(self, agent_id, val): def log_val(self, agent_id, val):
pass pass
def update(self, timeStep): def update(self, timeStep):
for i in range(10): #print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t)
for i in range(1):
self.t += timeStep self.t += timeStep
self._humanoid.setSimTime(self.t) self._humanoid.setSimTime(self.t)
if self.desiredPose: 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=False)
pos,orn=self._pybullet_client.getBasePositionAndOrientation(self._humanoid._sim_model) #pos,orn=self._pybullet_client.getBasePositionAndOrientation(self._humanoid._sim_model)
self._pybullet_client.resetBasePositionAndOrientation(self._humanoid._kin_model, [pos[0],pos[1]+2,pos[2]],orn) #self._pybullet_client.resetBasePositionAndOrientation(self._humanoid._kin_model, [pos[0]+3,pos[1],pos[2]],orn)
#print("desiredPositions=",self.desiredPose) #print("desiredPositions=",self.desiredPose)
taus = self._humanoid.computePDForces(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) self._humanoid.applyPDForces(taus)
self._pybullet_client.stepSimulation() self._pybullet_client.stepSimulation()
@@ -255,3 +280,13 @@ class PyBulletDeepMimicEnv(Env):
def check_valid_episode(self): def check_valid_episode(self):
#could check if limbs exceed velocity threshold #could check if limbs exceed velocity threshold
return true return true
def getKeyboardEvents(self):
return self._pybullet_client.getKeyboardEvents()
def isKeyTriggered(self, keys, key):
o = ord(key)
#print("ord=",o)
if o in keys:
return keys[ord(key)] & self._pybullet_client.KEY_WAS_TRIGGERED
return False

122
examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py vendored Normal file
View File

@@ -0,0 +1,122 @@
from pybullet_utils import bullet_client
import time
import math
import motion_capture_data
from pybullet_envs.deep_mimic.env import humanoid_stable_pd
import pybullet_data
import pybullet as p1
import humanoid_pose_interpolator
import numpy as np
pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
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)
#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"
mocapData.Load(motionPath)
timeStep = 1./600
useFixedBase=False
humanoid = humanoid_stable_pd.HumanoidStablePD(pybullet_client, mocapData, timeStep, useFixedBase)
isInitialized = True
pybullet_client.setTimeStep(timeStep)
pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
timeId = pybullet_client.addUserDebugParameter("time",0,10,0)
#startPose = humanoid_pose_interpolator.HumanoidPoseInterpolator()
#startPose.Reset(basePos=[-0.000000,0.889540,0.000000],baseOrn=[0.029215,-0.000525,-0.017963,0.999412],chestRot=[0.000432,0.000572,0.005500,0.999985],
# neckRot=[0.001660,-0.011168,-0.140597,0.990003],rightHipRot=[-0.024450,-0.000839,-0.038869,0.998945],rightKneeRot=[-0.014186],rightAnkleRot=[0.010757,-0.105223,0.035405,0.993760],
# rightShoulderRot=[-0.003003,-0.124234,0.073280,0.989539],rightElbowRot=[0.240463],leftHipRot=[-0.020920,-0.012925,-0.006300,0.999678],leftKneeRot=[-0.027859],
# leftAnkleRot=[-0.010764,0.105284,-0.009276,0.994341],leftShoulderRot=[0.055661,-0.019608,0.098917,0.993344],leftElbowRot=[0.148934],
# baseLinVel=[-0.340837,0.377742,0.009662],baseAngVel=[0.047057,0.285253,-0.248554],chestVel=[-0.016455,-0.070035,-0.231662],neckVel=[0.072168,0.097898,-0.059063],
# rightHipVel=[-0.315908,-0.131685,1.114815],rightKneeVel=[0.103419],rightAnkleVel=[-0.409780,-0.099954,-4.241572],rightShoulderVel=[-3.324227,-2.510209,1.834637],
# rightElbowVel=[-0.212299],leftHipVel=[0.173056,-0.191063,1.226781,0.000000],leftKneeVel=[-0.665322],leftAnkleVel=[0.282716,0.086217,-3.007098,0.000000],
# leftShoulderVel=[4.253144,2.038637,1.170750],leftElbowVel=[0.387993])
#
#targetPose = humanoid_pose_interpolator.HumanoidPoseInterpolator()
#targetPose.Reset(basePos=[0.000000,0.000000,0.000000],baseOrn=[0.000000,0.000000,0.000000,1.000000],chestRot=[-0.006711,0.007196,-0.027119,0.999584],neckRot=[-0.017613,-0.033879,-0.209250,0.977116],
# rightHipRot=[-0.001697,-0.006510,0.046117,0.998913],rightKneeRot=[0.366954],rightAnkleRot=[0.012605,0.001208,-0.187007,0.982277],rightShoulderRot=[-0.468057,-0.044589,0.161134,0.867739],
# rightElbowRot=[-0.593650],leftHipRot=[0.006993,0.017242,0.049703,0.998591],leftKneeRot=[0.395147],leftAnkleRot=[-0.008922,0.026517,-0.217852,0.975581],
# leftShoulderRot=[0.426160,-0.266177,0.044672,0.863447],leftElbowRot=[-0.726281])
#out_tau= [0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-33.338211,-1.381748,-118.708975,0.000000,-2.813919,-2.773850,-0.772481,0.000000,31.885372,2.658243,64.988216,0.000000,94.773133,1.784944,6.240010,5.407563,0.000000,-180.441290,-6.821173,-19.502417,0.000000,-44.518261,9.992627,-2.380950,53.057697,0.000000,111.728594,-1.218496,-4.630812,4.268995,0.000000,89.741829,-8.460265,-117.727884,0.000000,-79.481906]
#,mSimWorld->stepSimulation(timestep:0.001667, mParams.mNumSubsteps:2, subtimestep:0.000833)
#cImpPDController::CalcControlForces timestep=0.001667
def isKeyTriggered(keys, key):
o = ord(key)
if o in keys:
return keys[ord(key)] & pybullet_client.KEY_WAS_TRIGGERED
return False
animating = False
singleStep = False
#humanoid.initializePose(pose=startPose, phys_model = humanoid._sim_model, initBase=True, initializeVelocity=True)
t=0
while (1):
keys = pybullet_client.getKeyboardEvents()
#print(keys)
if isKeyTriggered(keys, ' '):
animating = not animating
if isKeyTriggered(keys, 'b'):
singleStep = True
if animating or singleStep:
singleStep = False
#t = pybullet_client.readUserDebugParameter(timeId)
#print("t=",t)
for i in range (1):
print("t=",t)
humanoid.setSimTime(t)
humanoid.computePose(humanoid._frameFraction)
pose = humanoid._poseInterpolator
#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()
#curPose = HumanoidPoseInterpolator()
#curPose.reset()
s = humanoid.getState()
#np.savetxt("pb_record_state_s.csv", s, delimiter=",")
taus = humanoid.computePDForces(desiredPose)
#print("taus=",taus)
humanoid.applyPDForces(taus)
pybullet_client.stepSimulation()
t+=1./600.
time.sleep(1./600.)

1
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py
View File

@@ -147,6 +147,7 @@ class PPOAgent(PGAgent):
def _decide_action(self, s, g): def _decide_action(self, s, g):
with self.sess.as_default(), self.graph.as_default(): with self.sess.as_default(), self.graph.as_default():
self._exp_action = self._enable_stoch_policy() and MathUtil.flip_coin(self.exp_params_curr.rate) self._exp_action = self._enable_stoch_policy() and MathUtil.flip_coin(self.exp_params_curr.rate)
#print("_decide_action._exp_action=",self._exp_action)
a, logp = self._eval_actor(s, g, self._exp_action) a, logp = self._eval_actor(s, g, self._exp_action)
return a[0], logp[0] return a[0], logp[0]

5
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py
View File

@@ -123,8 +123,7 @@ class RLAgent(ABC):
if self.need_new_action(): if self.need_new_action():
#print("update_new_action!!!") #print("update_new_action!!!")
self._update_new_action() self._update_new_action()
else:
print("no action???")
if (self._mode == self.Mode.TRAIN and self.enable_training): if (self._mode == self.Mode.TRAIN and self.enable_training):
self._update_counter += timestep self._update_counter += timestep
@@ -329,7 +328,9 @@ class RLAgent(ABC):
return return
def _update_new_action(self): def _update_new_action(self):
#print("_update_new_action!")
s = self._record_state() s = self._record_state()
#np.savetxt("pb_record_state_s.csv", s, delimiter=",")
g = self._record_goal() g = self._record_goal()
if not (self._is_first_step()): if not (self._is_first_step()):

15
examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py
View File

@@ -16,6 +16,7 @@ import sys
import random import random
update_timestep = 1./600. update_timestep = 1./600.
animating = True
def update_world(world, time_elapsed): def update_world(world, time_elapsed):
timeStep = 1./600. timeStep = 1./600.
@@ -74,9 +75,21 @@ def build_world(args, enable_draw):
world.reset() world.reset()
return world return world
if __name__ == '__main__': if __name__ == '__main__':
world = build_world(args, True) world = build_world(args, True)
while (world.env._pybullet_client.isConnected()): while (world.env._pybullet_client.isConnected()):
timeStep = 1./600. timeStep = 1./600.
update_world(world, timeStep) keys = world.env.getKeyboardEvents()
if world.env.isKeyTriggered(keys, ' '):
animating = not animating
if (animating):
update_world(world, timeStep)
#animating=False

193
examples/pybullet/gym/pybullet_utils/pd_controller_stable.py Normal file
View File

@@ -0,0 +1,193 @@
import numpy as np
class PDControllerStableMultiDof(object):
def __init__(self, pb):
self._pb = pb
def computeAngVel(self,ornStart, ornEnd, deltaTime, bullet_client):
dorn = bullet_client.getDifferenceQuaternion(ornStart,ornEnd)
axis,angle = bullet_client.getAxisAngleFromQuaternion(dorn)
angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
return angVel
#def computeAngVelRel(self,ornStart, ornEnd, deltaTime, bullet_client):
# ornStartConjugate = [-ornStart[0],-ornStart[1],-ornStart[2],ornStart[3]]
# pos_diff, q_diff =bullet_client.multiplyTransforms([0,0,0], ornStartConjugate, [0,0,0], ornEnd)
# axis,angle = bullet_client.getAxisAngleFromQuaternion(q_diff)
# angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
# return angVel
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
#print("q1=",q1)
#qdot1 = [0,0,0, 0,0,0,0]
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
#print("baseLinVel=",baseLinVel)
qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0]
#qError = [0,0,0, 0,0,0,0]
desiredOrn = [desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]]
axis1 = self._pb.getAxisDifferenceQuaternion(desiredOrn,curOrn)
angDiff = [0,0,0]#self.computeAngVel(curOrn, desiredOrn, 1, self._pb)
qError=[ desiredPositions[0]-curPos[0], desiredPositions[1]-curPos[1], desiredPositions[2]-curPos[2],angDiff[0],angDiff[1],angDiff[2],0]
target_pos = np.array(desiredPositions)
#np.savetxt("pb_target_pos.csv", target_pos, delimiter=",")
qIndex=7
qdotIndex=7
zeroAccelerations=[0,0,0, 0,0,0,0]
for i in range (numJoints):
js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
jointPos=js[0]
jointVel = js[1]
q1+=jointPos
if len(js[0])==1:
desiredPos=desiredPositions[qIndex]
qdiff=desiredPos - jointPos[0]
qError.append(qdiff)
zeroAccelerations.append(0.)
qdot1+=jointVel
qIndex+=1
qdotIndex+=1
if len(js[0])==4:
desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]]
#axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos)
#angDiff = self.computeAngVelRel(jointPos, desiredPos, 1, self._pb)
angDiff = self._pb.computeAngVelRel(jointPos, desiredPos, 1)
jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0]
qdot1+=jointVelNew
qError.append(angDiff[0])
qError.append(angDiff[1])
qError.append(angDiff[2])
qError.append(0)
desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]]
zeroAccelerations+=[0.,0.,0.,0.]
qIndex+=4
qdotIndex+=4
q = np.array(q1)
qerr = np.array(qError)
#np.savetxt("pb_qerro.csv",qerr,delimiter=",")
#np.savetxt("pb_q.csv", q, delimiter=",")
qdot=np.array(qdot1)
#np.savetxt("qdot.csv", qdot, delimiter=",")
qdotdesired = np.array(desiredVelocities)
qdoterr = qdotdesired-qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
#np.savetxt("pb_qError.csv", qError, delimiter=",")
#np.savetxt("pb_p.csv", p, delimiter=",")
d = Kd.dot(qdoterr)
#np.savetxt("pb_d.csv", d, delimiter=",")
#np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",")
M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1, flags=1)
M2 = np.array(M1)
#np.savetxt("M2.csv", M2, delimiter=",")
M = (M2 + Kd * timeStep)
#np.savetxt("pbM_tKd.csv",M, delimiter=",")
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
c = np.array(c1)
#np.savetxt("pb_C.csv",c, delimiter=",")
A = M
#p = [0]*43
#np.savetxt("pb_kp_dot_qError.csv", p)
#np.savetxt("pb_kd_dot_vError.csv", d)
b = p + d -c
#np.savetxt("pb_b_acc.csv",b, delimiter=",")
useNumpySolver = False
if useNumpySolver:
qddot = np.linalg.solve(A, b)
else:
dofCount = len(b)
print(dofCount)
qddot = self._pb.ldltSolve(bodyUniqueId, jointPositions=q1, b=b.tolist(), kd=kds, t=timeStep)
tau = p + d - Kd.dot(qddot) * timeStep
#print("len(tau)=",len(tau))
#np.savetxt("pb_tau_not_clamped.csv", tau, delimiter=",")
maxF = np.array(maxForces)
#print("maxF",maxF)
forces = np.clip(tau, -maxF , maxF )
#np.savetxt("pb_tau_clamped.csv", tau, delimiter=",")
return forces
class PDControllerStable(object):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = self._pb.getNumJoints(bodyUniqueId)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = []
qdot1 = []
zeroAccelerations = []
for i in range (numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
zeroAccelerations.append(0)
q = np.array(q1)
qdot=np.array(qdot1)
qdes = np.array(desiredPositions)
qdotdes = np.array(desiredVelocities)
qError = qdes - q
qdotError = qdotdes - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
d = Kd.dot(qdotError)
forces = p + d
M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
M2 = np.array(M1)
M = (M2 + Kd * timeStep)
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
c = np.array(c1)
A = M
b = -c + p + d
qddot = np.linalg.solve(A, b)
tau = p + d - Kd.dot(qddot) * timeStep
maxF = np.array(maxForces)
forces = np.clip(tau, -maxF , maxF )
#print("c=",c)
return tau

7
examples/pybullet/pybullet.c
View File

@@ -10711,11 +10711,11 @@ static PyMethodDef SpamMethods[] = {
{NULL, NULL, 0, NULL} /* Sentinel */ {NULL, NULL, 0, NULL} /* Sentinel */
}; };
///copied from CommonWindowInterface.h ///copied from CommonCallbacks.h
enum enum
{ {
B3G_ESCAPE = 27, B3G_ESCAPE = 27,
B3G_SPACE = 32,
B3G_F1 = 0xff00, B3G_F1 = 0xff00,
B3G_F2, B3G_F2,
B3G_F3, B3G_F3,
@@ -10745,7 +10745,7 @@ enum
B3G_SHIFT, B3G_SHIFT,
B3G_CONTROL, B3G_CONTROL,
B3G_ALT, B3G_ALT,
B3G_RETURN B3G_RETURN,
}; };
#if PY_MAJOR_VERSION >= 3 #if PY_MAJOR_VERSION >= 3
@@ -10959,6 +10959,7 @@ initpybullet(void)
PyModule_AddIntConstant(m, "B3G_CONTROL", B3G_CONTROL); PyModule_AddIntConstant(m, "B3G_CONTROL", B3G_CONTROL);
PyModule_AddIntConstant(m, "B3G_ALT", B3G_ALT); PyModule_AddIntConstant(m, "B3G_ALT", B3G_ALT);
PyModule_AddIntConstant(m, "B3G_RETURN", B3G_RETURN); PyModule_AddIntConstant(m, "B3G_RETURN", B3G_RETURN);
PyModule_AddIntConstant(m, "B3G_SPACE", B3G_SPACE);
PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE); PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX); PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);