Hi,

I would like to implement an unmaned underwater vehicle (UUV) simulator with

controllers in Orocos. RTT/KDL seems to have almost everything I need to do

so, except for two things wich seem to be partly missing:

- The pseudo-inverse of a Matrix

- Quaternions and the conversions from/to Euler-angle (RPY), Transform and

Rotation matrices.

I found some code about the pseudo-inverse (kdl/experimental/pinv.hpp) and

the quaternions [1]. I would like to have the status of these features?

Also, since KDL is using Eigen2 I would like to know why the

experimental/pinv.hpp don't use Eigen2/SVD ?

Thank you!

[1]

http://www.orocos.org/forum/orocos/orocos-users/kdl-patch-get-quaternion...

*Philippe Hamelin*, ing. jr, M. Ing

Chercheur / Researcher

Tel: (450) 652-8499 x2198

Fax: (450) 652-1316

*Institut de recherche d'Hydro-Québec*

*Unité robotique et civil*

1740, boul. Lionel-Boulet

Varennes (QC) J1X 1S1, Canada

## Pseudo-Inverse and Quaternion

On Fri, Jan 15, 2010 at 3:11 PM, Philippe Hamelin

<philippe [dot] hamelin [..] ...> wrote:

> Hi,

> I would like to implement an unmaned underwater vehicle (UUV) simulator with

> controllers in Orocos. RTT/KDL seems to have almost everything I need to do

> so, except for two things wich seem to be partly missing:

>

> - The pseudo-inverse of a Matrix

> - Quaternions and the conversions from/to Euler-angle (RPY), Transform and

> Rotation matrices.

>

> I found some code about the pseudo-inverse (kdl/experimental/pinv.hpp) and

> the quaternions [1]. I would like to have the status of these features?

> Also, since KDL is using Eigen2 I would like to know why the

> experimental/pinv.hpp don't use Eigen2/SVD ?

You can find two algorithms that can help you to calculate the

pseudo-inverse of a jacobian and which are also used in the

iksolvervel classes:

(apparently they are not included in the doxygen API pages)

src/utilities/svd_eigen_HH.hpp: this is a svd decomposition based on

Householder rotations

src/utilities/svc_eigen_Macie.hpp: this is a svd decomposition based

on Givens rotations as described in MaciejewskiKlein89: The Singular

Value Decomposition: Computation and Applications to Robotics

The latter uses the previous decomposition to start from and is

therefore a lot faster in a continuous control setting, where the

jacobians from the previous decomposition to the next do not change

much.

I tried the Eigen2/SVD functions, but they are not real-time safe due

to allocations in the algorithm itself. Maybe the TLSF solver can help

you there. Also the results from the Eigen2/SVD functions near and in

singular positions are far from satisfying.

The transformations from/to quaternion, from/to euler and rpy

representations are all included in the Rotation class. KDL itself

always used RotationMatrix representation internally .

Ruben

> Thank you!

>

> [1]

> http://www.orocos.org/forum/orocos/orocos-users/kdl-patch-get-quaternion...

>

> Philippe Hamelin, ing. jr, M. Ing

> Chercheur / Researcher

>

> Tel: (450) 652-8499 x2198

> Fax: (450) 652-1316

>

> Institut de recherche d'Hydro-Québec

> Unité robotique et civil

> 1740, boul. Lionel-Boulet

> Varennes (QC) J1X 1S1, Canada

>

## Pseudo-Inverse and Quaternion

On Fri, Jan 15, 2010 at 4:21 PM, Ruben Smits

<ruben [dot] smits [..] ...>wrote:

> On Fri, Jan 15, 2010 at 3:11 PM, Philippe Hamelin

> <philippe [dot] hamelin [..] ...> wrote:

> > Hi,

> > I would like to implement an unmaned underwater vehicle (UUV) simulator

> with

> > controllers in Orocos. RTT/KDL seems to have almost everything I need to

> do

> > so, except for two things wich seem to be partly missing:

> >

> > - The pseudo-inverse of a Matrix

> > - Quaternions and the conversions from/to Euler-angle (RPY),

> Transform and

> > Rotation matrices.

> >

> > I found some code about the pseudo-inverse (kdl/experimental/pinv.hpp)

> and

> > the quaternions [1]. I would like to have the status of these features?

> > Also, since KDL is using Eigen2 I would like to know why the

> > experimental/pinv.hpp don't use Eigen2/SVD ?

>

> You can find two algorithms that can help you to calculate the

> pseudo-inverse of a jacobian and which are also used in the

