It is common in control design to use the outputs of inverse kinematics to specify the set points for a controller. A robotic arm tasked with tracing a path with its end effector, for example, might have one such controller for each joint. Given the path to trace, an inverse kinematics solver determines the instantaneous joint angles to target, and from these the controllers can obtain the joint actuation torques to apply.

To support the kinematic analysis required of such tasks, Simscape Multibody
provides the `KinematicsSolver`

class.
Create a `KinematicsSolver`

object to obtain a suitable
representation of a model for analysis, then use the object functions to
formulate the kinematic problem to solve. You can call the functions from a
MATLAB Function block to incorporate the analysis with simulation or from a
MATLAB script to run the analysis outside simulation.

Consider the `sm_import_humanoid_robot_urdf`

model from
your Simscape Multibody installation. The model is of a walking robot
designed after the human body. Create a kinematic solver object for the
model:

```
sys = 'sm_import_humanoid_urdf';
ks = simscape.multibody.KinematicsSolver(sys)
```

The solver object is of the `KinematicsSolver`

class and
it stores, among other data, the variables of your analysis. Those
variables can be joint variables or frame variables.

Joint variables encode the states of joints—the translation and
rotation components of follower relative to base frames. Frame variables
encode the pose of a body, or, more precisely, the translation and
rotation components of a *frame* on that body
relative to another frame. The end effector and reference frames are
analogous to the follower and base frames of a joint.

List the joint variables in your solver object:

ks.jointVariables

The joint variables include all those present in the model when the solver object was constructed. If you update the model—add joints, say—then you must create a new solver object in order to capture those changes. Suppose your analysis is restricted to the left arm. The relevant joint variables are then your left wrist, elbow, shoulder frontal, and shoulder sagittal angles—or rows 8, 6, 7, and 2 of the joint variables table.

List the frame variables in your solver object:

ks.frameVariables

Frame variables depend on the choice of end effector, and this choice cannot be determined from your model alone. You must indicate which frame variables to use. As none have been given yet, none exist to show.

Let the left hand be the end effector. Take the **F**
frame of the hand to be the end effector frame and the world frame to be
your reference frame. You can refer to these frames as follower and
base. Add the *x*, *y*, and
*z* translation components, as measured from base
to follower, to your frame
variables:

base = 'sm_import_humanoid_urdf/World/W'; follower = 'sm_import_humanoid_urdf/left_hand/F'; ks.addFrameVariables('Hand', 'translation', base, follower)

Note that `base`

and `follower`

must
be the *full* pathnames of your frames. Note also
that your frame variables, as defined, are all of translation type. What
if you care about the end effector rotation as well? You can always add
(and remove) frame variables. To add the *x*,
*y*, and *z* rotation
components:

ks.addFrameVariables('Hand', 'rotation', base, follower)

`Hand`

is the variable group name. Notice the
**ID** column of your variable tables.
All variables, whether frame or joint, are named in the format
*group*.*type*.*component*.
The first frame variable listed is in group `Hand`

, of
type `Translation`

, and for component
*x*. In other words,
`Hand.Translation.x`

gives the
*x* translation of the `Hand`

follower frame relative to the base frame.

Likewise, the first joint variable listed belongs to group
`j1`

, is of type `Rz`

, and defines
component `q`

. That is, `j1.Rz.q`

gives the rotation angle (`q`

) of the
`Rz`

joint primitive—or of its follower frame
relative to its base frame. `Rz`

is a revolute joint
primitive with *z* for its rotation axis.

Your joint and frame variables are now adequate for analysis on the
left arm. The type of the analysis depends on which variables you
specify—your *targets*—and which variables you solve
for—your *outputs*. Target and output variables can
each comprise joint and frame variables. They are, in the general case,
subsets of all joint and frame variables.

Suppose you have targets for your joint angles and want the pose of the end effector frame. The problem is one of forward kinematics. Set the joint variables of the left arm as your targets and the frame variables of translation type as your output:

ks.addTargetVariables(ks.jointVariables.ID([2,6:8])) ks.addOutputVariables(ks.frameVariables.ID(1:3))

