/// <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 EpsonE2LScaraRobotReference()
{
// Arrange
var pose = new ScaraRobotPose();
// Act
var point = ScaraForwardKinematics(pose);
// Assert
Assert.That(point, Is.EqualTo(new Vector3(650, 0, 318)));
}
public void EpsonE2LScaraRobotTestRotations()
{
// Arrange
var pose = new ScaraRobotPose(-Math.PI / 2, -Math.PI / 2, 0, 318);
// Act
var point = ScaraForwardKinematics(pose);
// Assert
Assert.That(point, Is.EqualTo(new Vector3(-350, -300, 0)));
}
public void EpsonE2LScaraRobotLowerXyPlane()
{
// Arrange
var pose = new ScaraRobotPose {
Z = 318
};
// Act
var point = ScaraForwardKinematics(pose);
// Assert
Assert.That(point, Is.EqualTo(new Vector3(650, 0, 0)));
}
