示例#1
0
文件: PR2.py 项目: ttblue/rapprentice
    def follow_joint_trajectory(self, traj):
        traj = np.r_[np.atleast_2d(self.get_joint_positions()), traj]
        for i in [2,4,6]:
            traj[:,i] = np.unwrap(traj[:,i])

        times = retiming.retime_with_vel_limits(traj, self.vel_limits)
        times_up = np.arange(0,times[-1],.1)
        traj_up = mu.interp2d(times_up, times, traj)
        vels = ku.get_velocities(traj_up, times_up, .001)
        self.follow_timed_joint_trajectory(traj_up, vels, times_up)
示例#2
0
    def follow_joint_trajectory(self, traj):
        traj = np.r_[np.atleast_2d(self.get_joint_positions()), traj]
        for i in [2,4,6]:
            traj[:,i] = np.unwrap(traj[:,i])

        times = retiming.retime_with_vel_limits(traj, self.vel_limits)
        times_up = np.arange(0,times[-1],.1)
        traj_up = mu.interp2d(times_up, times, traj)
        vels = resampling.get_velocities(traj_up, times_up, .001)
        self.follow_timed_joint_trajectory(traj_up, vels, times_up)
def retime_hmats(lhmats, rhmats, max_cart_vel=.02, upsample_time=.1):
    """
    retimes hmats (4x4 transforms) for left and right grippers
    """
    assert len(lhmats) == len(rhmats)
    cart_traj = np.empty((len(rhmats), 6))
    for i in xrange(len(lhmats)):
        cart_traj[i,:3] = lhmats[i][:3,3]
        cart_traj[i,3:] = rhmats[i][:3,3]
    times    = retiming.retime_with_vel_limits(cart_traj, np.repeat(max_cart_vel, 6))
    times_up = np.linspace(0, times[-1], times[-1]/upsample_time) if times[-1] > upsample_time else times
    lhmats_up = resampling.interp_hmats(times_up, times, lhmats)
    rhmats_up = resampling.interp_hmats(times_up, times, rhmats)
    return (lhmats_up, rhmats_up)
def retime_traj(robot, inds, traj, max_cart_vel=.02, upsample_time=.1):
    """retime a trajectory so that it executes slowly enough for the simulation"""
    cart_traj = np.empty((len(traj), 6))
    leftarm, rightarm = robot.GetManipulator("leftarm"), robot.GetManipulator("rightarm")
    with robot:
        for i in range(len(traj)):
            robot.SetDOFValues(traj[i], inds)
            cart_traj[i,:3] = leftarm.GetTransform()[:3,3]
            cart_traj[i,3:] = rightarm.GetTransform()[:3,3]

    times = retiming.retime_with_vel_limits(cart_traj, np.repeat(max_cart_vel, 6))
    times_up = np.linspace(0, times[-1], times[-1]/upsample_time) if times[-1] > upsample_time else times
    traj_up = math_utils.interp2d(times_up, times, traj)
    return traj_up
def retime_hmats(lhmats, rhmats, max_cart_vel=.02, upsample_time=.1):
    """
    retimes hmats (4x4 transforms) for left and right grippers
    """
    assert len(lhmats) == len(rhmats)
    cart_traj = np.empty((len(rhmats), 6))
    for i in xrange(len(lhmats)):
        cart_traj[i, :3] = lhmats[i][:3, 3]
        cart_traj[i, 3:] = rhmats[i][:3, 3]
    times = retiming.retime_with_vel_limits(cart_traj,
                                            np.repeat(max_cart_vel, 6))
    times_up = np.linspace(
        0, times[-1], times[-1] /
        upsample_time) if times[-1] > upsample_time else times
    lhmats_up = resampling.interp_hmats(times_up, times, lhmats)
    rhmats_up = resampling.interp_hmats(times_up, times, rhmats)
    return (lhmats_up, rhmats_up)
