Source code for skbot.inverse_kinematics.gradient_descent

from .. import transform as tf
from .targets import Target
from typing import List
import numpy as np
from scipy.optimize import minimize, OptimizeResult, Bounds
import warnings


[docs]def gd( targets: List[Target], joints: List[tf.Joint], *, rtol: float = 1e-6, maxiter: int = 500, ): """L-BFGS-B based Gradient Descent. .. note:: This function will modify the objects in ``joints`` as a side effect. Use L-BFGS-B to find values for ``joints`` such that the sum of all target scores is minimal. L-BFGS-B is a quasi-Newton method that approximates both the targets Jacobian and Hessian. Parameters ---------- targets : List[Target] A list of quality measures that a successful pose minimizes. joints : List[joint] A list of 1DoF joints which should be adjusted to minimize ``targets``. rtol : float Relative tolerance for termination. If, after one iteration, the sum of scores has not improved by more than rtol the algorithm terminates and assumes that a local optimum has been found. maxiter : int The maximum number of iterations to perform. Returns ------- joint_values : List[float] The final parameters of each joint. Notes ----- Joint limits (min/max) are enforced as hard constraints throughout the optimization. A common cause of IK faulure is that the chosen initial condition is too far away from the desired target position. One indicator for this is that :func:`gd` converges based on ``rtol``, but the score of one or more targets isn't below ``atol``. """ joint_values = np.array([l.param for l in joints]) for target in targets: target._chain = tf.simplify_links(target._chain, keep_links=joints) atols = np.array([x.atol for x in targets]) bounds = Bounds( [x.lower_limit for x in joints], [x.upper_limit for x in joints], keep_feasible=True, ) def objective_function(joint_config: np.ndarray) -> float: for joint, value in zip(joints, joint_config): joint.param = value normalized_scores = np.array([x.score() / x.atol for x in targets]) return np.sum(normalized_scores) # return np.max(normalized_scores) # check if optimization is needed skip = False for target in targets: if target.score() > target.atol: break else: skip = True if not skip: result: OptimizeResult = minimize( objective_function, joint_values, bounds=bounds, method="L-BFGS-B", options={"maxiter": maxiter, "ftol": rtol}, ) if not result.success: warnings.warn( f"L-BFGS-B terminated abnormally with message `{result.message}`." ) scores = np.array([x.score() for x in targets]) if np.any(scores > atols): raise RuntimeError( f"IK failed. Reason: Local minimum doesn't reach one or more targets." ) for joint, value in zip(joints, result.x): joint.param = value joint_values = np.array([j.param for j in joints]) return joint_values