> iksolvervel classes:

> (apparently they are not included in the doxygen API pages)

>

> src/utilities/svd_eigen_HH.hpp: this is a svd decomposition based on

> Householder rotations

>

> src/utilities/svc_eigen_Macie.hpp: this is a svd decomposition based

> on Givens rotations as described in MaciejewskiKlein89: The Singular

> Value Decomposition: Computation and Applications to Robotics

>

> The latter uses the previous decomposition to start from and is

> therefore a lot faster in a continuous control setting, where the

> jacobians from the previous decomposition to the next do not change

> much.

>

Do you have some figures at hand of estimated speedups for a particular

setup?

Also, there is currently no continuous control implementation that

incorporates weight matrices and damping in KDL, right? (I didn't find

anything like it in the source)

>

> I tried the Eigen2/SVD functions, but they are not real-time safe due

> to allocations in the algorithm itself. Maybe the TLSF solver can help

> you there. Also the results from the Eigen2/SVD functions near and in

> singular positions are far from satisfying.

>

True, although it looks like the SVD module of Eigen is currently

undergoing a major overhaul :)

Adolfo

>

> The transformations from/to quaternion, from/to euler and rpy

> representations are all included in the Rotation class. KDL itself

> always used RotationMatrix representation internally .

>

>

> Ruben

>

> > Thank you!

> >

> > [1]

> >

> http://www.orocos.org/forum/orocos/orocos-users/kdl-patch-get-quaternion...

> >

> > Philippe Hamelin, ing. jr, M. Ing

> > Chercheur / Researcher

> >

> > Tel: (450) 652-8499 x2198

> > Fax: (450) 652-1316

> >

> > Institut de recherche d'Hydro-Québec

> > Unité robotique et civil

> > 1740, boul. Lionel-Boulet

> > Varennes (QC) J1X 1S1, Canada

> >

> --

> Orocos-Users mailing list

> Orocos-Users [..] ...

> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users

>

## Pseudo-Inverse and Quaternion

On Fri, 15 Jan 2010, Adolfo Rodríguez Tsouroukdissian wrote:

> On Fri, Jan 15, 2010 at 4:21 PM, Ruben Smits <ruben [dot] smits [..] ...> wrote:

> On Fri, Jan 15, 2010 at 3:11 PM, Philippe Hamelin

> <philippe [dot] hamelin [..] ...> wrote:

> > Hi,

> > I would like to implement an unmaned underwater vehicle (UUV) simulator

> with

> > controllers in Orocos. RTT/KDL seems to have almost everything I need to

> do

> > so, except for two things wich seem to be partly missing:

> >

> > - The pseudo-inverse of a Matrix

> > - Quaternions and the conversions from/to Euler-angle (RPY), Transform and

> > Rotation matrices.

> >

> > I found some code about the pseudo-inverse (kdl/experimental/pinv.hpp) and

> > the quaternions [1]. I would like to have the status of these features?

> > Also, since KDL is using Eigen2 I would like to know why the

> > experimental/pinv.hpp don't use Eigen2/SVD ?

>

> You can find two algorithms that can help you to calculate the

> pseudo-inverse of a jacobian and which are also used in the

> iksolvervel classes:

> (apparently they are not included in the doxygen API pages)

>

> src/utilities/svd_eigen_HH.hpp: this is a svd decomposition based on

> Householder rotations

>

> src/utilities/svc_eigen_Macie.hpp: this is a svd decomposition based

> on Givens rotations as described in MaciejewskiKlein89: The Singular

> Value Decomposition: Computation and Applications to Robotics

>

> The latter uses the previous decomposition to start from and is

> therefore a lot faster in a continuous control setting, where the

> jacobians from the previous decomposition to the next do not change

> much.

>

>

> Do you have some figures at hand of estimated speedups for a particular setup?

That depends... Your setup could work with just one iteration per control

period, which can mean a speedup of 10-100 compared to the case where you

do iterations per control period until you reach a full convergence to an

"eps" which is very small.

> Also, there is currently no continuous control implementation that incorporates weight

> matrices and damping in KDL, right? (I didn't find anything like it in the source)

KDL is not about _control_! A typical robotics control _application_ will

have a need for some of the KDL features, but that's not a reason to begin

to add controllers to KDL :-)

> I tried the Eigen2/SVD functions, but they are not real-time safe due

> to allocations in the algorithm itself. Maybe the TLSF solver can help

> you there. Also the results from the Eigen2/SVD functions near and in

> singular positions are far from satisfying.

