Forward and inverse kinematics

The namespace UnderAutomation.UniversalRobots.Kinematics computes the forward and inverse kinematics of Universal Robots cobots, without a connection to a robot. This page shows how to get the DH parameters of a robot, compute the position of the flange from the joint positions and back, and detect the singularities.

• Forward kinematics: 6 joint positions give the position and the orientation of the flange.
• Inverse kinematics: a position and an orientation of the flange give up to 8 sets of joint positions.

DH parameters

The Denavit-Hartenberg (DH) parameters describe the geometry of the arm. Each joint has four parameters: θ (rotation around z, the joint variable), d (offset along z), a (length along x) and α (rotation around x). On UR cobots, only a2, a3, d1, d4, d5 and d6 change between the models:

See the DH parameters of each model by Universal Robots.

Nominal or custom parameters

GetDhParametersFromModel returns the nominal parameters of a model (UR3 to UR30, CB-Series and e-Series). CustomUrDhParameters takes your own values.

static void Main(string[] args)
{
// Nominal DH parameters of a model
IUrDhParameters ur5e = KinematicsUtils.GetDhParametersFromModel(RobotModelsExtended.UR5e);

// Or your own values, in meters
IUrDhParameters custom = new CustomUrDhParameters(
  a2: -0.425,
  a3: -0.3922,
  d1: 0.1625,
  d4: 0.1333,
  d5: 0.0997,
  d6: 0.0996);
}
}

Parameters of a connected robot

The Primary Interface sends the DH parameters of the robot, with its calibration, in KinematicsInfo and ConfigurationData. Both implement IUrDhParameters.

static void Main(string[] args)
{
var robot = new UR();

// The Primary Interface is enabled by default
robot.Connect("192.168.0.1");

// Wait for the first packages (10 Hz)
Thread.Sleep(500);

// DH parameters of this robot, with its calibration
IUrDhParameters dh = robot.PrimaryInterface.KinematicsInfo;

// Current joint positions, in radians
JointDataPackageEventArgs j = robot.PrimaryInterface.JointData;
double[] joints =
{
  j.Base.Position, j.Shoulder.Position, j.Elbow.Position,
  j.Wrist1.Position, j.Wrist2.Position, j.Wrist3.Position
};

// Current position of the tool center point
Pose pose = robot.PrimaryInterface.CartesianInfo.AsPose();
}
}

Forward kinematics

ForwardKinematics takes 6 joint positions in radians. ToolTransform is the 4x4 transform of the flange in the base frame: Pose.From4x4MatrixToRotationVector converts it to a pose in meters and radians. Add the TCP offset of your tool to get the position of the tool center point.

static void Main(string[] args)
{
IUrDhParameters dh = KinematicsUtils.GetDhParametersFromModel(RobotModelsExtended.UR5e);

// Joint positions in radians: base, shoulder, elbow, wrist 1, wrist 2, wrist 3
double[] joints = { 0, -1.57, 1.57, -1.57, -1.57, 0 };

KinematicsResult result = KinematicsUtils.ForwardKinematics(joints, dh);

// 4x4 transform of the flange, as a pose: X, Y, Z in m, rotation vector in rad
Pose flange = Pose.From4x4MatrixToRotationVector(result.ToolTransform);

// Transform of each joint, alone and composed with the previous ones
TransformationSet local = result.IndividualLocalTransforms;
TransformationSet global = result.CumulativeGlobalTransforms;
}
}

IndividualLocalTransforms gives the transform of each joint alone, and CumulativeGlobalTransforms the transform of each joint composed with the previous ones.

Inverse kinematics

InverseKinematics takes the 4x4 transform of the flange and returns up to 8 solutions, of 6 joint positions each. GetNearestSolution picks the solution nearest to a reference, usually the current joint positions. The solutions close to a singularity are included.

static void Main(string[] args)
{
IUrDhParameters dh = KinematicsUtils.GetDhParametersFromModel(RobotModelsExtended.UR5e);

// Target of the flange: X, Y, Z in m, rotation vector in rad
var target = new Pose(0.4, -0.1, 0.3, 0, 3.14, 0);

// Up to 8 solutions, 6 joint positions in radians each
double[][] solutions = KinematicsUtils.InverseKinematics(target.FromRotationVectorTo4x4Matrix(), dh);

// The solution nearest to the current joint positions
double[] current = { 0, -1.57, 1.57, -1.57, -1.57, 0 };
double[] nearest = KinematicsUtils.GetNearestSolution(solutions, current);
}
}

Singularities

Near a singularity, the robot loses a degree of freedom: a small move of the tool needs a large move of a joint, and the inverse kinematics has no unique answer. GetSingularity returns the singularities near a set of joint positions: Wrist, Elbow, Shoulder, or None.

static void Main(string[] args)
{
IUrDhParameters dh = KinematicsUtils.GetDhParametersFromModel(RobotModelsExtended.UR5e);

// Joint positions in radians
double q2 = -1.57; // shoulder
double q3 = 0.0;   // elbow
double q4 = -1.57; // wrist 1
double q5 = 1.57;  // wrist 2

// The 4 joint positions are given in the order q2, q3, q4, q5
SingularityType singularity = KinematicsUtils.GetSingularity(q2, q3, q4, q5, dh);

// Flags: Wrist, Elbow, Shoulder, or None
if (singularity != SingularityType.None)
  Console.WriteLine("Close to a singularity: " + singularity);
}
}

Try it in the demo application

Everything on this page can be tried without writing code, in the Forward & Invert Kinematics page of the demo application.

The demo application is open source. The C# source of this page is KinematicsControl.cs.

Reference

Implementation

The solver is analytical (closed form), with the standard DH convention and the sequence of Chen et al., IEEE ICASI 2017. The forward kinematics is the product of the 6 homogeneous transforms of the joints. The inverse kinematics solves q1, then q5, q6, the sum q2 + q3 + q4, then q2, and q3 and q4 last. The singularities are detected from the factors of the determinant of the Jacobian: sin(q5) for the wrist, sin(q3) for the elbow, and a2·cos(q2) + a3·cos(q2 + q3) + d5·sin(q2 + q3 + q4) for the shoulder.

Reference: Chen et al., IEEE ICASI 2017, IEEE Xplore.