Exemplo n.º 1
0
    def readFromfile(self, filename='./poly.csv'):
        (Dt, xcoeff, ycoeff, zcoeff, wcoeff) = tj.ppFromfile(filename)
        px = pwp.PwPoly()
        px.loadFromData(xcoeff, Dt, Dt.size)

        py = pwp.PwPoly()
        py.loadFromData(ycoeff, Dt, Dt.size)

        pz = pwp.PwPoly()
        pz.loadFromData(zcoeff, Dt, Dt.size)

        pw = pwp.PwPoly()
        pw.loadFromData(wcoeff, Dt, Dt.size)
Exemplo n.º 2
0
    def gen_land(self, p=None):
        """
        Generate a landing trajectory
        """
        start_pos = posFromOdomMsg(self.current_odometry)
        start_vel = velFromOdomMsg(self.current_odometry)

        vland = 0.3

        if (p is not None):
            tg_prel = np.array(
                [p[0] - start_pos[0], p[1] - start_pos[1], -start_pos[2]])
            tg_p = np.copy(p)
            tg_p[2] = 0.0
            print("Landing in [{}, {}]\n".format(p[0], p[1]))
        else:
            tg_prel = np.array([0.0, 0.0, -start_pos[2]])
            tg_p = np.copy(start_pos)
            tg_p[2] = 0.0
            print("Landing in [{}, {}]\n".format(start_pos[0], start_pos[1]))

        t_end = (start_pos[2] / vland)
        tg_v = np.zeros((3))
        tg_a = np.zeros((3))

        (X, Y, Z, W) = genInterpolationMatrices(start_vel, tg_prel, tg_v, tg_a)

        # Times (Absolute and intervals)
        knots = np.array([0, t_end])  # One second each piece

        # Polynomial characteristic:  order
        ndeg = 7

        ppx = pw.PwPoly(X, knots, ndeg)
        ppy = pw.PwPoly(Y, knots, ndeg)
        ppz = pw.PwPoly(Z, knots, ndeg)
        ppw = pw.PwPoly(W, knots, ndeg)
        traj_obj = trj.Trajectory(ppx, ppy, ppz, ppw)

        return (traj_obj, t_end)
Exemplo n.º 3
0
    def gen_takeoff(self, h, t2go=None):
        """
        Generate a tracking trajectory to reach an absolute waypoint 
        with a given velocity and acceleration.
        """
        start_pos = posFromOdomMsg(self.current_odometry)

        if (start_pos[2] > 0.4):
            rospy.loginfo("Already Flying!")
            return (None, 0)

        vtakeoff = 0.3
        t_end = h / vtakeoff

        if (t2go is not None and t2go != 0.0):
            t_end = t2go
            vtakeoff = h / t2go

        tg_prel = np.array([0.0, 0.0, h])
        tg_v = np.zeros((3))
        tg_a = np.zeros((3))

        (X, Y, Z, W) = genInterpolationMatrices(np.zeros(3), tg_prel, tg_v,
                                                tg_a)

        # Times (Absolute and intervals)
        knots = np.array([0, t_end])  # One second each piece

        # Polynomial characteristic:  order
        ndeg = 7

        ppx = pw.PwPoly(X, knots, ndeg)
        ppy = pw.PwPoly(Y, knots, ndeg)
        ppz = pw.PwPoly(Z, knots, ndeg)
        ppw = pw.PwPoly(W, knots, ndeg)
        traj_obj = trj.Trajectory(ppx, ppy, ppz, ppw)

        return (traj_obj, t_end)
Exemplo n.º 4
0
    [0, tgt_a[2], 0.0],
    [0, np.nan, 0.0],
])

W = np.array([
    [0, np.nan, 0.0],
    [0, np.nan, 0.0],
    [0, np.nan, 0.0],
    [0, np.nan, 0.0],
])

# Times (Absolute and intervals)
knots = np.array([0, 2.0, 3])  # One second each piece

# Generate the polynomial
ppx = pw.PwPoly(X, knots, ndeg)
ppy = pw.PwPoly(Y, knots, ndeg)
ppz = pw.PwPoly(Z, knots, ndeg)
ppw = pw.PwPoly(W, knots, ndeg)

# Check (Evaluate polynomial)
tv = np.linspace(0, max(knots), 50)
(Xtj, Ytj, Ztj, Wtj, Zbtj) = tjh.TrajFromPW(tv, [0,1,2], \
        pwpolx=ppx, pwpoly=ppy, pwpolz=ppz, pwpolw = ppw)

