Coverage for src/pyroboplan/ik/differential_ik.py: 86%
110 statements
« prev ^ index » next coverage.py v7.6.12, created at 2025-03-02 22:03 -0500
1"""Utilities for differential IK."""
3import numpy as np
4import pinocchio
5import time
7from ..core.utils import (
8 check_collisions_at_state,
9 check_within_limits,
10 get_random_state,
11 get_random_collision_free_state,
12)
13from ..visualization.meshcat_utils import visualize_frame
15VIZ_INITIAL_RENDER_TIME = 0.5
16VIZ_SLEEP_TIME = 0.025
19class DifferentialIkOptions:
20 """Options for differential IK."""
22 def __init__(
23 self,
24 max_iters=200,
25 max_retries=10,
26 max_translation_error=1e-3,
27 max_rotation_error=1e-3,
28 damping=1e-3,
29 min_step_size=0.1,
30 max_step_size=0.5,
31 ignore_joint_indices=[],
32 joint_weights=None,
33 rng_seed=None,
34 ):
35 """
36 Initializes a set of differential IK options.
38 Parameters
39 ----------
40 max_iters : int
41 Maximum number of iterations per try.
42 max_retries : int
43 Maximum number of retries with random restarts.
44 If set to 0, only the initial state provided will be used.
45 max_translation_error : float
46 Maximum translation error, in meters, to consider IK solved.
47 max_rotation_error : float
48 Maximum rotation error, in radians, to consider IK solved.
49 damping : float
50 Damping value, between 0 and 1, for the Jacobian pseudoinverse.
51 Setting this to a nonzero value is using Levenberg-Marquardt.
52 min_step_size : float
53 Minimum gradient step size, between 0 and 1, based on ratio of current distance to target to initial distance to target.
54 To use a fixed step size, set both minimum and maximum values to be equal.
55 max_step_size : float
56 Maximum gradient step size, between 0 and 1, based on ratio of current distance to target to initial distance to target.
57 To use a fixed step size, set both minimum and maximum values to be equal.
58 joint_weights : list[float], optional
59 A list of relative weights for different joints, used in computing the Jacobian pseudoinverse.
60 If your robot has redundant joints, assigning a higher weight to some joints will cause them to move less than lower weight joints.
61 If not specified, all joints are weighted equally with unit weight.
62 ignore_joint_indices : list[int], optional
63 A list of joints to ignore changing when solving IK.
64 TODO: This should eventually be done through a concept of joint groups.
65 rng_seed : int, optional
66 Sets the seed for random number generation. Use to generate deterministic results.
67 """
68 self.max_iters = max_iters
69 self.max_retries = max_retries
70 self.max_translation_error = max_translation_error
71 self.max_rotation_error = max_rotation_error
72 self.damping = damping
73 self.min_step_size = min_step_size
74 self.max_step_size = max_step_size
75 self.joint_weights = joint_weights
76 self.ignore_joint_indices = ignore_joint_indices
77 self.rng_seed = rng_seed
80class DifferentialIk:
81 """
82 Differential IK solver.
84 This is a numerical IK solver that uses the manipulator's Jacobian to take first-order steps towards a solution.
85 It contains several of the common options such as damped least squares (Levenberg-Marquardt), random restarts, and nullspace projection.
87 Some good resources:
88 * https://motion.cs.illinois.edu/RoboticSystems/InverseKinematics.html
89 * https://homes.cs.washington.edu/~todorov/courses/cseP590/06_JacobianMethods.pdf
90 * https://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf
91 * http://www.diag.uniroma1.it/deluca/rob2_en/02_KinematicRedundancy_1.pdf
92 """
94 def __init__(
95 self,
96 model,
97 collision_model=None,
98 data=None,
99 collision_data=None,
100 visualizer=None,
101 options=DifferentialIkOptions(),
102 ):
103 """
104 Creates an instance of a DifferentialIk solver.
106 Parameters
107 ----------
108 model : `pinocchio.Model`
109 The model to use for this solver.
110 collision_model : `pinocchio.Model`, optional
111 The model to use for collision checking. If None, no collision checking takes place.
112 data : `pinocchio.Data`, optional
113 The model data to use for this solver. If None, data is created automatically.
114 collision_data : `pinocchio.GeometryData`, optional
115 The collision_model data to use for this solver. If None, data is created automatically.
116 visualizer : `pinocchio.visualize.meshcat_visualizer.MeshcatVisualizer`, optional
117 The visualizer to use for this solver.
118 options : `DifferentialIkOptions`, optional
119 The options to use for solving IK. If not specified, default options are used.
120 """
121 self.model = model
122 self.collision_model = collision_model
124 if not data:
125 data = model.createData()
126 self.data = data
127 if not collision_data and self.collision_model is not None:
128 collision_data = collision_model.createData()
129 self.collision_data = collision_data
131 self.visualizer = visualizer
132 self.options = options
134 def solve(
135 self,
136 target_frame,
137 target_tform,
138 init_state=None,
139 nullspace_components=[],
140 verbose=False,
141 ):
142 """
143 Solves an IK query.
145 Parameters
146 ----------
147 target_frame : str
148 The name of the target frame in the model.
149 target_tform : `pinocchio.SE3`
150 The desired transformation of the target frame in the model.
151 init_state : array-like, optional
152 The initial state to solve from. If not specified, a random initial state will be selected.
153 nullspace_components : list[function], optional
154 An optional list of nullspace components to use when solving.
155 These components must take the form `lambda model, q: component(model, q, <other_args>)`.
156 verbose : bool, optional
157 If True, prints additional information to the console.
159 Returns
160 -------
161 array-like or None
162 A list of joint configuration values with the solution, if one was found. Otherwise, returns None.
163 """
164 np.random.seed(self.options.rng_seed)
165 target_frame_id = self.model.getFrameId(target_frame)
167 # Get the active joint indices.
168 active_joint_indices = [
169 idx
170 for idx in range(self.model.nq)
171 if idx not in self.options.ignore_joint_indices
172 ]
173 num_active_joints = len(active_joint_indices)
175 # Create the joint weights.
176 if self.options.joint_weights is None:
177 # Use identity weights if they are not specified.
178 W = np.eye(num_active_joints)
179 elif len(self.options.joint_weights) != num_active_joints:
180 raise ValueError(
181 f"Joint weights, if specified, must have {num_active_joints} elements."
182 )
183 elif np.any(np.array(self.options.joint_weights) <= 0.0):
184 raise ValueError(f"All joint weights must be strictly positive.")
185 else:
186 # Invert the weights so that higher weight means less joint motion.
187 W = np.linalg.inv(np.diag(self.options.joint_weights))
189 # Create a random initial state, if not specified
190 if init_state is None:
191 init_state = get_random_state(self.model)
192 if self.visualizer: 192 ↛ 193line 192 didn't jump to line 193 because the condition on line 192 was never true
193 self.visualizer.displayFrames(True, frame_ids=[target_frame_id])
194 visualize_frame(self.visualizer, "ik_target_pose", target_tform)
196 # Initialize IK
197 solved = False
198 n_tries = 0
199 q_cur = init_state
200 initial_error_norm = None
202 if self.visualizer: 202 ↛ 203line 202 didn't jump to line 203 because the condition on line 202 was never true
203 self.visualizer.display(q_cur)
204 time.sleep(VIZ_INITIAL_RENDER_TIME) # Needed to render
206 while n_tries <= self.options.max_retries:
207 n_iters = 0
208 while n_iters < self.options.max_iters:
209 # Compute forward kinematics at the current state
210 pinocchio.framesForwardKinematics(self.model, self.data, q_cur)
211 cur_tform = self.data.oMf[target_frame_id]
213 # Check the error using actInv
214 error = target_tform.actInv(cur_tform)
215 error = -pinocchio.log(error).vector
216 if (
217 np.linalg.norm(error[:3]) < self.options.max_translation_error
218 and np.linalg.norm(error[3:]) < self.options.max_rotation_error
219 ):
220 # Wrap to the range -/+ pi, and then check joint limits and collision.
221 q_cur = (q_cur + np.pi) % (2 * np.pi) - np.pi
222 if check_within_limits(self.model, q_cur):
223 if self.collision_model is not None:
224 if check_collisions_at_state(
225 self.model,
226 self.collision_model,
227 q_cur,
228 self.data,
229 self.collision_data,
230 ):
231 if verbose: 231 ↛ 232line 231 didn't jump to line 232 because the condition on line 231 was never true
232 print(
233 "Solved within joint limits, but in collision."
234 )
235 else:
236 solved = True
237 if verbose: 237 ↛ 238line 237 didn't jump to line 238 because the condition on line 237 was never true
238 print(
239 "Solved, within joint limits, and collision-free!"
240 )
241 else:
242 solved = True
243 if verbose: 243 ↛ 244line 243 didn't jump to line 244 because the condition on line 243 was never true
244 print("Solved within joint limits!")
245 else:
246 if verbose: 246 ↛ 247line 246 didn't jump to line 247 because the condition on line 246 was never true
247 print("Solved, but outside joint limits.")
248 break
250 # Calculate the Jacobian for the active joints.
251 J = pinocchio.computeFrameJacobian(
252 self.model,
253 self.data,
254 q_cur,
255 target_frame_id,
256 pinocchio.ReferenceFrame.LOCAL,
257 )[:, active_joint_indices]
259 # Compute the (optionally damped and weighted) Jacobian pseudoinverse.
260 jjt = (J @ W @ J.T) + self.options.damping**2 * np.eye(6)
262 # Compute the gradient descent step size.
263 error_norm = np.linalg.norm(error)
264 if not initial_error_norm:
265 initial_error_norm = error_norm
266 alpha = self.options.min_step_size + (
267 1.0 - error_norm / initial_error_norm
268 ) * (self.options.max_step_size - self.options.min_step_size)
270 # Gradient descent step
271 if not nullspace_components:
272 q_step = alpha * W @ J.T @ np.linalg.solve(jjt, error)
273 else:
274 nullspace_term = sum(
275 [
276 comp(self.model, q_cur)[active_joint_indices]
277 for comp in nullspace_components
278 ]
279 )
280 q_step = alpha * (
281 W @ J.T @ (np.linalg.solve(jjt, error - J @ (nullspace_term)))
282 + nullspace_term
283 )
285 # Zero out the values for the ignored indices before returning.
286 for q, idx in zip(q_step, active_joint_indices):
287 q_cur[idx] += q
289 n_iters += 1
291 # Protect against numerical instability.
292 if np.any(np.isinf(q_cur)): 292 ↛ 293line 292 didn't jump to line 293 because the condition on line 292 was never true
293 print(f"Terminating due to numerical instability.")
294 break
296 if self.visualizer: 296 ↛ 297line 296 didn't jump to line 297 because the condition on line 296 was never true
297 self.visualizer.display(q_cur)
298 time.sleep(VIZ_SLEEP_TIME)
300 # Check results at the end of this try
301 if solved:
302 if verbose: 302 ↛ 303line 302 didn't jump to line 303 because the condition on line 302 was never true
303 print(f"Solved in {n_tries+1} tries.")
304 break
305 else:
306 if self.collision_model is not None:
307 q_cur = get_random_collision_free_state(
308 self.model, self.collision_model
309 )
310 else:
311 q_cur = get_random_state(self.model)
312 n_tries += 1
313 if verbose: 313 ↛ 314line 313 didn't jump to line 314 because the condition on line 313 was never true
314 print(f"Retry {n_tries}")
316 # Check final results
317 if solved:
318 return q_cur
319 else:
320 return None