예제 #1
0
    def grab_rope(self, lr, seg_samples=5):
        print colorize("TESTING FOR GRAB...", "magenta", True)
        nodes, ctl_pts = self.rope.GetNodes(), self.rope.GetControlPoints()
        graspable_inds = []
        for i in xrange(len(ctl_pts)-1):
            pts = math_utils.interp2d(np.linspace(0,1,seg_samples), [0,1], np.r_[np.reshape(ctl_pts[i], (1,3)), np.reshape(ctl_pts[i+1], (1,3))])
            if np.any([in_grasp_region(self.robot, lr, pt) for pt in pts]):
                graspable_inds.append(i)
        print colorize('graspable inds for %s: %s' % (lr, str(graspable_inds)), "magenta", True)
        if len(graspable_inds) == 0:
            return False


        robot_link = self.robot.GetLink("%s_gripper_l_finger_tip_link"%lr)
        rope_links = self.rope.GetKinBody().GetLinks()
        for i_node in graspable_inds:
            for i_cnt in range(max(0, i_node-1), min(len(nodes), i_node+2)):
                cnt = self.bt_env.AddConstraint({
                    "type": "generic6dof",
                    "params": {
                        "link_a": robot_link,
                        "link_b": rope_links[i_cnt],
                        "frame_in_a": np.linalg.inv(robot_link.GetTransform()).dot(rope_links[i_cnt].GetTransform()),
                        "frame_in_b": np.eye(4),
                        "use_linear_reference_frame_a": False,
                        "stop_erp": .8,
                        "stop_cfm": .1,
                        "disable_collision_between_linked_bodies": True,
                    }
                })
                self.constraints[lr].append(cnt)

        return True
예제 #2
0
def interp_hmats(newtimes, oldtimes, oldhmats):
    oldposes = openravepy.poseFromMatrices(oldhmats)
    newposes = np.empty((len(newtimes), 7))
    newposes[:,4:7] = mu.interp2d(newtimes, oldtimes, oldposes[:,4:7])
    newposes[:,0:4] = interp_quats(newtimes, oldtimes, oldposes[:,0:4])
    newhmats = openravepy.matrixFromPoses(newposes)
    return newhmats
def make_joint_traj(xyzs, quats, manip, ref_frame, targ_frame, filter_options = 0):
    "do ik and then fill in the points where ik failed"

    n = len(xyzs)
    assert len(quats) == n
    
    robot = manip.GetRobot()
    joint_inds = manip.GetArmIndices()
    robot.SetActiveDOFs(joint_inds)
    orig_joint = robot.GetActiveDOFValues()
    
    joints = []
    inds = []

    for i in xrange(0,n):
        mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
        joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame, filter_options)
        if joint is not None: 
            joints.append(joint)
            inds.append(i)
            robot.SetActiveDOFValues(joint)


    robot.SetActiveDOFValues(orig_joint)
    
    
    LOG.info("found ik soln for %i of %i points",len(inds), n)
    if len(inds) > 2:
        joints2 = mu.interp2d(np.arange(n), inds, joints)
        return joints2, inds
    else:
        return np.zeros((n, len(joints))), []
예제 #4
0
def interp_hmats(newtimes, oldtimes, oldhmats):
    oldposes = openravepy.poseFromMatrices(oldhmats)
    newposes = np.empty((len(newtimes), 7))
    newposes[:, 4:7] = mu.interp2d(newtimes, oldtimes, oldposes[:, 4:7])
    newposes[:, 0:4] = interp_quats(newtimes, oldtimes, oldposes[:, 0:4])
    newhmats = openravepy.matrixFromPoses(newposes)
    return newhmats
 def observe_cloud(self, upsample=0):
     pts = self.rope.GetControlPoints()
     if upsample == 0:
         return pts
     lengths = np.r_[0, self.rope.GetHalfHeights() * 2]
     summed_lengths = np.cumsum(lengths)
     assert len(lengths) == len(pts)
     return math_utils.interp2d(np.linspace(0, summed_lengths[-1], upsample*len(pts)), summed_lengths, pts)