> True, although it looks like the SVD module of Eigen is currently undergoing a major

> overhaul :)

Some of Ruben's implementations are currently still more advanced than what

Eigen can offer. But, hopefully, Orocos can "outsource" all is linear

algebra to Eigen as quickly as possible :-) So, we keep it on our radar,

and we stimulate contributors to first look at Eigen before trying another

linear algebra project, or reinvent their own wheel :-)

Herman

## Pseudo-Inverse and Quaternion

On Fri, Jan 15, 2010 at 5:18 PM, Herman Bruyninckx <

Herman [dot] Bruyninckx [..] ...> wrote:

> On Fri, 15 Jan 2010, Adolfo Rodríguez Tsouroukdissian wrote:

>

> On Fri, Jan 15, 2010 at 4:21 PM, Ruben Smits <

>> ruben [dot] smits [..] ...> wrote:

>> On Fri, Jan 15, 2010 at 3:11 PM, Philippe Hamelin

>> <philippe [dot] hamelin [..] ...> wrote:

>> > Hi,

>> > I would like to implement an unmaned underwater vehicle (UUV)

>> simulator

>> with

>> > controllers in Orocos. RTT/KDL seems to have almost everything I

>> need to

>> do

>> > so, except for two things wich seem to be partly missing:

>> >

>> > - The pseudo-inverse of a Matrix

>> > - Quaternions and the conversions from/to Euler-angle (RPY),

>> Transform and

>> > Rotation matrices.

>> >

>> > I found some code about the pseudo-inverse

>> (kdl/experimental/pinv.hpp) and

>> > the quaternions [1]. I would like to have the status of these

>> features?

>> > Also, since KDL is using Eigen2 I would like to know why the

>> > experimental/pinv.hpp don't use Eigen2/SVD ?

>>

>> You can find two algorithms that can help you to calculate the

>> pseudo-inverse of a jacobian and which are also used in the

>> iksolvervel classes:

>> (apparently they are not included in the doxygen API pages)

>>

>> src/utilities/svd_eigen_HH.hpp: this is a svd decomposition based on

>> Householder rotations

>>

>> src/utilities/svc_eigen_Macie.hpp: this is a svd decomposition based

>> on Givens rotations as described in MaciejewskiKlein89: The Singular

>> Value Decomposition: Computation and Applications to Robotics

>>

>> The latter uses the previous decomposition to start from and is

>> therefore a lot faster in a continuous control setting, where the

>> jacobians from the previous decomposition to the next do not change

>> much.

>>

>>

>> Do you have some figures at hand of estimated speedups for a particular

>> setup?

>>

>

> That depends... Your setup could work with just one iteration per control

> period, which can mean a speedup of 10-100 compared to the case where you

> do iterations per control period until you reach a full convergence to an

> "eps" which is very small.

>

>

> Also, there is currently no continuous control implementation that

>> incorporates weight

>> matrices and damping in KDL, right? (I didn't find anything like it in the

>> source)

>>

>

> KDL is not about _control_! A typical robotics control _application_ will

> have a need for some of the KDL features, but that's not a reason to begin

> to add controllers to KDL :-)

Sure, I wasn't implying that KDL is about control. Maybe I phrased the above

question wrong. What I meant (which you already answer above) is:

The Givens rotations-based algorithm can be a lot faster in a continuous

control setting. How much faster compared to other alternatives that do not

exploit results from previous decompositions (like the one based on

Householder rotations)? :)

>

>

> I tried the Eigen2/SVD functions, but they are not real-time safe due

>> to allocations in the algorithm itself. Maybe the TLSF solver can

>> help

>> you there. Also the results from the Eigen2/SVD functions near and in

>> singular positions are far from satisfying.

>> True, although it looks like the SVD module of Eigen is currently

>> undergoing a major

>> overhaul :)

>>

>

> Some of Ruben's implementations are currently still more advanced than what

> Eigen can offer. But, hopefully, Orocos can "outsource" all is linear

> algebra to Eigen as quickly as possible :-) So, we keep it on our radar,

> and we stimulate contributors to first look at Eigen before trying another

> linear algebra project, or reinvent their own wheel :-)

>

> Herman

> --

> Orocos-Users mailing list

> Orocos-Users [..] ...

> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users

>

>

## Pseudo-Inverse and Quaternion

On Fri, 15 Jan 2010, Philippe Hamelin wrote:

> I would like to implement an unmaned underwater vehicle (UUV) simulator with controllers

> in Orocos.

Great! I hope you will start a design discussion before spending time on

implementations :-)

