Coverage for src/pyroboplan/trajectory/trapezoidal_velocity.py: 79%
168 statements
« prev ^ index » next coverage.py v7.6.12, created at 2025-03-02 22:03 -0500
1import numpy as np
2import warnings
5class TrapezoidalVelocityTrajectorySingleDofContainer:
6 """
7 Helper data structure containing data for a single-dof trapezoidal velocity profile trajectory.
9 This class should not be used standalone.
10 """
12 def __init__(self):
13 self.times = []
14 self.positions = []
15 self.velocities = []
16 self.accelerations = []
19class TrapezoidalVelocityTrajectory:
20 """
21 Describes a trapezoidal velocity profile trajectory.
23 Some good resources:
24 * Chapter 9 of this book: https://hades.mech.northwestern.edu/images/7/7f/MR.pdf
25 * MATLAB documentation: https://www.mathworks.com/help/robotics/ref/trapveltraj.html
26 """
28 def __init__(self, q, qd_max, qdd_max, t_start=0.0):
29 """
30 Creates a trapezoidal velocity profile trajectory.
32 Parameters
33 ----------
34 q : array-like
35 A M-by-N array of M dimensions and N waypoints.
36 qd_max : array-like or float
37 The maximum velocity for each dimension (size N-by-1), or scalar for all dimensions.
38 qdd_max : array-like or float
39 The maximum acceleration for each dimension (size N-by-1), or scalar for all dimensions.
40 t_start : float, optional
41 The start time of the trajectory. Defaults to zero.
42 """
43 if len(q.shape) == 1:
44 q = q[np.newaxis, :]
45 self.waypoints = q
46 self.num_dims = q.shape[0]
48 if isinstance(qd_max, float) or len(qd_max) == 1:
49 qd_max = qd_max * np.ones(self.num_dims)
50 if isinstance(qdd_max, float) == 1:
51 qdd_max = qdd_max * np.ones(self.num_dims)
53 self.segment_times = [t_start]
54 self.single_dof_trajectories = [
55 TrapezoidalVelocityTrajectorySingleDofContainer()
56 for _ in range(self.num_dims)
57 ]
58 for dim, traj in enumerate(self.single_dof_trajectories):
59 traj.times.append(t_start)
60 traj.positions.append(q[dim][0])
61 traj.velocities.append(0.0)
63 # Iterate through all the waypoints
64 delta_q_vec = np.diff(q, axis=1)
65 for idx in range(delta_q_vec.shape[1]):
66 t_prev = self.segment_times[-1]
67 delta_q = delta_q_vec[:, idx]
68 delta_q_abs = np.abs(delta_q)
70 # Find the limiting velocity and acceleration by normalizing.
71 # This can lead to divisions by zero, which we can ignore here.
72 with np.errstate(divide="ignore"):
73 qd_max_norm = np.min(qd_max / delta_q_abs)
74 qdd_max_norm = np.min(qdd_max / delta_q_abs)
76 # If there is a zero displacement segment, skip it!
77 if not np.isfinite(qd_max_norm) or not np.isfinite(qdd_max_norm):
78 continue
80 accel = qdd_max_norm * delta_q
82 # Check if "bang-bang" control at constant acceleration is feasible.
83 # This is determined by seeing if the position moved by ramping up
84 # to max velocity and then immediately back down is greater than 1,
85 # in normalized coordinates.
86 #
87 # -------- qd_max
88 # /\
89 # / \ <--- slope = qdd_max
90 # / \
91 # <----> ---- 0.0
92 # t_segment
93 #
94 # Some key equations:
95 # qdd_max = 2 * qd_max / t_segment (slope at constant acceleration)
96 # delta_q = 0.5 * t_segment * qd_max = 1.0 (area under the triangle)
97 #
98 # By using normalized positions and substituting in the equations above,
99 # this is equivalent to checking that:
100 # qd_max ^ 2 >= qdd_max
102 if qd_max_norm**2 >= qdd_max_norm:
103 # This is possible without a zero-acceleration segment.
104 # We must now find the qd_peak value, which could be less than
105 # the maximum, to achieve a (normalized) delta_q value of 1.0.
106 #
107 # -------- qd_peak
108 # /\
109 # / \ <--- slope = qdd_peak
110 # / \
111 # <----> ---- 0.0
112 # t_segment
113 #
114 # The equations are the same as above:
115 # qdd_max = 2 * qd_peak / t_segment (slope at constant acceleration)
116 # delta_q = 0.5 * t_segment * qd_peak = 1.0 (area under the triangle)
118 qd_peak_norm = np.sqrt(qdd_max_norm)
119 t_segment = 2.0 * qd_peak_norm / qdd_max_norm
121 # Unpack into the separate dimensions by unnormalizing speed and acceleration.
122 qd_peak = qd_peak_norm * delta_q
123 for dim in range(self.num_dims):
124 q_prev = self.single_dof_trajectories[dim].positions[-1]
126 times = [t_prev + t_segment / 2.0, t_prev + t_segment]
127 positions = [
128 q_prev + delta_q[dim] / 2.0,
129 q_prev + delta_q[dim],
130 ]
131 velocities = [qd_peak[dim], 0.0]
132 accelerations = [accel[dim], -accel[dim]]
134 self.single_dof_trajectories[dim].times.extend(times)
135 self.single_dof_trajectories[dim].positions.extend(positions)
136 self.single_dof_trajectories[dim].velocities.extend(velocities)
137 self.single_dof_trajectories[dim].accelerations.extend(
138 accelerations
139 )
140 else:
141 # This requires a zero-acceleration segment as follows.
142 #
143 # --------- --- qd_max
144 # / \
145 # / \ <--- slope = qdd_max
146 # / \
147 # |<----->| --- 0
148 # t_zero_accel
149 # |<------------->|
150 # t_segment
152 t_const_accel = 2.0 * qd_max_norm / qdd_max_norm
153 delta_q_norm_const_accel = 0.5 * t_const_accel * qd_max_norm
154 delta_q_norm_zero_accel = 1.0 - delta_q_norm_const_accel
155 t_zero_accel = delta_q_norm_zero_accel / qd_max_norm
156 t_segment = t_const_accel + t_zero_accel
158 # Unpack into the separate dimensions by unnormalizing speed and acceleration.
159 qd_peak = qd_max_norm * delta_q
160 delta_q_const_accel = delta_q_norm_const_accel * delta_q
161 delta_q_zero_accel = delta_q_norm_zero_accel * delta_q
162 for dim in range(self.num_dims):
163 q_prev = self.single_dof_trajectories[dim].positions[-1]
165 times = [
166 t_prev + t_const_accel / 2.0,
167 t_prev + t_const_accel / 2.0 + t_zero_accel,
168 t_prev + t_const_accel + t_zero_accel,
169 ]
170 positions = [
171 q_prev + delta_q_const_accel[dim] / 2.0,
172 q_prev
173 + delta_q_const_accel[dim] / 2.0
174 + delta_q_zero_accel[dim],
175 q_prev + delta_q[dim],
176 ]
177 velocities = [qd_peak[dim], qd_peak[dim], 0.0]
178 accelerations = [accel[dim], 0.0, -accel[dim]]
180 self.single_dof_trajectories[dim].times.extend(times)
181 self.single_dof_trajectories[dim].positions.extend(positions)
182 self.single_dof_trajectories[dim].velocities.extend(velocities)
183 self.single_dof_trajectories[dim].accelerations.extend(
184 accelerations
185 )
187 self.segment_times.append(t_prev + t_segment)
189 def evaluate(self, t):
190 """
191 Evaluates the trajectory at a specific time.
193 Parameters
194 ----------
195 t : float
196 The time, in seconds, at which to evaluate the trajectory.
198 Returns
199 -------
200 tuple(array-like or float, array-like or float, array-like or float)
201 The tuple of (q, qd, qdd) trajectory values at the specified time.
202 If the trajectory is single-DOF, these will be scalars, otherwise they are arrays.
203 """
204 q = np.zeros(self.num_dims)
205 qd = np.zeros(self.num_dims)
206 qdd = np.zeros(self.num_dims)
208 # Handle edge cases.
209 if t < self.segment_times[0]:
210 warnings.warn("Cannot evaluate trajectory before its start time.")
211 return None
212 elif t > self.segment_times[-1]:
213 warnings.warn("Cannot evaluate trajectory after its end time.")
214 return None
216 for dim in range(self.num_dims):
217 segment_idx = 0
218 traj = self.single_dof_trajectories[dim]
219 evaluated_segment = False
220 while not evaluated_segment:
221 t_next = traj.times[segment_idx + 1]
222 t_prev = traj.times[segment_idx]
223 t_segment = t_next - t_prev
224 if t <= t_next:
225 q_prev = traj.positions[segment_idx]
226 t_prev = traj.times[segment_idx]
227 v_next = traj.velocities[segment_idx + 1]
228 v_prev = traj.velocities[segment_idx]
230 dt = t - t_prev
231 dv = (v_next - v_prev) * dt / t_segment
232 q[dim] = q_prev + (0.5 * dv + v_prev) * dt
233 qd[dim] = v_prev + dv
234 qdd[dim] = traj.accelerations[segment_idx]
235 evaluated_segment = True
236 else:
237 segment_idx += 1
239 # If the trajectory is single-DOF, return the values as scalars.
240 if len(q) == 1:
241 q = q[0]
242 qd = qd[0]
243 qdd = qdd[0]
244 return (q, qd, qdd)
246 def generate(self, dt):
247 """
248 Generates a full trajectory at a specified sample time.
250 Parameters
251 ----------
252 dt : float
253 The sample time, in seconds.
255 Returns
256 -------
257 tuple(array-like, array-like, array-like, array-like)
258 The tuple of (t, q, qd, qdd) trajectory values generated at the sample time.
259 The time vector is 1D, and the others are 2D (time and dimension).
260 """
262 # Initialize the data structure
263 t_start = self.segment_times[0]
264 t_final = self.segment_times[-1]
265 t_vec = np.arange(t_start, t_final, dt)
266 if t_vec[-1] != t_final: 266 ↛ 268line 266 didn't jump to line 268 because the condition on line 266 was always true
267 t_vec = np.append(t_vec, t_final)
268 num_pts = len(t_vec)
270 q = np.zeros([self.num_dims, num_pts])
271 qd = np.zeros([self.num_dims, num_pts])
272 qdd = np.zeros([self.num_dims, num_pts])
274 for dim in range(self.num_dims):
275 traj = self.single_dof_trajectories[dim]
276 segment_idx = 0
277 t_idx = 0
278 while t_idx < num_pts:
279 t_cur = t_vec[t_idx]
280 if segment_idx < len(traj.times) - 1: 280 ↛ 283line 280 didn't jump to line 283 because the condition on line 280 was always true
281 t_next = traj.times[segment_idx + 1]
282 else:
283 t_next = t_final
284 segment_idx -= 1
285 t_prev = traj.times[segment_idx]
287 if t_cur <= t_next:
288 q_prev = traj.positions[segment_idx]
289 v_next = traj.velocities[segment_idx + 1]
290 v_prev = traj.velocities[segment_idx]
292 dt = t_cur - t_prev
293 dv = (v_next - v_prev) * dt / (t_next - t_prev)
294 q[dim, t_idx] = q_prev + (0.5 * dv + v_prev) * dt
295 qd[dim, t_idx] = v_prev + dv
296 qdd[dim, t_idx] = traj.accelerations[segment_idx]
297 t_idx += 1
298 else:
299 segment_idx += 1
301 return t_vec, q, qd, qdd
303 def visualize(
304 self,
305 dt=0.01,
306 joint_names=None,
307 show_position=True,
308 show_velocity=False,
309 show_acceleration=False,
310 ):
311 """
312 Visualizes a generated trajectory with one figure window per dimension.
314 Parameters
315 ----------
316 dt : float, optional
317 The sample time at which to generate the trajectory.
318 This is needed to produce the position curve.
319 joint_names : list[str], optional
320 The joint names to use for the figure titles.
321 If unset, uses the text "Dimension 1", "Dimension 2", etc.
322 show_position : bool, optional
323 If true, shows the trajectory position values.
324 show_velocity : bool, optional
325 If true, shows the trajectory velocity values.
326 show_acceleration : bool, optional
327 If true, shows the trajectory acceleration values.
328 """
329 import matplotlib.pyplot as plt
331 vertical_line_scale_factor = 1.2
332 t_vec, q, _, _ = self.generate(dt)
333 for dim, traj in enumerate(self.single_dof_trajectories):
334 if joint_names is not None:
335 title = joint_names[dim]
336 else:
337 title = f"Dimension {dim + 1}"
338 plt.figure(title)
340 # Positions, velocities, and accelerations
341 legend = []
342 min_pos = min_vel = min_accel = np.inf
343 max_pos = max_vel = max_accel = -np.inf
344 if show_position:
345 plt.plot(t_vec, q[dim, :])
346 min_pos = np.min(traj.positions)
347 max_pos = np.max(traj.positions)
348 legend.append("Position")
349 if show_velocity:
350 plt.plot(traj.times, traj.velocities)
351 min_vel = np.min(traj.velocities)
352 max_vel = np.max(traj.velocities)
353 legend.append("Velocity")
354 if show_acceleration:
355 plt.stairs(traj.accelerations, edges=traj.times)
356 min_accel = np.min(traj.accelerations)
357 max_accel = np.max(traj.accelerations)
358 legend.append("Acceleration")
359 plt.legend(legend)
361 # Times
362 min_val = vertical_line_scale_factor * min([min_pos, min_vel, min_accel])
363 max_val = vertical_line_scale_factor * max([max_pos, max_vel, max_accel])
364 plt.vlines(
365 self.segment_times,
366 min_val,
367 max_val,
368 colors="k",
369 linestyles=":",
370 linewidth=1.0,
371 )
373 plt.show()