[SOLVED] MechanicalObject rotation

[BrunoB81HK]

Hi!

I’m currently working on a plugin to link a custom haptic device to a Sofa simulation. My goal is to move and rotate a mechanical object base on the position of my haptic device. So far, I implemented the translation and the force computation, but I can’t find how to apply a rotation that make sense. In my .scn file, the mechanical object is defined as such :

<Node name="Instr">
        <EulerImplicitSolver rayleighStiffness='0.1' rayleighMass='0.1'/>
        <CGLinearSolver threshold='1e-11' tolerance='1e-11' iterations='25'/>
        <MechanicalObject name='InstrDOFs' template='Rigid3d'/>
        <UniformMass name='InstrMass' totalMass='0.05'/>
        <LCPForceFeedback template="Rigid3d" name="ForceFeedback" listening="1" activate="1" forceCoef="1."/>

        <Node name='InstrCol'>
            <MechanicalObject name='InstrColModel' src='@../../InstrColMesh'/>
            <SphereCollisionModel contactStiffness='1' listRadius='0.0025'/>
            <RigidMapping input='@../InstrDOFs' output='@InstrColModel'/>
        </Node>

        <Node name='InstrVis'>
            <OglModel name='InstrVisModel' src='@../../InstrVisMesh'/>
            <RigidMapping input='@../InstrDOFs' output='@InstrVisModel'/>
        </Node>
    </Node>

In my plugin, I start by retrieving the pointer m_instrument to the mechanical object. From there, I compute the needed translation and apply it with m_instrument->applyTranslation(trans[0], trans[1], trans[2]). This part is working very well.

The next step is to apply the required rotation. My mechanical object is basically a long cylinder. I want the axis of the cylinder to pass through a fixed point in the simulation. This point stay static through the entire simulation. I can easily compute the required axis/angle pair to achieve my rotation. I then use m_instrument->applyRotation(q) to apply my rotation (q being a quaternion).

The problem is that the rotation is applied, but doesn’t match the one that I want. I don’t know if it as to do with reference frame, but I compute my quaternion in the general reference frame. I feel like the rotations might be applied in the mechanical object reference frame. I’m kinda lost on this one.

Thanks a lot for your help!

Bruno

[Hugo]

Hi @brunob81hk

I would first use for transformation a TransformEngine : this engine allows to apply a transformation (translation/rotation) and output the associated positions based on input positions. You can have a look at the example : TransformEngine. The TransformMatrixEngine might also be of interest.

I would recommend to decouple each transformation (translation // rotation).
I hope this helps.

Best wishes,

Hugo

[BrunoB81HK]

Hi @hugo

Thanks a lot for the answer!

I will try it later this week and I’ll keep you posted.

Have a nice day!

Bruno

[Hugo]

Hi @brunob81hk

Any update?
Best wishes,

Hugo

[BrunoB81HK]

Hi @hugo

Things are moving pretty fast here. This task is still in my backlog. I should be able to try it soon.

Have a nice day!

Bruno

[BrunoB81HK]

Hi @hugo

I finally had the time to thinker with a TransformEngine. It looks like its what I want, although I am not ale to use it from c++.

I now retrieve the pointer (m_instrument) to the TransformEngine instead of the MechanicalObject. I thought that I could use m_instrument->translation.setValue() to set the desired position, but m_instrument->translation is a protected member. I wanted to see if it was the only thing stopping me from using this method, so I set m_instrument->translation as a public member in TransformEngine.h. This method did work as I wanted it to.

Since I changed a part of the source code and I didn’t like it, I reverted the changes and tried to create my own TrexTransformEngine which is inherited from TransformEngine.

After a lot of tinkering, I came up with a solution. Here is my code :

#pragma once

#include <SofaGeneralEngine/TransformEngine.h>
#include <sofa/core/ObjectFactory.h>

namespace sofa::component::engine
{

template <class DataTypes>
class TrexTransformEngine : public TransformEngine<DataTypes>
{
public:
    SOFA_CLASS(SOFA_TEMPLATE(TrexTransformEngine, DataTypes), SOFA_TEMPLATE(TransformEngine, DataTypes));

protected:
    TrexTransformEngine() : TransformEngine<DataTypes>{ } {};
    ~TrexTransformEngine() override {}

public:
    void setTranslation(defaulttype::Vector3 trans)
    {
	this->translation.setValue(trans);
    }

    void setQuaternion(defaulttype::Quaternion quat)
    {
	this->quaternion.setValue(quat);
    }
};

int TrexTransformEngineClass = core::RegisterObject("Transform engine to help move the instrument.")
    .add< TrexTransformEngine<defaulttype::Rigid3Types> >(true) // default template
    .add< TrexTransformEngine<defaulttype::Vec1Types> >()
    .add< TrexTransformEngine<defaulttype::Vec2Types> >()
    .add< TrexTransformEngine<defaulttype::Vec3Types> >()
    .add< TrexTransformEngine<defaulttype::Rigid2Types> >()
    ;

template class TrexTransformEngine<defaulttype::Vec1Types>;
template class TrexTransformEngine<defaulttype::Vec2Types>;
template class TrexTransformEngine<defaulttype::Vec3Types>;
template class TrexTransformEngine<defaulttype::Rigid2Types>;
template class TrexTransformEngine<defaulttype::Rigid3Types>;

} //namespace sofa::component::engine

Is this a good way to do such a thing?

Thanks a lot for your help and have a nice day!

Bruno

[Hugo]

Hi @brunob81hk

Sorry but by reading throughout the topic again, I realize your purpose is to build a connector between an interface and a virtual object in the scene.

The way we usually do it is by building a C++ driver component (recovering information from your interface). As being a SOFA component, this driver has Data which a mechanical object could then be linked against.

Could you take a look at the Geomagic plugin (especially the class GeomagicDriver)?

Best

Hugo

[BrunoB81HK]

Hi @hugo

Just tried the method based on the Geomagic Driver and it works like a charm. This is everything I want.

Thanks a lot for your help and have a nice day!

Bruno

[Author]

Posts