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

add yapf style and apply yapf to format all Python files

Browse Source 
This recreates pull request #2192
This commit is contained in:
Erwin Coumans
2019-04-27 07:31:15 -07:00
parent c591735042
commit ef9570c315
347 changed files with 70304 additions and 22752 deletions
Download Patch File Download Diff File Expand all files Collapse all files

169
examples/pybullet/examples/pdControllerExplicit.py
View File

@@ -2,88 +2,97 @@ import numpy as np
class PDControllerExplicitMultiDof(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 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
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)
d = Kd.dot(qdoterr)
forces = p + d
maxF = np.array(maxForces)
forces = np.clip(forces, -maxF , maxF )
return forces
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 = [curPos[0], curPos[1], curPos[2], curOrn[0], curOrn[1], curOrn[2], curOrn[3]]
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)
d = Kd.dot(qdoterr)
forces = p + d
maxF = np.array(maxForces)
forces = np.clip(forces, -maxF, maxF)
return forces
class PDControllerExplicit(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 = []
for i in range (numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
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)
forces = Kp.dot(qError) + Kd.dot(qdotError)
maxF = np.array(maxForces)
forces = np.clip(forces, -maxF , maxF )
return forces
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 = []
for i in range(numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
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)
forces = Kp.dot(qError) + Kd.dot(qdotError)
maxF = np.array(maxForces)
forces = np.clip(forces, -maxF, maxF)
return forces