correct strike goal rotation

make distinction between world pos and relative goal pos

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_backflip_args_test.txt

@@ -0,0 +1,26 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+#--goal_type Strike
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_backflip.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_backflip.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spinkick_args_semi_trained.txt

@@ -0,0 +1,26 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_spinkick.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+--goal_type Strike
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files output/spinkick_partial_run/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_kick_goals_args.txt

@@ -0,0 +1,34 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 4
+--time_end_lim_max 4
+--time_end_lim_exp 10
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+--goal_type kick_random
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_kick.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spinkick_args_test.txt

@@ -0,0 +1,35 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+--goal_type Strike
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_spinkick.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+--model_files output/spinkick_partial_run/agent0_model.ckpt
+#Command: mpiexec -n 6 python DeepMimic_Optimizer.py --arg_file train_humanoid3d_spinkick_args_test.txt --num_workers 6

examples/pybullet/gym/pybullet_data/data/agents/ct_agent_humanoid_ppo.txt

@@ -27,7 +27,7 @@
 	"TDLambda": 0.95,
 	
 	"OutputIters": 10,
-	"IntOutputIters": 400,
+	"IntOutputIters": 50,
 	"TestEpisodes": 32,
 
 	"ExpAnnealSamples": 64000000,

examples/pybullet/gym/pybullet_envs/deep_mimic/env/goals.py

@@ -0,0 +1,129 @@
+from enum import Enum
+from abc import ABC, abstractmethod
+import random
+import math
+import pybullet as pb
+import numpy
+
+def randomVal(lowerBound, upperbound):
+    difference = upperbound - lowerBound
+    return lowerBound + random.random() * difference
+
+class GoalType(Enum):
+    NoGoal = 0
+    Strike = 1
+    TargetHeading = 2
+    Throw = 3
+    TerrainTraversal = 4
+
+    @staticmethod
+    def from_str(goal_type_str):
+        if goal_type_str == '':
+            return GoalType.NoGoal
+        else:
+            try:
+                return GoalType[goal_type_str]
+            except:
+                raise NotImplementedError
+
+class Goal(ABC):
+    def __init__(self, goal_type: GoalType):
+        self.goal_type = goal_type
+        self.generateGoalData()
+
+    @abstractmethod
+    def generateGoalData(self, modelPos=[0,0,0], modelOrient=[0,0,0,1]):
+        pass
+
+    @abstractmethod
+    def getTFData(self):
+        pass
+
+class NoGoal(Goal):
+    def __init__(self):
+        super().__init__(GoalType.NoGoal)
+
+    def generateGoalData(self, modelPos=[0,0,0], modelOrient=[0,0,0,1]):
+        self.goal_data = []
+
+    def getTFData(self):
+        return self.goal_data
+
+class StrikeGoal(Goal):
+    def __init__(self):
+        self.follow_rot = True
+        self.is_hit_prev = False
+        super().__init__(GoalType.Strike)
+
+    def generateGoalData(self, modelPos=[0,0,0], modelOrient=[0,0,0,1]):
+        # distance, height, rot
+        distance = randomVal(0.6, 0.8)
+        height = randomVal(0.8, 1.25)
+        rot = randomVal(-1, 1) # radians
+
+        self.is_hit = False
+
+        # The max distance from the target counting as a hit
+        self.hit_range = 0.2
+
+        # Transform to xyz coordinates for placement in environment
+        x = distance * math.cos(rot)
+        y = distance * math.sin(rot)
+        z = height
+
+        # Y axis up, z axis in different direction
+        self.goal_data = [-x, z, y]
+
+        if self.follow_rot:
+            # Take rotation of human model into account
+            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]]
+        else:
+            self.world_pos = [-x + modelPos[0], z, y + modelPos[2]]
+
+    def getTFData(self):
+        x = 0.0
+        if self.is_hit:
+            x = 1.0
+        return [x] + self.goal_data
+
+class TargetHeadingGoal(Goal):
+    def __init__(self):
+        super().__init__(GoalType.TargetHeading)
+
+    def generateGoalData(self, modelPos=[0,0,0], modelOrient=[0,0,0,1]):
+        # Direction: 2D unit vector
+        # speed: max speed
+        random_rot = random.random() * 2 * math.pi
+        x = math.cos(random_rot)
+        y = math.sin(random_rot)
+        velocity = randomVal(0, 0.5)
+        self.goal_data = [x, y, velocity]
+
+    def getTFData(self):
+        return self.goal_data
+
+def createGoal(goal_type: GoalType) -> Goal:
+    if goal_type == GoalType.NoGoal:
+        return NoGoal()
+    elif goal_type == GoalType.Strike:
+        return StrikeGoal()
+    elif goal_type == GoalType.TargetHeading:
+        return TargetHeadingGoal()
+    else:
+        raise NotImplementedError

examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_link_ids.py

@@ -0,0 +1,17 @@
+from enum import IntEnum
+
+class HumanoidLinks(IntEnum):
+    chest = 1
+    neck = 2
+    rightHip = 3
+    rightKnee = 4
+    rightAnkle = 5
+    rightShoulder = 6
+    rightElbow = 7
+    rightWrist = 8
+    leftHip = 9
+    leftKnee = 10
+    leftAnkle = 11
+    leftShoulder = 12
+    leftElbow = 13
+    leftWrist = 14

examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py

@@ -19,9 +19,11 @@ jointFrictionForce = 0
 
 class HumanoidStablePD(object):
 
-  def __init__(self, pybullet_client, mocap_data, timeStep, useFixedBase=True):
+  def __init__( self, pybullet_client, mocap_data, timeStep, 
+                useFixedBase=True, arg_parser=None):
     self._pybullet_client = pybullet_client
     self._mocap_data = mocap_data
+    self._arg_parser = arg_parser
     print("LOADING humanoid!")
 
     self._sim_model = self._pybullet_client.loadURDF(
@@ -135,7 +137,10 @@ class HumanoidStablePD(object):
     self._jointDofCounts = [4, 4, 4, 1, 4, 4, 1, 4, 1, 4, 4, 1]
 
     #only those body parts/links are allowed to touch the ground, otherwise the episode terminates
-    self._allowed_body_parts = [5, 11]
+    fall_contact_bodies = []
+    if self._arg_parser is not None:
+      fall_contact_bodies = self._arg_parser.parse_ints("fall_contact_bodies")
+    self._fall_contact_body_parts = fall_contact_bodies
 
     #[x,y,z] base position and [x,y,z,w] base orientation!
     self._totalDofs = 7
@@ -224,9 +229,9 @@ class HumanoidStablePD(object):
     keyFrameDuration = self._mocap_data.KeyFrameDuraction()
     cycleTime = self.getCycleTime()
     #print("self._motion_data.NumFrames()=",self._mocap_data.NumFrames())
-    self._cycleCount = self.calcCycleCount(t, cycleTime)
+    self.cycleCount = self.calcCycleCount(t, cycleTime)
     #print("cycles=",cycles)
-    frameTime = t - self._cycleCount * cycleTime
+    frameTime = t - self.cycleCount * cycleTime
     if (frameTime < 0):
       frameTime += cycleTime
 
@@ -264,9 +269,9 @@ class HumanoidStablePD(object):
     self.computeCycleOffset()
     oldPos = self._poseInterpolator._basePos
     self._poseInterpolator._basePos = [
-        oldPos[0] + self._cycleCount * self._cycleOffset[0],
-        oldPos[1] + self._cycleCount * self._cycleOffset[1],
-        oldPos[2] + self._cycleCount * self._cycleOffset[2]
+        oldPos[0] + self.cycleCount * self._cycleOffset[0],
+        oldPos[1] + self.cycleCount * self._cycleOffset[1],
+        oldPos[2] + self.cycleCount * self._cycleOffset[2]
     ]
     pose = self._poseInterpolator.GetPose()
 
@@ -598,7 +603,7 @@ class HumanoidStablePD(object):
         part = p[3]
       if (p[2] == self._sim_model):
         part = p[4]
-      if (part >= 0 and part not in self._allowed_body_parts):
+      if (part >= 0 and part in self._fall_contact_body_parts):
         #print("terminating part:", part)
         terminates = True
 
@@ -802,4 +807,7 @@ class HumanoidStablePD(object):
 
   def getSimModelBasePosition(self):
     return  self._pybullet_client\
-                .getBasePositionAndOrientation(self._sim_model)
+                .getBasePositionAndOrientation(self._sim_model)
+                
+  def getLinkPositionAndOrientation(self, link_id):
+    return tuple(self._pybullet_client.getLinkState(self._sim_model, link_id)[0:2])

examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py

@@ -6,6 +6,8 @@ from pybullet_utils import bullet_client
 import time
 from pybullet_envs.deep_mimic.env import motion_capture_data
 from pybullet_envs.deep_mimic.env import humanoid_stable_pd
+from pybullet_envs.deep_mimic.env.goals import GoalType, Goal, createGoal
+from pybullet_envs.deep_mimic.env.humanoid_link_ids import HumanoidLinks
 import pybullet_data
 import pybullet as p1
 import random
@@ -20,8 +22,14 @@ class PyBulletDeepMimicEnv(Env):
     self._isInitialized = False
     self._useStablePD = True
     self._arg_parser = arg_parser
+    self.goal = self.getGoal()
+    self.target_id = None
+
     self.reset()
 
+    if self.goal.goal_type == GoalType.Strike:
+      self.target_id = self.drawStrikeGoal()
+
   def reset(self):
 
     if not self._isInitialized:
@@ -55,7 +63,7 @@ class PyBulletDeepMimicEnv(Env):
       timeStep = 1. / 600.
       useFixedBase = False
       self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData,
-                                                           timeStep, useFixedBase)
+                                                           timeStep, useFixedBase, self._arg_parser)
       self._isInitialized = True
 
       self._pybullet_client.setTimeStep(timeStep)
