Create Constrained Inverse Kinematics Solver Using Inverse Kinematics Designer

This example shows how to create an inverse kinematics (IK) solver and constraints using the Inverse Kinematics Designer app. Inverse kinematics (IK) solvers are used in robotics to determine the joint configurations for a robot that satisfies certain constraints. A constrained IK solver can be used to enforce a variety of behaviors, such as end effector pose targets, joint position limits, and other kinematic constraints. In this example, a solver & constraints are designed to enforce a camera aiming behavior for the Willow Garage PR2. The PR2 includes a body that represents a camera sensor. By adding a constraint that forces the camera to point at the left gripper, the camera follows the gripper as it moves, ensuring that the camera will track any object that the gripper is acting on.

Start New Session

Click Apps > Inverse Kinematics Designer to start the app, or use the inverseKinematicsDesigner function:

inverseKinematicsDesigner

Click New Session to bring up the New Session dialog box to start a new app session. The dialog allows you to choose from a list of robots available in the robot library or provide custom rigid body trees. Custom rigid body trees must be available in the MATLAB workspace before loading them into the app.

Select Willow Garage PR2 from the Rigid body tree list, and click OK.

Position Robot in Scene Canvas

Once the app is loaded, use the controls in the Scene Canvas to reposition the robot to more comfortably use the space. Hover over the axes shown in the Scene Canvas to bring up the Axes Toolbar, shown with numbered components in the figure below.

Use these buttons to control the figure mode. The Rotate 3D control (labeled with a 1 in the figure above) to rotate the robot. In this mode, a scroll wheel or similar function will zoom the entire Scene Canvas, and can be useful to fill the screen. The Pan button (labeled with a 2), allows the user to pan the robot within the axes bounds, and has the same default zoom behavior as the Rotate 3D option. The Zoom In and Zoom Out modes (labeled with a 3 above) enable the user to zoom in within the axes bounds by clicking or using the scroll wheel zoom. Lastly, the Home control resets the view to the default one.

In the following GIF, all four modes are used to reposition the robot into a more usable position given screen space.

Zoom Mode

