From d4292fdac3fe71d404732a91849428732aabc8dd Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 10 Feb 2019 21:37:34 -0800 Subject: [PATCH] PyBullet deep_mimic fix computeAngVelRel --- .../env/humanoid_pose_interpolator.py | 2 +- .../pybullet_utils/pd_controller_stable.py | 26 ++++++++++++------- 2 files changed, 18 insertions(+), 10 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py index 18e5d0191..af0870ff5 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py @@ -14,7 +14,7 @@ class HumanoidPoseInterpolator(object): self._basePos = basePos self._baseLinVel = baseLinVel - print("HumanoidPoseInterpolator.Reset: baseLinVel = ", baseLinVel) + #print("HumanoidPoseInterpolator.Reset: baseLinVel = ", baseLinVel) self._baseOrn =baseOrn self._baseAngVel = baseAngVel diff --git a/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py b/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py index d0cee30bd..04424b9a1 100644 --- a/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py +++ b/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py @@ -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: