dynamics_kinematics

Uses

- dynamics_helper
- dynamics_quaternions
- dynamics_error_handling
- dynamics_geometry
- iso_fortran_env
- nonlin
- linalg

Interfaces

coordinate_system dh_forward_kinematics dh_jacobian dh_parameter_set dh_table

Derived Types

coordinate_system dh_parameter_set dh_table

Functions

dh_matrix dh_rotate_x dh_rotate_z dh_translate_x dh_translate_z jacobian_generating_vector solve_inverse_kinematics transform_inverse

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. The axes are normalized and the $x$ axis is selected along the common normal of the two joint axes. For intersecting axes, $\hat{x}_i = \frac{\hat{z}_{i-1} \times \hat{z}_i} {\left\lVert \hat{z}_{i-1} \times \hat{z}_i \right\rVert}.$

Arguments

Type	Intent		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	Intent	Optional		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	Intent	Optional	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	Intent		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	Intent		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	Intent		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	Intent		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	Intent		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	Intent		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	Intent	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	Intent	Optional	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	Intent	Optional	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 $\dot{\vec{q}}$ to the end-effector velocity $\dot{\vec{X}}$ by $\dot{\vec{X}} = J \dot{\vec{q}}$ .

Arguments

Type	Intent	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	Intent		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	Intent	Optional	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		Name
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. The axes are normalized and the $x$ axis is selected along the common normal of the two joint axes. For intersecting axes, $\hat{x}_i = \frac{\hat{z}_{i-1} \times \hat{z}_i} {\left\lVert \hat{z}_{i-1} \times \hat{z}_i \right\rVert}.$
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		Name
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	Intent		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	Intent	Optional	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	Intent	Optional	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	Intent	Optional	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	Intent	Optional	Attributes		Name
real(kind=real64),	intent(in)			::	d	The translation.

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

The 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, $\vec{d}$ , and the joint axis unit vector, $\vec{k}$ .

Arguments

Type	Intent		Name
real(kind=real64),	intent(in)	::	d(3)	The position vector of the end-effector, $\vec{d}$ , 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: $T_{i} T_{i+1} ... T_{n}$ .
real(kind=real64),	intent(in)	::	k(3)	The unit vector defining the joint axis, $\vec{k}$ , given in the base coordinate frame. This vector can be computed most easily by using the transformation matrix: $T = T_1 T_2 ... T_{i-1}$ and then computing $\vec{k_{i-1}} = T \hat{k}$ .
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) result(rst)

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

Arguments

Type	Intent	Optional	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.

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

An M-element array containing the computed joint variables.

public pure function transform_inverse(x) result(rst)

Computes the inverse of a 4-by-4 homogeneous transformation matrix.

Arguments

Type	Intent	Optional	Attributes		Name
real(kind=real64),	intent(in)			::	x(4,4)	The 4-by-4 transformation matrix.

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

The inverted 4-by-4 transformation matrix.

dynamics_kinematics Module

Contents

Uses

Contents

Variables

Interfaces

public interface coordinate_system

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

Arguments

Return Value type(coordinate_system)

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

Arguments

Return Value type(coordinate_system)

public interface dh_forward_kinematics

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

Arguments

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

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

Arguments

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

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

Arguments

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

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

Arguments

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

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

Arguments

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

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

Arguments

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

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

Arguments

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

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

Arguments

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

private pure function dh_forward_kinematics_dh_params(x) result(rst)

Arguments

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

private pure function dh_forward_kinematics_dh_table(x) result(rst)

Arguments

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

public interface dh_jacobian

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

Arguments

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

public interface dh_parameter_set

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

Arguments

Return Value type(dh_parameter_set)

public interface dh_table

private pure function dh_table_init(c) result(rst)

Arguments

Return Value type(dh_table)

Derived Types

type, public :: coordinate_system

Components

Constructor

type, public :: dh_parameter_set

Components

Constructor

type, public :: dh_table

Components

Constructor

Functions

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

Arguments

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

public pure function dh_rotate_x(alpha) result(rst)

Arguments

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

public pure function dh_rotate_z(theta) result(rst)

Arguments

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

public pure function dh_translate_x(a) result(rst)

Arguments

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

public pure function dh_translate_z(d) result(rst)