It is necessary to enter one of the given figure control modes to zoom in using the scroll wheel or a similar feature. As noted above, there are two such modes. When the Rotate 3D or Pan controls are active, the entire canvas is zoomed (i.e. the axes limits don't change). When the Zoom In or Zoom Out controls are active, the canvas is zoomed inside the figure bounds. By using these together, it's possible to use the canvas space as efficiently as possible for a task.

Choose Body for Robot Gripper

Choose the body to use as the robot gripper. There are many possible bodies in the gripper, so it is useful to click on them and select one that moves intuitively. There are several ways to inspect the robot.

Explore Robot Using Scene Panes

The Scene Browser is the panel to the left of the scene canvas. It displays all the bodies and joints in the robot, as well as any collision objects that have been loaded into the scene. By clicking on objects in the scene inspector, they will be highlighted in the Scene Canvas, and the details of the selected object are brought up in the Scene Inspector. The Scene Inspector is the rightmost panel, and shows object properties and states, such as the current pose.

To get started, select the body, l_gripper_palm_link, in the Scene Browser to select the palm of the left gripper.

To further verify its motion, right-click on the body and select Assign Pose To Marker Body. This assigns the marker to this body so that it may be moved around using an interactive marker pose target.

Move End Effector Using Marker Pose Constraint

The Inverse Kinematics Designer always creates a solver with one default constraint: the marker pose constraint. This constraint assigns the target pose for a specified end effector body. The body can be set via the right-click menus or in the toolstrip, or by editing the constraint and modifying the constraint details directly. Unlike other constraints, however, the marker pose constraint can be directly set using an interactive marker.

This tool allows the robot to be easily manipulated, which helps with solver verification. The constraint can also be disabled by unchecking the check box next to it in the Constraints Browser.

Point Camera at Gripper Body Using Aiming Constraint

Now that the app has been configured, add an aiming constraint to ensure that the camera is always pointing at the left gripper. An aiming constraint ensures that the Z axis of a designated body points at a given point in space, specified in reference to some other body. To add a constraint, click Add Constraint in the toolstrip to open the Constraint tab, and then select Aiming Constraint. This opens the aiming constraint parameters section of the Constraint tab.

Assign Constraint Parameters

To configure the aiming constraint, three main parameters must be set:

• Assign an End Effector body. The end effector is the body that aims at the reference point along its Z axis. Since the goal is to have the camera aim, it is necessary to choose a body on the head (the green body) with a forward-facing Z axis is chosen. Select high_def_optical_frame from the End Effector Body list, and observe that the corresponding body is now highlighted in green.

• Assign a Reference Body. The target point is defined with respect to a reference body. Since we have been using the palm to move the robot arm, select l_gripper_palm_link from the Reference Body list. Observe that the palm body is now highlighted in blue.

• Modify the Target Point. By default, the target point is at the origin of the reference body. However, suppose we want to instead aim at a point that is slightly offset from the palm, as though it were holding something. Set the Z Target to 0.1 and observe that the red X that indicates the target point is now offset along the reference body's Z-axis.

During constraint modification, the marker can be hidden to make the constraint changes easier to inspect. Disable the marker either by unchecking Marker Pose Target in the Constraints Browser, or by right-clicking on the marker pose constraint in the Constraints Browser and selecting Toggle Marker Display.

Once you are satisfied with the constraint, click Apply to add the constraint to the solver, and Close Constraint to exit the constraint tab.

Verify Constrained Behavior

Once the constraint has been applied, verify that it works as expected by moving the palm around and seeing if the camera follows it. To do this, first make sure that both constraints are enabled in the Constraints Browser (they should be checked). Next, use the marker to move the palm around, and see if the head follows.

As you move the marker, notice that the robot may not always hit the marker exactly. You can see whether a constraint has been met by looking at the Constraints Browser. When the constraint icon with a green check is shown, the constraint has been fully satisfied. However, when a constraint icon with a red X is shown, the constraint has not been met. For example, in the following snapshot, the Marker Pose Target is not met, while the aiming constraint has been met.

There are many reasons why a constraint might not be met, ranging from conflicts with other constraints to not enough solver iterations. Click the Report Status button to see why the solver exited. This opens a window that indicates details of the last solver call.

For example, in the situation above, clicking on the report indicates that the solver ran to the maximum number of iterations and then returned the best available solution. So it is recommended to call the solver again using the current conditions as the initial conditions. If that does not succeed, it is also possible that the pose is unachievable. For more information, refer to Resolving Constraint Conflict.

Tune Solver Settings

Based on the exit conditions, it may be helpful to modify the solver settings. For example, as more constraints are added, the maximum iterations should be increased to ensure that the solver has time to converge.

To update the settings, click Solver Settings, or select the Solver tab.

Select the Maximum Iterations, and change the value to 200. Click outside the box to confirm the change, then click Apply to Solver to apply changes.

Once the new solver settings have been applied, return to the Inverse Kinematics tab and click Refresh Solver to run the solver again. Click Report Status to verify that when a solution is achievable, the solution converges before the maximum iterations are hit.

Export Solver and Constraints

To use the solver object outside of the app, export it to the MATLAB® workspace. Click Export Solver and Constraints, which will bring up the Export Solver and Constraints dialog.

Set Solver name and Constraints cell array name or use the default values. Shift-click to select multiple constraints and click Export.

Once everything is selected, click Export to export these to the workspace.

Use Exported Solver

Use the exported solver object to constrain another configuration directly from the command line.

Note that the exported solver uses a robot with row format so the exported configurations are in row format. A handle to the rigid body tree is stored on the exported solver object.

load("ikSolverDesignExampleData.mat")

robot = ikSolver.RigidBodyTree;

% Assign a new target pose to the gripper
ikConstraints{1}.TargetTransform = trvec2tform([-.01 .04 1.45]);

% Call the solver with these two constraints
[qSol, info] = ikSolver(robot.homeConfiguration, ikConstraints{:});

% Display results
show(robot, qSol);