def __init__(self,
style,
window=None,
scale=0.06,
e_scale=4,
ca_scale=20,
a_scale=20,
at_scale=4,
ay_scale=20,
z_scale=2,
cv_scale=4,
cv_ratio=1,
abs_acc=False,
avoid_self=False,
**kwargs):
"""
Initialize Gram matrix style transfer environment
"""
super().__init__(**kwargs)
if window:
self._window = window
else:
if self._mocap._is_wrap:
self._window = int(self._mocap._cycletime * 60)
else:
assert (False and "style reference is not cyclic")
self._min_window = 7
self._scale = scale
self._history = []
char_file = "data/characters/humanoid3d_cmu.txt"
ctrl_file = "data/controllers/humanoid3d_cmu_ctrl.txt"
self._style_skeleton = HumanoidSkeleton(char_file, ctrl_file)
style_file = "data/motions/humanoid3d_%s.txt" % style
self._style_G = self.style_gram_matrix(style_file)
# regularization
self._e_scale = e_scale
self._ca_scale = ca_scale
self._a_scale = a_scale
self._at_scale = at_scale
self._ay_scale = ay_scale
self._z_off_scale = z_scale
self._cv_scale = cv_scale
self._cv_ratio = cv_ratio
self._abs_acc = abs_acc
self._avoid_self = avoid_self
self.init_params(style_file)
self._prev_cv = None
self._curr_cv = None
self._prev_vs = None
self._curr_vs = None
self._prev_omgs = None
self._curr_omgs = None
def __init__(self):
char_file = "data/characters/humanoid3d.txt"
ctrl_file = "data/controllers/humanoid3d_ctrl.txt"
self.__skeleton = HumanoidSkeleton(char_file, ctrl_file)
'''
self.__engine = engine_builder("pybullet", 1/600.0)
self.__character = Character(self.__skeleton, True)
self.__engine.add_object(self.__character)
'''
self.__parent_table = [-1, 0, 1, 0, 3, 4, 1, 6, 7, 0, 9, 10, 1, 12, 13]
self.__dof = [0, 3, 3, 3, 1, 3, 3, 1, 0, 3, 1, 3, 3, 1, 0]
self.__mirror = [0, 1, 2, 9, 10, 11, 12, 13, 14, 3, 4, 5, 6, 7, 8]
def __init__(self, seed=0, **kwargs):
from presets import preset
if "new_process" in kwargs and kwargs["new_process"] == True:
preset.override(kwargs["forked_preset"])
self.init_parameters()
env_settings = preset.env
self.latent_dim = preset.vae.latent_dim
self._rand = np.random.RandomState(seed)
self.sim_frequency = env_settings.simulation_frequency
self.control_frequency = env_settings.control_frequency
self._sim_step = 1.0 / self.sim_frequency
self._nsubsteps = int(self.sim_frequency / self.control_frequency)
# initialize kinematic parts
char_file = "data/characters/%s.txt" % env_settings.character
ctrl_file = "data/controllers/%s.txt" % env_settings.character_ctrl
self._skeleton = HumanoidSkeleton(char_file, ctrl_file)
# initialize simulation parts
self._engine = engine_builder(env_settings.physics_engine,
self._sim_step)
from sim import Plane
from sim import Character
self.plane = Plane()
self.character = Character(self._skeleton,
env_settings.enable_self_collision)
self._engine.add_object(self.plane)
self._engine.add_object(self.character)
self._enable_draw = env_settings.enable_rendering
if self._enable_draw:
self.__init_render()
self.a_min, self.a_max = self.build_action_bound()
self.observation_space = gym.spaces.Box(-np.inf, np.inf,
[self.get_state_size()],
np.float32)
self.action_space = gym.spaces.Box(self.a_min.astype(np.float32),
self.a_max.astype(np.float32),
dtype=np.float32)
self._mode = 0 # 0 for train and 1 for test
def init(self):
"""
initialize environment with bullet backend
"""
# mocap data
if self._model == "humanoid3d":
char_file = "data/characters/humanoid3d.txt"
ctrl_file = "data/controllers/humanoid3d_ctrl.txt"
self._skeleton = HumanoidSkeleton(char_file, ctrl_file)
self._mocap = HumanoidMocap(self._skeleton, self._motion_file)
print("mocap duration: %f" % self._mocap._cycletime)
elif self._model == "atlas":
char_file = "data/characters/atlas.txt"
ctrl_file = "data/controllers/atlas_ctrl.txt"
self._skeleton = HumanoidSkeleton(char_file, ctrl_file)
self._mocap = HumanoidMocap(self._skeleton, self._motion_file)
print("mocap duration: %f" % self._mocap._cycletime)
elif self._model == "atlas_jason":
char_file = "data/characters/atlas_jason.txt"
ctrl_file = "data/controllers/atlas_jason_ctrl.txt"
self._skeleton = HumanoidSkeleton(char_file, ctrl_file)
self._mocap = HumanoidMocap(self._skeleton, self._motion_file)
print("mocap duration: %f" % self._mocap._cycletime)
else:
assert (False)
self._visual = HumanoidVis(self._skeleton, self._model)
#color = [227/255, 170/255, 14/255, 1] # sim
color = [44 / 255, 160 / 255, 44 / 255, 1] # ref
self._char = self._visual.add_character("mocap", color)
self._visual.camera_follow(self._char, 2, 0, 0)
self._pybullet_client = self._visual._pybullet_client
self._play_speed = self._pybullet_client.addUserDebugParameter(
"play_speed", 0, 2, 1.0)
self._phase_ctrl = self._pybullet_client.addUserDebugParameter(
"frame", 0, 1, 0)
class FeatureConverter:
def __init__(self):
char_file = "data/characters/humanoid3d.txt"
ctrl_file = "data/controllers/humanoid3d_ctrl.txt"
self.__skeleton = HumanoidSkeleton(char_file, ctrl_file)
'''
self.__engine = engine_builder("pybullet", 1/600.0)
self.__character = Character(self.__skeleton, True)
self.__engine.add_object(self.__character)
'''
self.__parent_table = [-1, 0, 1, 0, 3, 4, 1, 6, 7, 0, 9, 10, 1, 12, 13]
self.__dof = [0, 3, 3, 3, 1, 3, 3, 1, 0, 3, 1, 3, 3, 1, 0]
self.__mirror = [0, 1, 2, 9, 10, 11, 12, 13, 14, 3, 4, 5, 6, 7, 8]
def quat_to_pos(self, frame):
self.__skeleton.set_pose(np.array(frame))
return list(self.__skeleton.get_feature())
def pos_to_quat(self, pos):
return list(self.__skeleton.inv_feature(pos))
def exp_to_quat(self, exp):
return self.__skeleton.exp_to_action(exp)
def quat_to_exp(self, quat, root=[0, 0, 0, 1, 0, 0, 0]):
return self.__skeleton.targ_pose_to_action(np.array(root + quat))
def pos_to_exp(self, pos):
quat = self.pos_to_quat(pos)
return self.quat_to_exp(quat)
def mirror(self, pos):
mirrored = list(pos).copy()
for i in range(15):
idx = i * 12
mirroridx = self.__mirror[i] * 12
for j in range(12):
mirrored[mirroridx +
j] = pos[idx + j] * (-1 if j % 3 == 2 else 1)
return mirrored
def mirror_root(self, root):
rot = R.from_quat([root[4], root[5], root[6], root[3]])
expmap = rot.as_rotvec()
expmap[0] = -expmap[0]
expmap[1] = -expmap[1]
q = R.from_rotvec(expmap).as_quat()
return [root[0], root[1], -root[2], q[3], q[0], q[1], q[2]]
def __init__(self,
task,
seed=0,
model="humanoid3d",
engine="pybullet",
contact="walk",
self_collision=True,
enable_draw=False,
record_contact=False,
record_torques=False):
self._task = task
self._rand = np.random.RandomState(seed)
self._model = model
self._ctrl_step = ACT_STEPTIME
self._nsubsteps = SUBSTEPS
self._sim_step = self._ctrl_step / self._nsubsteps
self._enable_draw = enable_draw
self._vis_step = VIS_STEPTIME
self._follow_character = True
self._record_contact = record_contact
# initialize kinematic parts
char_file = "data/characters/%s.txt" % self._model
ctrl_file = "data/controllers/%s_ctrl.txt" % self._model
motion_file = "data/motions/%s_%s.txt" % (self._model, self._task)
self._skeleton = HumanoidSkeleton(char_file, ctrl_file)
self._mocap = HumanoidMocap(self._skeleton, motion_file)
self.curr_phase = None
# initialize visual parts, need to be the first when using pybullet environment
if self._enable_draw:
# draw timer
self._prev_clock = time.perf_counter()
self._left_time = 0
# visual part
self._visual = HumanoidVis(self._skeleton, self._model)
cnt, pose, vel = self._mocap.slerp(0)
self._sim_char = self._visual.add_character(
"sim", [227 / 255, 170 / 255, 14 / 255, 1])
#self._kin_char = self._visual.add_character("kin", [1, 1, 1, 0.4])
self._kin_char = self._visual.add_character(
"kin", [44 / 255, 160 / 255, 44 / 255, 1])
self._visual.camera_follow(self._sim_char, 2, 180, 0)
# initialize simulation parts
self._engine = engine_builder(engine, self._skeleton, self_collision,
self._sim_step, self._model)
contact_parser = argparse.ArgumentParser()
contact_parser.add_argument("N",
type=int,
nargs="+",
help="allowed contact body ids")
contact_file = "data/contacts/%s_%s" % (self._model,
allowed_contacts[contact])
with open(contact_file) as fh:
s = fh.read().split()
args = contact_parser.parse_args(s)
allowed_body_ids = args.N
self._engine.set_allowed_fall_contact(allowed_body_ids)
self._contact_ground = False
# joint torque monitor # debug use
self._monitor_joints = record_torques
if self._monitor_joints:
self._monitored_joints = list(range(self._skeleton.num_joints))
self._engine.set_monitored_joints(self._monitored_joints)
# initialize reinfrocement learning action bound
self.a_min, self.a_max = self.build_action_bound()
self._max_t = max(20, self._mocap._cycletime)
self._task_t = 0.5
self._mode = 0 # 0 for max_t and 1 for task_t
class BaseEnv(ABC):
"""
environment abstraction
__init__
- build_action_bound (abstract)
get_state_size (abstract)
get_action_size (abstract)
get_reset_data (abstract)
reset (abstract)
step
- set_action (abstract)
- check_valid_episode
- record_state (abstract)
- record_info (abstract)
- calc_reward (abstract)
- is_episode_end
check_terminate (abstract)
check_fall
check_wrap_end
check_time_end
set_mode
set_task_t
"""
def __init__(self,
task,
seed=0,
model="humanoid3d",
engine="pybullet",
contact="walk",
self_collision=True,
enable_draw=False,
record_contact=False,
record_torques=False):
self._task = task
self._rand = np.random.RandomState(seed)
self._model = model
self._ctrl_step = ACT_STEPTIME
self._nsubsteps = SUBSTEPS
self._sim_step = self._ctrl_step / self._nsubsteps
self._enable_draw = enable_draw
self._vis_step = VIS_STEPTIME
self._follow_character = True
self._record_contact = record_contact
# initialize kinematic parts
char_file = "data/characters/%s.txt" % self._model
ctrl_file = "data/controllers/%s_ctrl.txt" % self._model
motion_file = "data/motions/%s_%s.txt" % (self._model, self._task)
self._skeleton = HumanoidSkeleton(char_file, ctrl_file)
self._mocap = HumanoidMocap(self._skeleton, motion_file)
self.curr_phase = None
# initialize visual parts, need to be the first when using pybullet environment
if self._enable_draw:
# draw timer
self._prev_clock = time.perf_counter()
self._left_time = 0
# visual part
self._visual = HumanoidVis(self._skeleton, self._model)
cnt, pose, vel = self._mocap.slerp(0)
self._sim_char = self._visual.add_character(
"sim", [227 / 255, 170 / 255, 14 / 255, 1])
#self._kin_char = self._visual.add_character("kin", [1, 1, 1, 0.4])
self._kin_char = self._visual.add_character(
"kin", [44 / 255, 160 / 255, 44 / 255, 1])
self._visual.camera_follow(self._sim_char, 2, 180, 0)
# initialize simulation parts
self._engine = engine_builder(engine, self._skeleton, self_collision,
self._sim_step, self._model)
contact_parser = argparse.ArgumentParser()
contact_parser.add_argument("N",
type=int,
nargs="+",
help="allowed contact body ids")
contact_file = "data/contacts/%s_%s" % (self._model,
allowed_contacts[contact])
with open(contact_file) as fh:
s = fh.read().split()
args = contact_parser.parse_args(s)
allowed_body_ids = args.N
self._engine.set_allowed_fall_contact(allowed_body_ids)
self._contact_ground = False
# joint torque monitor # debug use
self._monitor_joints = record_torques
if self._monitor_joints:
self._monitored_joints = list(range(self._skeleton.num_joints))
self._engine.set_monitored_joints(self._monitored_joints)
# initialize reinfrocement learning action bound
self.a_min, self.a_max = self.build_action_bound()
self._max_t = max(20, self._mocap._cycletime)
self._task_t = 0.5
self._mode = 0 # 0 for max_t and 1 for task_t
@abstractmethod
def get_state_size(self):
assert (False and "not implemented")
@abstractmethod
def get_action_size(self):
assert (False and "not implemented")
@abstractmethod
def build_action_bound(self):
assert (False and "not implemented")
@abstractmethod
def get_reset_data(self):
assert (False and "not implemented")
@abstractmethod
def reset(self, phase=None, state=None):
""" Reset environment
Inputs:
phase
state
Outputs:
obs
"""
assert (False and "not implemented")
def step(self, action):
""" Step environment
Inputs:
action
Outputs:
ob
r
done
info
"""
self.set_action(action)
self._contact_forces = []
self._joint_torques = []
# if not terminated during simulation
self._contact_ground = False
for i in range(self._nsubsteps):
self.update()
if self.check_fall():
self._contact_ground = True
is_fail = self.check_terminate()
ob = self.record_state()
r = 0 if is_fail else self.calc_reward()
done = is_fail or self.is_episode_end()
info = self.record_info()
return ob, r, done, info
@abstractmethod
def set_action(self, action):
assert (False and "not implemented")
def update(self):
# update draw
if self._enable_draw and self.time_to_draw(self._sim_step):
self.update_draw()
self._engine.step_sim(self._sim_step)
if self._record_contact:
self.update_contact_forces()
if self._monitor_joints:
self.update_joint_torques()
self.post_update()
def time_to_draw(self, timestep):
self._left_time -= timestep
if self._left_time < 0:
self._left_time += self._vis_step
return True
else:
return False
def update_draw(self):
# synchronize sim pose and kin pose
sim_pose, sim_vel = self._engine.get_pose()
kin_t = self.curr_phase * self._mocap._cycletime
cnt, kin_pose, kin_vel = self._mocap.slerp(kin_t)
# offset kinematic pose
kin_pose[:3] += cnt * self._mocap._cyc_offset
kin_pose[0] += 1.5
# wait until time
time_remain = self._vis_step - (time.perf_counter() - self._prev_clock)
if time_remain > 0:
time.sleep(time_remain)
self._prev_clock = time.perf_counter()
# draw on window
self._visual.set_pose(self._sim_char, sim_pose, sim_vel)
self._visual.set_pose(self._kin_char, kin_pose, kin_vel)
# adjust cameral pose
if self._follow_character:
self._visual.camera_follow(self._sim_char)
def update_contact_forces(self):
# draw contact forces
contact_forces = self._engine.report_contact_force()
self._contact_forces.append(contact_forces)
def draw_contact_force(self):
contact_forces = self._engine.report_contact_force()
for part, pos, force in contact_forces:
self._visual.visual_force(pos, force)
def update_joint_torques(self):
jt = self._engine.get_monitored_joint_torques()
self._joint_torques.append(jt)
def draw_joint_torques(self):
jt = self._engine.get_monitored_joint_torques()
pose, vel = self._engine.get_pose()
self._skeleton.set_pose(pose)
for i, torque in zip(self._monitored_joints, jt):
pos = self._skeleton.get_joint_pos(i)
self._visual.visual_force(pos, np.array(torque))
@abstractmethod
def record_info(self):
assert (False and "not implemented")
@abstractmethod
def post_update(self):
assert (False and "not implemented")
@abstractmethod
def record_state(self):
assert (False and "not implemented")
@abstractmethod
def calc_reward(self):
assert (False and "not implemented")
def is_episode_end(self):
""" Check if episode is end
episode will normally end if time excced max time
"""
return self.check_wrap_end() or self.check_time_end()
@abstractmethod
def check_terminate(self):
""" Check if simulation or task failed
"""
assert (False and "not implemented")
def check_fall(self):
""" Check if any not allowed body part contact ground
"""
# check fail
return self._engine.check_fall()
def check_valid_episode(self):
# TODO check valid
#return self._core.CheckValidEpisode()
return True
def check_time_end(self):
if self._mode == 0:
return self._engine.t >= self._task_t
elif self._mode == 1:
return self._engine.t >= self._max_t
else:
assert (False and "no supported mode")
def check_wrap_end(self):
""" Check if time is up for non-wrap motions
True if motion is non-wrap and reaches phase 1.0
False if motion is wrap or hasn't reaches phase 1.0
"""
loop_term = not self._mocap._is_wrap and self.curr_phase > 1.0
return loop_term
def set_mode(self, mode):
# 0 for train, 1 for test using max time
assert (mode >= 0 and mode < 2)
self._mode = mode
def set_task_t(self, t):
""" Set the max t an episode can have under training mode
"""
self._task_t = min(t, self._max_t)
def set_max_t(self, t):
""" Set the max t an episode can have under test mode
"""
self._max_t = t
def close(self):
self._engine.close()
class BaseEnv(ABC, gym.Env):
def __init__(self, seed=0, **kwargs):
from presets import preset
if "new_process" in kwargs and kwargs["new_process"] == True:
preset.override(kwargs["forked_preset"])
self.init_parameters()
env_settings = preset.env
self.latent_dim = preset.vae.latent_dim
self._rand = np.random.RandomState(seed)
self.sim_frequency = env_settings.simulation_frequency
self.control_frequency = env_settings.control_frequency
self._sim_step = 1.0 / self.sim_frequency
self._nsubsteps = int(self.sim_frequency / self.control_frequency)
# initialize kinematic parts
char_file = "data/characters/%s.txt" % env_settings.character
ctrl_file = "data/controllers/%s.txt" % env_settings.character_ctrl
self._skeleton = HumanoidSkeleton(char_file, ctrl_file)
# initialize simulation parts
self._engine = engine_builder(env_settings.physics_engine,
self._sim_step)
from sim import Plane
from sim import Character
self.plane = Plane()
self.character = Character(self._skeleton,
env_settings.enable_self_collision)
self._engine.add_object(self.plane)
self._engine.add_object(self.character)
self._enable_draw = env_settings.enable_rendering
if self._enable_draw:
self.__init_render()
self.a_min, self.a_max = self.build_action_bound()
self.observation_space = gym.spaces.Box(-np.inf, np.inf,
[self.get_state_size()],
np.float32)
self.action_space = gym.spaces.Box(self.a_min.astype(np.float32),
self.a_max.astype(np.float32),
dtype=np.float32)
self._mode = 0 # 0 for train and 1 for test
def init_parameters(self):
pass
def update_control_frequency(self, f):
self.control_frequency = f
self._nsubsteps = int(self.sim_frequency / f)
def __init_render(self):
self._ur_renderer = UnityRenderer(self._sim_step)
self._ur_renderer.register_object("character", self.character)
def __get_link_states(self):
low_states = self._engine.get_link_states(
self._skeleton.get_link_ids())
states = []
for i in range(len(low_states)):
state = list(low_states[i][0]) + list(low_states[i][1])
states.append(state)
return states
@abstractmethod
def get_state_size(self):
raise NotImplementedError
@abstractmethod
def get_action_size(self):
raise NotImplementedError
@abstractmethod
def build_action_bound(self):
raise NotImplementedError
@abstractmethod
def get_state_normalization_exclusion_list(self):
raise NotImplementedError
@abstractmethod
def reset(self):
raise NotImplementedError
def step(self, action):
""" Step environment
Inputs:
action
Outputs:
ob
r
done
info
"""
self.set_action(action)
# if not terminated during simulation
self.is_fail = False
for i in range(self._nsubsteps):
self.update()
if self.check_terminate():
self.is_fail = True
break
self.current_obs = self.record_state()
r = self.calc_reward()
done = self.is_fail or self.is_episode_end()
info = self.record_info()
return self.current_obs, r, done, info
@abstractmethod
def set_action(self, action):
raise NotImplementedError
def update(self):
if self._enable_draw:
self._ur_renderer.tick()
self._engine.step_sim()
self.post_update()
@abstractmethod
def record_info(self):
raise NotImplementedError
def post_update(self):
pass
@abstractmethod
def record_state(self):
raise NotImplementedError
@abstractmethod
def calc_reward(self):
raise NotImplementedError
@abstractmethod
def is_episode_end(self):
raise NotImplementedError
@abstractmethod
def check_terminate(self):
raise NotImplementedError
def check_not_allowed_contacts(self):
return self.character.check_not_allowed_contacts()
def set_mode(self, mode):
# 0 for train, 1 for test
assert (mode >= 0 and mode < 2)
self._mode = mode
def close(self):
self._engine.close()
class StyleGramEnv(SpacetimeBoundsEnv):
def __init__(self,
style,
window=None,
scale=0.06,
e_scale=4,
ca_scale=20,
a_scale=20,
at_scale=4,
ay_scale=20,
z_scale=2,
cv_scale=4,
cv_ratio=1,
abs_acc=False,
avoid_self=False,
**kwargs):
"""
Initialize Gram matrix style transfer environment
"""
super().__init__(**kwargs)
if window:
self._window = window
else:
if self._mocap._is_wrap:
self._window = int(self._mocap._cycletime * 60)
else:
assert (False and "style reference is not cyclic")
self._min_window = 7
self._scale = scale
self._history = []
char_file = "data/characters/humanoid3d_cmu.txt"
ctrl_file = "data/controllers/humanoid3d_cmu_ctrl.txt"
self._style_skeleton = HumanoidSkeleton(char_file, ctrl_file)
style_file = "data/motions/humanoid3d_%s.txt" % style
self._style_G = self.style_gram_matrix(style_file)
# regularization
self._e_scale = e_scale
self._ca_scale = ca_scale
self._a_scale = a_scale
self._at_scale = at_scale
self._ay_scale = ay_scale
self._z_off_scale = z_scale
self._cv_scale = cv_scale
self._cv_ratio = cv_ratio
self._abs_acc = abs_acc
self._avoid_self = avoid_self
self.init_params(style_file)
self._prev_cv = None
self._curr_cv = None
self._prev_vs = None
self._curr_vs = None
self._prev_omgs = None
self._curr_omgs = None
def init_params(self, motion_file):
mocap = HumanoidMocap(self._skeleton, motion_file)
get_bound = lambda p: min(p['max'], p['upper_fence'])
e_param = mocap.get_energy_statistics()
self._e_penalty = get_bound(e_param) * self._e_scale
com_param, body_params = mocap.get_acceleration_statistics(
self._abs_acc)
self._ca_penalty = get_bound(com_param) * self._ca_scale
self._a_penalties = map(get_bound, body_params)
self._a_penalties = np.array(list(self._a_penalties)) * self._a_scale
com_vel = mocap._com_vel
com_vel_l = np.sqrt((com_vel**2).sum(axis=1))
self._cv_goal = np.array([com_vel_l.mean(), 0, 0])
self._cv_penalty = max(com_vel_l.std(),
0.2 * com_vel_l.mean()) * self._cv_scale
self._cv_goal *= self._cv_ratio
self._cv_penalty *= self._cv_ratio
twist_params, yaw_params = mocap.get_angular_acceleration_statistics()
self._at_penalties = map(get_bound, twist_params)
self._at_penalties = np.array(list(
self._at_penalties)) * self._at_scale + 0.1
self._ay_penalties = map(get_bound, yaw_params)
self._ay_penalties = np.array(list(
self._ay_penalties)) * self._ay_scale + 0.1
def style_gram_matrix(self, motion_file):
# load style motion
style_mocap = HumanoidMocap(self._style_skeleton,
motion_file,
extend_none_wrap=False)
# transfer to features
if style_mocap._is_wrap:
clip_time = 25 * style_mocap._cycletime + 12 / 60
else:
clip_time = style_mocap._cycletime
length = int(clip_time * 60)
traj = []
for i in range(length):
cnt, pose, vel = style_mocap.slerp(i / 60)
pose[:3] += style_mocap._cyc_offset * cnt
pose[:3] *= 19
self._style_skeleton.set_pose(pose)
jointposes = self._style_skeleton.get_pos_for_gram()
traj.append(jointposes)
traj = np.array(traj)
feature = process_trajectory(traj).transpose()
dim, l = feature.shape
feature = feature.reshape(1, dim, l)
# to gram matrix
G = process_gram_matrix(feature)
return G
def get_state_size(self):
single_state = self._skeleton.build_state()
# 1 dim for phase, 2 for additional x, z, 1 for sim state (which contains y)
state_size = 1 + single_state.size
return state_size
def style_distance(self):
if (len(self._history) < self._min_window):
return 0
# turn history records to feature
traj = np.array(self._history)
feature = process_trajectory(traj)
feature = feature.transpose()
content_G = process_gram_matrix(feature)
err = np.square(content_G - self._style_G).mean()
err = np.sqrt(err)
return err
def get_foot_forces(self):
""" get foot supporting forces
Output:
FL vector, contact force on left foot
FR vector, contact force on right foot
"""
left_forces = []
right_forces = []
for sim_step in self._contact_forces:
l_f = np.array([0., 0., 0.])
r_f = np.array([0., 0., 0.])
for part, pos, force in sim_step:
if part == 5:
r_f += force
if part == 11:
l_f += force
left_forces.append(l_f)
right_forces.append(r_f)
left_forces = np.array(left_forces)
right_forces = np.array(right_forces)
FL = left_forces.mean(axis=0)
FR = right_forces.mean(axis=0)
return FL, FR
def get_CoMz_offset(self):
pos = self._skeleton.get_com_pos()
return pos[2]
def get_CoM_velocity(self):
vel = self._skeleton.get_com_vel()
return vel
def get_body_velocities(self):
vels = list(
map(self._skeleton.get_body_vel, range(self._skeleton.num_joints)))
vels = np.array(vels)
return vels
def get_joint_local_omgs(self):
omgs = list(
map(self._skeleton.get_joint_local_omg,
range(self._skeleton.num_joints)))
omgs = np.array(omgs)
return omgs
def get_kin_energy(self):
char = self._skeleton._kin_core.getCharacter()
kin_energy = KinematicCore.kinematicEnergy(char, True)
return kin_energy
def reset(self, phase=None, state=None):
self._history = []
return super().reset(phase, state)
def record_state(self):
"""
state is made up with 2 items:
- phase
- sim_state
"""
pose, vel = self._engine.get_pose()
# add to history and mantain features for style comparing
style_pose = pose.copy()
style_pose[:3] *= 19
self._style_skeleton.set_pose(style_pose)
pos_gram = self._style_skeleton.get_pos_for_gram()
if len(self._history) > 1:
pos_gram_mid = (pos_gram + self._history[-1]) / 2
self._history.append(pos_gram_mid)
self._history.append(pos_gram)
else:
# when start from [], first position
self._history.append(pos_gram)
if len(self._history) > self._window:
# pop out two frames, since the record freq is 60Hz
self._history.pop(0)
self._history.pop(0)
# build state using current reference root, and without global info
self._skeleton.set_pose(pose)
self._skeleton.set_vel(vel)
self._curr_sim_pose = pose
self._curr_sim_vel = vel
ori_pos = pose[:3]
ori_rot = pose[3:7]
sim_state = self._skeleton.build_state(ori_pos, ori_rot,
self._use_global_root_ori)
phase = self.curr_phase % 1.0
state = np.concatenate(([phase], sim_state))
# get com velocity and body parts velocity
vel = self.get_CoM_velocity()
self._prev_cv = self._curr_cv
self._curr_cv = vel.copy()
vels = self.get_body_velocities()
self._prev_vs = self._curr_vs
self._curr_vs = vels.copy()
# get joint local omg
omgs = self.get_joint_local_omgs()
self._prev_omgs = self._curr_omgs
self._curr_omgs = omgs.copy()
return state
def calc_reward(self):
# penalizing self contact
if self._avoid_self and self._engine.check_self_contact():
return 0
# style reward
err_s = self.style_distance()
r_s = np.exp(-err_s / self._scale)
# z offset, c velocity
z_off = self.get_CoMz_offset()
err_z = abs(z_off)
vel = self.get_CoM_velocity()
err_cv = np.linalg.norm(vel - self._cv_goal)
reg_off = np.array(
[err_z / self._z_off_scale, err_cv / self._cv_penalty])
# kinematic energy
kin_energy = self.get_kin_energy()
reg_energy = kin_energy / self._e_penalty
# linear acceleration
com_acc = (self._curr_cv - self._prev_cv) * 30
body_accs = (self._curr_vs - self._prev_vs) * 30
if not self._abs_acc:
body_accs = body_accs - com_acc
err_ca = abs(com_acc[1])
err_as = np.sqrt((body_accs**2).sum(axis=1))
reg_com = err_ca / self._ca_penalty
reg_as = err_as / self._a_penalties
# angular acceleration
angular_acc = (self._curr_omgs - self._prev_omgs) * 30
twist_acc = angular_acc[:, 1]
yaw_acc = angular_acc[:, [0, 2]]
twist_err = abs(twist_acc)
yaw_err = np.sqrt((yaw_acc**2).sum(axis=1))
reg_aa = np.concatenate(
[twist_err / self._at_penalties, yaw_err / self._ay_penalties])
# sum up all regularizations
regs = [
reg_off.mean(), reg_energy, reg_com,
reg_as.mean(),
reg_aa.mean()
]
regs = np.array(regs)
r_reg = np.exp(-regs.mean())
rwd = r_s * r_reg
#return np.array([r_s, r_reg])
return rwd
def record_info(self):
info = super().record_info()
info["trust_rwd"] = len(self._history) > self._min_window
return info
import sys, os
from utils.humanoid_kin import HumanoidSkeleton
from utils.humanoid_mocap import HumanoidMocap
from utils.motion_recorder import MotionRecorder
from ur import UnityRenderer
from sim import engine_builder, Plane, Character
from env.jumper.block import Block
from env.jumper.wall import Wall
from presets import preset
preset.load_default()
preset.experiment.env = "deepmimic"
char_file = "data/characters/humanoid3d.txt"
ctrl_file = "data/controllers/humanoid3d_ctrl.txt"
skeleton = HumanoidSkeleton(char_file, ctrl_file)
engine = engine_builder("pybullet", 1/600.0)
plane = Plane()
character = Character(skeleton, True)
engine.add_object(plane)
engine.add_object(character)
obj_map = {'character': character}
recorder = MotionRecorder(input_file=sys.argv[1])
if recorder.get('block'):
obj_map['block'] = Block()
if recorder.get('wall'):