> RTT/KDL seems to have almost everything I need to do so,

and that is...?

> except for two things wich seem to be partly missing:

>

> - The pseudo-inverse of a Matrix

> - Quaternions and the conversions from/to Euler-angle (RPY), Transform and Rotation

> matrices.

>

> I found some code about the pseudo-inverse (kdl/experimental/pinv.hpp) and the

> quaternions [1]. I would like to have the status of these features?

The really interesting IK solvers are not using explicit pseudo-inverse

computations, but have recursive algorithms to reach the same goal. For the

"instantaneous motion specification" develpments, pseudo-inverses are still

being used, so they will remain a supported feature. However, the vision is

to avoid their explicit use whenever better alternatives are available.

> Also, since KDL is using Eigen2 I would like to know why the

> experimental/pinv.hpp don't use Eigen2/SVD ?

Because (i) "This is not considered to be part of the stable public API

yet. Changes may happen in future releases. See Experimental parts of

Eigen" <http://eigen.tuxfamily.org/dox/group__SVD__Module.html>,

and (ii) we have not yet had the urgent need to add it :-)

Herman

> Thank you!

>

> [1]http://www.orocos.org/forum/orocos/orocos-users/kdl-patch-get-quaternion-orientation-ro

> tation-matrix

>

> Philippe Hamelin, ing. jr, M. Ing

> Chercheur / Researcher

>

> Tel: (450) 652-8499 x2198

> Fax: (450) 652-1316

>

> Institut de recherche d'Hydro-Québec

> Unité robotique et civil

> 1740, boul. Lionel-Boulet

> Varennes (QC) J1X 1S1, Canada

>

>

>

--

K.U.Leuven, Mechanical Eng., Mechatronics & Robotics Research Group

<http://people.mech.kuleuven.be/~bruyninc> Tel: +32 16 328056

EURON Coordinator (European Robotics Research Network) <http://www.euron.org>

Open Realtime Control Services <http://www.orocos.org>

Associate Editor JOSER <http://www.joser.org>, IJRR <http://www.ijrr.org>

## Pseudo-Inverse and Quaternion

2010/1/15 Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>

> On Fri, 15 Jan 2010, Philippe Hamelin wrote:

>

> I would like to implement an unmaned underwater vehicle (UUV) simulator

>> with controllers

>> in Orocos.

>>

> Great! I hope you will start a design discussion before spending time on

> implementations :-)

>

>

> RTT/KDL seems to have almost everything I need to do so,

>>

> and that is...?

>

>

Almost everything that is in RTT for control purposes and the Matrix/Vector

representations and operations from KDL.

>

> except for two things wich seem to be partly missing:

>>

>> - The pseudo-inverse of a Matrix

>> - Quaternions and the conversions from/to Euler-angle (RPY), Transform and

>> Rotation

>> matrices.

>>

>> I found some code about the pseudo-inverse (kdl/experimental/pinv.hpp) and

>> the

>> quaternions [1]. I would like to have the status of these features?

>>

>

> The really interesting IK solvers are not using explicit pseudo-inverse

> computations, but have recursive algorithms to reach the same goal. For the

> "instantaneous motion specification" develpments, pseudo-inverses are still

> being used, so they will remain a supported feature. However, the vision is

> to avoid their explicit use whenever better alternatives are available.

>

>

The pseudo-inverse is not required for the simulation of the UUV. However,

this is often used in the controller as a way to translate the desired

rigid-body forces/torques to appropriate thruster forces. This is somewhat a

classic energy minimization problem. It could be a good idea to add the

rigid body dynamics to KDL but i'm not sure it's the case for the

controllers.

>

> Also, since KDL is using Eigen2 I would like to know why the

>> experimental/pinv.hpp don't use Eigen2/SVD ?

>>

> Because (i) "This is not considered to be part of the stable public API

> yet. Changes may happen in future releases. See Experimental parts of

> Eigen" <http://eigen.tuxfamily.org/dox/group__SVD__Module.html>,

> and (ii) we have not yet had the urgent need to add it :-)

>

> Herman

>

>

> Thank you!

>>

>> [1]

>> http://www.orocos.org/forum/orocos/orocos-users/kdl-patch-get-quaternion...

>> tation-matrix

>>

>> Philippe Hamelin, ing. jr, M. Ing

>> Chercheur / Researcher

>>

>> Tel: (450) 652-8499 x2198

>> Fax: (450) 652-1316

>>

>> Institut de recherche d'Hydro-Québec

>> Unité robotique et civil

>> 1740, boul. Lionel-Boulet

