dynamics_kinematics Module


Uses


Contents

Variables

Interfaces

Derived Types

Functions


Variables

Type Visibility Attributes Name Initial
integer(kind=int32), public, parameter :: PRISMATIC_JOINT = 1

Defines a prismatic joint.
integer(kind=int32), public, parameter :: REVOLUTE_JOINT = 0

Defines a revolute joint.

Interfaces

public interface coordinate_system

  • private pure function define_link_csys(xim1, zim1, zi, rim1, ri) result(rst)

    Defines the DH coordinate system for the specified link.

    Arguments

    Type IntentOptional Attributes Name
    real(kind=real64), intent(in) :: xim1(3)

    The x-axis of the previous link.
    real(kind=real64), intent(in) :: zim1(3)

    The z-axis of the previous link. This is also the axis of the proximal joint of the current link.
    real(kind=real64), intent(in) :: zi(3)

    The axis of the distal joint of the current link.
    real(kind=real64), intent(in) :: rim1(3)

    The location of the proximal joint's center or home position.
    real(kind=real64), intent(in) :: ri(3)

    The location of the distal joint's center or home position.

    Return Value type(coordinate_system)

    The resulting coordinate system.

  • private pure function define_csys(i, j, k, o) result(rst)

    Defines a new coordinate_system object. It is the callers responsibility to ensure that the supplied vectors are orthogonal to one another.

    Arguments

    Type IntentOptional Attributes Name
    real(kind=real64), intent(in) :: i(3)

    The x-axis unit vector.
    real(kind=real64), intent(in) :: j(3)

    The y-axis unit vector.
    real(kind=real64), intent(in) :: k(3)

    The x-axis unit vector.
    real(kind=real64), intent(in), optional :: o(3)

    The location of the coordinate system within a reference coordinate system. If not supplied, a value of (0, 0, 0) will be used.

    Return Value type(coordinate_system)

    The new coordinate_system object.

public interface dh_forward_kinematics

  • 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.

  • private pure function dh_forward_kinematics_dh_params(x) 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
    class(dh_parameter_set), intent(in), dimension(:) :: x

    The list of DH parameters.

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

    The resulting 4-by-4 transformation matrix.

  • private pure function dh_forward_kinematics_dh_table(x) 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
    class(dh_table), intent(in) :: x

    The DH table.

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

    The resulting 4-by-4 transformation matrix.

public interface dh_jacobian

  • private function dh_build_jacobian(alpha, a, theta, d, jtypes) result(rst)

    Builds the Jacobian matrix for a linkage given the Denavit-Hartenberg parameters. The first entry in each array must be from the first link nearest ground. The Jacobian matrix relates the joint velocities to the end-effector velocity by .

    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.
    integer(kind=int32), intent(in), dimension(size(alpha)) :: jtypes

    The types of each joint. Must be either REVOLUTE_JOINT or PRISMATIC_JOINT. The code defaults to REVOLUTE_JOINT.

    Return Value real(kind=real64), allocatable, dimension(:,:)

    The resulting 6-by-N Jacobian matrix where N is the number of joint variables (i.e. the length of the input arrays).

public interface dh_parameter_set

  • private pure function dps_init(length, twist, offset, angle) result(rst)

    Constructs a new dh_parameter_set object.

    Arguments

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

    The link length.
    real(kind=real64), intent(in) :: twist

    The link twist.
    real(kind=real64), intent(in) :: offset

    The link offset.
    real(kind=real64), intent(in) :: angle

    The joint angle.

    Return Value type(dh_parameter_set)

    The new dh_parameter_set object.

public interface dh_table

  • private pure function dh_table_init(c) result(rst)

    Constructs a Denavit-Hartenberg table given the locations and orientations of a linkage.

    Arguments

    Type IntentOptional Attributes Name
    class(coordinate_system), intent(in), dimension(:) :: c

    An N-element array containing the coordinate frames for each of the N links of the linkage.

    Return Value type(dh_table)

    The resulting Denavit-Hartenberg table.


Derived Types

type, public ::  coordinate_system

Defines a 3D Cartesian coordinate system.

Components

Type Visibility Attributes Name Initial
real(kind=real64), public :: i(3)

The unit vector along the coordinate system's x-axis.
real(kind=real64), public :: j(3)

The unit vector along the coordinate system's y-axis.
real(kind=real64), public :: k(3)

The unit vector along the coordinate system's z-axis.
real(kind=real64), public :: origin(3)

The location of the origin of the coordinate system in the parent coordinate system.

Constructor

private pure function define_link_csys (xim1, zim1, zi, rim1, ri)

Defines the DH coordinate system for the specified link.
private pure function define_csys (i, j, k, o)

Defines a new coordinate_system object. It is the callers responsibility to ensure that the supplied vectors are orthogonal to one another.

type, public ::  dh_parameter_set

Describes a set of Denavit-Hartenberg parameters for a single joint.

Components

Type Visibility Attributes Name Initial
real(kind=real64), public :: joint_angle

The joint angle is the required rotation of the previous link's x-axis about the proximal joint's axis to become parallel to the current link's x-axis.
real(kind=real64), public :: link_length

