def getShortestPath(self, start_state, goal, set_velz=None):
traj = quadtraj.RapidTrajectory(
start_state.position.to_numpy_array().tolist(),
start_state.linear_velocity.to_numpy_array().tolist(),
start_state.linear_acceleration.to_numpy_array().tolist(),
self.gravity)
goal_pos = goal[:3]
traj.set_goal_position(goal_pos)
distance = np.linalg.norm(start_state.position.to_numpy_array() -
np.array(goal_pos))
traj.set_goal_acceleration([0, 0, 0])
if set_velz:
print("Special case with set vz: ", set_velz * goal[3:])
traj.set_goal_velocity(set_velz * goal[3:])
# traj.set_goal_acceleration_in_axis(2,0)
# print("PLANNING: Start: ", start_state.position.to_numpy_array().tolist())
# print("PLANNING: Start: ", start_state.linear_velocity.to_numpy_array().tolist())
# print("PLANNING: Start: ", start_state.linear_acceleration.to_numpy_array().tolist())
# print("PLANNING: g: ", self.gravity)
# print("PLANNING: Goal: ", goal_pos)
# print("PLANNING: Limits: ", self.fmin, self.fmax, self.wmax, self.minTimeSec)
return self.binaryCompute(traj, distance)
def givemetraj(self, pos0, vel0, acc0, posf, velf, accf, Tf):
traj = quadtraj.RapidTrajectory(pos0, vel0, acc0, self.gravity)
traj.set_goal_position(posf)
traj.set_goal_velocity(velf)
traj.set_goal_acceleration(accf)
# Run the algorithm, and generate the trajectory.
traj.generate(Tf)
return traj
def get_paths_list(self, pos_s,vec_s,acc_s, pos_g,vec_g,acc_g = [0,0,0],T_duration = 0):
if T_duration == 0 :
print ('Wrong duration time!!')
return
traj = quadtraj.RapidTrajectory(pos_s, vec_s, acc_s, self.gravity)
traj.set_goal_position(pos_g)
traj.set_goal_velocity(vec_g)
traj.set_goal_acceleration(acc_g)
traj.generate(T_duration)
floorPoint = [0,0,0] # a point on the floor
floorNormal = [0,0,1] # we want to be in this direction of the point (upwards)
positionFeasible = traj.check_position_feasibility(floorPoint, floorNormal)
#print("Position feasibility result: ", quadtraj.StateFeasibilityResult.to_string(positionFeasible), "(", positionFeasible, ")")
return traj
def generate_trajectory_cross_time(self, pos_err, spd_start, acc_start,
spd_end, acc_end, cross_time):
self.pos_err = pos_err
self.spd_start = spd_start
self.acc_start = acc_start
self.spd_end = spd_end
self.acc_end = acc_end
# self.cross_spd = cross_spd
self.rapid_traj = quadtraj.RapidTrajectory([0, 0, 0], spd_start,
acc_start, self.gravity)
self.rapid_traj.set_goal_position(pos_err)
self.rapid_traj.set_goal_velocity(spd_end)
self.rapid_traj.set_goal_acceleration(acc_end)
self.Tf = float(cross_time)
self.cross_spd = float(np.linalg.norm(pos_err) / self.Tf)
self.rapid_traj.generate(self.Tf)
def getExtendedPath(self, goal, set_velz=None):
assert (len(self.goalPosition) > 0)
traj = quadtraj.RapidTrajectory(self.goalPosition.tolist(),
self.goalVelocity.tolist(),
self.goalAccerleration.tolist(),
self.gravity)
goal_pos = goal[:3]
traj.set_goal_position(goal_pos)
traj.set_goal_acceleration([0, 0, 0])
if set_velz:
print("Special case with set vz: ", set_velz * goal[3:])
traj.set_goal_velocity(set_velz * goal[3:])
# print("PLANNING: Start: ", self.goalPosition.tolist())
# print("PLANNING: Vel: ", self.goalVelocity.tolist())
# print("PLANNING: Acc: ", self.goalAccerleration.tolist())
# print("PLANNING: Goal: ", goal_pos)
distance = np.linalg.norm(self.goalPosition - np.array(goal_pos))
return self.binaryCompute(traj, distance)
# Define the duration: Tf = 1 timeArr = np.linspace(3,5,N_times) # Define the input limits: fmin = 5 #[m/s**2] fmax = 25 #[m/s**2] wmax = 20 #[rad/s] minTimeSec = 0.02 #[s] # Define how gravity lies: gravity = [0,0,-9.81] traj = quadtraj.RapidTrajectory(pos0, vel0, acc0, gravity) traj.set_goal_position(posf) traj.set_goal_velocity(velf) traj.set_goal_acceleration(accf) # Note: if you'd like to leave some states free, there are two options to # encode this. As exmample, we will be leaving the velocity in `x` (axis 0) # free: # # Option 1: # traj.set_goal_velocity_in_axis(1,velf_y); # traj.set_goal_velocity_in_axis(2,velf_z); # # Option 2: # traj.set_goal_velocity([None, velf_y, velf_z])
def approach_trajectory(self, pos_start, vel_start, acc_start, if_draw=1):
pos0 = [float(pos_start[0]),
float(pos_start[1]),
float(pos_start[2])] # position
vel0 = [float(vel_start[0]),
float(vel_start[1]),
float(vel_start[2])] # position
acc0 = [float(acc_start[0]),
float(acc_start[1]),
float(acc_start[2])] # position
posf = self.np_matrix_to_float_list(self.cross_path_p_0)
velf = self.np_matrix_to_float_list(self.cross_path_v_0)
# accf = [0, 0, -9.80] # acceleration
# accf = [0, 0, -9.80] # acceleration
# accf = self.np_matrix_to_float_list(self.base_e[2]*-1.0)
accf = self.np_matrix_to_float_list(self.g_vec - self.g_sub_vec[2])
# accf = self.base_e[2].T.tolist()[0] # acceleration
gravity = [0, 0, -9.80]
self.rapid_traj = quadtraj.RapidTrajectory(pos0, vel0, acc0, gravity)
self.rapid_traj.set_goal_position(posf)
self.rapid_traj.set_goal_velocity(velf)
self.rapid_traj.set_goal_acceleration(accf)
print('Pos0 = ', pos0)
print('Posf = ', posf)
print('Norm = ',
LA.norm(np.matrix(np.matrix(posf) - np.matrix(pos0))))
self.Tf = LA.norm(
np.matrix(np.matrix(posf) - np.matrix(pos0))) / self.para_cross_spd
self.rapid_traj.generate(self.Tf)
self.rapid_nn_input_vector = np.zeros((17, 1))
self.rapid_pos_err = self.cross_path_p_0 - np.matrix(pos0).T
self.rapid_pos_start = np.matrix(pos0).T
self.rapid_spd_start = np.matrix(vel0).T
self.rapid_acc_start = np.matrix(acc0).T
self.rapid_spd_end = np.matrix(velf).T
self.rapid_acc_end = np.matrix(accf).T
self.rapid_cross_time = self.Tf
self.rapid_nn_input_vector[1, :] = self.para_cross_spd
self.rapid_nn_input_vector[(2, 3, 4), :] = self.rapid_pos_err
self.rapid_nn_input_vector[(5, 6, 7), :] = self.rapid_spd_start
self.rapid_nn_input_vector[(8, 9, 10), :] = self.rapid_acc_start
self.rapid_nn_input_vector[(11, 12, 13), :] = self.rapid_spd_end
self.rapid_nn_input_vector[(14, 15, 16), :] = self.rapid_acc_end
# print(self.rapid_nn_input_vector)
if (if_draw):
numPlotPoints = 100
time = np.linspace(0, self.Tf, numPlotPoints)
position = np.zeros([numPlotPoints, 3])
velocity = np.zeros([numPlotPoints, 3])
acceleration = np.zeros([numPlotPoints, 3])
thrust = np.zeros([numPlotPoints, 1])
ratesMagn = np.zeros([numPlotPoints, 1])
plane_acc = np.zeros([numPlotPoints, 3])
plane_x = np.zeros([numPlotPoints, 3])
plane_y = np.zeros([numPlotPoints, 3])
plane_z = np.zeros([numPlotPoints, 3])
for i in range(numPlotPoints):
t = time[i]
position[i, :] = self.rapid_traj.get_position(t)
velocity[i, :] = self.rapid_traj.get_velocity(t)
acceleration[i, :] = self.rapid_traj.get_acceleration(t)
plane_acc[i, :] = acceleration[i, :] - gravity
thrust[i] = self.rapid_traj.get_thrust(t)
vec_camera2center = np.matrix(
np.array(self.center) - position[i, :]).T
Pitch_vec = (
vec_camera2center -
(self.vector_project(vec_camera2center,
np.matrix(plane_acc[i, :]).T)))
plane_x[i, :] = Pitch_vec.T.tolist()[0]
plane_y[i, :] = np.cross(Pitch_vec.T, np.matrix(
plane_acc[i, :])).tolist()[0]
plane_z[i, :] = -plane_acc[i, :]
plane_x[i, :] = plane_x[i, :] / LA.norm(plane_x[i, :])
plane_y[i, :] = plane_y[i, :] / LA.norm(plane_y[i, :])
plane_z[i, :] = plane_z[i, :] / LA.norm(plane_z[i, :])
if (np.mod(i, 10) == 0 or i == numPlotPoints - 1):
draw_arrow(self.ax3d,
position[i, :],
plane_acc[i, :],
1,
label='',
color='r')
# draw_arrow(self.ax3d, position[i, :], plane_x[i, :], 2, label='', color='g')
# draw_arrow(self.ax3d, position[i, :], plane_y[i, :], 1, label='', color='b')
self.quadrotor_painter.plot_plane(position[i, :],
plane_x[i, :],
plane_y[i, :],
plane_z[i, :])
# ratesMagn[i] = np.linalg.norm(rapid_traj.get_body_rates(t))
self.ax3d.plot3D(position[:, 0],
position[:, 1],
position[:, 2],
'g--',
label="approach_path")
self.ax3d.legend()
def main():
# # Initiate subscriber
# rospy.init_node('trajectory_subscriber', anonymous = True);
# rospy.Subscriber('DESIRED_WAYPOINTS', ,callback);
# rospy.spin();
#
# Predefined gates
with open('gate_locations_0.yaml') as handle:
gate_locations = yaml.safe_load(handle)
Gate1 = np.mean(gate_locations['Gate1']['location'], axis=0)
Gate2 = np.mean(gate_locations['Gate2']['location'], axis=0)
Gate3 = np.mean(gate_locations['Gate3']['location'], axis=0)
Gate4 = np.mean(gate_locations['Gate4']['location'], axis=0)
Gate5 = np.mean(gate_locations['Gate5']['location'], axis=0)
Gate6 = np.mean(gate_locations['Gate6']['location'], axis=0)
Gate7 = np.mean(gate_locations['Gate7']['location'], axis=0)
Gate8 = np.mean(gate_locations['Gate8']['location'], axis=0)
Gate9 = np.mean(gate_locations['Gate9']['location'], axis=0)
Gate10 = np.mean(gate_locations['Gate10']['location'], axis=0)
Gate11 = np.mean(gate_locations['Gate11']['location'], axis=0)
Gate12 = np.mean(gate_locations['Gate12']['location'], axis=0)
Gate13 = np.mean(gate_locations['Gate13']['location'], axis=0)
Gate14 = np.mean(gate_locations['Gate14']['location'], axis=0)
Gate15 = np.mean(gate_locations['Gate15']['location'], axis=0)
Gate16 = np.mean(gate_locations['Gate16']['location'], axis=0)
Gate17 = np.mean(gate_locations['Gate17']['location'], axis=0)
Gate18 = np.mean(gate_locations['Gate18']['location'], axis=0)
Gate19 = np.mean(gate_locations['Gate19']['location'], axis=0)
Gate20 = np.mean(gate_locations['Gate20']['location'], axis=0)
Gate21 = np.mean(gate_locations['Gate21']['location'], axis=0)
Gate22 = np.mean(gate_locations['Gate22']['location'], axis=0)
Gate23 = np.mean(gate_locations['Gate23']['location'], axis=0)
gate = [
Gate1, Gate2, Gate3, Gate4, Gate5, Gate6, Gate7, Gate8, Gate9, Gate10,
Gate11, Gate12, Gate13, Gate14, Gate15, Gate16, Gate17, Gate18, Gate19,
Gate20, Gate21, Gate22, Gate23
]
GATE_ORDER = [19, 10, 21, 2, 13, 9, 14, 1, 22, 15, 23, 6]
# Define the trajectory starting state:
pos0 = [-1, -30, 1] #position
vel0 = [0, 0, 0] #velocity
acc0 = [0, 0, 0] #acceleration
# Define the goal state:
posf = gate[GATE_ORDER[0]] # position
velf = [5, 5, 0] # velocity
accf = [0, 0, 0] # acceleration
# Define the duration:
Tf = 5
# Define the input limits:
fmin = 5 #[m/s**2]
fmax = 25 #[m/s**2]
wmax = 20 #[rad/s]
minTimeSec = 0.02 #[s]
# Define how gravity lies:
gravity = [0, 0, -9.81]
# Trajectory generation
position = list()
velocity = list()
acceleration = list()
thrust = list()
ratesMagn = list()
for i in range(len(GATE_ORDER)):
traj = quadtraj.RapidTrajectory(pos0, vel0, acc0, gravity)
traj.set_goal_position(gate[GATE_ORDER[i] - 1])
traj.set_goal_velocity(velf)
traj.set_goal_acceleration(accf)
traj.generate(Tf)
N = 100
time = np.linspace(0, Tf, N)
for j in range(N):
t = time[j]
position.append(traj.get_position(t))
velocity.append(traj.get_velocity(t))
acceleration.append(traj.get_acceleration(t))
thrust.append(traj.get_thrust(t))
ratesMagn.append(np.linalg.norm(traj.get_body_rates(t)))
###############
# Feasibility #
###############
# Test input feasibility
inputsFeasible = traj.check_input_feasibility(fmin, fmax, wmax,
minTimeSec)
# Test whether we fly into the floor
floorPoint = [0, 0, 0]
# a point on the floor
floorNormal = [0, 0, 1]
# we want to be in this direction of the point (upwards)
positionFeasible = traj.check_position_feasibility(
floorPoint, floorNormal)
for i in range(3):
print("Axis #", i)
print("\talpha = ", traj.get_param_alpha(i), "\tbeta = ",
traj.get_param_beta(i), "\tgamma = ",
traj.get_param_gamma(i))
print("Total cost = ", traj.get_cost())
print("Input feasibility result: ",
quadtraj.InputFeasibilityResult.to_string(inputsFeasible), "(",
inputsFeasible, ")")
print("Position feasibility result: ",
quadtraj.StateFeasibilityResult.to_string(positionFeasible), "(",
positionFeasible, ")")
# Reset using last point
pos0 = position[-1]
vel0 = velocity[-1]
acc0 = acceleration[-1]
traj.reset()
plot(Tf, GATE_ORDER, N, position, velocity, acceleration, thrust,
ratesMagn, fmin, fmax, wmax)
def main():
# # Initiate subscriber
# rospy.init_node('trajectory_subscriber', anonymous = True);
# rospy.Subscriber('DESIRED_WAYPOINTS', ,callback);
# rospy.spin();
# Define the trajectory starting state:
pos0 = [0, 0, 0] # position
vel0 = [0, 0, 0] # velocity
acc0 = [0, 0, 0] # acceleration
# Define the goal state:
posf = [20, -20, 17] # position
velf = [5, 5, 0] # velocity
accf = [0, 0, 0] # acceleration
desired_yaw = 10
# Define the duration:
tmin = 0.1
# minimum time, has to be > 0
tmax = 10
# maximum time
Tf = np.linspace(tmin, tmax, 1000)
# Define the input limits:
fmin = 5 #[m/s**2]
fmax = 25 #[m/s**2]
wmax = 20 #[rad/s]
minTimeSec = 0.02 #[s]
tol = 10
# Error tolerance
# Define how gravity lies:
gravity = [0, 0, -9.81]
#########################
# Trajectory generation #
#########################
position = list()
velocity = list()
acceleration = list()
thrust = list()
ratesMagn = list()
bodyRates = list()
attitude = list()
N = 100
# Grid search optimization
for i in range(len(Tf)):
traj = quadtraj.RapidTrajectory(pos0, vel0, acc0, gravity)
traj.set_goal_position(posf)
traj.set_goal_velocity([2, 10, -5])
traj.set_goal_acceleration([0, 0, 0])
traj.generate(Tf[i])
cost = traj.get_cost()
# To get (p, q, r), call traj.get_body_rates(t) and traj.get_rotation_matrix(psi, theta,phi)
# (p, q, r) get_body_rates returns body rates at time t in the inertial frame. These are to be rotated by the rotation matrix.
# print((np.degrees(traj.get_final_yaw(tmin, Tf[i], N))))
#and (traj.check_final_yaw(tmin,Tf[i], N) - desired_yaw <= tol)
if (traj.check_input_feasibility(fmin, fmax, wmax, minTimeSec) == 0):
print(Tf[i])
break
# Obtain the trajectory with the lowest time
timeToGo = Tf[i]
time = np.linspace(tmin, timeToGo, N)
print(timeToGo)
for j in range(N):
t = time[j]
position.append(traj.get_position(t))
velocity.append(traj.get_velocity(t))
acceleration.append(traj.get_acceleration(t))
bodyRates.append(traj.get_body_rates(t, dt=timeToGo / N))
attitude.append(traj.get_pose(t, dt=timeToGo / N))
thrust.append(traj.get_thrust(t))
ratesMagn.append(np.linalg.norm(traj.get_body_rates(t)))
print(np.matmul(traj.get_rotation_matrix(1, 2, 3), traj.get_body_rates(t)))
###############
# Feasibility #
###############
# Test input feasibility
inputsFeasible = traj.check_input_feasibility(fmin, fmax, wmax, minTimeSec)
# Test whether we fly into the floor or any planes
floorPoint = [0, 0, 0]
# a point on the floor
floorNormal = [0, 0, 1]
# we want to be in this direction of the point (upwards)
positionFeasible = traj.check_position_feasibility(floorPoint, floorNormal)
for i in range(3):
print("Axis #", i)
print("\talpha = ", traj.get_param_alpha(i), "\tbeta = ",
traj.get_param_beta(i), "\tgamma = ", traj.get_param_gamma(i))
print("Total cost = ", traj.get_cost())
print("Input feasibility result: ",
quadtraj.InputFeasibilityResult.to_string(inputsFeasible), "(",
inputsFeasible, ")")
print("Position feasibility result: ",
quadtraj.StateFeasibilityResult.to_string(positionFeasible), "(",
positionFeasible, ")")
# Reset
traj.reset()
plot(timeToGo, N, position, velocity, acceleration, thrust, ratesMagn,
fmin, fmax, wmax)