@@ -86,11 +94,15 @@ class PyBulletDeepMimicEnv(Env):
     self.removeThrownObjects()
 
     self._humanoid.setSimTime(startTime)
+    self.prevCycleCount = self._humanoid.cycleCount
 
     self._humanoid.resetPose()
     #this clears the contact points. Todo: add API to explicitly clear all contact points?
     #self._pybullet_client.stepSimulation()
     self._humanoid.resetPose()
+    # generate new goal
+    humanPos, humanOrient = self._humanoid.getLinkPositionAndOrientation(0)
+    self.goal.generateGoalData(humanPos, humanOrient)
     self.needs_update_time = self.t - 1  #force update
 
   def get_num_agents(self):
@@ -145,7 +157,7 @@ class PyBulletDeepMimicEnv(Env):
     return np.array(out_scale)
 
   def get_goal_size(self, agent_id):
-    return 0
+    return len(self.goal.getTFData())
 
   def get_action_size(self, agent_id):
     ctrl_size = 43  #numDof
@@ -153,13 +165,16 @@ class PyBulletDeepMimicEnv(Env):
     return ctrl_size - root_size
 
   def build_goal_norm_groups(self, agent_id):
-    return np.array([])
+    # Perform normalization on goal data
+    return np.array([0] * len(self.goal.getTFData()))
 
   def build_goal_offset(self, agent_id):
-    return np.array([])
+    # no offset
+    return np.array([0] * len(self.goal.getTFData()))
 
   def build_goal_scale(self, agent_id):
-    return np.array([])
+    # no scale
+    return np.array([1] * len(self.goal.getTFData()))
 
   def build_action_offset(self, agent_id):
     out_offset = [0] * self.get_action_size(agent_id)
@@ -232,18 +247,26 @@ class PyBulletDeepMimicEnv(Env):
     return np.array(state)
 
   def record_goal(self, agent_id):
-    return np.array([])
+    return np.array(self.goal.getTFData())
 
   def calc_reward(self, agent_id):
     kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
     reward = self._humanoid.getReward(kinPose)
+
+    mimic_weight = 0.7
+    goal_weight = 0.3
+
+    if self.goal.goal_type == GoalType.Strike:
+      linkPos, linkOrient = self._humanoid.getLinkPositionAndOrientation(HumanoidLinks.rightAnkle)
+      reward = mimic_weight * reward + goal_weight * self.calcStrikeGoalReward(linkPos)
+
     return reward
 
   def set_action(self, agent_id, action):
     #print("action=",)
     #for a in action:
     #  print(a)
-    np.savetxt("pb_action.csv", action, delimiter=",")
+    # np.savetxt("pb_action.csv", action, delimiter=",")
     self.desiredPose = self._humanoid.convertActionToPose(action)
     #we need the target root positon and orientation to be zero, to be compatible with deep mimic
     self.desiredPose[0] = 0
@@ -255,7 +278,7 @@ class PyBulletDeepMimicEnv(Env):
     self.desiredPose[6] = 0
     target_pose = np.array(self.desiredPose)
 
