コード例 #1
0
ファイル: sim_util.py プロジェクト: shhuang/large-scale-mmql
def sim_full_traj_maybesim(sim_env, full_traj, animate=False, interactive=False):
    def sim_callback(i):
        sim_env.sim.step()

    animate_speed = 10 if animate else 0

    traj, dof_inds = full_traj

    # make the trajectory slow enough for the simulation
    traj = ropesim.retime_traj(sim_env.robot, dof_inds, traj)

    # in simulation mode, we must make sure to gradually move to the new starting position
    sim_env.robot.SetActiveDOFs(dof_inds)
    curr_vals = sim_env.robot.GetActiveDOFValues()
    transition_traj = np.r_[[curr_vals], [traj[0]]]
    unwrap_in_place(transition_traj)
    transition_traj = ropesim.retime_traj(sim_env.robot, dof_inds, transition_traj, max_cart_vel=.05)
    animate_traj.animate_traj(transition_traj, sim_env.robot, restore=False, pause=interactive,
        callback=sim_callback, step_viewer=animate_speed)
    traj[0] = transition_traj[-1]
    unwrap_in_place(traj)

    animate_traj.animate_traj(traj, sim_env.robot, restore=False, pause=interactive,
        callback=sim_callback, step_viewer=animate_speed)
    if sim_env.viewer:
        sim_env.viewer.Step()
    return True
コード例 #2
0
def exec_traj_sim(bodypart2traj, animate):
    def sim_callback(i):
        Globals.sim.step()

    dof_inds = []
    trajs = []
    for (part_name, traj) in bodypart2traj.items():
        manip_name = {"larm": "leftarm", "rarm": "rightarm"}[part_name]
        dof_inds.extend(Globals.robot.GetManipulator(manip_name).GetArmIndices())
        trajs.append(traj)
    full_traj = np.concatenate(trajs, axis=1)
    Globals.robot.SetActiveDOFs(dof_inds)

    # make the trajectory slow enough for the simulation
    #orig full_traj = ropesim.retime_traj(Globals.robot, dof_inds, full_traj)
    full_traj = ropesim.retime_traj(Globals.robot, dof_inds, full_traj, max_cart_vel=.01)

    # in simulation mode, we must make sure to gradually move to the new starting position
    curr_vals = Globals.robot.GetActiveDOFValues()
    transition_traj = np.r_[[curr_vals], [full_traj[0]]]
    unwrap_in_place(transition_traj)
    transition_traj = ropesim.retime_traj(Globals.robot, dof_inds, transition_traj, max_cart_vel=.05)
    #transition_traj = ropesim.retime_traj(Globals.robot, dof_inds, transition_traj, max_cart_vel=.005)
    animate_traj.animate_traj(transition_traj, Globals.robot, restore=False, pause=False,
                              callback=sim_callback, step_viewer=animate)
    full_traj[0] = transition_traj[-1]
    unwrap_in_place(full_traj)

    animate_traj.animate_traj(full_traj, Globals.robot, restore=False, pause=False,
                              callback=sim_callback, step_viewer=animate)
    return True
コード例 #3
0
ファイル: do_task.py プロジェクト: ankush-me/rapprentice
def exec_traj_maybesim(bodypart2traj):
    if args.animation:
        dof_inds = []
        trajs = []
        for (part_name, traj) in bodypart2traj.items():
            manip_name = {"larm":"leftarm","rarm":"rightarm"}[part_name]
            dof_inds.extend(Globals.robot.GetManipulator(manip_name).GetArmIndices())            
            trajs.append(traj)
        full_traj = np.concatenate(trajs, axis=1)
        Globals.robot.SetActiveDOFs(dof_inds)
        animate_traj.animate_traj(full_traj, Globals.robot, restore=False,pause=True)
    if args.execution:
        pr2_trajectories.follow_body_traj(Globals.pr2, bodypart2traj)
コード例 #4
0
ファイル: do_task.py プロジェクト: rgleichman/rapprentice
def exec_traj_maybesim(bodypart2traj):
    if args.animation:
        dof_inds = []
        trajs = []
        for (part_name, traj) in bodypart2traj.items():
            manip_name = {"larm": "leftarm", "rarm": "rightarm"}[part_name]
            dof_inds.extend(
                Globals.robot.GetManipulator(manip_name).GetArmIndices())
            trajs.append(traj)
        full_traj = np.concatenate(trajs, axis=1)
        Globals.robot.SetActiveDOFs(dof_inds)
        animate_traj.animate_traj(full_traj,
                                  Globals.robot,
                                  restore=False,
                                  pause=True)
    if args.execution:
        pr2_trajectories.follow_body_traj(Globals.pr2, bodypart2traj)
コード例 #5
0
ファイル: sim_util.py プロジェクト: shhuang/large-scale-mmql
def sim_full_traj_maybesim(sim_env,
                           full_traj,
                           animate=False,
                           interactive=False):
    def sim_callback(i):
        sim_env.sim.step()

    animate_speed = 10 if animate else 0

    traj, dof_inds = full_traj

    # make the trajectory slow enough for the simulation
    traj = ropesim.retime_traj(sim_env.robot, dof_inds, traj)

    # in simulation mode, we must make sure to gradually move to the new starting position
    sim_env.robot.SetActiveDOFs(dof_inds)
    curr_vals = sim_env.robot.GetActiveDOFValues()
    transition_traj = np.r_[[curr_vals], [traj[0]]]
    unwrap_in_place(transition_traj)
    transition_traj = ropesim.retime_traj(sim_env.robot,
                                          dof_inds,
                                          transition_traj,
                                          max_cart_vel=.05)
    animate_traj.animate_traj(transition_traj,
                              sim_env.robot,
                              restore=False,
                              pause=interactive,
                              callback=sim_callback,
                              step_viewer=animate_speed)
    traj[0] = transition_traj[-1]
    unwrap_in_place(traj)

    animate_traj.animate_traj(traj,
                              sim_env.robot,
                              restore=False,
                              pause=interactive,
                              callback=sim_callback,
                              step_viewer=animate_speed)
    if sim_env.viewer:
        sim_env.viewer.Step()
    return True
