Forward and inverse kinematics

This page explains the offline kinematics of the Yaskawa SDK: forward kinematics (joint angles to flange position) and inverse kinematics (flange position to every set of joint angles) of 6-axis Motoman arms and cobots. The computation runs on the PC, without a controller and without a connection, for 169 robot models.

What the kinematics do

Both take the geometry of the robot as DhParameters: from the catalog of models, from the ALL.PRM file of the controller, or from your own values. See Kinematics models.

Use cases: check that a target can be reached before you send a move, choose the posture of a move, compute a position in a simulation or a planner, or compare a position read on the controller with a model.

Conventions

• Joint angles are in degrees, with the signs of the pendant. They are not encoder pulses: the conversion from pulses depends on the robot and is not done by the SDK.
• The Cartesian position is the flange (tool 0) in the robot frame. Add your tool yourself, with CartesianPosition.ToHomogeneousMatrix() and a matrix product.
• The robot frame has its origin on the S axis, at the height of the L axis, X forward and Z up.
• The angles follow the Yaskawa convention: the orientation matrix is R = Rz(Rz) * Ry(Ry) * Rx(Rx), the same as the pendant.

Forward kinematics

ForwardKinematics(joints, dh) returns the flange position for a set of joint angles.

static void Main()
{
// Geometry of the robot model
DhParameters dh = DhParameters.FromArmKinematicModel(ArmKinematicModels.GP7);

// Joint angles in degrees (S, L, U, R, B, T), with the signs of the pendant
var joints = new JointsAngles(0, 0, 0, 0, -90, 0);

// Position of the flange in the robot frame: mm and degrees
CartesianPosition flange = KinematicsUtils.ForwardKinematics(joints, dh);
Console.WriteLine(flange); // X=480, Y=0, Z=405, Rx=-180, Ry=0, Rz=0: the flange points down
}
}

JointsAngles takes the 6 angles in its constructor, or an array. S, L, U, R, B, T and Values read and change them.

Inverse kinematics

InverseKinematics(position, dh) returns every set of joint angles that puts the flange at the position. The angles are in the range (-180, 180]. The array is empty when the position cannot be reached.

static void Main()
{
DhParameters dh = DhParameters.FromArmKinematicModel(ArmKinematicModels.GP7);

// Flange position in the robot frame: mm and degrees
var target = new CartesianPosition(400, 100, 300, 180, 0, 0);

// Every joint solution, angles in (-180, 180]. Empty if the position cannot be reached.
JointsAngles[] solutions = KinematicsUtils.InverseKinematics(target, dh);

foreach (JointsAngles solution in solutions)
    Console.WriteLine(solution); // S=..., L=..., U=..., R=..., B=..., T=...
}
}

The joint limits of the robot are not checked: remove the solutions outside the limits of your robot, then choose one. See Inverse kinematics.

Supported robots

The SDK chooses the solver from the DH parameters:

These structures are not supported: T axis offset from the B axis (MA1400, MA1550, MA1800, MA1900), painting wrists with tilted axes (MPX2600, MPX3500), R axis parallel to the L and U axes (MPXL2600), palletizing robots, SCARA, delta and 7-axis robots. For them, InverseKinematics throws a NotSupportedException.

Accuracy and speed

• Each solution is checked with the forward kinematics before it is returned: the error is below 0.00001 mm.
• The catalog gives the nominal geometry of each model. The controller can use calibrated values: compare a few positions with your robot before you rely on an offline result.
• The SDK is tested with poses measured on real GP7 and HC10 robots.
• An inverse kinematics takes about 20 µs for a GP7 and 100 µs for an HC10 on a desktop PC: 10000 targets of a path are checked in a fraction of a second.

Reference

What to read next

• Inverse kinematics: the solutions, the postures and how to choose one.
• Kinematics models: DH parameters from the catalog, from the controller or from your values.