Run a Kinematic Analysis on a Model

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.

Forward Kinematics

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)

Joint and Frame Variables

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.

Target and Output Variables

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];

Analysis Output

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?

Inverse Kinematics

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.

Guess 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;

Improved Analysis Output

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.