inverseKinematics

Create inverse kinematic solver

Description

The inverseKinematics System object™ creates an inverse kinematic (IK) solver to calculate joint configurations for a desired end-effector pose based on a specified rigid body tree model. Create a rigid body tree model for your robot using the rigidBodyTree class. This model defines all the joint constraints that the solver enforces. If a solution is possible, the joint limits specified in the robot model are obeyed.

To specify more constraints besides the end-effector pose, including aiming constraints, position bounds, or orientation targets, consider using generalizedInverseKinematics. This object allows you to compute multiconstraint IK solutions.

For closed-form analytical IK solutions, see analyticalInverseKinematics.

To compute joint configurations for a desired end-effector pose:

1. Create the inverseKinematics object and set its properties.

2. Call the object with arguments, as if it were a function.

To learn more about how System objects work, see What Are System Objects?

Creation

Syntax

ik = inverseKinematics

ik = inverseKinematics(PropertyName=Value)

Description

ik = inverseKinematics creates an inverse kinematic solver. To use the solver, specify a rigid body tree model in the RigidBodyTree property.

ik = inverseKinematics(PropertyName=Value) specifies properties of the inverse kinematics solver using one or more optional name-value arguments. For example, SolverAlgorithm="fminconsqp" uses the fmincon SQP solver as the inverse kinematics solver.

Properties

Unless otherwise indicated, properties are nontunable, which means you cannot change their values after calling the object. Objects lock when you call them, and the release function unlocks them.

If a property is tunable, you can change its value at any time.

For more information on changing property values, see System Design in MATLAB Using System Objects.

RigidBodyTree — Rigid body tree model

rigidBodyTree object

Rigid body tree model, specified as a rigidBodyTree object. If you modify your rigid body tree model, reassign the rigid body tree to this property. For example:

Create IK solver and specify the rigid body tree.

ik = inverseKinematics('RigidBodyTree',rigidbodytree)

Modify the rigid body tree model.

addBody(rigidbodytree,rigidBody('body1'),'base')

Reassign the rigid body tree to the IK solver. If the solver or the step function is called before modifying the rigid body tree model, use release to allow the property to be changed.

ik.RigidBodyTree = rigidbodytree;

SolverAlgorithm — Algorithm for solving inverse kinematics

"BFGSGradientProjection" (default) | "LevenbergMarquardt" | "fminconsqp"

Algorithm for solving inverse kinematics, specified as "BFGSGradientProjection", "LevenbergMarquardt", or "fminconsqp". For details on each algorithm, see Inverse Kinematics Algorithms.

The "fminconsqp" algorithm requires Optimization Toolbox™.

SolverParameters — Parameters associated with algorithm

structure

Parameters associated with the specified algorithm, specified as a structure. The fields in the structure are specific to the algorithm. See Solver Parameters.

Usage

Syntax

[configSol,solInfo] = ik(endeffector,pose,weights,initialguess)

Description

[configSol,solInfo] = ik(endeffector,pose,weights,initialguess) finds a joint configuration that achieves the specified end-effector pose. Specify an initial guess for the configuration and your desired weights on the tolerances for the six components of pose. Solution information related to execution of the algorithm, solInfo, is returned with the joint configuration solution, configSol.

Input Arguments

endeffector — End-effector name

character vector

End-effector name, specified as a character vector. The end effector must be a frame on the rigidBodyTree object specified in the inverseKinematics System object.

pose — End-effector pose

4-by-4 homogeneous transform

End-effector pose, specified as a 4-by-4 homogeneous transform. This transform defines the desired position and orientation of the rigid body specified in the endeffector property.

weights — Weight for pose tolerances

six-element vector

Weight for pose tolerances, specified as a six-element vector. The first three elements correspond to the weights on the error in orientation for the desired pose. The last three elements correspond to the weights on the error in xyz position for the desired pose.

initialguess — Initial guess of robot configuration

structure array | vector

Initial guess of robot configuration, specified as a structure array or vector. Use this initial guess to help guide the solver to a desired robot configuration. The solution is not guaranteed to be close to this initial guess.

To use the vector form, set the DataFormat property of the object assigned in the RigidBodyTree property to either "row" or "column".

Output Arguments

configSol — Robot configuration solution

structure array | vector

Robot configuration, returned as a structure array. The structure array contains these fields:

• JointName — Character vector for the name of the joint specified in the RigidBodyTree robot model

• JointPosition — Position of the corresponding joint

This joint configuration is the computed solution that achieves the desired end-effector pose within the solution tolerance.

Note

For revolute joints, if the joint limits exceed a range of 2*pi, where joint position wrapping occurs, then the returned joint position is the one closest to the joint's lower bound.

To use the vector form, set the DataFormat property of the object assigned in the RigidBodyTree property to either "row" or "column".

