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.
MATLAB® supports two algorithms for achieving an IK solution: the BFGS projection algorithm and the Levenberg-Marquardt algorithm. Both 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.
To set your algorithm, specify the SolverAlgorithm property
as either 'BFGSGradientProjection' or 'LevenbergMarquardt'.
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 the Levenberg-Marquardt method.
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 algorithm may yield faster
results. Otherwise, use the BFGS Gradient Projection instead.
Each algorithm has specific tunable parameters to improve solutions.
These parameters are specified in the SolverParameters property
of the object.
The solver parameters for the BFGS algorithm have the following fields:
MaxIterations — Maximum
number of iterations allowed. The default is 1500.
MaxTime — Maximum number
of seconds that the algorithm runs before timing out. The default
is 10.
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 in rigidBodyTree. 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 and MaxTime 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 10–14.
The solver parameters for the LM algorithm have the following 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 the UseErrorDamping parameter.
UseErrorDamping — 1 (default),
Indicator of whether damping is used. Set this parameter to false to
disable dampening.
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').
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 the StepToleranceSize field
of the SolverParameters 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 the StepToleranceSize field
of the SolverParameters property.
5 — The change in end-effector
pose error is below the ErrorChangeTolerance field
of the SolverParameters property.
[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.
generalizedInverseKinematics | inverseKinematics | rigidBodyTree