KinematicsSolver

Solve kinematics problems for a multibody model

Description

KinematicsSolver objects allow users to formulate and numerically solve kinematics problems for their Simscape™ Multibody™ models. You can use the object to solve standard forward and inverse kinematics problems, as well as more general problems with closed-loop kinematic systems and multiple targets.

A kinematics problem is formulated using kinematic variables. These variables have scalar values that specify the relationships between frames in the corresponding Simscape Multibody model. There are two types of kinematic variables: joint and frame. Joint variables correspond to joint position and velocity states and are created automatically when the object is constructed. You can view the joint variables using the jointPositionVariables and jointVelocityVariables object functions. Frame variables correspond to position and velocity relationships between arbitrary frames in the model and must be defined using the addFrameVariables object function. Once defined, they can be viewed using the frameVariables object function.

To formulate a kinematics problem, you must assign roles for the relevant kinematic variables. There are three roles: targets, initial guesses, and outputs. Variables are assigned to these roles using the addTargetVariables , addInitialGuessVariables, and addOutputVariables object functions. To solve the problem with the assigned variables, use the solve object function. Starting from an initial state, the solver attempts to find a final state of the system consistent with the values of the target variables. The initial state is synthesized using the values of the initial guess variables. The initial states that are not specified by initial guess variables are initialized to zero. The values of the output variables are derived from the final state returned by the solver. If the solver is unable to find a final state that satisfies all the targets, it tries to at least return a state that is kinematically feasible.

Creation

Syntax

ks = simscape.multibody.KinematicsSolver(modelName)

ks = simscape.multibody.KinematicsSolver(___,Name,Value)

Description

ks = simscape.multibody.KinematicsSolver(modelName) creates a KinematicsSolver object for the model named in modelName. The object contains a representation of the model suitable for kinematic analysis. The representation is a snapshot of the model as it is when the object is created. Subsequent changes to the model do not carry over to the object. Create a new object, if necessary to capture those changes.

The model must contain a Simscape Multibody network, and you need to load the model into memory before creating its KinematicsSolver object. If blocks of the model have MATLAB^® variables, you need to numerically define those variables in the model workspace or MATLAB workspace. The KinematicsSolver object ignores any contacts and several parameters of joint blocks, like State Targets, Limits, Actuation, and Mode Configuration. For example, during an analysis, two bodies can penetrate each other even though there is a Spatial Contact Force block that connects them. Block parameters set to Run-Time are evaluated when creating the object and cannot be modified afterward.

A KinematicsSolver object is a handle object. A variable created from it contains not a copy of the object but a reference to it. The variable acts as a pointer or handle. Modifying a handle modifies also the object and all of its remaining handles. Copying a KinematicsSolver object and adding a frame variable to the copy, for example, adds that frame variable to the object and so also to any other handles it might have.

ks = simscape.multibody.KinematicsSolver(___,Name,Value) creates a KinematicsSolver object with additional options specified by one or more Name,Value pair arguments.

Properties

MaxIterations — Maximum number of solver iterations

100 (default) | positive integer

Maximum number of solver iterations, specified as a positive integer. You can specify this property after creating a KinematicsSolver object.

Example: 50

Data Types: double | int

ModelName — Model Name

string scalar | character vector

Simscape Multibody model name from which the object derives. This property is read-only.

Example: "FourBar"

Data Types: char | string

Name-Value Pair Arguments

Specify optional pairs of arguments as Name1=Value1,...,NameN=ValueN, where Name is the argument name and Value is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Before R2021a, use commas to separate each name and value, and enclose Name in quotes.

Example: "DefaultAngleUnit","rad"

"DefaultAngleUnit" — Default angle unit of new kinematic variables

"deg" (default) | string scalar | character vector

Default angle unit of new kinematic variables, specified as the comma-separated pair consisting of "DefaultAngleUnit" and a character vector or string scalar. When you change the "DefaultAngleUnit" property, the change applies only to new kinematic variables. Existing variables are not changed.

Example: "DefaultAngleUnit","rad"

Data Types: char | string

"DefaultAngularVelocityUnit" — Default angular velocity unit of new kinematic variables

"deg/s" (default) | string scalar | character vector

Default angle unit of new kinematic variables, specified as the comma-separated pair consisting of "DefaultAngularVelocityUnit" and a character vector or string scalar. When you change the "DefaultAngularVelocityUnit" property, the change applies only to new kinematic variables. Existing variables are not changed.

Example: "DefaultAngularVelocityUnit","rad/s"

Data Types: char | string

"DefaultLengthUnit" — Default length unit of new kinematic variables

"m" (default) | string scalar | character vector

Default angle unit of new kinematic variables, specified as the comma-separated pair consisting of "DefaultLengthUnit" and a character vector or string scalar. When you change the "DefaultLengthUnit" property, the change applies only to new kinematic variables. Existing variables are not changed.

Example: "DefaultLengthUnit","in"

Data Types: char | string

"DefaultLinearVelocityUnit" — Default linear velocity unit of new kinematic variables

"m/s" (default) | string scalar | character vector

Default angle unit of new kinematic variables, specified as the comma-separated pair consisting of "DefaultLinearVelocityUnit" and a character vector or string scalar. When you change the "DefaultLinearVelocityUnit" property, the change applies only to new kinematic variables. Existing variables are not changed.

Example: "DefaultLinearVelocityUnit","in/s"

Data Types: char | string

Object Functions

Listing Variables

frameVariables	List kinematic variables associated with frame pairs
initialGuessVariables	List all kinematic variables assigned as initial guesses
jointVelocityVariables	List all kinematic variables associated with joint velocities
jointPositionVariables	List all kinematic variables associated with joint positions
outputVariables	List all kinematic variables assigned as outputs
targetVariables	List kinematic variables assigned as targets

Adding and Configuring Variables

addFrameVariables	Create kinematic variables from select frame pair in KinematicsSolver object
addInitialGuessVariables	Assign kinematic variables from KinematicsSolver object as guesses
addOutputVariables	Assign kinematic variables from the KinematicsSolver object as outputs
addTargetVariables	Assign kinematic variables from KinematicsSolver object as targets
setVariableUnit	Change physical unit of kinematic variable

Removing Variables

removeFrameVariables	Drop select frame variables from the KinematicsSolver object
removeInitialGuessVariables	Drop select guess variables from the KinematicsSolver object
removeOutputVariables	Drop select output variables from the KinematicsSolver object
removeTargetVariables	Drop select target variables from the KinematicsSolver object

Clearing Variables

clearFrameVariables	Drop all frame variables from the KinematicsSolver object
clearInitialGuessVariables	Drop all guess variables from the KinematicsSolver object
clearOutputVariables	Drop all output variables from the KinematicsSolver object
clearTargetVariables	Drop all target variables from the KinematicsSolver object

Solving

solve

Run kinematic analysis for KinematicsSolver object

Code Generation

generateCode

Generate C code to run kinematic analysis on KinematicsSolver object

Viewing Solution

viewSolution	Open Kinematics Solver Viewer window to visualize KinematicsSolver solution
closeViewer	Close the Kinematics Solver Viewer window

Examples

Run Forward Kinematics on Humanoid Robot Arm

This example shows how to compute forward kinematics for the left arm of a humanoid robot. Specifically, it computes the position of the robot wrist for specified angles of the shoulder and elbow joints.

• Open this example, load the humanoid robot model into memory, and create a KinematicsSolver object for the model. The object contains a kinematic representation of the model and a list of all the joint variables that it contains.

mdl = "ImportedURDF";
load_system(mdl);
fk = simscape.multibody.KinematicsSolver(mdl);

• Add to the object, fk, a group of frame variables for the left wrist. Specify the world frame as base and the B frame of the left_wrist joint as follower. Name the frame variable group Wrist.

base = "ImportedURDF/World/W";
follower = "ImportedURDF/left_wrist/B";
addFrameVariables(fk,"Wrist","translation",base,follower);
addFrameVariables(fk,"Wrist","rotation",base,follower);
frameVariables(fk)

ans=6×4 table
             ID                       Base                      Follower              Unit 
    _____________________    ______________________    ___________________________    _____

    "Wrist.Translation.x"    "ImportedURDF/World/W"    "ImportedURDF/left_wrist/B"    "m"  
    "Wrist.Translation.y"    "ImportedURDF/World/W"    "ImportedURDF/left_wrist/B"    "m"  
    "Wrist.Translation.z"    "ImportedURDF/World/W"    "ImportedURDF/left_wrist/B"    "m"  
    "Wrist.Rotation.x"       "ImportedURDF/World/W"    "ImportedURDF/left_wrist/B"    "deg"
    "Wrist.Rotation.y"       "ImportedURDF/World/W"    "ImportedURDF/left_wrist/B"    "deg"
    "Wrist.Rotation.z"       "ImportedURDF/World/W"    "ImportedURDF/left_wrist/B"    "deg"

The paths in base and follower are the full block paths from the root of the model to the selected port of a desired block. This example selects the W port of the World Frame block as the base and the B port of the left_wrist joint block as the follower. The object now has six frame variables—three for the x, y, and z translation components and three for the x, y, and z rotation components.

• Use jointPositionVariables(fk) to list all joint position variables. Assign as targets the joint position variables for the elbow ( j2.Rz.q), shoulder frontal (j6.Rz.q), and shoulder sagittal (j7.Rz.q).

jointPositionVariables(fk)