# Save the polynomial coefficients on file
x_coeff = ppx.getCoeffMat()
y_coeff = ppy.getCoeffMat()
z_coeff = ppz.getCoeffMat()
w_coeff = ppy.getCoeffMat()
Exemplo n.º 5
0
print('Number of constraint on the flat output = {:d}.'.format(nconstr))

# Build the constraint matrix
X = np.array([
    [0, 1.0, 0.6],
    [0, 0.02, 0.0],
    [0, -9.0, 0.0],
    [0, np.nan, 0.0],
])

# Times (Absolute and intervals)
knots = np.array([0, 1.5, 3])  # One second each piece
Dt = knots[1:len(knots)] - knots[0:len(knots) - 1]

# Generate the polynomial
ppx = pw.PwPoly(X, knots, ndeg)

wps = ppx.getWaypoints()
print("Waypoints: ", wps)
knts = ppx.getKnots()
print("Knots: ", knts)
cfs = ppx.getCoeffMat()
print("Coefficients: ", cfs)

print('\n')
knots = np.array([0, 2, 3.5])
print("Changing the knots position: ", knots)
ppx.moveKnots(knots)
cfs = ppx.getCoeffMat()
print("Coefficients: ", cfs)
Exemplo n.º 6
0
def handle_genImpTrjAuto(req):
    """
    Generate a impact trajectory just specifying the modulus of the 
    acceleration, speed and the time to go.
    
    It uses a 7 degree polynomial, such that the trajectory is also
    uploadable on the vehicle for onboard tracking.
    """
    ndeg = 7
    a_norm = req.a_norm
    v_norm = req.v_norm

    start_pos = np.array([
        current_odometry.pose.pose.position.x,
        current_odometry.pose.pose.position.y,
        current_odometry.pose.pose.position.z
    ])
    start_orientation = np.array([
        current_odometry.pose.pose.orientation.w,
        current_odometry.pose.pose.orientation.x,
        current_odometry.pose.pose.orientation.y,
        current_odometry.pose.pose.orientation.z
    ])

    tg_pos = np.array([
        current_target.pose.position.x, current_target.pose.position.y,
        current_target.pose.position.z
    ])

    tg_q = np.array([
        current_target.pose.orientation.w, current_target.pose.orientation.x,
        current_target.pose.orientation.y, current_target.pose.orientation.z
    ])

    start_yaw = quat2yaw(start_orientation)
    tg_yaw = quat2yaw(tg_q)

    rospy.loginfo("On Target in " + str(req.t2go) + " sec!")
    rospy.loginfo("Target = [" + str(tg_pos[0]) + " " + str(tg_pos[1]) + " " +
                  str(tg_pos[2]) + "]")
    rospy.loginfo("Vehicle = [" + str(start_pos[0]) + " " + str(start_pos[1]) +
                  " " + str(start_pos[2]) + "]")

    # Generate the interpolation matrices
    (X, Y, Z, W, relknots) = genInterpolProblem(tg_pos - start_pos, tg_q,
                                                tg_yaw - start_yaw, v_norm,
                                                a_norm)
    # Generate the knots vector
    t_impact = req.t2go
    knots = relknots + t_impact
    knots = np.hstack(([0.0], knots))

    # Generate the polynomial
    ppx = pw.PwPoly(X, knots, ndeg)
    ppy = pw.PwPoly(Y, knots, ndeg)
    ppz = pw.PwPoly(Z, knots, ndeg)
    ppw = pw.PwPoly(W, knots, ndeg)

    frequency = rospy.get_param('~freq', 30.0)

    my_traj = trj.Trajectory(ppx, ppy, ppz, ppw)

    Dt = knots[1:len(knots)] - knots[0:len(knots) - 1]

    my_traj.writeTofile(Dt, '/tmp/toTarget.csv')

    t = Thread(target=rep_trajectory,
               args=(my_traj, start_pos, max(knots), frequency)).start()

    #    (Dt, polysX, polysY, polysZ, polysW) = tj.ppFromfile('/tmp/toTarget.csv')
    #
    #    my_traj = trj.Trajectory(polysX, polysY, polysZ, polysW)
    #    t = Thread(target=rep_trajectory, args=(my_traj,start_pos, max(knots), frequency)).start()

    return True