示例#6
0
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
def follow_body_traj(pr2, bodypart2traj, wait=True, base_frame="/base_footprint", speed_factor=1):

    rospy.loginfo("following trajectory with bodyparts %s", " ".join(bodypart2traj.keys()))

    name2part = {"lgrip": pr2.lgrip, "rgrip": pr2.rgrip, "larm": pr2.larm, "rarm": pr2.rarm, "base": pr2.base}
    for partname in bodypart2traj:
        if partname not in name2part:
            raise Exception("invalid part name %s" % partname)

    #### Go to initial positions #######

    for (name, part) in name2part.items():
        if name in bodypart2traj:
            part_traj = bodypart2traj[name]
            if name == "lgrip" or name == "rgrip":
                part.set_angle(np.squeeze(part_traj)[0])
            elif name == "larm" or name == "rarm":
                part.goto_joint_positions(part_traj[0])
            elif name == "base":
                part.goto_pose(part_traj[0], base_frame)
    pr2.join_all()

    #### Construct total trajectory so we can retime it #######

    n_dof = 0
    trajectories = []
    vel_limits = []
    acc_limits = []
    bodypart2inds = {}
    for (name, part) in name2part.items():
        if name in bodypart2traj:
            traj = bodypart2traj[name]
            if traj.ndim == 1:
                traj = traj.reshape(-1, 1)
            trajectories.append(traj)
            vel_limits.extend(part.vel_limits)
            acc_limits.extend(part.acc_limits)
            bodypart2inds[name] = range(n_dof, n_dof + part.n_joints)
            n_dof += part.n_joints

    trajectories = np.concatenate(trajectories, 1)

    vel_limits = np.array(vel_limits) * speed_factor

    times = retiming.retime_with_vel_limits(trajectories, vel_limits)
    times_up = np.linspace(0, times[-1], int(np.ceil(times[-1] / 0.1)))
    traj_up = mu.interp2d(times_up, times, trajectories)

    #### Send all part trajectories ###########

    for (name, part) in name2part.items():
        if name in bodypart2traj:
            part_traj = traj_up[:, bodypart2inds[name]]
            if name == "lgrip" or name == "rgrip":
                part.follow_timed_trajectory(times_up, part_traj.flatten())
            elif name == "larm" or name == "rarm":
                vels = ku.get_velocities(part_traj, times_up, 0.001)
                part.follow_timed_joint_trajectory(part_traj, vels, times_up)
            elif name == "base":
                part.follow_timed_trajectory(times_up, part_traj, base_frame)

    if wait:
        pr2.join_all()

    return True
示例#8
0
def follow_body_traj(pr2,
                     bodypart2traj,
                     wait=True,
                     base_frame="/base_footprint",
                     speed_factor=1):

    rospy.loginfo("following trajectory with bodyparts %s",
                  " ".join(bodypart2traj.keys()))

    name2part = {
        "lgrip": pr2.lgrip,
        "rgrip": pr2.rgrip,
        "larm": pr2.larm,
        "rarm": pr2.rarm,
        "base": pr2.base
    }
    for partname in bodypart2traj:
        if partname not in name2part:
            raise Exception("invalid part name %s" % partname)

    #### Go to initial positions #######

    for (name, part) in name2part.items():
        if name in bodypart2traj:
            part_traj = bodypart2traj[name]
            if name == "lgrip" or name == "rgrip":
                part.set_angle(np.squeeze(part_traj)[0])
            elif name == "larm" or name == "rarm":
                part.goto_joint_positions(part_traj[0])
            elif name == "base":
                part.goto_pose(part_traj[0], base_frame)
    pr2.join_all()

    #### Construct total trajectory so we can retime it #######

    n_dof = 0
    trajectories = []
    vel_limits = []
    acc_limits = []
    bodypart2inds = {}
    for (name, part) in name2part.items():
        if name in bodypart2traj:
            traj = bodypart2traj[name]
            if traj.ndim == 1: traj = traj.reshape(-1, 1)
            trajectories.append(traj)
            vel_limits.extend(part.vel_limits)
            acc_limits.extend(part.acc_limits)
            bodypart2inds[name] = range(n_dof, n_dof + part.n_joints)
            n_dof += part.n_joints

    trajectories = np.concatenate(trajectories, 1)

    vel_limits = np.array(vel_limits) * speed_factor

    times = retiming.retime_with_vel_limits(trajectories, vel_limits)
    times_up = np.linspace(0, times[-1], int(np.ceil(times[-1] / .1)))
    traj_up = mu.interp2d(times_up, times, trajectories)

    #### Send all part trajectories ###########

    for (name, part) in name2part.items():
        if name in bodypart2traj:
            part_traj = traj_up[:, bodypart2inds[name]]
            if name == "lgrip" or name == "rgrip":
                part.follow_timed_trajectory(times_up, part_traj.flatten())
            elif name == "larm" or name == "rarm":
                vels = ku.get_velocities(part_traj, times_up, .001)
                part.follow_timed_joint_trajectory(part_traj, vels, times_up)
            elif name == "base":
                part.follow_timed_trajectory(times_up, part_traj, base_frame)

    if wait: pr2.join_all()

    return True