ans=18×4 table
        ID           JointType                      BlockPath                   Unit 
    __________    ________________    ______________________________________    _____

    "j1.Rz.q"     "Revolute Joint"    "ImportedURDF/left_ankle"                 "deg"
    "j2.Rz.q"     "Revolute Joint"    "ImportedURDF/left_elbow"                 "deg"
    "j3.Rz.q"     "Revolute Joint"    "ImportedURDF/left_hip_frontal"           "deg"
    "j4.Rz.q"     "Revolute Joint"    "ImportedURDF/left_hip_sagittal"          "deg"
    "j5.Rz.q"     "Revolute Joint"    "ImportedURDF/left_knee"                  "deg"
    "j6.Rz.q"     "Revolute Joint"    "ImportedURDF/left_shoulder_frontal"      "deg"
    "j7.Rz.q"     "Revolute Joint"    "ImportedURDF/left_shoulder_sagittal"     "deg"
    "j8.Rz.q"     "Revolute Joint"    "ImportedURDF/left_wrist"                 "deg"
    "j9.Rz.q"     "Revolute Joint"    "ImportedURDF/right_ankle"                "deg"
    "j10.Rz.q"    "Revolute Joint"    "ImportedURDF/right_elbow"                "deg"
    "j11.Rz.q"    "Revolute Joint"    "ImportedURDF/right_hip_frontal"          "deg"
    "j12.Rz.q"    "Revolute Joint"    "ImportedURDF/right_hip_sagittal"         "deg"
    "j13.Rz.q"    "Revolute Joint"    "ImportedURDF/right_knee"                 "deg"
    "j14.Rz.q"    "Revolute Joint"    "ImportedURDF/right_shoulder_frontal"     "deg"
    "j15.Rz.q"    "Revolute Joint"    "ImportedURDF/right_shoulder_sagittal"    "deg"
    "j16.Rz.q"    "Revolute Joint"    "ImportedURDF/right_wrist"                "deg"
      ⋮

targetIDs = ["j2.Rz.q";"j6.Rz.q";"j7.Rz.q"];
addTargetVariables(fk,targetIDs);

• Assign all of the frame variables as output variables.

outputIDs = ["Wrist.Translation.x";"Wrist.Translation.y";...
    "Wrist.Translation.z";"Wrist.Rotation.x";"Wrist.Rotation.y";"Wrist.Rotation.z"];
addOutputVariables(fk,outputIDs);

• Solve the forward kinematics problem given the elbow, shoulder frontal, and shoulder sagittal joint angles of 30, 45, and 45 degrees.

targets = [30,45,45];
[outputVec,statusFlag] = solve(fk,targets)

outputVec = 6×1

    0.2196
    0.0584
   -0.0983
  135.0000
    0.0027
  -15.0000

statusFlag = 
1

The solve function returns the values of the output variables. The values are sorted in the same order as the output variables. The units are the defaults of m for translation components, and deg for rotation components. The statusFlag shows that all model constraints and target variables are satisfied.

• View the Solution.

viewSolution(fk);

• Close the viewer.

closeViewer(fk);

Run Inverse Kinematics on Humanoid Robot Arm

This example shows how to compute inverse kinematics for the right arm of a humanoid robot. Specifically, it computes the angles of the elbow and shoulder joints corresponding to a desired wrist position. Since this problem has multiple solutions, initial guesses for the shoulder joint angles are used to bias the solver towards a desirable solution.

• Open this example, load the humanoid robot model into memory, and create a KinematicsSolver object for the model. The object contains a kinematic representation of the model and a list of all the joint variables that it contains.

mdl = "ImportedURDF";
load_system(mdl);
ik = simscape.multibody.KinematicsSolver(mdl);

• Add to the object, ik, a group of frame variables for the right wrist. Specify the world frame as base and the B frame of the right_wrist joint as follower and. Name the frame variable group Wrist.

base = "ImportedURDF/World/W";
follower = "ImportedURDF/right_wrist/B";
addFrameVariables(ik,"Wrist","translation",base,follower);
frameVariables(ik)

ans=3×4 table
             ID                       Base                       Follower              Unit
    _____________________    ______________________    ____________________________    ____

    "Wrist.Translation.x"    "ImportedURDF/World/W"    "ImportedURDF/right_wrist/B"    "m" 
    "Wrist.Translation.y"    "ImportedURDF/World/W"    "ImportedURDF/right_wrist/B"    "m" 
    "Wrist.Translation.z"    "ImportedURDF/World/W"    "ImportedURDF/right_wrist/B"    "m" 

The paths in base and follower are the full block paths from the root of the model to the selected port of a desired block. This example selects the W port of the World Frame block as the base and the B port of the right_wrist joint block as the follower. The object now has three frame variables for x, y, and z translation components.