solInfo — Solution information

structure

Solution information, returned as a structure. The solution information structure contains these 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').

Object Functions

To use an object function, specify the System object as the first input argument. For example, to release system resources of a System object named obj, use this syntax:

release(obj)

Common to All System Objects

step	Run System object algorithm
release	Release resources and allow changes to System object property values and input characteristics
reset	Reset internal states of System object

Examples

Solve Inverse Kinematics Using fmincon SQP

Load a robot and create an inverse kinematics solver for it. Set the solver algorithm to the fmincon SQP algorithm.

robot = loadrobot("universalUR5",DataFormat="row");
ik = inverseKinematics(RigidBodyTree=robot,SolverAlgorithm="fminconsqp");

Set the last body of the robot as the end effector, set a target pose, set the weights, and set the initial guess configuration.

ee = robot.BodyNames{end};
poseTarget = se3([0 pi/2 -pi/2],"eul","ZYX",[0 0.7 0.3]);
weights = [1 1 1 0.8 0.8 0.8];
initGuessConfig = [pi/2 0 0 0 0 0];

Show the robot in the initial guess configuration, and plot the target pose.

show(robot,initGuessConfig);
axis([-0.5 0.5 -0.1 0.9 -0.1 0.8])
hold on
plotTransforms(poseTarget,FrameSize=0.2);
title("Initial Guess Configuration and Pose Target")

Solve for a configuration that reaches the pose target using the specified weights and initial guess configuration.

[config,solninfo] = ik(ee,tform(poseTarget),weights,initGuessConfig);
show(robot,config,PreservePlot=false);
title("End-Effector Target Pose Achieved")
hold off

solninfo.Status

ans = 
'success'

Generate Joint Positions to Achieve End-Effector Position

Load a PUMA 560 manipulator from the Robotics System Toolbox™ loadrobot and show the robot model in a figure.

puma = loadrobot("puma560");
show(puma,homeConfiguration(puma),PreservePlot=false);
axis([-1 1 -1 1 0 2])
title("PUMA 560 Home Configuration")

The desired position for the end effector of the robot to reach is [-0.5 0.5 0.75]. Inverse Kinematics uses a desired pose to solve for joint configurations, so first convert the position into an SE(3) homogeneous transformation matrix with the desired translation. Then plot the pose transformation.

pos = [-0.5 0.5 0.75];
poseTF = trvec2tform(pos);
hold on
plotTransforms(pos,eul2quat([0 0 0]),FrameSize=0.2);
title("PUMA 560 and End-Effector Target Position")

Create an inverseKinematics System object™ for the puma robot model. Specify weights for the rotation and the position of the pose. Because the goal is for the end-effector to reach that position with no constraint on orientation, set orientation-angle weights to zero so that the orientation solution does not matter to the IK solver. Use the home configuration of the robot as an initial guess.

ik = inverseKinematics("RigidBodyTree",puma);
weights = [0 0 0 1 1 1];
initialguess = homeConfiguration(puma);

Calculate the joint positions using the ik System object. Use the last link in the robot model, link7, as the end effector.

[configSoln,solnInfo] = ik("link7",poseTF,weights,initialguess);

Show the generated solution configuration, which now reaches the goal position.

show(puma,configSoln,PreservePlot=false);
title("End-Effector Target Position Achieved")

Note that for most IK problems, there are multiple configurations that can reach the desired pose target. Because the solver is optimization-based, the solver could approach a solution that does not actually reach the desired pose. If this occurs, the solver automatically restarts and uses a random configuration as the initial guess. This means that running the function more than once for the same pose target could yield different configurations that all reach the pose target. To avoid randomness, you can either set the random number generator seed or you can use an initial guess that is closer to the solution while also disabling AllowRandomRestart.

ik.SolverParameters.AllowRandomRestart = false

If you must find all possible solutions, use the analyticalInverseKinematics object.

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.

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

When using code generation, you must specify the RigidBodyTree property to define the robot on construction of the object. For example:

ik = inverseKinematics('RigidBodyTree',robotModel);

You also cannot change the SolverAlgorithm property after creation. To specify the solver algorithm on creation, use:

ik = inverseKinematics('RigidBodyTree',robotModel,...
        'SolverAlgorithm','LevenbergMarquardt');

Version History

Introduced in R2016b

R2025a: fmincon available as IK solver algorithm

The inverseKinematics System object now supports using the fmincon SQP solver as the IK solver. See the SolverAlgorithm property for more information.

This algorithm requires Optimization Toolbox.

R2025a: Specify endeffector frame in inverseKinematics system object

You can now specify the endeffector input argument as a frame name instead of a body name in the rigid body tree.

R2019b: inverseKinematics was renamed

The inverseKinematics object was renamed from robotics.InverseKinematics. Use inverseKinematics for all object creation.