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

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

@@ -14,7 +14,7 @@ class HumanoidPoseInterpolator(object):
self._basePos = basePos self._basePos = basePos
self._baseLinVel = baseLinVel self._baseLinVel = baseLinVel
print("HumanoidPoseInterpolator.Reset: baseLinVel = ", baseLinVel) #print("HumanoidPoseInterpolator.Reset: baseLinVel = ", baseLinVel)
self._baseOrn =baseOrn self._baseOrn =baseOrn
self._baseAngVel = baseAngVel self._baseAngVel = baseAngVel

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] 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): def quatMul(self, q1, q2):
# ornStartConjugate = [-ornStart[0],-ornStart[1],-ornStart[2],ornStart[3]] return [ q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1],
# pos_diff, q_diff =bullet_client.multiplyTransforms([0,0,0], ornStartConjugate, [0,0,0], ornEnd) q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2],
# axis,angle = bullet_client.getAxisAngleFromQuaternion(q_diff) q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0],
# angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime] q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2]]
# return angVel
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): def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
@@ -62,8 +70,8 @@ class PDControllerStableMultiDof(object):
if len(js[0])==4: if len(js[0])==4:
desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]] desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]]
#axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos) #axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos)
#angDiff = self.computeAngVelRel(jointPos, desiredPos, 1, self._pb) angDiff = self.computeAngVelRel(jointPos, desiredPos, 1, self._pb)
angDiff = self._pb.computeAngVelRel(jointPos, desiredPos, 1) #angDiff = self._pb.computeAngVelRel(jointPos, desiredPos, 1)
jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0] jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0]
qdot1+=jointVelNew qdot1+=jointVelNew
@@ -131,7 +139,7 @@ class PDControllerStableMultiDof(object):
#np.savetxt("pb_b_acc.csv",b, delimiter=",") #np.savetxt("pb_b_acc.csv",b, delimiter=",")
useNumpySolver = False useNumpySolver = True
if useNumpySolver: if useNumpySolver:
qddot = np.linalg.solve(A, b) qddot = np.linalg.solve(A, b)
else: else: