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

implement stablePD control version of testLaikago, in preparation for quadruped DeepMimic

Browse Source
This commit is contained in:
Erwin Coumans
2019-04-05 16:45:33 -07:00
parent 3146f6276b
commit 4ae24083ee
4 changed files with 656 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
examples/pybullet/gym/pybullet_utils/pd_controller_stable.py
View File

@@ -165,11 +165,25 @@ class PDControllerStable(object):
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)
numBaseDofs = 0
numPosBaseDofs=0
baseMass = self._pb.getDynamicsInfo(bodyUniqueId,-1)[0]
curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
q1 = []
qdot1 = []
zeroAccelerations = []
qError=[]
if (baseMass>0):
numBaseDofs=6
numPosBaseDofs=7
q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
qdot1=[0]*numBaseDofs
zeroAccelerations = [0]*numBaseDofs
angDiff=[0,0,0]
qError=[ desiredPositions[0]-curPos[0], desiredPositions[1]-curPos[1], desiredPositions[2]-curPos[2],angDiff[0],angDiff[1],angDiff[2]]
numJoints = self._pb.getNumJoints(bodyUniqueId)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
for i in range (numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
@@ -178,7 +192,10 @@ class PDControllerStable(object):
qdot=np.array(qdot1)
qdes = np.array(desiredPositions)
qdotdes = np.array(desiredVelocities)
qError = qdes - q
#qError = qdes - q
for j in range(numJoints):
qError.append(desiredPositions[j+numPosBaseDofs]-q1[j+numPosBaseDofs])
#print("qError=",qError)
qdotError = qdotdes - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)