コード例 #6
0
ファイル: do_task.py プロジェクト: ttblue/rapprentice
def exec_traj_maybesim(bodypart2traj, speed_factor=0.5):
    if args.animation:
        """
        dof_inds = []
        trajs = []
        for (part_name, traj) in bodypart2traj.items():
            manip_name = {"larm":"leftarm","rarm":"rightarm"}[part_name]
            dof_inds.extend(Globals.robot.GetManipulator(manip_name).GetArmIndices())            
            trajs.append(traj)
        full_traj = np.concatenate(trajs, axis=1)
        Globals.robot.SetActiveDOFs(dof_inds)
        animate_traj.animate_traj(full_traj, Globals.robot, restore=False,pause=True)
        """
        name2part = {"lgrip":Globals.pr2.lgrip,
                     "rgrip":Globals.pr2.rgrip,
                     "larm":Globals.pr2.larm,
                     "rarm":Globals.pr2.rarm,
                     "base":Globals.pr2.base}
        dof_inds = []
        trajs = []
        vel_limits = []
        for (part_name, traj) in bodypart2traj.items():
            manip_name = {"larm":"leftarm","rarm":"rightarm"}[part_name]
            vel_limits.extend(name2part[part_name].vel_limits)
            dof_inds.extend(Globals.robot.GetManipulator(manip_name).GetArmIndices())
            if traj.ndim == 1: traj = traj.reshape(-1,1)            
            trajs.append(traj)
        
        trajectories = np.concatenate(trajs, 1)
        print trajectories.shape
        times = retiming.retime_with_vel_limits(trajectories, np.array(vel_limits))
        times_up = np.linspace(0, times[-1], int(np.ceil(times[-1]/.1)))
        full_traj = mu.interp2d(times_up, times, trajectories)
        print full_traj.shape
        
        Globals.robot.SetActiveDOFs(dof_inds)
        animate_traj.animate_traj(full_traj, Globals.robot, restore=False,pause=True)
        #return True
    if args.execution:
        pr2_trajectories.follow_body_traj(Globals.pr2, bodypart2traj, speed_factor=speed_factor)
        return True
コード例 #7
0
    rave_traj = [
        r2r.convert(row)
        for row in asarray(seg_info["joint_states"]["position"])
    ]
    robot.SetActiveDOFs(r2r.rave_inds)
    robot.SetActiveDOFValues(rave_traj[0])

    handles = []
    T_w_k = berkeley_pr2.get_kinect_transform(robot)
    o = T_w_k[:3, 3]
    x = T_w_k[:3, 0]
    y = T_w_k[:3, 1]
    z = T_w_k[:3, 2]

    handles.append(env.drawarrow(o, o + .3 * x, .005, (1, 0, 0, 1)))
    handles.append(env.drawarrow(o, o + .3 * y, .005, (0, 1, 0, 1)))
    handles.append(env.drawarrow(o, o + .3 * z, .005, (0, 0, 1, 1)))

    XYZ_k = clouds.depth_to_xyz(np.asarray(seg_info["depth"]), berkeley_pr2.f)
    Twk = asarray(seg_info["T_w_k"])
    XYZ_w = XYZ_k.dot(Twk[:3, :3].T) + Twk[:3, 3][None, None, :]
    RGB = np.asarray(seg_info["rgb"])
    handles.append(
        env.plot3(XYZ_w.reshape(-1, 3), 2,
                  RGB.reshape(-1, 3)[:, ::-1] / 255.))

    animate_traj.animate_traj(rave_traj, robot, pause=not args.nopause)

    print "DONE"
    trajoptpy.GetViewer(env).Idle()
コード例 #8
0
ファイル: animate_demo.py プロジェクト: ankush-me/rapprentice
    
    r2r = ros2rave.RosToRave(robot, seg_info["joint_states"]["name"])
    rave_traj = [r2r.convert(row) for row in asarray(seg_info["joint_states"]["position"])]
    robot.SetActiveDOFs(r2r.rave_inds)
    robot.SetActiveDOFValues(rave_traj[0])
    
    handles = []
    T_w_k = berkeley_pr2.get_kinect_transform(robot)
    o = T_w_k[:3,3]
    x = T_w_k[:3,0]
    y = T_w_k[:3,1]
    z = T_w_k[:3,2]

    handles.append(env.drawarrow(o, o+.3*x, .005,(1,0,0,1)))
    handles.append(env.drawarrow(o, o+.3*y, .005,(0,1,0,1)))
    handles.append(env.drawarrow(o, o+.3*z, .005,(0,0,1,1)))
    
    cloud_xyz = np.squeeze(seg_info["cloud_xyz"])
    handles.append(env.plot3(cloud_xyz,5))
    
    animate_traj.animate_traj(rave_traj, robot, pause = not args.nopause)
    
    print "DONE"
    trajoptpy.GetViewer(env).Idle()