add blender test

correct grab goal settings
enable different links for different goal types

.gitignore

@@ -20,6 +20,9 @@ pip-selfcheck.json
 /dist
 *.eggs
 
+# Virtualenv
+.venv/*
+.gitignore
 # CMake
 CMakeFiles/
 CMakeCache.txt

build_visual_studio_vr_pybullet_double.bat

@@ -5,15 +5,15 @@ IF NOT EXIST bin\openvr64pi.dll  copy examples\ThirdPartyLibs\openvr\bin\win64\o
 #aargh, see https://github.com/ValveSoftware/openvr/issues/412
 
 
-#find a python version (hopefully just 1) and use this
+REM #find a python version (hopefully just 1) and use this
-dir c:\python* /b /ad > tmp1234.txt
+REM dir c:\python* /b /ad > tmp1234.txt
 
-set /p myvar1= < tmp1234.txt
+REM set /p myvar1= < tmp1234.txt
-set myvar=c:/%myvar1%
+REM set myvar=c:/%myvar1%
-del tmp1234.txt
+REM del tmp1234.txt
 
 rem you can also override and hardcode the Python path like this (just remove the # hashmark in next line)
-rem SET myvar=c:\python-3.5.2
+SET myvar=C:\Users\Bart\AppData\Local\Programs\Python\Python36
 
 cd build3
 

examples/pybullet/examples/humanoidMotionCapture.py

@@ -42,7 +42,9 @@ timeStep = 1. / 600.
 
 p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
 
-path = pybullet_data.getDataPath() + "/data/motions/humanoid3d_backflip.txt"
+path = pybullet_data.getDataPath() + "/data/motions/blender_ik_test.txt"
+# path = pybullet_data.getDataPath() + "/data/motions/humanoid3d_zero_pos.txt"
+# path = pybullet_data.getDataPath() + "/data/motions/humanoid3d_backflip.txt"
 #path	=	pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt"
 #path	=	pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
 

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,27 @@
+--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/spinkick_no_rot.ckpt
+#--model_files output/spinkick_partial_run/agent0_model.ckpt

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

@@ -0,0 +1,27 @@
+--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/humanoid_grasp_test.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+--goal_type Grab
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files output/grasp_goal_partial_run/agent0_model.ckpt
+#--model_files output/spinkick_partial_run/agent0_model.ckpt

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

@@ -0,0 +1,27 @@
+--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/humanoid_grasp_test.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+--goal_type NoGoal
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files output/grasp_partial_run/agent0_model.ckpt
+#--model_files output/spinkick_partial_run/agent0_model.ckpt

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

@@ -0,0 +1,25 @@
+--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_run.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/agent0_model.ckpt

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

@@ -0,0 +1,25 @@
+--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_walk.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/output/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_run_continue.txt

@@ -0,0 +1,33 @@
+--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
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_run.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/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/args/train_humanoid_grasp_args.txt

@@ -0,0 +1,33 @@
+--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
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid_grasp_test.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_humanoid_grasp_goal.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 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 Grab
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid_grasp_test.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/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_data/data/motions/humanoid3d_zero_pos.txt

@@ -0,0 +1,16 @@
+{
+"Loop": "wrap",
+"Frames":
+[
+[0.062500, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000,0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000],
+[0.062500, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000,0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000],
+[0.062500, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000,0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000],
+[0.062500, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000,0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000],
+[0.062500, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000,0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000],
+[0.062500, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000,0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000],
+[0.062500, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000,0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000],
+[0.062500, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000,0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000],
+[0.062500, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000,0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000],
+[0.062500, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000,0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000,	1.000000, 0.000000, 0.000000, 0.000000, 0.000000]
+]
+}

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

@@ -0,0 +1,168 @@
+from enum import Enum
+from abc import ABC, abstractmethod
+import random
+import math
+import pybullet as pb
+import numpy
+from pybullet_envs.deep_mimic.env.humanoid_link_ids import HumanoidLinks
+
+class RandomBounds:
+    def __init__(self, lower_bound: float, upper_bound: float):
+        self.upper_bound = upper_bound
+        self.lower_bound = lower_bound
+        self.difference = upper_bound - lower_bound
+
+    def generate(self) -> float:
+        return self.lower_bound + random.random() * self.difference
+    
+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
+    Grab = 5
+
+    @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 Strike(Goal):
+    def __init__(self,
+                distance_RB: RandomBounds,
+                height_RB: RandomBounds,
+                rot_RB: RandomBounds,
+                hit_range = 0.2,
+                follow_rot = True,
+                target_link = HumanoidLinks.rightAnkle):
+        self.follow_rot = follow_rot
+        self.is_hit_prev = False
+        self.distance_RB = distance_RB
+        self.height_RB = height_RB
+        self.rot_RB = rot_RB
+        self.hit_range = hit_range
+        self.target_link = target_link
+        super().__init__(GoalType.Strike)
+    
+    def generateGoalData(self,
+                        modelPos=[0,0,0],
+                        modelOrient=[0,0,0,1]):
+        # distance, height, rot
+        distance = self.distance_RB.generate()
+        height = self.height_RB.generate()
+        rot = self.rot_RB.generate() # radians
+
+        self.is_hit = False
+
+        # 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 robot 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 Kick(Strike):
+    def __init__(self):
+        distance_RB = RandomBounds(0.6, 0.8)
+        height_RB = RandomBounds(0.8, 1.25)
+        rot_RB = RandomBounds(-1, 1)
+        super().__init__(distance_RB, height_RB, rot_RB, hit_range=0.2)
+
+class Grab(Strike):
+    def __init__(self):
+        distance_RB = RandomBounds(0.4, 0.6)
+        height_RB = RandomBounds(0.8, 1.1)
+        rot_RB = RandomBounds(3.14159 - 0.5, 3.14159 + 0.5)
+        hit_range = 0.1
+        target_link = HumanoidLinks.rightWrist
+        super().__init__(distance_RB, height_RB, rot_RB, hit_range=hit_range, follow_rot=False, target_link=target_link)
+
+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 Kick()
+    elif goal_type == GoalType.TargetHeading:
+        return TargetHeadingGoal()
+    elif goal_type == GoalType.Grab:
+        return Grab()
+    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[0] + self.cycleCount * self._cycleOffset[0],
-        oldPos[1] + self._cycleCount * self._cycleOffset[1],
+        oldPos[1] + self.cycleCount * self._cycleOffset[1],
-        oldPos[2] + self._cycleCount * self._cycleOffset[2]
+        oldPos[2] + self.cycleCount * self._cycleOffset[2]
     ]
     pose = self._poseInterpolator.GetPose()
 
@@ -704,6 +705,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):
@@ -931,3 +935,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, Kick, Grab
+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(self.goal.target_link)
+      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()
+    if self.target_id is not None: # TODO: check goal type
+      self.updateDrawStrikeGoal()
 
     for i in range(1):
       self.t += timeStep
@@ -325,3 +354,114 @@ 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):
+    if self.goal.goal_type == GoalType.Strike:
+      linkPos = self._humanoid.getLinkPositionAndOrientation(self.goal.target_link)[0]
+      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/env/testHumanoid.py

@@ -26,7 +26,9 @@ pybullet_client.setPhysicsEngineParameter(numSolverIterations=10)
 
 mocapData = motion_capture_data.MotionCaptureData()
 #motionPath = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
-motionPath = pybullet_data.getDataPath() + "/data/motions/humanoid3d_backflip.txt"
+# motionPath = pybullet_data.getDataPath() + "/data/motions/humanoid3d_backflip.txt"
+# motionPath = pybullet_data.getDataPath() + "/data/motions/blender_ik_test.txt"
+motionPath = pybullet_data.getDataPath() + "/data/motions/humanoid_grasp_test.txt"
 mocapData.Load(motionPath)
 timeStep = 1. / 600
 useFixedBase = False

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

@@ -74,7 +74,7 @@ class RLAgent(ABC):
 
     self._output_dir = ""
     self._int_output_dir = ""
-    self.output_iters = 100
+    self.output_iters = 50
     self.int_output_iters = 100
 
     self.train_return = 0.0
@@ -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.__class__.__name__)
           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/runcommand.txt

@@ -0,0 +1,60 @@
+python testrl.py --arg_file run_humanoid3d_backflip_args.txt
+python testrl.py --arg_file run_humanoid3d_spinkick_args_semi_trained.txt
+python testrl.py --arg_file run_humanoid_run_semi_trained.txt
+python testrl.py --arg_file run_humanoid_grasp_semi_trained.txt
+python testrl.py --arg_file run_humanoid_grasp_goal_semi_trained.txt
+
+
+scp -r acro@10.11.98.170:Git/bullet3/examples/pybullet/gym/pybullet_envs/deep_mimic/output C:/UntrackedGit/bulletFork/bullet3/examples/pybullet/gym/pybullet_envs/deep_mimic/output
+scp -r acro@10.11.98.170:Git/bullet3/examples/pybullet/gym/pybullet_envs/deep_mimic/output C:/UntrackedGit/bulletFork/bullet3/examples/pybullet/gym/pybullet_data/output
+scp -r acro@10.11.98.170:Git/bullet3/examples/pybullet/gym/pybullet_envs/deep_mimic/output C:/UntrackedGit/bulletFork/bullet3/examples/pybullet/gym/pybullet_data/output/spinkick_partial_run
+scp -r acro@10.11.98.170:Git/bullet3/examples/pybullet/gym/pybullet_envs/deep_mimic/output C:/UntrackedGit/bulletFork/bullet3/examples/pybullet/gym/pybullet_data/output/grasp_partial_run
+scp -r acro@10.11.98.170:Git/bullet3/examples/pybullet/gym/pybullet_envs/deep_mimic/output C:/UntrackedGit/bulletFork/bullet3/examples/pybullet/gym/pybullet_data/output/grasp_goal_partial_run
+scp -r acro@10.11.98.170:Git/bullet3/examples/pybullet/gym/pybullet_envs/deep_mimic/output C:/UntrackedGit/bulletFork/bullet3/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d
+
+scp -r -P 33001 acro@127.0.0.1:Git/bullet3/examples/pybullet/gym/pybullet_envs/deep_mimic/output C:/UntrackedGit/bulletFork/bullet3/examples/pybullet/gym/pybullet_data/output/grasp_partial_run
+scp -r -P 33001 acro@127.0.0.1:Git/bullet3/examples/pybullet/gym/pybullet_envs/deep_mimic/output C:/UntrackedGit/bulletFork/bullet3/examples/pybullet/gym/pybullet_data/output/grasp_goal_partial_run
+
+cd Git/bullet3/examples/pybullet/gym/pybullet_envs/deep_mimic
+mpiexec -n 16 python3 DeepMimic_Optimizer.py --arg_file train_humanoid3d_kick_goals_args.txt --num_workers 16
+mpiexec -n 16 python3 DeepMimic_Optimizer.py --arg_file train_humanoid3d_spinkick_args.txt --num_workers 16
+mpiexec -n 16 python3 DeepMimic_Optimizer.py --arg_file train_humanoid3d_walk_args.txt --num_workers 16
+mpiexec -n 16 python DeepMimic_Optimizer.py --arg_file train_humanoid3d_run_args.txt --num_workers 16
+mpiexec -n 16 python DeepMimic_Optimizer.py --arg_file train_humanoid_grasp_args.txt --num_workers 16
+mpiexec -n 16 python DeepMimic_Optimizer.py --arg_file train_humanoid_grasp_goal.txt --num_workers 16
+
+// "python3" may have to be "python" instead in previous commands!
+
+// To install pybullet from a local package editably:
+pip install --editable .
+
+// Remote server setup:
+    // setup virtual environment:
+    virtualenv env
+    pip install gym
+    pip install tensorflow==1.14.0
+
+    // build and install pybullet locally:
+    // In bullet3 root folder:
+    ./build_cmake_pybullet_double.sh
+    pip install --editable .
+
+    // Install openMPI:
+    https://www.open-mpi.org/
+    sudo apt install libopenmpi-dev
+    pip install mpi4py
+
+
+// To run training after closing terminal:
+https://linuxize.com/post/how-to-use-linux-screen/
+sudo apt install screen
+screen -r // To return to screen
+ctrl + a -> d // To detach from screen
+ctrl + a -> K // To kill screen (Or type: exit)
+
+ssh acro@10.11.98.170
+
+
+// enable virtualenv:
+source /home/acro/Git/bullet3/env/bin/activate
+

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):
@@ -94,6 +94,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