The link length is the distance between the proximal and distal joint axes as measured along the link's x-axis.
real(kind=real64), public :: link_offset

The link offset is the distance between the previous link's x-axis and the current link's x-axis as measured along the axis of the proximal joint.
real(kind=real64), public :: link_twist

The link twist is the required rotation of the proximal joint axis about the link's x-axis to become parallel to the distal joint's axis.

Constructor

private pure function dps_init (length, twist, offset, angle)

Constructs a new dh_parameter_set object.

type, public ::  dh_table

Describes a Denavit-Hartenberg table.

Components

Type Visibility Attributes Name Initial
type(dh_parameter_set), public, allocatable, dimension(:) :: parameters

A collection of DH parameters for each joint in the linkage.

Constructor

private pure function dh_table_init (c)

Constructs a Denavit-Hartenberg table given the locations and orientations of a linkage.

Functions

public pure function dh_matrix(alpha, a, theta, d) result(rst)

Computes the Denavit-Hartenberg transformation matrix for the specified DH parameters.

Arguments

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

The link twist angle, 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) :: a

The link length as measured along the link's x-axis.
real(kind=real64), intent(in) :: theta

The joint angle, 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) :: d

The joint offset 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.

public pure function dh_rotate_x(alpha) result(rst)

Computes the Denavit-Hartenberg matrix for a local x-axis rotation.

Arguments

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

The rotation angle, in radians.

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

The matrix.

public pure function dh_rotate_z(theta) result(rst)

Computes the Denavit-Hartenberg matrix for a local z-axis rotation.

Arguments

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

The rotation angle, in radians.

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

The matrix.

public pure function dh_translate_x(a) result(rst)

Computes the Denavit-Hartenberg matrix for a local x-axis translation.

Arguments

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

The translation.

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

The matrix.

public pure function dh_translate_z(d) result(rst)

Computes the Denavit-Hartenberg matrix for a local z-axis translation.

Arguments

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

The translation.

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

The matrix.

public pure function identity_4() result(rst)

Computes a 4-by-4 identity matrix.

Arguments

None

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

The resulting identity matrix.

public pure function jacobian_generating_vector(d, k, R, jtype) result(rst)

Computes a single Jacobian generating vector given the position vector of the link origin, , and the joint axis unit vector, .

Read more…

Arguments

Type IntentOptional Attributes Name
real(kind=real64), intent(in) :: d(3)

The position vector of the end-effector, , relative to the link coordinate frame given in the base coordinate frame. An easy way to compute this vector is to extract the first 3 elements of the 4th column of the transformation matrix: .
real(kind=real64), intent(in) :: k(3)

The unit vector defining the joint axis, , given in the base coordinate frame. This vector can be computed most easily by using the transformation matrix: and then computing .
real(kind=real64), intent(in) :: R(3,3)

The rotation matrix defining the orientation of the link coordinate frame relative to the base coordinate frame.
integer(kind=int32), intent(in) :: jtype

The joint type. Must be either REVOLUTE_JOINT or PRISMATIC_JOINT. If incorrectly specified, the code defaults to a REVOLUTE_JOINT type.

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

The resulting 6-element Jacobian generating vector.

public function solve_inverse_kinematics(mdl, qo, constraints, df, slvr, ib, jfcn, qmax, qmin, args, err) result(rst)

Solves the inverse kinematics problem for a linkage. An iterative solution procedure is utilized.

Arguments

Type IntentOptional Attributes Name
procedure(vecfcn), intent(in), pointer :: mdl

A routine used to compute the forward kinematics for the linkage given the current joint variable estimates.
real(kind=real64), intent(in), dimension(:) :: qo

An M-element array containing an initial estimate of the M joint variables.
real(kind=real64), intent(in), target, dimension(:) :: constraints

An N-element array containing the target values (constraints) for each of the N kinematic equations in the model. N must be at least equal to M (the number of joint variables).
real(kind=real64), intent(out), optional, target, dimension(:) :: df

An optional N-element array that, if supplied, can be used to retrieve the residuals of each of the N kinematic equations.
class(constrained_equation_solver), intent(inout), optional, target :: slvr

An optional solver that can be used in place of the default Levenberg-Marquardt solver.
type(iteration_behavior), intent(out), optional :: ib

An optional output that can be used to gather information on the solver.
procedure(jacobianfcn), intent(in), optional, pointer :: jfcn

A routine that can be used to provide the Jacobian matrix for the linkage.
real(kind=real64), intent(in), optional, dimension(size(qo)) :: qmax

An optional set of upper limits on each of the joint variables. If not provided, the default is the output of the 'huge' intrinsic function.
real(kind=real64), intent(in), optional, dimension(size(qo)) :: qmin

An optional set of lower limits on each of the joint variables. If not provided, the default is the negative of the output of the 'huge' intrinsic function.
class(*), intent(inout), optional, target :: args

An optional argument that can be used to communicate with mdl.
class(errors), intent(inout), optional, target :: err

An errors-based object that if provided can be used to retrieve information relating to any errors encountered during execution.

Return Value real(kind=real64), allocatable, dimension(:)

An M-element array containing the computed joint variables.