>> Varennes (QC) J1X 1S1, Canada

>>

>>

>>

>>

> --

> K.U.Leuven, Mechanical Eng., Mechatronics & Robotics Research Group

> <http://people.mech.kuleuven.be/~bruyninc<http://people.mech.kuleuven.be/%7Ebruyninc>>

> Tel: +32 16 328056

> EURON Coordinator (European Robotics Research Network) <

> http://www.euron.org>

> Open Realtime Control Services <http://www.orocos.org>

> Associate Editor JOSER <http://www.joser.org>, IJRR <http://www.ijrr.org>

## Pseudo-Inverse and Quaternion

On Fri, 15 Jan 2010, Philippe Hamelin wrote:

>

> 2010/1/15 Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>

> On Fri, 15 Jan 2010, Philippe Hamelin wrote:

>

> I would like to implement an unmaned underwater vehicle (UUV)

> simulator with controllers

> in Orocos.

>

> Great! I hope you will start a design discussion before spending time on

> implementations :-)

>

> RTT/KDL seems to have almost everything I need to do so,

>

> and that is...?

>

> Almost everything that is in RTT for control purposes and the Matrix/Vector

> representations and operations from KDL.

I meant: what is it exactly that you want to achieve? :-) (Not how you

think to achieve it, because there might be better ways... :-)

> except for two things wich seem to be partly missing:

>

> - The pseudo-inverse of a Matrix

> - Quaternions and the conversions from/to Euler-angle (RPY),

> Transform and Rotation

> matrices.

>

> I found some code about the pseudo-inverse

> (kdl/experimental/pinv.hpp) and the

> quaternions [1]. I would like to have the status of these

> features?

>

> The really interesting IK solvers are not using explicit pseudo-inverse

> computations, but have recursive algorithms to reach the same goal. For the

> "instantaneous motion specification" develpments, pseudo-inverses are still

> being used, so they will remain a supported feature. However, the vision is

> to avoid their explicit use whenever better alternatives are available.

>

> The pseudo-inverse is not required for the simulation of the UUV. However, this is often

> used in the controller as a way to translate the desired rigid-body forces/torques to

> appropriate thruster forces. This is somewhat a classic energy minimization problem. It

> could be a good idea to add the rigid body dynamics to KDL but i'm not sure it's the

> case for the controllers.

It is, of course: especially in the case of UUVs, the thrusters really

feel the dynamics a lot, and a dynamics-weighted inverse will give a much better

result than a generic pseudo inverse that basically sets the inertia to

that of a sphere with homogeneous mass distribution.

In addition, the Right Way (in my opinion :-)) to tackle this problem is to

attach virtual attractors to the trajectory you want your UUV to follow and

then to optimize the constrained optimization problem (following from

energy minimization with the constraints given by (i) the actuators, (ii)

the virtual attractors, and (iii) the 'driver' that you use to pull your

robot along the trajectory.

The certainly Wrong Way is to treat this underwater robot control problem

as the traditional motion control problem for machine tools or robots...

Herman

## Pseudo-Inverse and Quaternion

2010/1/15 Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>

> On Fri, 15 Jan 2010, Philippe Hamelin wrote:

>

>

>> 2010/1/15 Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>

>> On Fri, 15 Jan 2010, Philippe Hamelin wrote:

>>

>> I would like to implement an unmaned underwater vehicle (UUV)

>> simulator with controllers

>> in Orocos.

>>

>> Great! I hope you will start a design discussion before spending time on

>> implementations :-)

>>

>> RTT/KDL seems to have almost everything I need to do so,

>>

>> and that is...?

>>

>> Almost everything that is in RTT for control purposes and the

>> Matrix/Vector

>> representations and operations from KDL.

>>

>

> I meant: what is it exactly that you want to achieve? :-) (Not how you

> think to achieve it, because there might be better ways... :-)

>

>

The first step is to simulate the dynamic and kinematic models of a rigid

body. More specifically, the dynamic model has the same generalized form as

a manipulator, i.e.:

tau = M*q_dotdot + C*q_dot

where tau is the force/torque vector, M is the inertia matrix and C is the

Coriolis/centripetal forces. So I have to integrate the 6 differential

equations (D.E.) of q_dotdot = inv(M) * (tau - C*q_dot).

For the kinematic model, this is somewhat different than manipulators

because there's singularities in the Euler angles (RPY for pitch=+/- pi/2)

used to define the position/orientation of the rigid-body in a earth-fixed

reference frame. Thus, the kinematic model is composed of 7 D.E. : 3 for the

position and 4 for the quaternion. The quaternion maps the velocity from the

body-fixed frame to the earth-fixed frame. Since the mapping has no

singularity, it can always be inverted to obtain the orientation of the

rigid-body expressed in the earth-fixed reference frame. Briefly, the

quaternion is integrated instead of the Euler angles to avoid any

singularities.

When considering the particular case of a UUV, there's added mass and

inertia (hydrodynamics effects) to matrix "M" and the "C" matrix is also

modified to include gravity and Buoyancy forces. However, the simulation

procedure remains the same thing.

For the simulation of the rigid body, I don't currently know exactly what

parts of KDL should I use and what's missing. In our actual simulator (which

don't actually use Orocos), we have a single class representing the

rigid-body dynamics with a Runge-Kutta 4 integration. We have our own

vector/matrix library but we don't want to use it when we will port the

simulator to Orocos. I know that all vector/Matrix operations could be done

with Eigen2, but I don't figure yet if all the primitives (ex: 3x4 Jacobian

Matrix for Quaternion mapping to Euler angle) I would need are there. I

think I will first write a short document describing the differential

equations for the dynamic and kinematics models. Then, it will be clearer

for everyone what's needed and what's missing.

>

> except for two things wich seem to be partly missing:

>>

>> - The pseudo-inverse of a Matrix

>> - Quaternions and the conversions from/to Euler-angle (RPY),

>> Transform and Rotation

>> matrices.

>>

>> I found some code about the pseudo-inverse

>> (kdl/experimental/pinv.hpp) and the

>> quaternions [1]. I would like to have the status of these

>> features?

>>

>> The really interesting IK solvers are not using explicit pseudo-inverse

>> computations, but have recursive algorithms to reach the same goal. For

>> the

>> "instantaneous motion specification" develpments, pseudo-inverses are

>> still

>> being used, so they will remain a supported feature. However, the vision

>> is

>> to avoid their explicit use whenever better alternatives are available.

>>

>> The pseudo-inverse is not required for the simulation of the UUV. However,

>> this is often

>> used in the controller as a way to translate the desired rigid-body

>> forces/torques to

>> appropriate thruster forces. This is somewhat a classic energy

>> minimization problem. It

>> could be a good idea to add the rigid body dynamics to KDL but i'm not

>> sure it's the

>> case for the controllers.

>>

>

> It is, of course: especially in the case of UUVs, the thrusters really

> feel the dynamics a lot, and a dynamics-weighted inverse will give a much

> better

> result than a generic pseudo inverse that basically sets the inertia to

> that of a sphere with homogeneous mass distribution.

>

> In addition, the Right Way (in my opinion :-)) to tackle this problem is to

> attach virtual attractors to the trajectory you want your UUV to follow and

> then to optimize the constrained optimization problem (following from

> energy minimization with the constraints given by (i) the actuators, (ii)

> the virtual attractors, and (iii) the 'driver' that you use to pull your

> robot along the trajectory.

>

> The certainly Wrong Way is to treat this underwater robot control problem

> as the traditional motion control problem for machine tools or robots...

>

> Herman

> --

> Orocos-Users mailing list

> Orocos-Users [..] ...

> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users

>

>

## Pseudo-Inverse and Quaternion

On Fri, 15 Jan 2010, Philippe Hamelin wrote:

> 2010/1/15 Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>

> On Fri, 15 Jan 2010, Philippe Hamelin wrote:

>

> 2010/1/15 Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>

> On Fri, 15 Jan 2010, Philippe Hamelin wrote:

> I would like to implement an unmaned underwater

> vehicle (UUV)

> simulator with controllers

> in Orocos.

> Great! I hope you will start a design discussion before spending

> time on

> implementations :-)

> RTT/KDL seems to have almost everything I need to do so,

> and that is...?

> Almost everything that is in RTT for control purposes and the

> Matrix/Vector

> representations and operations from KDL.

>

> I meant: what is it exactly that you want to achieve? :-) (Not how you

> think to achieve it, because there might be better ways... :-)

>

> The first step is to simulate the dynamic and kinematic models of a rigid body. More

> specifically, the dynamic model has the same generalized form as a manipulator, i.e.:

>

> tau = M*q_dotdot + C*q_dot

>

> where tau is the force/torque vector, M is the inertia matrix and C is the

> Coriolis/centripetal forces. So I have to integrate the 6 differential equations (D.E.)

> of q_dotdot = inv(M) * (tau - C*q_dot).

Ok, that I understand.

