next up previous contents
Next: inertia Up: The Robot and mRobot Previous: Dynamics   Contents

Subsections

acceleration

Syntax

ReturnMatrix acceleration(const ColumnVector & q, 
                          const ColumnVector & qp,
                          const ColumnVector & tau);

ReturnMatrix acceleration(const ColumnVector & q, 
                          const ColumnVector & qp,
                          const ColumnVector & tau_cmd, 
                          const ColumnVector & Fext,
                          const ColumnVector & Next)

Description

This function computes $\ddot{\mbox{\boldmath$ q $}}$ from $\mbox{\boldmath$ q $}$, $\dot{\mbox{\boldmath$ q $}}$ and $\mbox{\boldmath$ \tau $}$ which is the forward dynamics problem. Walker and Orin [11] presented methods to compute the inverse dynamics. A simplified RNE version computing
$\displaystyle \mbox{\boldmath$ \tau $}$ $\textstyle =$ $\displaystyle \mbox{\boldmath$ D(q) $} \ddot{\mbox{\boldmath$ q $}}$ (2.67)

is implemented in the function torque_novelocity. By evaluating this equation $n$ times, one can compute $\mbox{\boldmath$ D(q) $}$ (the inertia function), use the full RNE to compute $\mbox{\boldmath$ C(q, $}\dot{\mbox{\boldmath$ q $}}) + \mbox{\boldmath$ G(q) $}$ and then solve the equation :
$\displaystyle \ddot{\mbox{\boldmath$ q $}}$ $\textstyle =$ $\displaystyle \mbox{\boldmath$ D^{-1}(q) $} \left [ \mbox{\boldmath$ \tau $} - ...
...ldmath$ C(q, $}\dot{\mbox{\boldmath$ q $}})
- \mbox{\boldmath$ G(q) $} \right ]$ (2.68)

Return Value

ColumnVector



Richard Gourdeau 2004-07-06