jacobian_generating_vector Function

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}$.

For a revolute joint:

$$\vec{c_{i}} = \left( \begin{matrix} R \left( \hat{k} \times \vec{d_{i-1}} \right) \\ \vec{k_{i-1}} \end{matrix} \right)$$

For a prismatic joint:

$$\vec{c_{i}} = \left( \begin{matrix} \vec{k_{i-1}} \\ 0 \end{matrix} \right)$$

The Jacobian matrix is then constructed from the Jacobian generating vectors as follows.

$$J = \left[ \begin{matrix} \vec{c_1} & \vec{c_2} & ... & \vec{c_n} \end{matrix} \right]$$

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.

Contents