Only one thing remains to complete the analysis formulation—numerical values for the target variables. The variables and their units are by default in degrees. Let the left elbow joint be at a 30-degree angle, the left shoulder frontal and left shoulder sagittal joints at a 45-degree angle each, and the left wrist joint at a 15-degree angle:

targets = [30, 45, 45, 15];

Solve for the output variables and examine the results:

outputVec = ks.solve(targets)

outputVec = 0.2120 0.0505 -0.1273

`outputVec`

is the vector of solutions for the output
variables—the translation components, in the default unit of
`m`

, of the left hand **F** frame
relative to the world frame. This vector answers the question most
likely to drive such an analysis: where is the end effector when the
joints are at some given angles?

Often, you have targets for your frame variables—or your end effector—and want joint angles consistent with those targets. The problem is now one of inverse kinematics, and the solution, if one is found, is likely one of several possible. Begin by clearing target and output variables from previous analyses:

ks.clearTargetVariables ks.clearOutputVariables

The `clearTargetVariables`

and
`clearOutputVariables`

delete *all*
target and output variables from your solver object. Similar methods exist
for other variables. You can also remove subsets of variables. To remove
frame variables of rotation type—rows 4–6 in the frame variables
table:

ks.removeFrameVariables(ks.frameVariables.ID([4:6]))

The `removeFrameVariables`

method deletes only the
specified variables. Set your frame variables as targets and your relevant
joint variables—those of the left arm—as
output:

ks.addTargetVariables(ks.frameVariables.ID) ks.addOutputVariables(ks.jointVariables.ID([2,6:8]))

The robot is small, and the upper and lower arms are each on the order of
`8 cm`

long. With this measure in mind, set your frame
translation variables to `[16, -12, 0] cm`

, or, in the
default units of length, `[0.16, -0.12, 0] m`

. These values
are relative to the World frame, which is located at the base of the head,
with x-axis pointing left, *y*-axis pointing back, and
*z*-axis pointing
up:

targets = [0.16, -0.12, 0];

Solve for the output variables and examine the results:

outputVec = ks.solve(targets)

outputVec = 52.8406 -108.3936 7.0457 15.0565

The output is now a vector of joint angles in the default unit of degrees.
The angles are each for the follower frame of the joint relative to the base
frame. To interpret the angles, it helps to visualize the model and check
the orientation of the joint base frame. You can use
`set_param`

function to enable position state targets
and to assign the calculated angles to the respective
joints:

for i = 1:length(outputVec) path = ks.outputVariables.JointVariableInfo.BlockPath(i); angle = num2str(outputVec(i)); set_param(path, 'PositionTargetSpecify', 'on',... 'PositionTargetValue', angle); end

Update the block diagram (**Simulation** > **Update Diagram**) to open Mechanics Explorer and visualize the model in its new
initial state. The hand—the end effector—is in the right place, but the
elbow has an unnatural bend. You can correct the bend—or attempt to correct
it—by specifying guesses for your output variables.

Guesses, themselves variables, serve to guide the solver toward a reasonable assembly configuration. They can be joint variables or frame variables and are, in the general case, a mix of both. Set the elbow joint variable—the only that needs guidance—as a guess variable:

ks.addInitialGuessVariables(ks.jointVariables.ID(2))

Think of guess variables as low-priority joint state targets. They are not always satisfied—for example, if two guess variables are in conflict. Experiment with guess values to find some that work. Here, to give the elbow a natural bend, it suffices to reverse the sign of the calculated elbow angle:

guesses = -50;

Solve for the output variables using both target and guess variables as input:

outputVec = ks.solve(targets,guesses);

Update the joint position targets as before:

for i = 1:length(outputVec) path = ks.outputVariables.JointVariableInfo.BlockPath(i); angle = num2str(outputVec(i)); set_param(path, 'PositionTargetSpecify', 'on',... 'PositionTargetValue', angle); end

Update the block diagram to visualize the results. The hand should remain where it was, with the left elbow now facing away from the torso.