Inverse Kinematics Algorithms
The inverseKinematics
and generalizedInverseKinematics
classes give you access to inverse kinematics
(IK) algorithms. You can use these algorithms to generate a robot configuration that
achieves specified goals and constraints for the robot. This robot configuration is a list
of joint positions that are within the position limits of the robot model and do not violate
any constraints the robot has.
Choose an Algorithm
MATLAB® supports three algorithms for achieving an IK solution: the BFGS
projection algorithm, the Levenberg-Marquardt algorithm, and the
fmincon
sequential quadratic programming (SQP) algorithm. All
three algorithms are iterative, gradient-based optimization methods that start from an
initial guess at the solution and seek to minimize a specific cost function. If either
algorithm converges to a configuration where the cost is close to zero within a
specified tolerance, it has found a solution to the inverse kinematics problem. However,
for some combinations of initial guesses and desired end effector poses, the algorithm
may exit without finding an ideal robot configuration. To handle this, the algorithm
utilizes a random restart mechanism. If enabled, the random restart mechanism restarts
the iterative search from a random robot configuration whenever that search fails to
find a configuration that achieves the desired end effector pose. These random restarts
continue until either a qualifying IK solution is found, the maximum time has elapsed,
or the iteration limit is reached.
The fmincon
SQP algorithm is particularly well-suited for handling
constraints in the optimization problem, making it a robust choice for complex IK
scenarios with multiple constraints.
To set your algorithm, specify the SolverAlgorithm
property as
"BFGSGradientProjection"
,
"LevenbergMarquardt"
, or "fminconsqp"
.
BFGS Gradient Projection
The Broyden-Fletcher-Goldfarb-Shanno (BFGS) gradient projection algorithm is a quasi-Newton method that uses the gradients of the cost function from past iterations to generate approximate second-derivative information. The algorithm uses this second-derivative information in determining the step to take in the current iteration. A gradient projection method is used to deal with boundary limits on the cost function that the joint limits of the robot model create. The direction calculated is modified so that the search direction is always valid.
This method is the default algorithm and is more robust at finding solutions than the Levenberg-Marquardt method. It is more effective for configurations near joint limits or when the initial guess is not close to the solution. If your initial guess is close to the solution and a quicker solution is needed, consider either the fmincon SQP or Levenberg-Marquardt methods.
Levenberg-Marquardt
The Levenberg-Marquardt (LM) algorithm variant used in the
InverseKinematics
class is an error-damped least-squares
method. The error-damped factor helps to prevent the algorithm from escaping a local
minimum. The LM algorithm is optimized to converge much faster if the initial guess
is close to the solution. However the algorithm does not handle arbitrary initial
guesses well. Consider using this algorithm for finding IK solutions for a series of
poses along a desired trajectory of the end effector. Once a robot configuration is
found for one pose, that configuration is often a good initial guess at an IK
solution for the next pose in the trajectory. In this situation, the LM or
fmincon
SQP algorithms may yield faster results. Otherwise,
use the BFGS Gradient Projection instead.
fmincon
SQP
The fmincon
algorithm variant used for inverse kinematics
solvers is the sequential quadratic programming (SQP) method, which excels at
solving nonlinear objective functions. This method requires fewer iterations to
converge on a solution than the BFGS projection or Levenberg-Marquardt algorithm,
without the need for random restarts. For more information about the
fmincon
SQP solver, see the fmincon
(Optimization Toolbox) function.
This algorithm requires Optimization Toolbox™.
Solver Parameters
Each algorithm has specific tunable parameters to improve solutions.
These parameters are specified in the SolverParameters
property
of the object.
BFGS Gradient Projection
The solver parameters structure for the BFGS algorithm has these fields:
MaxIterations
— Maximum number of iterations allowed. The default is 1500.GradientTolerance
— Threshold on the gradient of the cost function. The algorithm stops if the magnitude of the gradient falls below this threshold. Must be a positive scalar.SolutionTolerance
— Threshold on the magnitude of the error between the end-effector pose generated from the solution and the desired pose. The weights specified for each component of the pose in the object are included in this calculation. Must be a positive scalar.EnforceJointLimits
— Indicator if joint limits are considered in calculating the solution.JointLimits
is a property of the robot model inrigidBodyTree
. By default, joint limits are enforced.AllowRandomRestarts
— Indicator if random restarts are allowed. Random restarts are triggered when the algorithm approaches a solution that does not satisfy the constraints. A randomly generated initial guess is used.MaxIteration
andMaxTime
are still obeyed. By default, random restarts are enabled.StepTolerance
— Minimum step size allowed by the solver. Smaller step sizes usually mean that the solution is close to convergence. The default is 1e–14.MaxTime
— Maximum number of seconds that the algorithm runs before timing out. The default is 10.
Levenberg-Marquardt
The solver parameters structure for the LM algorithm has these extra fields in addition to what the BFGS Gradient Projection method requires:
ErrorChangeTolerance
— Threshold on the change in end-effector pose error between iterations. The algorithm returns if the changes in all elements of the pose error are smaller than this threshold. Must be a positive scalar.DampingBias
— A constant term for damping. The LM algorithm has a damping feature controlled by this constant that works with the cost function to control the rate of convergence. To disable damping, use theUseErrorDamping
parameter.UseErrorDamping
— 1 (default), Indicator of whether damping is used. Set this parameter tofalse
to disable dampening.
fmincon
SQP
The solver parameters structure for the fmincon
SQP algorithm
has these fields:
MaxIterations
— Maximum number of iterations allowed. The default is 1500. This maps to theMaxIterations
option of thefmincon
function.SolutionTolerance
— Threshold on the magnitude of the error between the end-effector pose generated from the solution and the desired pose. This calculation includes the weights specified for each component of the pose in the object. Specify this as a positive scalar. This maps to theObjectiveLimit
option of thefmincon
function.StepTolerance
— Minimum step size allowed by the solver. Smaller step sizes usually result in a solution closer to convergence. The default is 1e–6. This maps to theStepTolerance
option of thefmincon
function.GradientTolerance
— Threshold on the gradient of the cost function. The algorithm stops if the magnitude of the gradient falls below this threshold. Specify this as a positive scalar. This maps to theOptimalityTolerance
option of thefmincon
function.EnforceJointLimits
— Indicates whether to consider joint limits when calculating the solution.JointLimits
is a property ofrigidBodyTree
robot models. By default, the algorithm enforces joint limits. This maps to thelb
(Optimization Toolbox) andub
(Optimization Toolbox) arguments of thefmincon
function.ConstraintTolerance
— Threshold on constraint violations. This indicates how much violation in joint position limits the solver allows. The default is 1e-6 and must be a nonnegative numeric scalar. This maps to theConstraintTolerance
option of thefmincon
function.
For more information about the fmincon
options that these
parameters map to, see the options
(Optimization Toolbox) argument of the
fmincon
function.
Solution Information
While using the inverse kinematics algorithms, each call on the object returns solution information about how the algorithm performed. The solution information is provided as a structure with the following fields:
Iterations
— Number of iterations run by the algorithm.NumRandomRestarts
— Number of random restarts because algorithm got stuck in a local minimum.PoseErrorNorm
— The magnitude of the pose error for the solution compared to the desired end effector pose.ExitFlag
— Code that gives more details on the algorithm execution and what caused it to return. For the exit flags of each algorithm type, see Exit Flags.Status
— Character vector describing whether the solution is within the tolerance ('success'
) or the best possible solution the algorithm could find ('best available'
).
Exit Flags
In the solution information, the exit flags give more details on the execution of
the specific algorithm. Look at the Status
property of the object
to find out if the algorithm was successful. Each exit flag code has a defined
description.
"BFGSGradientProjection"
algorithm exit flags:
1
— Local minimum found.2
— Maximum number of iterations reached.3
— Algorithm timed out during operation.4
— Minimum step size. The step size is below theStepToleranceSize
field of theSolverParameters
property.5
— No exit flag. Relevant to"LevenbergMarquardt"
algorithm only.6
— Search direction invalid.7
— Hessian is not positive semidefinite.
"LevenbergMarquardt"
algorithm exit flags:
1
— Local minimum found.2
— Maximum number of iterations reached.3
— Algorithm timed out during operation.4
— Minimum step size. The step size is below theStepToleranceSize
field of theSolverParameters
property.5
— The change in end-effector pose error is below theErrorChangeTolerance
field of theSolverParameters
property.
"fminconsqp"
algorithm exit flags:
1
— First-order optimality measure was less thanGradientTolerance
, and maximum constraint violation was less thanConstraintTolerance
.0
— Number of iterations exceededMaxIterations
, or number of function evaluations exceeded maximum default value for MaxFunctionEvaluations (Optimization Toolbox).-1
— Stopped by an output function or plot function.-2
— No feasible solution was found.-3
— Objective function at current iteration went belowSolutionTolerance
, and maximum constraint violation was less thanConstraintTolerance
.
See the exitflag
(Optimization Toolbox) argument of the
fmincon
function for more information.
References
[1] Badreddine, Hassan, Stefan Vandewalle, and Johan Meyers. "Sequential Quadratic Programming (SQP) for Optimal Control in Direct Numerical Simulation of Turbulent Flow." Journal of Computational Physics. 256 (2014): 1–16. doi:10.1016/j.jcp.2013.08.044.
[2] Bertsekas, Dimitri P. Nonlinear Programming. Belmont, MA: Athena Scientific, 1999.
[3] Goldfarb, Donald. "Extension of Davidon’s Variable Metric Method to Maximization Under Linear Inequality and Equality Constraints." SIAM Journal on Applied Mathematics. Vol. 17, No. 4 (1969): 739–64. doi:10.1137/0117067.
[4] Nocedal, Jorge, and Stephen Wright. Numerical Optimization. New York, NY: Springer, 2006.
[5] Sugihara, Tomomichi. "Solvability-Unconcerned Inverse Kinematics by the Levenberg–Marquardt Method." IEEE Transactions on Robotics Vol. 27, No. 5 (2011): 984–91. doi:10.1109/tro.2011.2148230.
[6] Zhao, Jianmin, and Norman I. Badler. "Inverse Kinematics Positioning Using Nonlinear Programming for Highly Articulated Figures." ACM Transactions on Graphics Vol. 13, No. 4 (1994): 313–36. doi:10.1145/195826.195827.
See Also
rigidBodyTree
| generalizedInverseKinematics
| inverseKinematics