-    np.savetxt("pb_target_pose.csv", target_pose, delimiter=",")
+    # np.savetxt("pb_target_pose.csv", target_pose, delimiter=",")
 
     #print("set_action: desiredPose=", self.desiredPose)
 
@@ -266,6 +289,9 @@ class PyBulletDeepMimicEnv(Env):
     #print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t)
     self._pybullet_client.setTimeStep(timeStep)
     self._humanoid._timeStep = timeStep
+    self.updateGoal(self._humanoid.getLinkPositionAndOrientation(HumanoidLinks.rightAnkle)[0])
+    if self.target_id is not None: # TODO: check goal type
+      self.updateDrawStrikeGoal()
 
     for i in range(1):
       self.t += timeStep
@@ -367,4 +393,68 @@ class PyBulletDeepMimicEnv(Env):
 
     self._humanoid.thrown_body_ids = []
 
-    self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_RENDERING,1)
+    self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_RENDERING,1)
+  
+  def getGoal(self):
+    goal_type_str = self._arg_parser.parse_string("goal_type")
+    return createGoal(GoalType.from_str(goal_type_str))
+
+  def calcStrikeGoalReward(self, linkPos):
+    if self.goal.is_hit:
+      return 1
+    else:
+      goalPos = self.goal.world_pos
+      distanceSquared = sum([(x - y)**2 for (x, y) in zip(goalPos, linkPos)])
+      return math.exp(-4*distanceSquared)
+
+  def updateGoal(self, linkPos):
+    if self.goal.goal_type == GoalType.Strike:
+      if self.prevCycleCount != self._humanoid.cycleCount:
+        # generate new goal
+        humanPos, humanOrient = self._humanoid.getLinkPositionAndOrientation(0)
+        self.goal.generateGoalData(humanPos, humanOrient)
+        self.prevCycleCount = self._humanoid.cycleCount
+
+      goalPos = self.goal.world_pos
+      distance = sum([(x - y)**2 for (x, y) in zip(goalPos, linkPos)]) ** 0.5
+
+      if distance <= self.goal.hit_range:
+        self.goal.is_hit = True
+
+  def drawStrikeGoal(self):
+    vis_id = self._pybullet_client.createVisualShape(
+                shapeType=self._pybullet_client.GEOM_SPHERE,
+                radius=self.goal.hit_range,
+                rgbaColor=[1,0,0,0.5])
+
+    obj_id = self._pybullet_client.createMultiBody(
+                baseVisualShapeIndex=vis_id,
+                basePosition=self.goal.world_pos)
+    return obj_id
+
+  def updateDrawStrikeGoal(self):
+    current_pos = self._pybullet_client.getBasePositionAndOrientation(self.target_id)[0]
+    target_pos = self.goal.world_pos
+
+    if target_pos != current_pos:
+      self._pybullet_client.resetBasePositionAndOrientation(
+        self.target_id,
+        target_pos,
+        [0, 0, 0, 1]
+      )
+    if self.goal.is_hit != self.goal.is_hit_prev:
+      self.goal.is_hit_prev = self.goal.is_hit
+      if self.goal.is_hit:
+        # Color green
+        self._pybullet_client.changeVisualShape(
+          self.target_id,
+          -1,
+          rgbaColor=[0, 1, 0, 0.5]
+        )
+      else:
+        # Color red
+        self._pybullet_client.changeVisualShape(
+          self.target_id,
+          -1,
+          rgbaColor=[1, 0, 0, 0.5]
+        )

examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py

@@ -556,6 +556,7 @@ class RLAgent(ABC):
           self.logger.log_tabular("Test_Return", self.avg_test_return)
           self.logger.log_tabular("State_Mean", s_mean)
           self.logger.log_tabular("State_Std", s_std)
+          self.logger.log_tabular("Goal_Type", self.world.env.goal.goal_type)
           self.logger.log_tabular("Goal_Mean", g_mean)
           self.logger.log_tabular("Goal_Std", g_std)
           self._log_exp_params()

examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py

@@ -22,7 +22,7 @@ animating = True
 def update_world(world, time_elapsed):
   timeStep = update_timestep
   world.update(timeStep)
-  reward = world.env.calc_reward(agent_id=0)
+  # reward = world.env.calc_reward(agent_id=0)
   #print("reward=",reward)
   end_episode = world.env.is_episode_end()
   if (end_episode):