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

PyBullet deep_mimic fix computeAngVelRel

Browse Source
This commit is contained in:
erwincoumans
2019-02-10 21:37:34 -08:00
parent bb15800964
commit d4292fdac3
2 changed files with 18 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -12,12 +12,20 @@ class PDControllerStableMultiDof(object):
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 quatMul(self, q1, q2):
return [ q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1],
q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2],
q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0],
q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2]]
def computeAngVelRel(self,ornStart, ornEnd, deltaTime, bullet_client):
ornStartConjugate = [-ornStart[0],-ornStart[1],-ornStart[2],ornStart[3]]
q_diff = self.quatMul(ornStartConjugate, ornEnd)#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):
@@ -62,8 +70,8 @@ class PDControllerStableMultiDof(object):
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)
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
@@ -131,7 +139,7 @@ class PDControllerStableMultiDof(object):
#np.savetxt("pb_b_acc.csv",b, delimiter=",")
useNumpySolver = False
useNumpySolver = True
if useNumpySolver:
qddot = np.linalg.solve(A, b)
else: