def main():
MOTION_ONLY = False
env_name = 'walk'
ppo = PPO(env_name, 0, visualize_only=True)
# if not MOTION_ONLY:
# ppo.LoadModel('model/' + env_name + '.pt')
ppo.env.Resets(False)
ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(ppo.env.phase_frame))
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer('MotionModel', yr.VpModelRenderer(ppo.env.ref_skel, (150,150,255), yr.POLYGON_FILL))
if not MOTION_ONLY:
viewer.doc.addRenderer('controlModel', yr.VpModelRenderer(ppo.env.skel, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('contact', yr.VectorsRenderer(rd_contact_forces, rd_contact_positions, (255,0,0)))
def postCallback(frame):
ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame))
def simulateCallback(frame):
state = ppo.env.GetState(0)
action_dist, _ = ppo.model(torch.tensor(state.reshape(1, -1)).float())
action = action_dist.loc.detach().numpy()
res = ppo.env.Steps(action)
# res = ppo.env.Steps(np.zeros_like(action))
# print(frame, ppo.env.Ref_skel.current_frame, ppo.env.world.time()*ppo.env.ref_motion.fps)
# print(frame, res[0][0])
# if res[0][0] > 0.46:
# ppo.env.continue_from_now_by_phase(0.2)
# if res[2]:
# print(frame, 'Done')
# ppo.env.reset()
# contact rendering
contacts = ppo.env.world.collision_result.contacts
del rd_contact_forces[:]
del rd_contact_positions[:]
for contact in contacts:
rd_contact_forces.append(contact.f/1000.)
rd_contact_positions.append(contact.p)
if MOTION_ONLY:
viewer.setPostFrameCallback_Always(postCallback)
viewer.setMaxFrame(len(ppo.env.ref_motion)-1)
else:
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(3000)
viewer.startTimer(1./30.)
viewer.show()
Fl.run()
def main():
fGraph = build_graph()
def get_info(x):
temp = x.split()
return tuple([int(temp[0]), int(temp[1]), float(temp[2])])
motion_idx = [None, None]
with open('mg.txt', 'r') as f:
s = f.read().splitlines(False)
total_num = int(s[0])
motion_info = list(map(get_info, s[1:]))
pydart.init()
world = [
pydart.World(1. / 1200., "../DartDeep/data/woody_with_ground_v2.xml")
for _ in range(2)
]
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer('motionModel[0]',
yr.DartRenderer(world[0], (255, 240, 255)))
viewer.doc.addRenderer('motionModel[1]', yr.DartRenderer(world[1]))
motion_state = [0]
def callback_0(_):
motion_state[0] = 0
def callback_1(_):
motion_state[0] = 1
def callback_2(_):
motion_state[0] = 2
viewer.objectInfoWnd.addBtn('0', callback_0)
viewer.objectInfoWnd.addBtn('1', callback_1)
viewer.objectInfoWnd.addBtn('2', callback_2)
def postCallback(frame):
for i in range(2):
world[i].skeletons[1].set_positions(
fGraph.motion_frames.get_q(motion_info[frame][i]))
print(frame, motion_info[frame])
viewer.setPreFrameCallback_Always(postCallback)
viewer.setMaxFrame(total_num - 1)
viewer.startTimer(1. / 30.)
viewer.show()
Fl.run()
def main():
MOTION_ONLY = False
pydart.init()
env_name = 'multi'
ppo = PPO(env_name, 1, visualize_only=True)
if not MOTION_ONLY:
ppo.LoadModel('model/' + env_name + '.pt')
ppo.env.specify_motion_num(1)
ppo.env.Resets(False)
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
dart_world = ppo.env.world
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer(
'MotionModel',
yr.DartRenderer(ppo.env.ref_world, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'contact',
yr.VectorsRenderer(rd_contact_forces, rd_contact_positions,
(255, 0, 0)))
def preCallback(frame):
ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame))
def simulateCallback(frame):
state = ppo.env.GetState(0)
action_dist, _ = ppo.model(torch.tensor(state.reshape(1, -1)).float())
action = action_dist.loc.detach().numpy()
res = ppo.env.Steps(action)
# contact rendering
contacts = ppo.env.world.collision_result.contacts
del rd_contact_forces[:]
del rd_contact_positions[:]
for contact in contacts:
rd_contact_forces.append(contact.f / 1000.)
rd_contact_positions.append(contact.p)
if MOTION_ONLY:
viewer.setPreFrameCallback_Always(preCallback)
else:
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(len(ppo.env.ref_motion) - 1)
viewer.startTimer(1 / ppo.env.ref_motion.fps)
viewer.show()
Fl.run()
def main(filenames):
motion = []
for filename in filenames:
motion.append(yf.readBvhFile(filename))
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
viewer = hsv.hpSimpleViewer(title='Bvh Viewer', rect=(0, 0, 1200, 800), viewForceWnd=False)
# viewer.doc.addRenderer('MotionModel', yr.DartRenderer(ppo.env.ref_world, (150,150,255), yr.POLYGON_FILL))
for motion_idx in range(len(motion)):
viewer.doc.addRenderer('motion'+str(motion_idx), yr.JointMotionRenderer(motion[motion_idx], (randrange(256), randrange(256), randrange(256))))
def preCallback(frame):
for _motion in motion:
_motion.frame = min(frame, len(_motion)-1)
viewer.setPostFrameCallback_Always(preCallback)
viewer.setMaxFrame(max([len(_motion)-1 for _motion in motion]))
viewer.startTimer(1./30.)
viewer.show()
Fl.run()
def main():
from fltk import Fl
from PyCommon.modules.GUI import hpSimpleViewer as hsv
from PyCommon.modules.Renderer import ysRenderer as yr
pydart.init()
args = mujoco_arg_parser().parse_args()
logger.configure(dir="./run/")
model, env = load(args.env, num_timesteps=10000000, seed=args.seed)
logger.log("Running trained model")
obs = np.zeros((env.num_envs, ) + env.observation_space.shape)
obs[:] = env.reset()
env.venv.envs[0].env.rsi = False
dart_world = env.venv.envs[0].env.world
viewer = hsv.hpSimpleViewer(viewForceWnd=False)
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dart_world, (150, 150, 255), yr.POLYGON_FILL))
def simulateCallback(frame):
actions = model.step(obs)
res = env.step(actions[0])
obs[:] = res[0]
done = res[2]
# if done[0]:
# break
# env.render()
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(3000)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def main():
CURRENT_CHECK = False
RSI = True
MOTION_ONLY = False
SKELETON_ON = False
CAMERA_TRACKING = True
PD_PLOT = False
PUSH = False
pydart.init()
env_name = 'walk'
env_name = 'walk_repeated'
# env_name = 'walk_fast'
# env_name = 'walk_sukiko'
# env_name = 'walk_u_turn'
# env_name = '1foot_contact_run'
# env_name = 'round_girl'
# env_name = 'fast_2foot_hop'
# env_name = 'slow_2foot_hop'
# env_name = 'long_broad_jump'
# env_name = 'short_broad_jump'
# env_name = 'n_kick'
# env_name = 'jump'
gain_p0 = []
gain_p1 = []
gain_p2 = []
gain_p3 = []
gain_p4 = []
MA_DUR = 5
ma_gain_p0 = []
ma_gain_p1 = []
ma_gain_p2 = []
ma_gain_p3 = []
ma_gain_p4 = []
rfoot_contact_ranges = []
lfoot_contact_ranges = []
ppo = PPO(env_name, 0, visualize_only=True)
if not MOTION_ONLY and not CURRENT_CHECK:
ppo.LoadModel('model/' + env_name + '.pt')
# ppo.LoadModel('model/' + 'param' + '.pt')
# ppo.LoadModel('model_test/' + env_name + '.pt')
# ppo.LoadModel('model_test/' + 'param' + '.pt')
elif not MOTION_ONLY and CURRENT_CHECK:
env_model_dir = []
for dir_name in sorted(os.listdir()):
if env_name in dir_name:
env_model_dir.append(dir_name)
pt_names = os.listdir(env_model_dir[-1])
pt_names.pop(pt_names.index('log.txt'))
pt_names.sort(key=lambda f: int(os.path.splitext(f)[0]))
pt_names.append('1171.pt')
ppo.LoadModel(env_model_dir[-1] + '/' + pt_names[-1])
print(env_model_dir[-1] + '/' + pt_names[-1])
ppo.env.dart_skel_file = '../data/wd2_seg_rough_terrain.xml'
ppo.env.rsi_num = 560
ppo.env.hard_reset()
ppo.env.Resets(RSI)
ppo.env.ref_skel.set_positions(
ppo.env.ref_motion.get_q(ppo.env.phase_frame))
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
rd_exf = [None]
rd_exf_pos = [None]
dart_world = ppo.env.world
skel = dart_world.skeletons[1]
viewer_w, viewer_h = 960, 1080
viewer = hsv.hpSimpleViewer(rect=(0, 0, viewer_w + 300, 1 + viewer_h + 55),
viewForceWnd=False)
# viewer.doc.addRenderer('MotionModel', yr.DartRenderer(ppo.env.ref_world, (150,150,255), yr.POLYGON_FILL))
control_model_renderer = None
if not MOTION_ONLY:
control_model_renderer = yr.DartRenderer(dart_world, (255, 240, 255),
yr.POLYGON_FILL)
viewer.doc.addRenderer('controlModel', control_model_renderer)
for shape_idx in range(
ppo.env.world.skeletons[0].body(0).num_shapenodes() // 2):
control_model_renderer.geom_colors[0][0][shape_idx] = (128, 128,
128)
# viewer.doc.addRenderer('contact', yr.VectorsRenderer(rd_contact_forces, rd_contact_positions, (255,0,0)))
viewer.doc.addRenderer(
'extraForceEnable',
yr.WideArrowRenderer(rd_exf,
rd_exf_pos, (255, 0, 0),
lineWidth=.05,
fromPoint=False))
def makeEmptyBasicSkeletonTransformDict(init=None):
Ts = dict()
Ts['pelvis'] = init
Ts['spine_ribs'] = init
Ts['head'] = init
Ts['thigh_R'] = init
Ts['shin_R'] = init
Ts['foot_heel_R'] = init
Ts['foot_R'] = init
Ts['heel_R'] = init
Ts['outside_metatarsal_R'] = init
Ts['outside_phalanges_R'] = init
Ts['inside_metatarsal_R'] = init
Ts['inside_phalanges_R'] = init
Ts['upper_limb_R'] = init
Ts['lower_limb_R'] = init
Ts['thigh_L'] = init
Ts['shin_L'] = init
Ts['foot_heel_L'] = init
Ts['foot_L'] = init
Ts['heel_L'] = init
Ts['outside_metatarsal_L'] = init
Ts['outside_phalanges_L'] = init
Ts['inside_metatarsal_L'] = init
Ts['inside_phalanges_L'] = init
Ts['upper_limb_L'] = init
Ts['lower_limb_L'] = init
return Ts
skeleton_renderer = None
if SKELETON_ON:
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08)
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0.8, -0.02, 0.))
skeleton_renderer = yr.BasicSkeletonRenderer(
makeEmptyBasicSkeletonTransformDict(np.eye(4)),
color=(230, 230, 230),
offset_draw=(0., -0.0, 0.))
viewer.doc.addRenderer('skeleton', skeleton_renderer)
def simulateCallback(frame):
state = ppo.env.GetState(0)
action_dist, v = ppo.model(torch.tensor(state.reshape(1, -1)).float())
action = action_dist.loc.detach().numpy()
value = v.detach().numpy()
if PUSH and 110 < frame < 123:
ppo.env.applied_force = np.array([0., 0., 50.])
ppo.env.applied_force_offset = np.array([-0.11, 0., 0.])
del rd_exf[:]
del rd_exf_pos[:]
rd_exf.append(ppo.env.applied_force / 100.)
rd_exf_pos.append(
ppo.env.skel.body('Spine').to_world(
ppo.env.applied_force_offset))
elif PUSH and 195 < frame < 208:
ppo.env.applied_force = np.array([0., 0., 80.])
ppo.env.applied_force_offset = np.array([-0.11, 0., 0.])
del rd_exf[:]
del rd_exf_pos[:]
rd_exf.append(ppo.env.applied_force / 100.)
rd_exf_pos.append(
ppo.env.skel.body('Spine').to_world(
ppo.env.applied_force_offset))
elif PUSH and 281 < frame < 294:
ppo.env.applied_force = np.array([0., 0., 100.])
ppo.env.applied_force_offset = np.array([-0.11, 0., 0.])
del rd_exf[:]
del rd_exf_pos[:]
rd_exf.append(ppo.env.applied_force / 100.)
rd_exf_pos.append(
ppo.env.skel.body('Spine').to_world(
ppo.env.applied_force_offset))
else:
del rd_exf[:]
del rd_exf_pos[:]
ppo.env.applied_force = np.zeros(3)
res = ppo.env.Steps(action)
# for gain plotting
gain_p0.append(res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_x')])
gain_p1.append(
res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_0_0_x')])
gain_p2.append(
res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_0_0_0_x')])
gain_p3.append(
res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_0_1_0_x')])
gain_p4.append(
res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_1_0_x')])
ma_gain_p0.append(
sum(gain_p0[-MA_DUR:]) /
MA_DUR if len(gain_p0) >= MA_DUR else sum(gain_p0) / len(gain_p0))
ma_gain_p1.append(
sum(gain_p1[-MA_DUR:]) /
MA_DUR if len(gain_p1) >= MA_DUR else sum(gain_p1) / len(gain_p1))
ma_gain_p2.append(
sum(gain_p2[-MA_DUR:]) /
MA_DUR if len(gain_p2) >= MA_DUR else sum(gain_p2) / len(gain_p2))
ma_gain_p3.append(
sum(gain_p3[-MA_DUR:]) /
MA_DUR if len(gain_p3) >= MA_DUR else sum(gain_p3) / len(gain_p3))
ma_gain_p4.append(
sum(gain_p4[-MA_DUR:]) /
MA_DUR if len(gain_p4) >= MA_DUR else sum(gain_p4) / len(gain_p4))
if any([
'RightFoot' in body.name
for body in ppo.env.world.collision_result.contacted_bodies
]):
if rfoot_contact_ranges and rfoot_contact_ranges[-1][
1] == frame - 1:
rfoot_contact_ranges[-1][1] = frame
else:
rfoot_contact_ranges.append([frame, frame])
# res = ppo.env.Steps(np.zeros_like(action))
ppo.env.world.collision_result.update()
# print(frame, ppo.env.Ref_skel.current_frame, ppo.env.world.time()*ppo.env.ref_motion.fps)
# print(frame, res[0][0])
# if res[0][0] > 0.46:
# ppo.env.continue_from_now_by_phase(0.2)
# print(frame, ' '.join(["{:0.1f}".format(400. * exp(log(400.) * rate/10.)) for rate in action[0][ppo.env.skel.ndofs-6:]]))
if res[2]:
print(frame, 'Done')
ppo.env.reset()
contacts = ppo.env.world.collision_result.contacts
# contact body rendering
if control_model_renderer is not None:
skel_idx = dart_world.skel.id
for body_idx in range(
dart_world.skeletons[skel_idx].num_bodynodes()):
for shape_idx in range(dart_world.skeletons[skel_idx].body(
body_idx).num_shapenodes()):
control_model_renderer.geom_colors[skel_idx][body_idx][
shape_idx] = control_model_renderer.totalColor
for contact in contacts:
body_idx, geom_idx = (
contact.bodynode_id1, contact.shape_id1
) if dart_world.skeletons[contact.skel_id1].body(
contact.bodynode_id1).name != 'ground' else (
contact.bodynode_id2, contact.shape_id2)
body = dart_world.skeletons[skel_idx].body(body_idx)
visual = sum(shapenode.has_visual_aspect()
for shapenode in body.shapenodes)
collision = sum(shapenode.has_collision_aspect()
for shapenode in body.shapenodes)
if visual == collision:
control_model_renderer.geom_colors[skel_idx][body_idx][
geom_idx - visual] = (255, 0, 0)
else:
control_model_renderer.geom_colors[skel_idx][body_idx][
(geom_idx - visual) // 2] = (255, 0, 0)
if PD_PLOT:
fig = plt.figure(1)
plt.clf()
fig.add_subplot(5, 1, 1)
for rfoot_contact_range in rfoot_contact_ranges:
plt.axvspan(rfoot_contact_range[0],
rfoot_contact_range[1],
facecolor='0.5',
alpha=0.3)
plt.ylabel('ankle')
plt.plot(range(len(ma_gain_p0)), ma_gain_p0)
fig.add_subplot(5, 1, 2)
for rfoot_contact_range in rfoot_contact_ranges:
plt.axvspan(rfoot_contact_range[0],
rfoot_contact_range[1],
facecolor='0.5',
alpha=0.3)
plt.ylabel('talus')
plt.plot(range(len(ma_gain_p1)), ma_gain_p1)
fig.add_subplot(5, 1, 3)
for rfoot_contact_range in rfoot_contact_ranges:
plt.axvspan(rfoot_contact_range[0],
rfoot_contact_range[1],
facecolor='0.5',
alpha=0.3)
plt.ylabel('thumb')
plt.plot(range(len(ma_gain_p2)), ma_gain_p2)
fig.add_subplot(5, 1, 4)
for rfoot_contact_range in rfoot_contact_ranges:
plt.axvspan(rfoot_contact_range[0],
rfoot_contact_range[1],
facecolor='0.5',
alpha=0.3)
plt.ylabel('phalange')
plt.plot(range(len(ma_gain_p3)), ma_gain_p3)
fig.add_subplot(5, 1, 5)
for rfoot_contact_range in rfoot_contact_ranges:
plt.axvspan(rfoot_contact_range[0],
rfoot_contact_range[1],
facecolor='0.5',
alpha=0.3)
plt.ylabel('heel')
plt.plot(range(len(ma_gain_p4)), ma_gain_p4)
plt.show()
plt.pause(0.001)
# contact rendering
contacts = ppo.env.world.collision_result.contacts
del rd_contact_forces[:]
del rd_contact_positions[:]
for contact in contacts:
rd_contact_forces.append(contact.f / 1000.)
rd_contact_positions.append(contact.p)
# render skeleton
if SKELETON_ON:
Ts = dict()
Ts['pelvis'] = skel.joint('j_Hips').get_local_transform()
Ts['thigh_R'] = skel.joint('j_RightUpLeg').get_local_transform()
Ts['shin_R'] = skel.joint('j_RightLeg').get_local_transform()
Ts['foot_R'] = skel.joint('j_RightFoot').get_local_transform()
Ts['foot_heel_R'] = skel.joint('j_RightFoot').get_local_transform()
Ts['heel_R'] = np.eye(4)
Ts['outside_metatarsal_R'] = skel.joint(
'j_RightFoot_foot_0_0').get_local_transform()
Ts['outside_phalanges_R'] = skel.joint(
'j_RightFoot_foot_0_0_0').get_local_transform()
# Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1'])
Ts['inside_metatarsal_R'] = np.eye(4)
Ts['inside_phalanges_R'] = skel.joint(
'j_RightFoot_foot_0_1_0').get_local_transform()
Ts['spine_ribs'] = skel.joint('j_Spine').get_local_transform()
Ts['head'] = skel.joint('j_Spine1').get_local_transform()
Ts['upper_limb_R'] = skel.joint('j_RightArm').get_local_transform()
# Ts['upper_limb_R'] = np.dot(skel.joint('j_RightArm').get_local_transform(), mm.SO3ToSE3(mm.rotX(pi/6.)))
Ts['lower_limb_R'] = skel.joint(
'j_RightForeArm').get_local_transform()
# Ts['lower_limb_R'] = np.dot(skel.joint('j_RightForeArm').get_local_transform(), mm.SO3ToSE3(mm.rotY(-pi/6.)))
Ts['thigh_L'] = skel.joint('j_LeftUpLeg').get_local_transform()
Ts['shin_L'] = skel.joint('j_LeftLeg').get_local_transform()
Ts['foot_L'] = skel.joint('j_LeftFoot').get_local_transform()
Ts['foot_heel_L'] = skel.joint('j_LeftFoot').get_local_transform()
Ts['heel_L'] = np.eye(4)
Ts['outside_metatarsal_L'] = skel.joint(
'j_LeftFoot_foot_0_0').get_local_transform()
Ts['outside_phalanges_L'] = skel.joint(
'j_LeftFoot_foot_0_0_0').get_local_transform()
# Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1'])
Ts['inside_metatarsal_L'] = np.eye(4)
Ts['inside_phalanges_L'] = skel.joint(
'j_LeftFoot_foot_0_1_0').get_local_transform()
Ts['upper_limb_L'] = skel.joint('j_LeftArm').get_local_transform()
# Ts['upper_limb_L'] = np.dot(skel.joint('j_LeftArm').get_local_transform(), mm.SO3ToSE3(mm.rotX(pi/6.)))
Ts['lower_limb_L'] = skel.joint(
'j_LeftForeArm').get_local_transform()
# Ts['lower_limb_L'] = np.dot(skel.joint('j_LeftForeArm').get_local_transform(), mm.SO3ToSE3(mm.rotY(pi/6.)))
skeleton_renderer.appendFrameState(Ts)
if MOTION_ONLY:
viewer.setMaxFrame(len(ppo.env.ref_motion) - 1)
if CAMERA_TRACKING:
cameraTargets = [None] * (viewer.getMaxFrame() + 1)
def postCallback(frame):
ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame - 1))
ppo.env.ref_motion.frame = frame - 1
if CAMERA_TRACKING:
if cameraTargets[frame] is None:
cameraTargets[frame] = ppo.env.ref_skel.body(0).com()
viewer.setCameraTarget(cameraTargets[frame])
viewer.setPostFrameCallback_Always(postCallback)
else:
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(3000)
if CAMERA_TRACKING:
cameraTargets = [None] * (viewer.getMaxFrame() + 1)
def postFrameCallback_Always(frame):
if CAMERA_TRACKING:
if cameraTargets[frame] is None:
cameraTargets[frame] = ppo.env.skel.body(0).com()
viewer.setCameraTarget(cameraTargets[frame])
viewer.setPostFrameCallback_Always(postFrameCallback_Always)
viewer.startTimer(1. / 30.)
viewer.show()
Fl.run()
def main():
win = SimonGame(700, 730)
win.show()
Fl.run()
def main():
MOTION_ONLY = False
CURRENT_CHECK = False
cvw.vp_init()
env_name = 'walk'
ppo = PPO(env_name, 0, visualize_only=True)
if not MOTION_ONLY and not CURRENT_CHECK:
# ppo.LoadModel('model/' + env_name + '.pt')
ppo.LoadModel('model/' + 'param' + '.pt')
elif not MOTION_ONLY and CURRENT_CHECK:
env_model_dir = []
for dir_name in sorted(os.listdir()):
if 'walk' in dir_name:
env_model_dir.append(dir_name)
pt_names = os.listdir(env_model_dir[-1])
pt_names.pop(pt_names.index('log.txt'))
pt_names.sort(key=lambda f: int(os.path.splitext(f)[0]))
ppo.LoadModel(env_model_dir[-1] + '/' + pt_names[-1])
# ppo.LoadModel(env_model_dir[-1]+'/'+'5324.pt')
print(pt_names[-1])
ppo.env.Resets(False)
ppo.env.ref_skel.set_q(ppo.env.ref_motion.get_q(ppo.env.phase_frame))
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer(
'MotionModel',
yr.VpModelRenderer(ppo.env.ref_skel, (150, 150, 255), yr.POLYGON_FILL))
control_model_renderer = None
if not MOTION_ONLY:
control_model_renderer = yr.VpModelRenderer(ppo.env.skel,
(255, 240, 255),
yr.POLYGON_FILL)
viewer.doc.addRenderer('controlModel', control_model_renderer)
viewer.doc.addRenderer(
'contact',
yr.VectorsRenderer(rd_contact_forces, rd_contact_positions,
(255, 0, 0)))
def postCallback(frame):
ppo.env.ref_skel.set_q(ppo.env.ref_motion.get_q(frame))
ppo.env.ref_motion.frame = frame - 1
def simulateCallback(frame):
state = ppo.env.GetState(0)
action_dist, _ = ppo.model(torch.tensor(state.reshape(1, -1)).float())
action = action_dist.loc.detach().numpy()
res = ppo.env.Steps(action)
# res = ppo.env.Steps(np.zeros_like(action))
# print(frame, ppo.env.Ref_skel.current_frame, ppo.env.world.time()*ppo.env.ref_motion.fps)
# print(frame, res[0][0])
# if res[0][0] > 0.46:
# ppo.env.continue_from_now_by_phase(0.2)
if res[2]:
print(frame, 'Done')
ppo.env.reset()
control_model_renderer._model = ppo.env.skel
# contact rendering
del rd_contact_forces[:]
del rd_contact_positions[:]
# for contact in contacts:
# rd_contact_forces.append(contact.f/1000.)
# rd_contact_positions.append(contact.p)
if MOTION_ONLY:
viewer.setPostFrameCallback_Always(postCallback)
viewer.setMaxFrame(len(ppo.env.ref_motion) - 1)
else:
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(3000)
viewer.startTimer(1. / 30.)
viewer.show()
Fl.run()
def main():
MOTION_ONLY = False
pydart.init()
env_name = 'multi'
ppo = PPO_MULTI(env_name, 0, visualize_only=True)
if not MOTION_ONLY:
ppo.LoadModel('model/param.pt')
ppo.env.specify_motion_num(5)
ppo.env.Resets(False)
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
rd_targets = [None]
dart_world = ppo.env.world
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
# viewer = hsv.hpSimpleViewer(rect=[0, 0, 960+300, 1+1080+55], viewForceWnd=False)
viewer.doc.addRenderer(
'MotionModel',
yr.DartRenderer(ppo.env.ref_world, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'contact',
yr.VectorsRenderer(rd_contact_forces, rd_contact_positions,
(255, 0, 0)))
viewer.doc.addRenderer('targets',
yr.PointsRenderer(rd_targets, color=(0, 0, 0)))
viewer.setMaxFrame(3000)
cameraTargets = [None] * (viewer.getMaxFrame() + 1)
def preCallback(frame):
ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame))
def simulateCallback(frame):
state = ppo.env.GetState(0)
action_dist, _ = ppo.model(state.reshape(1, -1))
action = action_dist.loc.detach().numpy()
res = ppo.env.Steps(action)
del rd_targets[:]
rd_targets.extend(ppo.env.goals_in_world_frame)
# res = ppo.env.step_after_training(action[0])
if res[2]:
print(frame, 'Done')
ppo.env.reset()
# contact rendering
contacts = ppo.env.world.collision_result.contacts
del rd_contact_forces[:]
del rd_contact_positions[:]
for contact in contacts:
rd_contact_forces.append(contact.f / 1000.)
rd_contact_positions.append(contact.p)
def postFrameCallback_Always(frame):
if cameraTargets[frame] is None:
cameraTargets[frame] = np.asarray(
dart_world.skel.body(0).to_world())
viewer.setCameraTarget(cameraTargets[frame])
if MOTION_ONLY:
viewer.setPreFrameCallback_Always(preCallback)
else:
viewer.setSimulateCallback(simulateCallback)
viewer.setPostFrameCallback_Always(postFrameCallback_Always)
viewer.startTimer(1 / ppo.env.ref_motion.fps)
viewer.show()
Fl.run()
def main():
MOTION_ONLY = False
pydart.init()
env_name = 'walk'
ppo = PPO(env_name, 1, visualize_only=True)
ppo.env.visualize = True
if not MOTION_ONLY:
ppo.LoadModel('model/' + env_name + '.pt')
ppo.env.Resets(False)
ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q_by_time(ppo.env.time_offset))
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
dart_world = ppo.env.world
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer('MotionModel', yr.DartRenderer(ppo.env.ref_world, (150,150,255), yr.POLYGON_FILL))
motion_state = [0]
if not MOTION_ONLY:
viewer.doc.addRenderer('controlModel', yr.DartRenderer(dart_world, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('contact', yr.VectorsRenderer(rd_contact_forces, rd_contact_positions, (255,0,0)))
def callback_0(_):
motion_state[0] = 0
def callback_1(_):
motion_state[0] = 1
def callback_2(_):
motion_state[0] = 2
viewer.objectInfoWnd.addBtn('0', callback_0)
viewer.objectInfoWnd.addBtn('1', callback_1)
viewer.objectInfoWnd.addBtn('2', callback_2)
def preCallback(frame):
ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame))
def simulateCallback(frame):
print(frame)
state = ppo.env.GetState(0)
action_dist, _ = ppo.model(torch.tensor(state.reshape(1, -1)).float())
action = action_dist.loc.detach().numpy()
ppo.env.visualize_select_motion = motion_state[0]
res = ppo.env.Steps(action)
# print(res[0][0])
# res = ppo.env.Steps(np.zeros_like(action))
# print(frame, ppo.env.ref_skel.current_frame, ppo.env.world.time()*ppo.env.ref_motion.fps)
# print(frame, res[0][0])
# if res[0][0] > 0.46:
# ppo.env.continue_from_now_by_phase(0.2)
# if res[2]:
# print(frame, 'Done')
# ppo.env.reset()
# contact rendering
contacts = ppo.env.world.collision_result.contacts
del rd_contact_forces[:]
del rd_contact_positions[:]
for contact in contacts:
rd_contact_forces.append(contact.f/1000.)
rd_contact_positions.append(contact.p)
if MOTION_ONLY:
viewer.setPreFrameCallback_Always(preCallback)
viewer.setMaxFrame(ppo.env.motion_len-1)
else:
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(3000)
# viewer.setMaxFrame(len(ppo.env.ref_motion)-1)
viewer.startTimer(1./30.)
viewer.show()
Fl.run()
def main2():
import PyCommon.modules.Motion.ysMotionAnalysis as yma
pydart.init()
world = [
pydart.World(1. / 1200., "../DartDeep/data/woody_with_ground_v2.xml")
for _ in range(2)
]
motion_files = ['wd2_jump0.bvh']
motion_files = ['woody_walk_normal.bvh']
motion_files = ['walk_left_90degree.bvh']
motion = yf.readBvhFileAsBvh(motion_files[0]).toPmLinearMotion(1., False)
# 70, 112, 156 right heel strike
mg = hmg.MotionGraph()
mg.add_motion(motion)
mg.build()
total_num = len(mg.transition_processed)
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer('motionModel[0]',
yr.DartRenderer(world[0], (255, 240, 255)))
viewer.doc.addRenderer('motionModel[1]', yr.DartRenderer(world[1]))
motion_state = [0]
def callback_0(_):
motion_state[0] = 0
def callback_1(_):
motion_state[0] = 1
def callback_2(_):
motion_state[0] = 2
viewer.objectInfoWnd.addBtn('0', callback_0)
viewer.objectInfoWnd.addBtn('1', callback_1)
viewer.objectInfoWnd.addBtn('2', callback_2)
hRef = 0.1
vRef = 0.4
lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef)
rc = yma.getElementContactStates(motion, 'RightFoot', hRef, vRef)
lindex = motion[0].skeleton.getElementIndex('LeftFoot')
rindex = motion[0].skeleton.getElementIndex('RightFoot')
def postCallback(frame):
transition = mg.transition_processed[frame]
world[0].skeletons[1].set_positions(
motion.get_q(transition.motion_from_idx))
world[1].skeletons[1].set_positions(
motion.get_q(transition.motion_to_idx))
print(frame, transition.motion_from_idx, transition.motion_to_idx,
transition.dist)
print(motion.getPosition(lindex))
print(lc[transition.motion_from_idx], rc[transition.motion_from_idx])
print(lc[transition.motion_to_idx], rc[transition.motion_to_idx])
viewer.setPreFrameCallback_Always(postCallback)
viewer.setMaxFrame(total_num - 1)
viewer.startTimer(1. / 30.)
viewer.show()
all_node_set = set()
dot = Digraph()
for i in range(len(mg.transition_processed)):
all_node_set.add(mg.transition_processed[i].motion_from_idx)
all_node_set.add(mg.transition_processed[i].motion_to_idx)
all_node_list = list(all_node_set)
all_node_list.sort()
for i in all_node_list:
dot.node(str(i))
all_edge_set = set()
for i in range(len(all_node_list) - 1):
all_edge_set.add((all_node_list[i], all_node_list[i + 1]))
for i in range(len(mg.transition_processed)):
all_edge_set.add((mg.transition_processed[i].motion_from_idx,
mg.transition_processed[i].motion_to_idx))
for i in list(all_edge_set):
dot.edge(str(i[0]), str(i[1]))
# print(dot.source)
dot.render('test', view=True)
dot.pipe()
Fl.run()
def main():
MOTION_ONLY = False
pydart.init()
env_name = 'walk'
ppo = PPO(env_name, 0, visualize_only=True)
if not MOTION_ONLY:
ppo.LoadModel('model/param.pt')
# ppo.LoadModel('walk_model_10240127/'+'181'+'.pt')
ppo.generate_rnn_motion()
ppo.envs_send_rnn_motion()
ppo.env.Resets(False)
# ppo.replace_motion_num = ppo.rnn_len
# ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(ppo.env.phase_frame))
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
rd_target_position = [None]
rd_com = [None]
dart_world = ppo.env.world
viewer = hsv.hpSimpleViewer(rect=[0, 0, 1280 + 300, 720 + 1 + 55],
viewForceWnd=False)
viewer.doc.addRenderer(
'MotionModel',
yr.DartRenderer(ppo.env.ref_world, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer('target',
yr.PointsRenderer(rd_target_position, (0, 255, 0)))
viewer.doc.addRenderer('motion',
yr.JointMotionRenderer(ppo.env.ref_motion))
if not MOTION_ONLY:
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'contact',
yr.VectorsRenderer(rd_contact_forces, rd_contact_positions,
(255, 0, 0)))
viewer.doc.addRenderer('CM_plane',
yr.PointsRenderer(rd_com, (0, 0, 255)))
last_frame = [0]
q = ppo.env.ref_skel.q
def simulateCallback(frame):
ppo.env.ref_motion.frame = frame - last_frame[0]
del rd_target_position[:]
del rd_com[:]
rd_target_position.append(
ppo.env.goals_in_world_frame[ppo.env.phase_frame])
com = ppo.env.skel.com()
com[1] = 0.
rd_com.append(com)
state = ppo.env.GetState(0)
action_dist, _ = ppo.model(torch.tensor(state.reshape(1, -1)).float())
action = action_dist.loc.detach().numpy()
# res = ppo.env.Steps(np.zeros_like(action))
res = ppo.env.Steps(action)
# res = [False, False, False]
# print(res[1])
# res = ppo.env.Steps(np.zeros_like(action))
# print(frame, ppo.env.ref_skel.current_frame, ppo.env.world.time()*ppo.env.ref_motion.fps)
# print(frame, res[0][0])
# if res[0][0] > 0.46:
# ppo.env.continue_from_now_by_phase(0.2)
# print(ppo.env.goal)
if res[2]:
print(frame, 'Done')
last_frame[0] = frame
ppo.generate_rnn_motion()
ppo.envs_send_rnn_motion()
ppo.env.reset()
# contact rendering
contacts = ppo.env.world.collision_result.contacts
del rd_contact_forces[:]
del rd_contact_positions[:]
for contact in contacts:
rd_contact_forces.append(contact.f / 1000.)
rd_contact_positions.append(contact.p)
# viewer.setPreFrameCallback_Always(preCallback)
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(3000)
viewer.startTimer(1. / 30.)
viewer.show()
Fl.run()