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)
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)
[0, np.nan, 0.0], ]) mid_time = 1.5 end_time = 2.0 # Times (Absolute and intervals) knots = np.array([0, mid_time, end_time]) # One second each piece Dt = knots[1:3] - knots[0:2] # 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) traj = tr.Trajectory(ppx, ppy, ppz, ppw) print("Evaluating Trajectory in 0") print(traj.eval(0.0, [0])) print("Evaluating Trajectory at " + str(1.5)) print(traj.eval(1.5, [0, 1])) print("\nEvaluating trajectory at the end point") print(traj.eval(end_time, [0, 1, 2])) traj.writeTofile(Dt, filename='./trjfile.csv') traj.readFromfile(filename='./trjfile.csv') print("Evaluating loaded trajectory at" + str(1.5)) print(traj.eval(1.5, [0, 1])) print("\nEvaluating loaded trajectory at the end point") print(traj.eval(end_time, [0, 1, 2]))
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
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
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