Orientantion in End-Effector

[Oscar]

I’m currently using SOFA with python. Using the “QPInverseProblemSolver” I’m trying to command a robot by giving the end-effector a desired position and orientation. The end-effector is defined by an EffectorNode, and the desired position and orientation is defined using a GoalNode as shown below:

   goalNode = rootNode.createChild('GoalNode')
    goalNode.createObject('EulerImplicit', rayleighStiffness=0.02, printLog=False, rayleighMass=0.01, firstOrder='1')
    goalNode.createObject('SparseLDLSolver', name='ldl')
    mo = goalNode.createObject('MechanicalObject', name='mo_goal', template='Vec3d', position='0 0 0', rotation='0 0 0') 
    goalNode.createObject('UniformMass')
    goalNode.createObject('GenericConstraintCorrection', name='GCC', solverName='ldl')

...

    EffectorNode = MecaModelNode.createChild('EffectorNode')
    EffectorNode.createObject('MechanicalObject', template='Vec3d', position='0 0 0', rotation='0 0 0' )
    EffectorNode.createObject('RigidMapping',  rigidIndexPerPoint='0')
    EffectorNode.createObject('PositionEffector', template='Vec3d', useDirections='1 1 1', use_orientation=True, indices='0', effectorGoal='@../../GoalNode/mo_goal.position')
    MecaModelNode.createObject('AdaptiveBeamForceFieldAndMass', massDensity=0.000155, name='LinkForceField', interpolation='@interpolation')

For a pure translation the end-effector reaches the desired position. However, a pure rotation seems not to work correctly for the end-effector. So I would like to know how to give a specific orientation to the end-effector?

Kind regards,
Oscar

[Hugo]

Hi @oscarg29 and welcome on the forum!

First of all, it must be noticed that you are using several private plugins, not available in the open core of SOFA. Is it correct? (I see AdaptiveBeamForceFieldAndMass or PositionEffector)

When you say that you apply a translation on the end-effector, where are you defining it in the scene file?

For translation/rotation transformation, we usually use the TransformEngine.

Hugo

[Author]

Posts