dynamics_kinematics Module

Uses

- nonlin
- dynamics_helper
- dynamics_error_handling
- iso_fortran_env
- ferror

Contents

Variables

PRISMATIC_JOINT REVOLUTE_JOINT

Interfaces

dh_forward_kinematics dh_jacobian

Functions

dh_matrix dh_rotate_x dh_rotate_z dh_translate_x dh_translate_z identity_4 jacobian_generating_vector solve_inverse_kinematics

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

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	Optional	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).

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

Arguments

Type	Intent	Optional	Attributes		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, args, err) 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 error in the kinematics equations based upon the current solution estimate.
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(least_squares_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.
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.