Forward kinematics: Findling the positions of points on lynx robot

Question

I have a task to find position of 5 points on lynx robot. I need to write below function. This function takes theta values as input.

function [ pos ] = lynx_fk( theta1, theta2, theta3, theta4, theta5 )

I have also given the kinematics chain to decode a(r), alpha and d values.

I have already written a function which computes the dh matrix, which is as follows.

function A = compute_dh_matrix(r, alpha, d, theta).

Now i'm confused on how to find the positins of points? My approach is: From the DH matrix i have to extract the values of translation vector(A[1,4], A[2,4] and A[3,4]).

Plese comment and correct my approach.

Thanks for your time.

Answer 1

it is hard to answer since you did not specify the 5 points you are looking for.

The DH Matrix is a transformation matrix between two coordinate systems. If you are looking for a point on a rigid body, you should identify the DH transformation chain leading to the coordinate system associated with that rigid body. I.e. if you are looking for the coordinates in world frame of the center of mass on linkage 3, you should identify the chain of transformation matrices from the world frame to linkage 3:

$A_{03}= A_{01} \times A_{12} \times A_{23}$

Next you will need the vector pointing from the frame asociated with linkage 3 to the center of mass:

$ v_{CoM} = [x_{CoM}, y_{CoM}, z_{CoM}]$

The coordinates of the center of mass in World frame will be:

$$ \left[ \begin{array}{c @{{}={}} c c @{{}+{}} c c} v_{CoM}^{world} \\ 1 \end{array} \right]= A_{03} \times \left[ \begin{array}{c @{{}={}} c c @{{}+{}} c c} v_{CoM} \\ 1 \end{array} \right] $$

or expanded:

$$ \left[ \begin{array}{c @{{}={}} c c @{{}+{}} c c} x_{CoM}^{world} \\ y_{CoM}^{world} \\ z_{CoM}^{world} \\ 1 \end{array} \right]= A_{03} \times \left[ \begin{array}{c @{{}={}} c c @{{}+{}} c c} x_{CoM} \\ y_{CoM} \\ z_{CoM} \\ 1 \end{array} \right] $$

iff $v_{CoM} = [0, 0 ,0]$ you can take the elements of the last column of the transfromation matrix directly.