/// <summary>
/// Compute the coordinates of the end activator for a EPSON SCARA robot.
/// </summary>
/// <param name="pose">
/// The robot pose.
/// </param>
/// <returns>
/// The coordinates of the end effector.
/// </returns>
private static Vector3 ScaraForwardKinematics(ScaraRobotPose pose)
{
// Here we create the three rotation quaternions
// Note that we are directly using the Plücker coordinates of the joint axes.
// However, rotation would work for axes defined by an orientation and a point belonging to the line.
var r1 = DualQuaternion.CreateRotation(pose.Theta1, Vector3.UnitZ, new Vector3(0, 0, 0));
var r2 = DualQuaternion.CreateRotation(pose.Theta2, Vector3.UnitZ, new Vector3(300, 0, 0));
var r3 = DualQuaternion.CreateRotation(pose.Theta3, Vector3.UnitZ, new Vector3(650, 0, 0));
// Fourth joint is translation
var t = DualQuaternion.CreateTranslation(pose.Z, -Vector3.UnitZ);
// The end effector position for the reference configuration
var endEffector = DualQuaternion.CreateTranslation(new Vector3(650, 0, 318));
// Here we combine the four displacements into a single one and return it.
// Notice we also append the end-effector at the reference configuration.
// The order of application of the transformations is right-most is applied first.
var displacements = DualQuaternion.Multiply(r1, r2, r3, t, endEffector);
// Here we basically use the action f4g (specified in libdq’s manual) to transform the origin point.
// This will give us the forward kinematics of the robot.
var result = displacements.TransformPoint();
return(result);
}