> For the kinematic model, this is somewhat different than manipulators because there's

> singularities in the Euler angles (RPY for pitch=+/- pi/2) used to define the

> position/orientation of the rigid-body in a earth-fixed reference frame.

Oops, the same singularities occur for all frame orientations, including

those used for manipulators. In addition, there are 12 possible Euler angle

sets, hence a corresponding number of different singularities :-(

> Thus, the

> kinematic model is composed of 7 D.E. : 3 for the position and 4 for the quaternion. The

> quaternion maps the velocity from the body-fixed frame to the earth-fixed frame. Since

> the mapping has no singularity, it can always be inverted to obtain the orientation of

> the rigid-body expressed in the earth-fixed reference frame. Briefly, the quaternion is

> integrated instead of the Euler angles to avoid any singularities.

You have to take into account the quaternion constraint during this

inversion, which you undoubtedly do :-)

> When considering the particular case of a UUV, there's added mass and inertia

> (hydrodynamics effects) to matrix "M" and the "C" matrix is also modified to include

> gravity and Buoyancy forces. However, the simulation procedure remains the same thing.

> For the simulation of the rigid body, I don't currently know exactly what parts of KDL

> should I use and what's missing.

You should not use KDL at all! Since KDL is designed for _chains_, and not

for one single rigid body :-) Of course, it can solve that problem too, but

does not really have a real advantage. _Unless_ you add more constraints

than just the instantaneous "joint" (read, thruster) "torques"; more in

particular, the ones coming from the _trajectory_. These are important,

since your rigid body has to satisfy the conservation of angular momentum

constraint, and it's better to optimize this over a trajectory than only

instantaneously.

> In our actual simulator (which don't actually use Orocos),

Which is probably the best approach :-)

> we have a single class representing the rigid-body dynamics with a Runge-Kutta

> 4 integration. We have our own vector/matrix library but we don't want to use it when we

> will port the simulator to Orocos. I know that all vector/Matrix operations could be

> done with Eigen2, but I don't figure yet if all the primitives (ex: 3x4 Jacobian Matrix

> for Quaternion mapping to Euler angle) I would need are there. I think I will first

> write a short document describing the differential equations for the dynamic and

> kinematics models. Then, it will be clearer for everyone what's needed and what's

> missing.

>

Fine :-)

[...]

Herman

## Pseudo-Inverse and Quaternion

2010/1/15 Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>

> On Fri, 15 Jan 2010, Philippe Hamelin wrote:

>

> 2010/1/15 Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>

>> On Fri, 15 Jan 2010, Philippe Hamelin wrote:

>>

>> 2010/1/15 Herman Bruyninckx <

>> Herman [dot] Bruyninckx [..] ...>

>> On Fri, 15 Jan 2010, Philippe Hamelin wrote:

>> I would like to implement an unmaned underwater

>> vehicle (UUV)

>> simulator with controllers

>> in Orocos.

>> Great! I hope you will start a design discussion before

>> spending

>> time on

>> implementations :-)

>> RTT/KDL seems to have almost everything I need to do so,

>> and that is...?

>> Almost everything that is in RTT for control purposes and the

>> Matrix/Vector

>> representations and operations from KDL.

>>

>> I meant: what is it exactly that you want to achieve? :-) (Not how you

>> think to achieve it, because there might be better ways... :-)

>>

>> The first step is to simulate the dynamic and kinematic models of a rigid

>> body. More

>> specifically, the dynamic model has the same generalized form as a

>> manipulator, i.e.:

>>

>> tau = M*q_dotdot + C*q_dot

>>

>> where tau is the force/torque vector, M is the inertia matrix and C is the

>> Coriolis/centripetal forces. So I have to integrate the 6 differential

>> equations (D.E.)

>> of q_dotdot = inv(M) * (tau - C*q_dot).

>>

>

> Ok, that I understand.

>

>

> For the kinematic model, this is somewhat different than manipulators

>> because there's

>> singularities in the Euler angles (RPY for pitch=+/- pi/2) used to define

>> the

>> position/orientation of the rigid-body in a earth-fixed reference frame.

>>

>

> Oops, the same singularities occur for all frame orientations, including

> those used for manipulators. In addition, there are 12 possible Euler angle

> sets, hence a corresponding number of different singularities :-(

>

>

> Thus, the

>> kinematic model is composed of 7 D.E. : 3 for the position and 4 for the

>> quaternion. The

>> quaternion maps the velocity from the body-fixed frame to the earth-fixed

>> frame. Since

>> the mapping has no singularity, it can always be inverted to obtain the

>> orientation of

>> the rigid-body expressed in the earth-fixed reference frame. Briefly, the

>> quaternion is

>> integrated instead of the Euler angles to avoid any singularities.

>>

>

> You have to take into account the quaternion constraint during this

> inversion, which you undoubtedly do :-)

