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

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