forked from davidcGIThub/trajectory_generator
-
Notifications
You must be signed in to change notification settings - Fork 0
/
test_intermediate_waypoints.py
104 lines (86 loc) · 4.09 KB
/
test_intermediate_waypoints.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
from bsplinegenerator.bsplines import BsplineEvaluation
from trajectory_generation.trajectory_generator import TrajectoryGenerator
from trajectory_generation.constraint_data_structures.safe_flight_corridor import SFC_Data, get3DRotationAndTranslationFromPoints
from trajectory_generation.path_plotter import set_axes_equal
from trajectory_generation.constraint_data_structures.waypoint_data import Waypoint, WaypointData, plot2D_waypoints
from trajectory_generation.constraint_data_structures.dynamic_bounds import DerivativeBounds, TurningBound
from trajectory_generation.constraint_data_structures.obstacle import Obstacle, plot_2D_obstacles
import time
dimension = 2
# max_curvature = 1
order = 3
traj_objective_type = "minimal_acceleration_path"
# traj_objective_type = "minimal_velocity_path"
# traj_objective_type = "minimal_distance_path"
sfc_data = None
# obstacles = [Obstacle(center=np.array([[5.5],[7]]), radius=1)]
obstacles = None
max_turning_bound = 1 #angular rate
turning_bound = TurningBound(max_turning_bound,"angular_rate")
turning_bound = None
max_velocity = 1.5
# max_velocity = None
# max_acceleration = 5
max_acceleration = None
derivative_bounds = DerivativeBounds(max_velocity, max_acceleration)
# derivative_bounds = None
### 1st path
waypoint_1 = Waypoint(location=np.array([[3 ],[4]]),velocity = np.array([[1],[0]]))
waypoint_2 = Waypoint(location=np.array([[1.5],[6]]))
waypoint_3 = Waypoint(location=np.array([[3.4],[8]]))
waypoint_4 = Waypoint(location=np.array([[2 ],[10]]),velocity = np.array([[0],[1]]))
waypoint_sequence = (waypoint_1, waypoint_2, waypoint_3, waypoint_4)
waypoint_data = WaypointData(waypoint_sequence)
traj_gen = TrajectoryGenerator(dimension, 11)
start_time_1 = time.time()
initial_control_points = None
initial_scale_factor = None
control_points, scale_factor = traj_gen.generate_trajectory(waypoint_data, derivative_bounds,
turning_bound, sfc_data, obstacles, traj_objective_type)
print("control_points: " , control_points)
print("scale_factor: ", scale_factor)
end_time_1 = time.time()
spline_start_time_1 = 0
bspline = BsplineEvaluation(control_points, order, spline_start_time_1, scale_factor, False)
end_time_spline = bspline.get_end_time()
## spline 1 data
number_data_points = 10000
spline_data, time_data = bspline.get_spline_data(number_data_points)
curvature_data, time_data = bspline.get_spline_curvature_data(number_data_points)
velocity_data, time_data = bspline.get_derivative_magnitude_data(number_data_points,1)
acceleration_data, time_data = bspline.get_derivative_magnitude_data(number_data_points,2)
angular_rate_data, time_data = bspline.get_angular_rate_data(number_data_points)
centripetal_acceleration_data, time_data = bspline.get_centripetal_acceleration_data(number_data_points)
path_length = bspline.get_arc_length(number_data_points)
start_velocity = bspline.get_derivative_at_time_t(0,1)
start_acceleration = bspline.get_derivative_at_time_t(0,2)
print("path_length: " , path_length)
print("computation time: " , end_time_1 - start_time_1)
velocity_matrix, time_data = bspline.get_spline_derivative_data(number_data_points,1)
acceleration_matrix, time_data = bspline.get_spline_derivative_data(number_data_points,2)
plt.figure()
ax = plt.axes()
# ax.scatter(control_points[0,:], control_points[1,:], color="tab:orange")
ax.plot(spline_data[0,:], spline_data[1,:], color = "tab:blue")
plot2D_waypoints(waypoint_data, ax)
set_axes_equal(ax,dimension)
plt.title("Optimized Path")
plt.show()
if turning_bound is not None:
turn_data = []
if turning_bound.bound_type == "angular_rate":
turn_data = angular_rate_data
elif turning_bound.bound_type == "curvature":
turn_data = curvature_data
elif turning_bound.bound_type == "centripetal_acceleration":
turn_data = centripetal_acceleration_data
turn_title = turning_bound.bound_type
plt.figure()
plt.plot(time_data, turn_data,color = "b")
# plt.plot(time_data, acceleration_data,color = "g")
plt.plot(time_data, max_turning_bound + turn_data*0)
plt.title(turn_title)
plt.show()