Exemplo n.º 7
0
def handle_genGotoTrj(req):
    tg_p = req.target_p
    tg_v = req.target_v
    tg_a = req.target_a
    t_impact = req.tg_time

    # Times (Absolute and intervals)
    knots = np.array([0, t_impact])  # One second each piece

    # Polynomial characteristic:  order
    ndeg = 7
    nconstr = 4

    X = np.array([
        [0.0, tg_p[0]],
        [0.0, tg_v[0]],
        [0.0, tg_a[0]],
        [0.0, 0.0],
    ])

    Y = np.array([
        [0.0, tg_p[1]],
        [0.0, tg_v[1]],
        [0.0, tg_a[1]],
        [0.0, 0.0],
    ])

    Z = np.array([
        [0.0, tg_p[2]],
        [0.0, tg_v[2]],
        [0.0, tg_a[2]],
        [0.0, 0.0],
    ])

    W = np.array([
        [0.0, 0.0],
        [0.0, 0.0],
        [0.0, 0.0],
        [0.0, 0.0],
    ])

    ppx = pw.PwPoly(X, knots, ndeg)
    ppy = pw.PwPoly(Y, knots, ndeg)
    ppz = pw.PwPoly(Z, knots, ndeg)
    ppw = pw.PwPoly(W, knots, ndeg)

    frequency = rospy.get_param('~freq', 30.0)

    my_traj = trj.Trajectory(ppx, ppy, ppz, ppw)

    start_pos = [
        current_odometry.pose.pose.position.x,
        current_odometry.pose.pose.position.y,
        current_odometry.pose.pose.position.z
    ]

    t = Thread(target=rep_trajectory,
               args=(my_traj, start_pos, t_impact, frequency)).start()

    if (tg_v.all() == 0.0 and tg_a.all() == 0.0):
        my_traj.writeTofile(Dt, '/tmp/goToTrajectory.csv')

    return True
Exemplo n.º 8
0
def handle_genTrackTrj(req):

    start_pos = np.array([
        current_odometry.pose.pose.position.x,
        current_odometry.pose.pose.position.y,
        current_odometry.pose.pose.position.z
    ])

    start_vel = np.array([
        current_odometry.twist.twist.linear.x,
        current_odometry.twist.twist.linear.y,
        current_odometry.twist.twist.linear.z
    ])

    t_impact = req.tg_time

    tg_v = req.target_v
    tg_a = req.target_a

    # A little logic considering what could have been requested
    if (req.ref == "Absolute"):
        tg_p = req.target_p
        tg_prel = tg_p - start_pos
    elif (req.ref == "Relative"):
        tg_prel = req.target_p
        # Detect if you are requesting a landing
        if ((start_pos[2] + tg_prel[2]) <= 0):
            # I have to redefine because I cannot mutate tuples
            tg_prel = [tg_prel[0], tg_prel[1], -start_pos[2]]
            # Automatically calculate the time to land, if not specified
            if (t_impact == 0.0):
                t_impact = (start_pos[2] / 0.3)
            tg_v = np.zeros((3))
            tg_a = np.zeros((3))
    else:
        rospy.loginfo("Error passing reference to guidance")

    # Times (Absolute and intervals)
    knots = np.array([0, t_impact])  # One second each piece
    Dt = knots[1:len(knots)] - knots[0:len(knots) - 1]

    # Polynomial characteristic:  order
    ndeg = 7
    nconstr = 4

    X = np.array([
        [0.0, tg_prel[0]],
        [start_vel[0], tg_v[0]],
        [0.0, tg_a[0]],
        [0.0, 0.0],
    ])

    Y = np.array([
        [0.0, tg_prel[1]],
        [start_vel[1], tg_v[1]],
        [0.0, tg_a[1]],
        [0.0, 0.0],
    ])

    Z = np.array([
        [0.0, tg_prel[2]],
        [start_vel[2], tg_v[2]],
        [0.0, tg_a[2]],
        [0.0, 0.0],
    ])

    W = np.array([
        [0.0, 0.0],
        [0.0, 0.0],
        [0.0, 0.0],
        [0.0, 0.0],
    ])

    ppx = pw.PwPoly(X, knots, ndeg)
    ppy = pw.PwPoly(Y, knots, ndeg)
    ppz = pw.PwPoly(Z, knots, ndeg)
    ppw = pw.PwPoly(W, knots, ndeg)

    frequency = rospy.get_param('~freq', 30.0)

    my_traj = trj.Trajectory(ppx, ppy, ppz, ppw)

    my_traj.writeTofile(Dt, '/tmp/toTarget.csv')

    t = Thread(target=rep_trajectory,
               args=(my_traj, start_pos, t_impact, frequency)).start()

    return True