• Assign all of the frame variables as target variables.

targetIDs = ["Wrist.Translation.x";"Wrist.Translation.y";"Wrist.Translation.z"];
addTargetVariables(ik,targetIDs);

• Use jointPositionVariables(ik) to list all joint position variables.

jointPositionVariables(ik)

ans=18×4 table
        ID           JointType                      BlockPath                   Unit 
    __________    ________________    ______________________________________    _____

    "j1.Rz.q"     "Revolute Joint"    "ImportedURDF/left_ankle"                 "deg"
    "j2.Rz.q"     "Revolute Joint"    "ImportedURDF/left_elbow"                 "deg"
    "j3.Rz.q"     "Revolute Joint"    "ImportedURDF/left_hip_frontal"           "deg"
    "j4.Rz.q"     "Revolute Joint"    "ImportedURDF/left_hip_sagittal"          "deg"
    "j5.Rz.q"     "Revolute Joint"    "ImportedURDF/left_knee"                  "deg"
    "j6.Rz.q"     "Revolute Joint"    "ImportedURDF/left_shoulder_frontal"      "deg"
    "j7.Rz.q"     "Revolute Joint"    "ImportedURDF/left_shoulder_sagittal"     "deg"
    "j8.Rz.q"     "Revolute Joint"    "ImportedURDF/left_wrist"                 "deg"
    "j9.Rz.q"     "Revolute Joint"    "ImportedURDF/right_ankle"                "deg"
    "j10.Rz.q"    "Revolute Joint"    "ImportedURDF/right_elbow"                "deg"
    "j11.Rz.q"    "Revolute Joint"    "ImportedURDF/right_hip_frontal"          "deg"
    "j12.Rz.q"    "Revolute Joint"    "ImportedURDF/right_hip_sagittal"         "deg"
    "j13.Rz.q"    "Revolute Joint"    "ImportedURDF/right_knee"                 "deg"
    "j14.Rz.q"    "Revolute Joint"    "ImportedURDF/right_shoulder_frontal"     "deg"
    "j15.Rz.q"    "Revolute Joint"    "ImportedURDF/right_shoulder_sagittal"    "deg"
    "j16.Rz.q"    "Revolute Joint"    "ImportedURDF/right_wrist"                "deg"
      ⋮

Assign as output variables the joint position variables for the elbow ( j10.Rz.q), shoulder frontal (j14.Rz.q), and shoulder sagittal (j15.Rz.q).

outputIDs = ["j10.Rz.q";"j14.Rz.q";"j15.Rz.q"];
addOutputVariables(ik,outputIDs);

• Compute the joint angles for the elbow and shoulder corresponding to a wrist position of [-0.16,-0.12,0] m.

targets = [-0.16,-0.12,0];
[outputVec,statusFlag] = solve(ik,targets)

outputVec = 3×1

  -52.8384
  -71.6077
  172.9586

statusFlag = 
1

The solve function returns the values of the output variables—the angles of the elbow, shoulder frontal, and shoulder sagittal, each in the default units of deg. The statusFlag shows that all model constraints and target are satisfied.

• Visualize the solution in the Kinematics Solver Viewer and determine if it is reasonable. At the MATLAB® command prompt, enter:

viewSolution(ik);

In the Click Front View button to view the result.

Although the solver found a solution that puts the right wrist in the right location, the viewer shows that the arm has an unnatural pose. You can specify the joints of the shoulder as guess variables to have a better solution.

• Set the shoulder frontal and shoulder sagittal joint variables as guess variables and run the analysis once again for rotations of [90,90] deg.

guessesIDs = ["j14.Rz.q","j15.Rz.q"];
guesses = [90,90];
addInitialGuessVariables(ik,guessesIDs);
[outputVec,statusFlag] = solve(ik,targets,guesses)

outputVec = 3×1

  -52.8384
  108.3891
   55.5025

statusFlag = 
1

The solve function returns a new solution for the joint angles.

Click the Update Visualization button to update the Kinematics Solver Viewer to visualize the results.

• Close the viewer.

closeViewer(ik);

Version History

Introduced in R2019a

R2023a: KinematicsSolver class no longer supports saving or loading objects

Simscape Multibody no longer supports saving or loading a KinematicsSolver object. If you save a KinematicsSolver object to a MAT-file by using the save function, MATLAB issues a warning. The saved KinematicsSolver object corresponds to an empty model with no kinematic variables.

When loading a KinematicsSolver object from a MAT-file, MATLAB issues a warning and the loaded object corresponds to an empty model with no kinematic variables. All properties of the loaded object have default values except the ModelName property which is the string DefaultEmpty.

Instead of saving a KinematicsSolver object, you can save the commands that create the object to a MATLAB script. To recreate the KinematicsSolver object, you can run the saved script.