Pseudo-Inverse and Quaternion

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

Ruben Smits's picture

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 > 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
>