Merge branch 'master' of github.com:erwincoumans/bullet3 into erwin_master

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

@@ -149,6 +149,7 @@ class HumanoidStablePD(object):
     self.setSimTime(0)
 
     self.resetPose()
+    self.thrown_body_ids = []
 
   def resetPose(self):
     #print("resetPose with self._frame=", self._frame, " and self._frameFraction=",self._frameFraction)
@@ -254,9 +255,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
 
@@ -294,9 +295,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()
 
@@ -705,6 +706,9 @@ class HumanoidStablePD(object):
       #ignore self-collision
       if (p[1] == p[2]):
         continue
+      # ignore collisions with thrown objects
+      if p[1] in self.thrown_body_ids or p[2] in self.thrown_body_ids:
+        continue
       if (p[1] == self._sim_model):
         part = p[3]
       if (p[2] == self._sim_model):
@@ -929,3 +933,10 @@ class HumanoidStablePD(object):
     #print("com_reward=",com_reward)
 
     return reward
+
+  def getSimModelBasePosition(self):
+    return  self._pybullet_client\
+                .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:
@@ -82,12 +90,19 @@ class PyBulletDeepMimicEnv(Env):
     startTime = float(rn) / rnrange * self._humanoid.getCycleTime()
     self.t = startTime
 
+    # Remove all the thrown objects
+    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):
@@ -142,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
@@ -150,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)
@@ -229,11 +247,19 @@ 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):
@@ -263,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
@@ -325,3 +354,113 @@ class PyBulletDeepMimicEnv(Env):
     if o in keys:
       return keys[ord(key)] & self._pybullet_client.KEY_WAS_TRIGGERED
     return False
+
+  def hitWithObject(self):
+    # Spawn an object with velocity hitting the model
+    r = random.random()
+    distance = 3.0
+    rand_angle = r * 2 * math.pi - math.pi
+
+    position, orientation = \
+      self._humanoid.getSimModelBasePosition()
+
+    # Remember, in bullet: Y direction is "up". X and Z direction are
+    # in the horizontal plane.
+    ball_position = [ distance * math.cos(rand_angle) + position[0],
+                      position[1],
+                      distance * math.sin(rand_angle)+ position[2]]
+
+    visualShapeId = self._pybullet_client.createVisualShape(
+      shapeType=self._pybullet_client.GEOM_SPHERE,
+      radius=0.1)
+    
+    collisionShapeId = self._pybullet_client.createCollisionShape(
+      shapeType=self._pybullet_client.GEOM_SPHERE,
+      radius=0.1)
+
+    body = self._pybullet_client.createMultiBody(
+      baseMass=1,
+      baseCollisionShapeIndex=collisionShapeId,
+      baseVisualShapeIndex=visualShapeId,
+      basePosition=ball_position)
+
+    distance_scale = 10
+    ball_velocity = [distance_scale * (position[i] - ball_position[i]) for i in range(3)]
+    self._pybullet_client.resetBaseVelocity(body, linearVelocity=ball_velocity)
+
+    self._humanoid.thrown_body_ids.append(body)
+
+  def removeThrownObjects(self):
+    # Disable gui so that object removal does not cause stutter
+    self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_RENDERING,0)
+
+    for objectId in self._humanoid.thrown_body_ids:
+      self._pybullet_client.removeBody(objectId)
+
+    self._humanoid.thrown_body_ids = []
+
+    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

@@ -23,7 +23,7 @@ step = False
 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):
@@ -92,6 +92,9 @@ if __name__ == '__main__':
       animating = not animating
     if world.env.isKeyTriggered(keys, 'i'):
       step = True
+    if world.env.isKeyTriggered(keys, 'x'):
+      # print("Throwing object.")
+      world.env.hitWithObject()
     if (animating or step):
       update_world(world, timeStep)
       step = False