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

correct strike goal rotation

Browse Source
This commit is contained in:
Bart Moyaers
2019-07-08 13:58:47 +02:00
parent e6d4cfc7da
commit 75e20522b5
1 changed files with 16 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
examples/pybullet/gym/pybullet_envs/deep_mimic/env/goals.py vendored
View File

@@ -51,7 +51,7 @@ class NoGoal(Goal):
class StrikeGoal(Goal):
def __init__(self):
self.follow_rot = False
self.follow_rot = True
self.is_hit_prev = False
super().__init__(GoalType.Strike)
@@ -76,16 +76,20 @@ class StrikeGoal(Goal):
if self.follow_rot:
# Take rotation of human model into account
eulerAngles = pb.getEulerFromQuaternion(modelOrient)
# Only Y angle matters
eulerAngles = [0, eulerAngles[1], 0]
yQuat = pb.getQuaternionFromEuler(eulerAngles)
rotMatList = pb.getMatrixFromQuaternion(yQuat)
rotMatList = pb.getMatrixFromQuaternion(modelOrient)
rotMat = numpy.array([rotMatList[0:3], rotMatList[3:6], rotMatList[6:9]])
vec = numpy.array(self.goal_data)
rotatedVec = numpy.dot(rotMat, vec)
self.world_pos = rotatedVec.tolist()
# Correct distance and height after rotation
curr_distance = (self.world_pos[0] ** 2 + self.world_pos[2] ** 2) ** 0.5
factor = distance / curr_distance
self.world_pos[0] *= factor
self.world_pos[1] = height
self.world_pos[2] *= factor
# Add translation of agent in world
self.world_pos = [ self.world_pos[0] + modelPos[0],
self.world_pos[1],
self.world_pos[2] + modelPos[2]]