예제 #6
0
 def bad_inds(x1, t1):
     ibad = np.flatnonzero( (np.abs(mu.interp2d(l, l1, x1) - x) > tol).any(axis=1) )
     jbad1 = np.flatnonzero((np.abs(x1[1:] - x1[:-1]) > max_change[None,:]).any(axis=1))
     if len(ibad) == 0 and len(jbad1) == 0: return []
     else:
         lbad = l[ibad]
         jbad = np.unique(np.searchsorted(l1, lbad)) - 1
         jbad = np.union1d(jbad, jbad1)
         return jbad
예제 #7
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)
예제 #8
0
 def bad_inds(x1, t1):
     ibad = np.flatnonzero(
         (np.abs(mu.interp2d(l, l1, x1) - x) > tol).any(axis=1))
     jbad1 = np.flatnonzero(
         (np.abs(x1[1:] - x1[:-1]) > max_change[None, :]).any(axis=1))
     if len(ibad) == 0 and len(jbad1) == 0: return []
     else:
         lbad = l[ibad]
         jbad = np.unique(np.searchsorted(l1, lbad)) - 1
         jbad = np.union1d(jbad, jbad1)
         return jbad
예제 #9
0
def retime_traj(robot, inds, traj, base_hmats, 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)
    
    if base_hmats != None:
        base_traj = [mat_to_base_pose(base_hmat) for base_hmat in base_hmats]
        base_traj_up = math_utils.interp2d(times_up, times, base_traj)
        base_hmats_up = [base_pose_to_mat(base_pose) for base_pose in base_traj_up]
    else:
        base_hmats_up = None
    
    return traj_up, base_hmats_up
예제 #10
0
def get_traj_collisions(bodypart_traj, robot, n=100):

    traj = []

    for group in bodypart_traj:
        traj.append(bodypart_traj[group])

    traj_up = mu.interp2d(np.linspace(0, 1, n), np.linspace(0, 1, len(traj)), traj)

    env = robot.GetEnv()
    col_times = []
    for (i, row) in enumerate(traj_up):
        robot.SetActiveDOFValues(row)
        col_now = env.CheckCollision(robot)
        if col_now:
            col_times.append(i)
    return col_times
예제 #11
0
def adaptive_resample2(x, tol, max_change=-1, min_steps=3):
    
    print type(x)
    print x.shape
    """
    resample original signal with a small number of waypoints so that the the sparsely sampled function, 
    when linearly interpolated, deviates from the original function by less than tol at every time

    input:
    x: 2D array in R^(t x k)  where t is the number of timesteps
    tol: tolerance. either a single scalar or a vector of length k
    max_change: max change in the sparsely sampled signal at each timestep
    min_steps: minimum number of timesteps in the new trajectory. (usually irrelevant)

    output:
    new_times, new_x

    assuming that the old signal has times 0,1,2,...,len(x)-1
    this gives the new times, and the new signal
    """

    x = np.asarray(x)
    assert x.ndim == 2

    if np.isscalar(tol): 
        tol = np.ones(x.shape[1])*tol
    else:
        tol = np.asarray(tol)
        assert tol.ndim == 1 and tol.shape[0] == x.shape[1]

    times = np.arange(x.shape[0])

    if max_change == -1: 
        max_change = np.ones(x.shape[1]) * np.inf
    elif np.isscalar(max_change): 
        max_change = np.ones(x.shape[1]) * max_change
    else:
        max_change = np.asarray(max_change)
        assert max_change.ndim == 1 and max_change.shape[0] == x.shape[1]

    dl = mu.norms(x[1:] - x[:-1],1)
    l = np.cumsum(np.r_[0,dl])

    def bad_inds(x1, t1):
        ibad = np.flatnonzero( (np.abs(mu.interp2d(l, l1, x1) - x) > tol).any(axis=1) )
        jbad1 = np.flatnonzero((np.abs(x1[1:] - x1[:-1]) > max_change[None,:]).any(axis=1))
        if len(ibad) == 0 and len(jbad1) == 0: return []
        else:
            lbad = l[ibad]
            jbad = np.unique(np.searchsorted(l1, lbad)) - 1
            jbad = np.union1d(jbad, jbad1)
            return jbad

    l1 = np.linspace(0,l[-1],min_steps)
    for _ in xrange(20):
        x1 = mu.interp2d(l1, l, x)
        bi = bad_inds(x1, l1)
        if len(bi) == 0:
            return np.interp(l1, l, times), x1
        else:
            l1 = np.union1d(l1, (l1[bi] + l1[bi+1]) / 2 )

    raise Exception("couldn't subdivide enough. something funny is going on. check your input data")
