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)
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, 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()
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)
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
