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. In the Modeling tab, click Update Model. 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.