Example #1
0
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()
Example #2
0
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()
Example #3
0
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()
Example #4
0
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()
Example #5
0
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()
Example #6
0
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()
Example #7
0
def main():
    win = SimonGame(700, 730)
    win.show()
    Fl.run()
Example #8
0
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()
Example #9
0
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()
Example #10
0
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()
Example #11
0
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()
Example #12
0
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()