dh_forward_kinematics Interface

public interface dh_forward_kinematics

Contents


Module Procedures

private pure function dh_forward_kinematics_2(T1, T2) result(rst)

Assembles all of the individual link transformation matrices into a single transformation matrix locating the end-effector in the parent coordinate system for the overall mechanism.

Arguments

Type IntentOptional Attributes Name
real(kind=real64), intent(in) :: T1(4,4)

The transformation matrix for the first link nearest ground in the linkage.

real(kind=real64), intent(in) :: T2(4,4)

The transformation matrix for the second link in the linkage.

Return Value real(kind=real64), (4,4)

The resulting transformation matrix.

private pure function dh_forward_kinematics_3(T1, T2, T3) result(rst)

Assembles all of the individual link transformation matrices into a single transformation matrix locating the end-effector in the parent coordinate system for the overall mechanism.

Arguments

Type IntentOptional Attributes Name
real(kind=real64), intent(in) :: T1(4,4)

The transformation matrix for the first link nearest ground in the linkage.

real(kind=real64), intent(in) :: T2(4,4)

The transformation matrix for the second link in the linkage.

real(kind=real64), intent(in) :: T3(4,4)

The transformation matrix for the third link in the linkage.

Return Value real(kind=real64), (4,4)

The resulting transformation matrix.

private pure function dh_forward_kinematics_4(T1, T2, T3, T4) result(rst)

Assembles all of the individual link transformation matrices into a single transformation matrix locating the end-effector in the parent coordinate system for the overall mechanism.

Arguments

Type IntentOptional Attributes Name
real(kind=real64), intent(in) :: T1(4,4)

The transformation matrix for the first link nearest ground in the linkage.

real(kind=real64), intent(in) :: T2(4,4)

The transformation matrix for the second link in the linkage.

real(kind=real64), intent(in) :: T3(4,4)

The transformation matrix for the third link in the linkage.

real(kind=real64), intent(in) :: T4(4,4)

The transformation matrix for the fourth link in the linkage.

Return Value real(kind=real64), (4,4)

The resulting transformation matrix.

private pure function dh_forward_kinematics_5(T1, T2, T3, T4, T5) result(rst)

Assembles all of the individual link transformation matrices into a single transformation matrix locating the end-effector in the parent coordinate system for the overall mechanism.

Arguments

Type IntentOptional Attributes Name
real(kind=real64), intent(in) :: T1(4,4)

The transformation matrix for the first link nearest ground in the linkage.

real(kind=real64), intent(in) :: T2(4,4)

The transformation matrix for the second link in the linkage.

real(kind=real64), intent(in) :: T3(4,4)

The transformation matrix for the third link in the linkage.

real(kind=real64), intent(in) :: T4(4,4)

The transformation matrix for the fourth link in the linkage.

real(kind=real64), intent(in) :: T5(4,4)

The transformation matrix for the fifth link in the linkage.

Return Value real(kind=real64), (4,4)

The resulting transformation matrix.

private pure function dh_forward_kinematics_6(T1, T2, T3, T4, T5, T6) result(rst)

Assembles all of the individual link transformation matrices into a single transformation matrix locating the end-effector in the parent coordinate system for the overall mechanism.

Arguments

Type IntentOptional Attributes Name
real(kind=real64), intent(in) :: T1(4,4)

The transformation matrix for the first link nearest ground in the linkage.

real(kind=real64), intent(in) :: T2(4,4)

The transformation matrix for the second link in the linkage.

real(kind=real64), intent(in) :: T3(4,4)

The transformation matrix for the third link in the linkage.

real(kind=real64), intent(in) :: T4(4,4)

The transformation matrix for the fourth link in the linkage.

real(kind=real64), intent(in) :: T5(4,4)

The transformation matrix for the fifth link in the linkage.

real(kind=real64), intent(in) :: T6(4,4)

The transformation matrix for the sixth link in the linkage.

Return Value real(kind=real64), (4,4)

The resulting transformation matrix.

private pure function dh_forward_kinematics_7(T1, T2, T3, T4, T5, T6, T7) result(rst)

Assembles all of the individual link transformation matrices into a single transformation matrix locating the end-effector in the parent coordinate system for the overall mechanism.

Arguments

Type IntentOptional Attributes Name
real(kind=real64), intent(in) :: T1(4,4)

The transformation matrix for the first link nearest ground in the linkage.

real(kind=real64), intent(in) :: T2(4,4)

The transformation matrix for the second link in the linkage.

real(kind=real64), intent(in) :: T3(4,4)

The transformation matrix for the third link in the linkage.

real(kind=real64), intent(in) :: T4(4,4)

The transformation matrix for the fourth link in the linkage.

real(kind=real64), intent(in) :: T5(4,4)

The transformation matrix for the fifth link in the linkage.

real(kind=real64), intent(in) :: T6(4,4)

The transformation matrix for the sixth link in the linkage.

real(kind=real64), intent(in) :: T7(4,4)

The transformation matrix for the seventh link in the linkage.

Return Value real(kind=real64), (4,4)

The resulting transformation matrix.

private pure function dh_forward_kinematics_8(T1, T2, T3, T4, T5, T6, T7, T8) result(rst)

Assembles all of the individual link transformation matrices into a single transformation matrix locating the end-effector in the parent coordinate system for the overall mechanism.

Arguments

Type IntentOptional Attributes Name
real(kind=real64), intent(in) :: T1(4,4)

The transformation matrix for the first link nearest ground in the linkage.

real(kind=real64), intent(in) :: T2(4,4)

The transformation matrix for the second link in the linkage.

real(kind=real64), intent(in) :: T3(4,4)

The transformation matrix for the third link in the linkage.

real(kind=real64), intent(in) :: T4(4,4)

The transformation matrix for the fourth link in the linkage.

real(kind=real64), intent(in) :: T5(4,4)

The transformation matrix for the fifth link in the linkage.

real(kind=real64), intent(in) :: T6(4,4)

The transformation matrix for the sixth link in the linkage.

real(kind=real64), intent(in) :: T7(4,4)

The transformation matrix for the seventh link in the linkage.

real(kind=real64), intent(in) :: T8(4,4)

The transformation matrix for the eigth link in the linkage.

Return Value real(kind=real64), (4,4)

The resulting transformation matrix.

private pure function dh_forward_kinematics_array(alpha, a, theta, d) result(rst)

Assembles all of the individual link transformation matrices into a single transformation matrix locating the end-effector in the parent coordinate system for the overall mechanism. The first entry must be from the first link nearest ground.

Arguments

Type IntentOptional Attributes Name
real(kind=real64), intent(in), dimension(:) :: alpha

The link twist angles, in radians. This angle is the required rotation of the z(i-1) axis about the link's x-axis to become parallel with the link's z-axis.

real(kind=real64), intent(in), dimension(size(alpha)) :: a

The link lengths as measured along the link's x-axis.

real(kind=real64), intent(in), dimension(size(alpha)) :: theta

The joint angles, in radians. This angle is the required rotation of the z(i-1) axis about the z(i-1) axis to become parallel with the link's x-axis.

real(kind=real64), intent(in), dimension(size(alpha)) :: d

The joint offsets distance measured as the distance between the x(i-1) axis and the link's x-axis along the z(i-1) axis.

Return Value real(kind=real64), (4,4)

The resulting 4-by-4 transformation matrix.