>

>

> When considering the particular case of a UUV, there's added mass and

>> inertia

>> (hydrodynamics effects) to matrix "M" and the "C" matrix is also modified

>> to include

>> gravity and Buoyancy forces. However, the simulation procedure remains the

>> same thing.

>>

>

> For the simulation of the rigid body, I don't currently know exactly what

>> parts of KDL

>> should I use and what's missing.

>>

>

> You should not use KDL at all! Since KDL is designed for _chains_, and not

> for one single rigid body :-) Of course, it can solve that problem too, but

> does not really have a real advantage. _Unless_ you add more constraints

> than just the instantaneous "joint" (read, thruster) "torques"; more in

> particular, the ones coming from the _trajectory_. These are important,

> since your rigid body has to satisfy the conservation of angular momentum

> constraint, and it's better to optimize this over a trajectory than only

> instantaneously.

>

>

It's clear that I won't use the _chains_ from KDL. But there's some parts

that I could use such as Quaternion, SVD and maybe pseudo-inverse.

>

> In our actual simulator (which don't actually use Orocos),

>>

>

> Which is probably the best approach :-)

>

>

> we have a single class representing the rigid-body dynamics with a

>> Runge-Kutta

>> 4 integration. We have our own vector/matrix library but we don't want to

>> use it when we

>> will port the simulator to Orocos. I know that all vector/Matrix

>> operations could be

>> done with Eigen2, but I don't figure yet if all the primitives (ex: 3x4

>> Jacobian Matrix

>> for Quaternion mapping to Euler angle) I would need are there. I think I

>> will first

>> write a short document describing the differential equations for the

>> dynamic and

>> kinematics models. Then, it will be clearer for everyone what's needed and

>> what's

>> missing.

>>

>>

> Fine :-)

> [...]

>

> Herman

> --

> Orocos-Users mailing list

> Orocos-Users [..] ...

> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users

>

>

## Pseudo-Inverse and Quaternion

On Fri, 15 Jan 2010, Philippe Hamelin wrote:

[...]

> For the simulation of the rigid body, I don't currently know exactly

> what parts of KDL

> should I use and what's missing.

>

> You should not use KDL at all! Since KDL is designed for _chains_, and not

> for one single rigid body :-) Of course, it can solve that problem too, but

> does not really have a real advantage. _Unless_ you add more constraints

> than just the instantaneous "joint" (read, thruster) "torques"; more in

> particular, the ones coming from the _trajectory_. These are important,

> since your rigid body has to satisfy the conservation of angular momentum

> constraint, and it's better to optimize this over a trajectory than only

> instantaneously.

>

> It's clear that I won't use the _chains_ from KDL. But there's some parts that I could

> use such as Quaternion, SVD and maybe pseudo-inverse.

Okay... but then Orocos/KDL becomes a large dependency, without offering

anything "exclusive" :-)

Herman

## Pseudo-Inverse and Quaternion

2010/1/15 Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>

> On Fri, 15 Jan 2010, Philippe Hamelin wrote:

>

> [...]

> > For the simulation of the rigid body, I don't currently know

> exactly

> > what parts of KDL

> > should I use and what's missing.

> >

> > You should not use KDL at all! Since KDL is designed for _chains_, and

> not

> > for one single rigid body :-) Of course, it can solve that problem too,

> but

> > does not really have a real advantage. _Unless_ you add more constraints

> > than just the instantaneous "joint" (read, thruster) "torques"; more in

> > particular, the ones coming from the _trajectory_. These are important,

> > since your rigid body has to satisfy the conservation of angular momentum

> > constraint, and it's better to optimize this over a trajectory than only

> > instantaneously.

> >

> > It's clear that I won't use the _chains_ from KDL. But there's some parts

> that I could

> > use such as Quaternion, SVD and maybe pseudo-inverse.

>

> Okay... but then Orocos/KDL becomes a large dependency, without offering

> anything "exclusive" :-)

>

>

I agree that it's a large dependency. I would like to avoid the recoding of

yet another Rotation/Transform/EulerAngle library. After some searches,

Eigen2 seems to have most of them. Maybe Eigen2 could just be the unique

dependancy...

> Herman

> --

> Orocos-Users mailing list

> Orocos-Users [..] ...

> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users

>