/// <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);
}
public void SimpleDualQuaternionTest2()
{
// Translation 5 units in X direction
var translation = DualQuaternion.CreateTranslation(5, Vector3.UnitX);
// Rotation of PI/2 around Z axis centered on origin - We are using Plücker coordinates to describe the axis of rotation
var rotation = DualQuaternion.CreateRotationPlucker(Math.PI / 2f, Vector3.UnitZ, Vector3.Zero);
// We compose displacements, translation is applied first
var displacement = rotation * translation;
// We apply the point transform (Clifford conjugation for points) on the origin point using the displacement
var point = displacement.TransformPoint();
// Assert
Assert.That(point, Is.EqualTo(new Vector3(0, 5, 0)));
}