Coverage for src/pyroboplan/planning/cartesian_planner.py: 92%
92 statements
« prev ^ index » next coverage.py v7.6.12, created at 2025-03-02 22:03 -0500
1"""Utilities for Cartesian (or task space) motion planning."""
3import numpy as np
4import pinocchio
5from scipy.spatial.transform import Rotation, Slerp
7from pyroboplan.trajectory.trapezoidal_velocity import TrapezoidalVelocityTrajectory
10class CartesianPlannerOptions:
11 """Options for Cartesian planning."""
13 def __init__(
14 self,
15 use_trapezoidal_scaling=True,
16 max_linear_velocity=1.0,
17 max_linear_acceleration=1.0,
18 max_angular_velocity=1.0,
19 max_angular_acceleration=1.0,
20 ):
21 """
22 Initializes a set of Cartesian planning options.
24 Parameters
25 ----------
26 use_trapezoidal_scaling : bool
27 If True, uses trapezoidal velocity time scaling.
28 Otherwise, uses linear time scaling.
29 max_linear_velocity : float
30 The maximum linear velocity magnitude, in meters per second.
31 max_linear_acceleration : float
32 The maximum linear acceleration magnitude, in meters per second squared.
33 max_angular_velocity : float
34 The maximum angular velocity magnitude, in radians per second.
35 max_angular_acceleration : float
36 The maximum angular acceleration magnitude, in radians per second squared.
37 """
38 self.use_trapezoidal_scaling = use_trapezoidal_scaling
40 if max_linear_velocity <= 0:
41 raise ValueError("Max linear velocity must be positive.")
42 self.max_linear_velocity = max_linear_velocity
43 if max_linear_acceleration <= 0:
44 raise ValueError("Max linear acceleration must be positive.")
45 self.max_linear_acceleration = max_linear_acceleration
46 if max_angular_velocity <= 0:
47 raise ValueError("Max angular velocity must be positive.")
48 self.max_angular_velocity = max_angular_velocity
49 if max_angular_acceleration <= 0:
50 raise ValueError("Max angular acceleration must be positive.")
51 self.max_angular_acceleration = max_angular_acceleration
54class CartesianPlanner:
55 """
56 Cartesian (or task space) motion planner.
58 This tool will plan a Cartesian trajectory through a set of poses,
59 and then use an inverse kinematics solver to incrementally solve for valid
60 robot joint configurations to achieve this task-space trajectory.
62 Some good resources:
63 * Slides: https://www.diag.uniroma1.it/~deluca/rob1_en/14_TrajectoryPlanningCartesian.pdf
64 * PickNik blog: https://picknik.ai/cartesian%20planners/moveit/motion%20planning/2021/01/07/guide-to-cartesian-planners-in-moveit.html
65 """
67 def __init__(
68 self, model, target_frame, tforms, ik_solver, options=CartesianPlannerOptions()
69 ):
70 """
71 Initializes a Cartesian motion planner instance.
73 Parameters
74 ----------
75 model : `pinocchio.Model`
76 The model to use for motion planning.
77 target_frame : str
78 The name of the target frame in the model defining the Cartesian motion.
79 tforms : list[`pinocchio.SE3`]
80 A list of pose waypoints, in SE3, for the target frame to move through.
81 ik_solver : any
82 Any valid inverse kinematics solver from this library.
83 options : `pyroboplan.planning.cartesian_planner.CartesianPlannerOptions`, optional
84 A set of Cartesian planning options.
85 If not set, a default set of options will be used.
86 """
87 self.model = model
88 self.target_frame = target_frame
89 self.tforms = tforms
90 self.ik_solver = ik_solver
91 self.options = options
93 # Generate the segments
94 cur_time = 0.0
95 self.segments = []
96 for idx in range(1, len(tforms)):
97 tform_start = tforms[idx - 1]
98 tform_end = tforms[idx]
100 # Figure out a time scaling.
101 tform_diff = tform_end.actInv(tform_start)
102 linear_distance = np.linalg.norm(tform_diff.translation)
103 angular_distance = Rotation.from_matrix(tform_diff.rotation).magnitude()
105 if options.use_trapezoidal_scaling: 105 ↛ 139line 105 didn't jump to line 139 because the condition on line 105 was always true
106 # Use trapezoidal scaling limited by both the linear and angular velocities and accelerations.
107 if linear_distance > 0: 107 ↛ 115line 107 didn't jump to line 115 because the condition on line 107 was always true
108 normalized_linear_accel = (
109 options.max_linear_acceleration / linear_distance
110 )
111 normalized_linear_vel = (
112 options.max_linear_velocity / linear_distance
113 )
114 else:
115 normalized_linear_accel = normalized_linear_vel = np.inf
117 if angular_distance > 0:
118 normalized_angular_accel = (
119 options.max_angular_acceleration / angular_distance
120 )
121 normalized_angular_vel = (
122 options.max_angular_velocity / angular_distance
123 )
124 else:
125 normalized_angular_accel = normalized_angular_vel = np.inf
127 max_normalized_accel = min(
128 normalized_linear_accel, normalized_angular_accel
129 )
130 max_normalized_vel = min(normalized_linear_vel, normalized_angular_vel)
132 traj = TrapezoidalVelocityTrajectory(
133 np.array([0.0, 1.0]), max_normalized_vel, max_normalized_accel
134 )
135 final_time = cur_time + traj.segment_times[-1]
136 self.segments.append((traj, final_time))
137 else:
138 # Use constant velocity at the specified max
139 constant_vel_time = max(
140 linear_distance / options.max_linear_velocity,
141 angular_distance / options.max_angular_velocity,
142 )
143 final_time = cur_time + constant_vel_time
144 self.segments.append((None, final_time))
146 cur_time = final_time
148 def generate(self, q_init, dt):
149 """
150 Generates a set of joint trajectories from the Cartesian plan at a specified sample time.
152 Parameters
153 ----------
154 q_init : array-like
155 The starting joint configuration for the robot.
156 This is used as the initial condition for solving inverse kinematics.
157 dt : float
158 The sample time to use for trajectory generation, in seconds.
160 Returns
161 -------
162 tuple(bool, array-like, array-like)
163 The tuple of (success, t_vec, q_vec) corresponding to a success metric,
164 the vector of times, and vector of joint positions, respectively.
165 """
166 self.generated_tforms = []
168 t_start = 0
169 t_final = self.segments[-1][1]
170 t_vec = np.arange(t_start, t_final, dt)
171 if t_vec[-1] != t_final: 171 ↛ 173line 171 didn't jump to line 173 because the condition on line 171 was always true
172 t_vec = np.append(t_vec, t_final)
173 num_pts = len(t_vec)
175 q = np.zeros([self.model.nq, num_pts])
176 q[:, 0] = q_init
178 q_cur = q_init
179 idx = 0
180 segment_idx = 0
181 t_prev = 0
182 t_final_cur = 0
184 while idx < num_pts:
185 t = t_vec[idx]
187 traj_cur, t_final_cur = self.segments[segment_idx]
188 tform_prev = self.tforms[segment_idx]
189 tform_next = self.tforms[segment_idx + 1]
190 slerp = Slerp(
191 [0.0, 1.0],
192 Rotation.concatenate(
193 [
194 Rotation.from_matrix(tform_prev.rotation),
195 Rotation.from_matrix(tform_next.rotation),
196 ]
197 ),
198 )
200 if t <= t_final_cur:
201 # Find the normalized interpolation time.
202 dt = t - t_prev
203 if self.options.use_trapezoidal_scaling: 203 ↛ 207line 203 didn't jump to line 207 because the condition on line 203 was always true
204 dt = min(dt, traj_cur.segment_times[-1]) # Avoids numerical issues?
205 alpha, _, _ = traj_cur.evaluate(dt)
206 else:
207 alpha = dt / (t_final_cur - t_prev)
209 # Interpolate position and rotation separately.
210 p = (
211 alpha * tform_next.translation
212 + (1.0 - alpha) * tform_prev.translation
213 )
214 R = slerp(alpha).as_matrix()
215 tform = pinocchio.SE3(R, p)
216 self.generated_tforms.append(tform)
218 # Solve IK.
219 q_sol = self.ik_solver.solve(self.target_frame, tform, init_state=q_cur)
220 if q_sol is not None:
221 q[:, idx] = q_sol
222 else:
223 print(f"Failed to solve on point {idx + 1} out of {num_pts}")
224 return False, t_vec, q
226 idx += 1
227 else:
228 segment_idx += 1
229 t_prev = t_final_cur
231 return True, t_vec, q