next up previous contents
Next: inv_kin_rhino Up: The Robot and mRobot Previous: Kinematics   Contents

Subsections

inv_kin

Syntax

ReturnMatrix inv_kin(const Matrix & Tobj,
                     const int mj = 0);
ReturnMatrix inv_kin(const Matrix & Tobj, 
                     const int mj, bool & converge);

Description

The inverse kinematic model is computed using a Newton-Raphson technique. If mj == 0, it is based on the following [6]:
$\displaystyle {}^0 \mbox{\boldmath$ T $}_n (\mbox{\boldmath$ q $}^*)$ $\textstyle =$ $\displaystyle {}^0 \mbox{\boldmath$ T $}_n (\mbox{\boldmath$ q $} + \delta \mbo...
...x{\boldmath$ T $} ( \delta \mbox{\boldmath$ q $}) = \mbox{\boldmath$ T $}_{obj}$ (2.35)
$\displaystyle \delta \mbox{\boldmath$ T $} ( \delta \mbox{\boldmath$ q $})$ $\textstyle =$ $\displaystyle ({}^0 \mbox{\boldmath$ T $}_n (\mbox{\boldmath$ q $}))^{-1} \mbox{\boldmath$ T $}_{obj} =
\mbox{\boldmath$ I + \Delta $}$ (2.36)
$\displaystyle \mbox{\boldmath$ \Delta $}$ $\textstyle =$ $\displaystyle \left[\begin{array}{cccc}
0 & -\delta_z & \delta_y & d_x \\
\del...
...x & d_y \\
-\delta_y & \delta_x & 0 & d_z \\
0 & 0 & 0 & 0 \end{array}\right]$ (2.37)
$\displaystyle {}^n \delta \mbox{\boldmath$ \chi $}$ $\textstyle =$ $\displaystyle \left[\begin{array}{cccccc}
d_x & d_y & d_z & \delta_x & \delta_y & \delta_z
\end{array}\right]^T$ (2.38)
$\displaystyle {}^n \delta \mbox{\boldmath$ \chi $}$ $\textstyle \approx$ $\displaystyle {}^n \mbox{\boldmath$ J $}(\mbox{\boldmath$ q $}) \delta \mbox{\boldmath$ q $}$ (2.39)

If mj == 1, it is based on the following Taylor expansion [6,7]:
$\displaystyle {}^0 \mbox{\boldmath$ T $}_n (\mbox{\boldmath$ q $}^*)$ $\textstyle = {}^0 \mbox{\boldmath$ T $}_n (\mbox{\boldmath$ q $} + \delta \mbox{\boldmath$ q $}) \approx$ $\displaystyle {}^0 \mbox{\boldmath$ T $}_n (\mbox{\boldmath$ q $}) + \sum_{i=1}^{n} \frac{\partial {}^0 \mbox{\boldmath$ T $}_n}{\partial q_i} \delta q_i$ (2.40)

The function dTdqi computes these partial derivatives.

Given the desired position represented by the homogeneous transform Tobj, this function return the column vector of joint variables that is corresponding to this position. On return, the value converge is true if the procedure has converge to values that give the correct position and false otherwise.

Note: mj == 0 is faster ( $\approx 1.8\times$) than mj == 1. Also, mj == 1 might converge when mj == 0 does not.

Return Value

ColumnVector


next up previous contents
Next: inv_kin_rhino Up: The Robot and mRobot Previous: Kinematics   Contents
Richard Gourdeau 2004-07-06