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.
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
@@ -11,6 +11,7 @@ typedef void (*b3RenderCallback)();
|
||||
enum
|
||||
{
|
||||
B3G_ESCAPE = 27,
|
||||
B3G_SPACE = 32,
|
||||
B3G_F1 = 0xff00,
|
||||
B3G_F2,
|
||||
B3G_F3,
|
||||
@@ -40,7 +41,8 @@ enum
|
||||
B3G_SHIFT,
|
||||
B3G_CONTROL,
|
||||
B3G_ALT,
|
||||
B3G_RETURN
|
||||
B3G_RETURN,
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -67,6 +67,11 @@ int getSpecialKeyFromVirtualKeycode(int virtualKeyCode)
|
||||
|
||||
switch (virtualKeyCode)
|
||||
{
|
||||
case VK_SPACE:
|
||||
{
|
||||
keycode = B3G_SPACE;
|
||||
break;
|
||||
}
|
||||
case VK_RETURN:
|
||||
{
|
||||
keycode = B3G_RETURN;
|
||||
|
||||
@@ -2,8 +2,8 @@
|
||||
<link name="base" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0 0 0" />
|
||||
<mass value = "0.00100" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
<mass value = "0.0001" />
|
||||
<inertia ixx = "0.0001" ixy = "0" ixz = "0" iyy = "0.0001" iyz = "0" izz = "0.0001" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="root" >
|
||||
|
||||
14
examples/pybullet/gym/pybullet_data/plane_transparent.mtl
Normal file
14
examples/pybullet/gym/pybullet_data/plane_transparent.mtl
Normal 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
18
examples/pybullet/gym/pybullet_data/plane_transparent.obj
Normal 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
29
examples/pybullet/gym/pybullet_data/plane_transparent.urdf
Normal 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>
|
||||
|
||||
@@ -1 +1 @@
|
||||
from pybullet_envs.deep_mimic.humanoid_deepmimic_gym_env import HumanoidDeepMimicGymEnv
|
||||
|
||||
|
||||
@@ -1,45 +1,51 @@
|
||||
from pybullet_envs.minitaur.envs import bullet_client
|
||||
from pybullet_utils import bullet_client
|
||||
import math
|
||||
|
||||
class HumanoidPoseInterpolator(object):
|
||||
def __init__(self):
|
||||
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._baseLinVel = [0,0,0]
|
||||
self._baseOrn = [0,0,0,1]
|
||||
self._baseAngVel = [0,0,0]
|
||||
self._basePos = basePos
|
||||
self._baseLinVel = baseLinVel
|
||||
print("HumanoidPoseInterpolator.Reset: baseLinVel = ", baseLinVel)
|
||||
self._baseOrn =baseOrn
|
||||
self._baseAngVel = baseAngVel
|
||||
|
||||
self._chestRot = [0,0,0,1]
|
||||
self._chestVel = [0,0,0]
|
||||
self._neckRot = [0,0,0,1]
|
||||
self._neckVel = [0,0,0]
|
||||
self._chestRot = chestRot
|
||||
self._chestVel =chestVel
|
||||
self._neckRot = neckRot
|
||||
self._neckVel = neckVel
|
||||
|
||||
self._rightHipRot = [0,0,0,1]
|
||||
self._rightHipVel = [0,0,0]
|
||||
self._rightKneeRot = [0]
|
||||
self._rightKneeVel = [0]
|
||||
self._rightAnkleRot = [0,0,0,1]
|
||||
self._rightAnkleVel = [0,0,0]
|
||||
self._rightHipRot = rightHipRot
|
||||
self._rightHipVel = rightHipVel
|
||||
self._rightKneeRot =rightKneeRot
|
||||
self._rightKneeVel = rightKneeVel
|
||||
self._rightAnkleRot = rightAnkleRot
|
||||
self._rightAnkleVel = rightAnkleVel
|
||||
|
||||
self._rightShoulderRot = [0,0,0,1]
|
||||
self._rightShoulderVel = [0,0,0]
|
||||
self._rightElbowRot = [0]
|
||||
self._rightElbowVel = [0]
|
||||
self._rightShoulderRot =rightShoulderRot
|
||||
self._rightShoulderVel = rightShoulderVel
|
||||
self._rightElbowRot = rightElbowRot
|
||||
self._rightElbowVel = rightElbowVel
|
||||
|
||||
self._leftHipRot = [0,0,0,1]
|
||||
self._leftHipVel = [0,0,0]
|
||||
self._leftKneeRot = [0]
|
||||
self._leftKneeVel = [0]
|
||||
self._leftAnkleRot = [0,0,0,1]
|
||||
self._leftAnkleVel = [0,0,0]
|
||||
self._leftHipRot = leftHipRot
|
||||
self._leftHipVel = leftHipVel
|
||||
self._leftKneeRot = leftKneeRot
|
||||
self._leftKneeVel = leftKneeVel
|
||||
self._leftAnkleRot =leftAnkleRot
|
||||
self._leftAnkleVel = leftAnkleVel
|
||||
|
||||
self._leftShoulderRot = [0,0,0,1]
|
||||
self._leftShoulderVel = [0,0,0]
|
||||
self._leftElbowRot = [0]
|
||||
self._leftElbowVel = [0]
|
||||
self._leftShoulderRot = leftShoulderRot
|
||||
self._leftShoulderVel = leftShoulderVel
|
||||
self._leftElbowRot =leftElbowRot
|
||||
self._leftElbowVel = leftElbowVel
|
||||
|
||||
def ComputeLinVel(self,posStart, posEnd, deltaTime):
|
||||
vel = [(posEnd[0]-posStart[0])/deltaTime,(posEnd[1]-posStart[1])/deltaTime,(posEnd[2]-posStart[2])/deltaTime]
|
||||
@@ -50,6 +56,14 @@ class HumanoidPoseInterpolator(object):
|
||||
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 NormalizeVector(self, vec):
|
||||
length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2]
|
||||
@@ -121,17 +135,17 @@ class HumanoidPoseInterpolator(object):
|
||||
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
|
||||
chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
|
||||
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]]
|
||||
neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
|
||||
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]]
|
||||
rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
|
||||
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]]
|
||||
rightKneeRotEnd = [frameDataNext[20]]
|
||||
@@ -141,12 +155,12 @@ class HumanoidPoseInterpolator(object):
|
||||
rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
|
||||
rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]]
|
||||
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]]
|
||||
rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
|
||||
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]]
|
||||
rightElbowRotEnd = [frameDataNext[29]]
|
||||
@@ -156,7 +170,7 @@ class HumanoidPoseInterpolator(object):
|
||||
leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
|
||||
leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]]
|
||||
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]]
|
||||
leftKneeRotEnd = [frameDataNext[34]]
|
||||
@@ -166,12 +180,12 @@ class HumanoidPoseInterpolator(object):
|
||||
leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
|
||||
leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
|
||||
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]]
|
||||
leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]]
|
||||
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]]
|
||||
leftElbowRotEnd = [frameDataNext[43]]
|
||||
|
||||
@@ -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
|
||||
import math
|
||||
|
||||
@@ -24,10 +24,10 @@ class HumanoidStablePD(object):
|
||||
print("LOADING humanoid!")
|
||||
|
||||
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(
|
||||
"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)
|
||||
for j in range (self._pybullet_client.getNumJoints(self._sim_model)):
|
||||
@@ -64,7 +64,7 @@ class HumanoidStablePD(object):
|
||||
for dof in self._jointDofCounts:
|
||||
self._totalDofs += dof
|
||||
self.setSimTime(0)
|
||||
self._maxForce = 6000
|
||||
|
||||
self.resetPose()
|
||||
|
||||
def resetPose(self):
|
||||
@@ -146,7 +146,9 @@ class HumanoidStablePD(object):
|
||||
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)
|
||||
#print("self._poseInterpolator.Slerp(", frameFraction,")=", pose)
|
||||
return pose
|
||||
|
||||
|
||||
@@ -154,27 +156,34 @@ class HumanoidStablePD(object):
|
||||
pose = self._poseInterpolator.ConvertFromAction(self._pybullet_client, action)
|
||||
return pose
|
||||
|
||||
def computePDForces(self, desiredPositions, desiredVelocities = None):
|
||||
def computePDForces(self, desiredPositions, desiredVelocities, maxForces):
|
||||
if desiredVelocities==None:
|
||||
desiredVelocities = [0]*self._totalDofs
|
||||
|
||||
|
||||
taus = self._stablePD.computePD(bodyUniqueId=self._sim_model,
|
||||
jointIndices = self._jointIndicesAll,
|
||||
desiredPositions = desiredPositions,
|
||||
desiredVelocities = desiredVelocities,
|
||||
kps = self._kpOrg,
|
||||
kds = self._kdOrg,
|
||||
maxForces = [self._maxForce]*self._totalDofs,
|
||||
maxForces = maxForces,
|
||||
timeStep=self._timeStep)
|
||||
return taus
|
||||
|
||||
def applyPDForces(self, taus):
|
||||
dofIndex=7
|
||||
scaling = 1
|
||||
for index in range (len(self._jointIndicesAll)):
|
||||
jointIndex = self._jointIndicesAll[index]
|
||||
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:
|
||||
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]
|
||||
|
||||
|
||||
@@ -257,6 +266,13 @@ class HumanoidStablePD(object):
|
||||
for l in linkPosLocal:
|
||||
stateVector.append(l)
|
||||
#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[0])
|
||||
stateVector.append(linkOrnLocal[1])
|
||||
@@ -268,9 +284,13 @@ class HumanoidStablePD(object):
|
||||
ls = self._pybullet_client.getLinkState(self._sim_model, j, computeLinkVelocity=True)
|
||||
linkLinVel = ls[6]
|
||||
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)
|
||||
for l in linkAngVel:
|
||||
for l in linkAngVelLocal:
|
||||
stateVector.append(l)
|
||||
|
||||
#print("stateVector len=",len(stateVector))
|
||||
@@ -284,6 +304,9 @@ class HumanoidStablePD(object):
|
||||
pts = self._pybullet_client.getContactPoints()
|
||||
for p in pts:
|
||||
part = -1
|
||||
#ignore self-collision
|
||||
if (p[1]==p[2]):
|
||||
continue
|
||||
if (p[1]==self._sim_model):
|
||||
part=p[3]
|
||||
if (p[2]==self._sim_model):
|
||||
|
||||
@@ -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
|
||||
@@ -2,9 +2,9 @@ import numpy as np
|
||||
import math
|
||||
from pybullet_envs.deep_mimic.env.env import Env
|
||||
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 motion_capture_data
|
||||
from pybullet_envs.deep_mimic.env import motion_capture_data
|
||||
from pybullet_envs.deep_mimic.env import humanoid_stable_pd
|
||||
import pybullet_data
|
||||
import pybullet as p1
|
||||
@@ -20,7 +20,10 @@ class PyBulletDeepMimicEnv(Env):
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.t = 0
|
||||
|
||||
|
||||
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)
|
||||
@@ -29,7 +32,7 @@ class PyBulletDeepMimicEnv(Env):
|
||||
|
||||
self._pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
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)
|
||||
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_Y_AXIS_UP,1)
|
||||
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._mocapData = motion_capture_data.MotionCaptureData()
|
||||
motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
|
||||
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
|
||||
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
|
||||
motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
|
||||
self._mocapData.Load(motionPath)
|
||||
timeStep = 1./600
|
||||
useFixedBase=False
|
||||
@@ -48,7 +51,7 @@ class PyBulletDeepMimicEnv(Env):
|
||||
|
||||
self._pybullet_client.setTimeStep(timeStep)
|
||||
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
|
||||
|
||||
|
||||
|
||||
selfCheck = False
|
||||
if (selfCheck):
|
||||
@@ -65,16 +68,17 @@ class PyBulletDeepMimicEnv(Env):
|
||||
#self._pybullet_client.stepSimulation()
|
||||
time.sleep(timeStep)
|
||||
#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
|
||||
rn = random.randint(0,rnrange)
|
||||
startTime = float(rn)/rnrange * self._humanoid.getCycleTime()
|
||||
|
||||
|
||||
self._humanoid.setSimTime(startTime)
|
||||
|
||||
self._humanoid.resetPose()
|
||||
#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):
|
||||
return self._num_agents
|
||||
@@ -198,12 +202,14 @@ class PyBulletDeepMimicEnv(Env):
|
||||
self._mode = mode
|
||||
|
||||
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):
|
||||
state = self._humanoid.getState()
|
||||
state[1]=state[1]+0.008
|
||||
#print("record_state=",state)
|
||||
|
||||
return np.array(state)
|
||||
|
||||
|
||||
@@ -216,24 +222,43 @@ class PyBulletDeepMimicEnv(Env):
|
||||
return reward
|
||||
|
||||
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)
|
||||
#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)
|
||||
|
||||
def log_val(self, agent_id, val):
|
||||
pass
|
||||
|
||||
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._humanoid.setSimTime(self.t)
|
||||
|
||||
if self.desiredPose:
|
||||
kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
|
||||
self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=False)
|
||||
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)
|
||||
#kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
|
||||
#self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=False)
|
||||
#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)
|
||||
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._pybullet_client.stepSimulation()
|
||||
|
||||
@@ -255,3 +280,13 @@ class PyBulletDeepMimicEnv(Env):
|
||||
def check_valid_episode(self):
|
||||
#could check if limbs exceed velocity threshold
|
||||
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
122
examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py
vendored
Normal 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.)
|
||||
@@ -147,6 +147,7 @@ class PPOAgent(PGAgent):
|
||||
def _decide_action(self, s, g):
|
||||
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)
|
||||
#print("_decide_action._exp_action=",self._exp_action)
|
||||
a, logp = self._eval_actor(s, g, self._exp_action)
|
||||
return a[0], logp[0]
|
||||
|
||||
|
||||
@@ -123,8 +123,7 @@ class RLAgent(ABC):
|
||||
if self.need_new_action():
|
||||
#print("update_new_action!!!")
|
||||
self._update_new_action()
|
||||
else:
|
||||
print("no action???")
|
||||
|
||||
|
||||
if (self._mode == self.Mode.TRAIN and self.enable_training):
|
||||
self._update_counter += timestep
|
||||
@@ -329,7 +328,9 @@ class RLAgent(ABC):
|
||||
return
|
||||
|
||||
def _update_new_action(self):
|
||||
#print("_update_new_action!")
|
||||
s = self._record_state()
|
||||
#np.savetxt("pb_record_state_s.csv", s, delimiter=",")
|
||||
g = self._record_goal()
|
||||
|
||||
if not (self._is_first_step()):
|
||||
|
||||
@@ -16,6 +16,7 @@ import sys
|
||||
import random
|
||||
|
||||
update_timestep = 1./600.
|
||||
animating = True
|
||||
|
||||
def update_world(world, time_elapsed):
|
||||
timeStep = 1./600.
|
||||
@@ -74,9 +75,21 @@ def build_world(args, enable_draw):
|
||||
world.reset()
|
||||
return world
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
world = build_world(args, True)
|
||||
while (world.env._pybullet_client.isConnected()):
|
||||
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
193
examples/pybullet/gym/pybullet_utils/pd_controller_stable.py
Normal 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
|
||||
@@ -9286,7 +9286,7 @@ static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args
|
||||
int physicsClientId = 0;
|
||||
int hasQuatStart = 0;
|
||||
int hasQuatEnd = 0;
|
||||
|
||||
|
||||
static char* kwlist[] = { "quaternionStart", "quaternionEnd", "physicsClientId", NULL };
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatStartObj, &quatEndObj, &physicsClientId))
|
||||
{
|
||||
@@ -10711,11 +10711,11 @@ static PyMethodDef SpamMethods[] = {
|
||||
{NULL, NULL, 0, NULL} /* Sentinel */
|
||||
};
|
||||
|
||||
///copied from CommonWindowInterface.h
|
||||
|
||||
///copied from CommonCallbacks.h
|
||||
enum
|
||||
{
|
||||
B3G_ESCAPE = 27,
|
||||
B3G_SPACE = 32,
|
||||
B3G_F1 = 0xff00,
|
||||
B3G_F2,
|
||||
B3G_F3,
|
||||
@@ -10745,7 +10745,7 @@ enum
|
||||
B3G_SHIFT,
|
||||
B3G_CONTROL,
|
||||
B3G_ALT,
|
||||
B3G_RETURN
|
||||
B3G_RETURN,
|
||||
};
|
||||
|
||||
#if PY_MAJOR_VERSION >= 3
|
||||
@@ -10959,6 +10959,7 @@ initpybullet(void)
|
||||
PyModule_AddIntConstant(m, "B3G_CONTROL", B3G_CONTROL);
|
||||
PyModule_AddIntConstant(m, "B3G_ALT", B3G_ALT);
|
||||
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_BOX", GEOM_BOX);
|
||||
|
||||
Reference in New Issue
Block a user