예제 #12
0
def interp_quats(newtimes, oldtimes, oldquats):
    "should actually do slerp"
    quats_unnormed = mu.interp2d(newtimes, oldtimes, oldquats)
    return mu.normr(quats_unnormed)
def follow_body_traj(pr2, bodypart2traj, wait=True, base_frame = "/base_footprint", speed_factor=1):    

    LOG.info("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 = resampling.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
예제 #14
0
def adaptive_resample2(x, tol, max_change=-1, min_steps=3):

    print type(x)
    print x.shape
    """
    resample original signal with a small number of waypoints so that the the sparsely sampled function, 
    when linearly interpolated, deviates from the original function by less than tol at every time

    input:
    x: 2D array in R^(t x k)  where t is the number of timesteps
    tol: tolerance. either a single scalar or a vector of length k
    max_change: max change in the sparsely sampled signal at each timestep
    min_steps: minimum number of timesteps in the new trajectory. (usually irrelevant)

    output:
    new_times, new_x

    assuming that the old signal has times 0,1,2,...,len(x)-1
    this gives the new times, and the new signal
    """

    x = np.asarray(x)
    assert x.ndim == 2

    if np.isscalar(tol):
        tol = np.ones(x.shape[1]) * tol
    else:
        tol = np.asarray(tol)
        assert tol.ndim == 1 and tol.shape[0] == x.shape[1]

    times = np.arange(x.shape[0])

    if max_change == -1:
        max_change = np.ones(x.shape[1]) * np.inf
    elif np.isscalar(max_change):
        max_change = np.ones(x.shape[1]) * max_change
    else:
        max_change = np.asarray(max_change)
        assert max_change.ndim == 1 and max_change.shape[0] == x.shape[1]

    dl = mu.norms(x[1:] - x[:-1], 1)
    l = np.cumsum(np.r_[0, dl])

    def bad_inds(x1, t1):
        ibad = np.flatnonzero(
            (np.abs(mu.interp2d(l, l1, x1) - x) > tol).any(axis=1))
        jbad1 = np.flatnonzero(
            (np.abs(x1[1:] - x1[:-1]) > max_change[None, :]).any(axis=1))
        if len(ibad) == 0 and len(jbad1) == 0: return []
        else:
            lbad = l[ibad]
            jbad = np.unique(np.searchsorted(l1, lbad)) - 1
            jbad = np.union1d(jbad, jbad1)
            return jbad

    l1 = np.linspace(0, l[-1], min_steps)
    for _ in xrange(20):
        x1 = mu.interp2d(l1, l, x)
        bi = bad_inds(x1, l1)
        if len(bi) == 0:
            return np.interp(l1, l, times), x1
        else:
            l1 = np.union1d(l1, (l1[bi] + l1[bi + 1]) / 2)

    raise Exception(
        "couldn't subdivide enough. something funny is going on. check your input data"
    )
예제 #15
0
def interp_quats(newtimes, oldtimes, oldquats):
    "should actually do slerp"
    quats_unnormed = mu.interp2d(newtimes, oldtimes, oldquats)
    return mu.normr(quats_unnormed)