Пример #1
0
def follow_body_traj(pr2,
                     traj,
                     times=None,
                     r_arm=False,
                     l_arm=False,
                     r_gripper=False,
                     l_gripper=False,
                     head=False,
                     base=False,
                     base_frame="/odom_combined",
                     wait=True):
    assert isinstance(pr2, PR2.PR2)
    pr2.update_rave()
    if r_arm:
        pr2.rarm.goto_joint_positions(traj["r_arm"][0])
    if l_arm:
        pr2.larm.goto_joint_positions(traj["l_arm"][0])
    if r_gripper:
        pr2.rgrip.set_angle(traj["r_gripper"][0])
    if l_gripper:
        pr2.lgrip.set_angle(traj["l_gripper"][0])
    if head:
        raise NotImplementedError
    if base:
        print "warning: not going to initial base position (goto position not implemented)"
        #pr2.base.goto_pose(traj["base"][0], base_frame)
    pr2.join_all()

    if times is None:
        vel_limits = np.r_[pr2.rarm.vel_limits, pr2.larm.vel_limits, .02, .02,
                           pr2.head.vel_limits * vel_mult, [.2, .2, .2]]
        acc_limits = np.r_[pr2.rarm.acc_limits, pr2.larm.acc_limits, np.inf,
                           np.inf, pr2.head.acc_limits, [1, 1, 1]]
        #times, inds = retiming.retime(flatten_compound_dtype(traj), vel_limits, acc_limits)
        times, inds = retiming.retime2(flatten_compound_dtype(traj),
                                       vel_limits)
        traj = traj[inds]

    if r_arm:
        pr2.rarm.follow_timed_joint_trajectory(
            traj["r_arm"], ku.get_velocities(traj["r_arm"], times, .001),
            times)
    if l_arm:
        pr2.larm.follow_timed_joint_trajectory(
            traj["l_arm"], ku.get_velocities(traj["l_arm"], times, .001),
            times)
    if r_gripper:
        pr2.rgrip.follow_timed_trajectory(times, traj["r_gripper"])
    if l_gripper:
        pr2.lgrip.follow_timed_trajectory(times, traj["l_gripper"])
    if head:
        pr2.head.follow_timed_trajectory(times, traj["head"])
    if base:
        pr2.base.follow_timed_trajectory(times,
                                         traj["base"],
                                         frame_id=base_frame)

    if wait: pr2.join_all()
Пример #2
0
def follow_body_traj(pr2, traj, times=None, r_arm = False, l_arm = False, r_gripper = False, l_gripper = False, head = False, base = False, 
                     base_frame = "/odom_combined", wait=True):
    assert isinstance(pr2, PR2.PR2)
    pr2.update_rave()
    if r_arm:
        pr2.rarm.goto_joint_positions(traj["r_arm"][0])
    if l_arm:
        pr2.larm.goto_joint_positions(traj["l_arm"][0])
    if r_gripper:
        pr2.rgrip.set_angle(traj["r_gripper"][0])
    if l_gripper:
        pr2.lgrip.set_angle(traj["l_gripper"][0])
    if head:
        raise NotImplementedError
    if base:
        print "warning: not going to initial base position (goto position not implemented)"
        #pr2.base.goto_pose(traj["base"][0], base_frame)
    pr2.join_all()
    
    if times is None:
        vel_limits = np.r_[pr2.rarm.vel_limits, pr2.larm.vel_limits, .02, .02, pr2.head.vel_limits*vel_mult, [.2,.2,.2]]
        acc_limits = np.r_[pr2.rarm.acc_limits, pr2.larm.acc_limits, np.inf, np.inf, pr2.head.acc_limits, [1,1,1]]
        #times, inds = retiming.retime(flatten_compound_dtype(traj), vel_limits, acc_limits)
        times, inds = retiming.retime2(flatten_compound_dtype(traj), vel_limits)
        traj = traj[inds]

    if r_arm:
        pr2.rarm.follow_timed_joint_trajectory(traj["r_arm"],ku.get_velocities(traj["r_arm"],times,.001),times)
    if l_arm:
        pr2.larm.follow_timed_joint_trajectory(traj["l_arm"],ku.get_velocities(traj["l_arm"],times,.001),times)
    if r_gripper:
        pr2.rgrip.follow_timed_trajectory(times, traj["r_gripper"])
    if l_gripper:
        pr2.lgrip.follow_timed_trajectory(times, traj["l_gripper"])
    if head:
        pr2.head.follow_timed_trajectory(times, traj["head"])
    if base:
        pr2.base.follow_timed_trajectory(times, traj["base"], frame_id = base_frame)
                    
    if wait: pr2.join_all()
Пример #3
0
def follow_body_traj(pr2, traj, times=None, r_arm = False, l_arm = False, r_gripper = False, l_gripper = False, head = False, base = False):
    isinstance(pr2, PR2.PR2)
    pr2.update_rave()
    if r_arm:
        pr2.rarm.goto_joint_positions(traj["r_arm"][0])
    if l_arm:
        pr2.larm.goto_joint_positions(traj["l_arm"][0])
    if r_gripper:
        pr2.rgrip.set_angle(traj["r_gripper"][0])
    if l_gripper:
        pr2.lgrip.set_angle(traj["l_gripper"][0])
    if head or base:
        raise NotImplementedError
    pr2.join_all()
    
    vel_mult = .2
    
    if times is None:
        vel_limits = np.r_[pr2.rarm.vel_limits*vel_mult, pr2.larm.vel_limits*vel_mult, .02, .02, pr2.head.vel_limits*vel_mult, [.2,.2,.2]]
        acc_limits = np.r_[pr2.rarm.acc_limits, pr2.larm.acc_limits, np.inf, np.inf, pr2.head.acc_limits, [1,1,1]]
        #times, inds = retiming.retime(flatten_compound_dtype(traj), vel_limits, acc_limits)
        times, inds = retiming.retime2(flatten_compound_dtype(traj), vel_limits)
        traj = traj[inds]

    if r_arm:
        pr2.rarm.follow_timed_joint_trajectory(traj["r_arm"],ku.get_velocities(traj["r_arm"],times,.001),times)
    if l_arm:
        pr2.larm.follow_timed_joint_trajectory(traj["l_arm"],ku.get_velocities(traj["l_arm"],times,.001),times)
    if r_gripper:
        pr2.rgrip.follow_timed_trajectory(times, traj["r_gripper"])
    if l_gripper:
        pr2.lgrip.follow_timed_trajectory(times, traj["l_gripper"])
    if head:
        pr2.head.follow_timed_trajectory(times, traj["head"])
    if base:
        pr2.base.follow_timed_trajectory(times, traj["base"], frame_id = "/map")
    
        
        
    pr2.join_all()