Constrain and control movements of an object

[dlouis]

Hello everyone,

First, thanks for providing this nice tool that SOFA is.

My question is:
I have the mesh of a beam (basically just a rectangular parallelepiped). Using SOFAPYTHON3, I would like to
1) attach rigidly one face of the beam to a plate (or any other object), and then
2) move this plate (in translation and rotation) according to specific events triggered by my code (NOT by keyboard events).
For instance, depending on how the free end of the beam moves, the plate (which is fixed to the beam at the other end) would move differently. Between each time step, the state of the beam is re-evaluated, and the movement of the plate would be calculated accordingly.
3) To calculate 2), I will need to retrieve the position and orientation of some vertices and faces of my mesh. Is it possible directly with some SOFA functions ?

Could you please point me towards the easiest ways to achieve 1) , 2) and 3) ? I already looked at some examples but I do not always get what each function does, so if you could tell me directly which functions I would need with some sort of pseudo-code, that would be great !

Louis

[Hugo]

Hey @dlouis

I think a good example of what you would like is given in the MechanicalMatrixMapper.pyscn.

In the above-mentioned scene, if you add the following :

Plate = RigidNode.createChild('Plate')
        Plate.createObject("MeshGmshLoader", name="plateLoader", filename="mesh/simple_square.msh", translation="-5 -5 0")
        Plate.createObject("TriangleSetTopologyContainer", name="topo", src="@plateLoader")
        Plate.createObject("MechanicalObject", name="plateMO", template="Vec3d", showObject="1")
        Plate.createObject("TriangleCollisionModel")
        Plate.createObject("RigidMapping")

you would then have a beam with a plate at the extremity.
Best

Hugo

[dlouis]

Hi @Hugo,

Thanks for your help !
Indeed it seems to work, but I need to adapt it to my use case because I want to control the end with the plate, as if the plate was a gripper.

But in the meantime, I came across another and (I think) simpler way to do what I want: using beamFEMForceField.

I can run this simple seen, modified from “BeamForceField.scn” (I just removed some lines):

<?xml version="1.0"?>

<!-- BeamFEMForceField example -->
<Node name="root" dt="0.01">
    <RequiredPlugin pluginName="SofaOpenglVisual"/>
    <RequiredPlugin pluginName='SofaMiscMapping'/> 
    <RequiredPlugin pluginName='SofaGeneralLinearSolver'/>
    <RequiredPlugin pluginName='SofaGeneralTopology'/>
    <RequiredPlugin pluginName='SofaImplicitOdeSolver'/>
    <RequiredPlugin pluginName='SofaGeneralSimpleFem'/>
    <RequiredPlugin pluginName='SofaBoundaryCondition'/>
    <RequiredPlugin pluginName='SofaDenseSolver'/>
    <RequiredPlugin pluginName='SofaMeshCollision'/>
    <RequiredPlugin pluginName='SofaRigid'/>
    <RequiredPlugin pluginName='SofaConstraint'/>
    <RequiredPlugin pluginName='SofaLoader'/>
    
    <VisualStyle displayFlags="showBehaviorModels hideForceFields showCollisionModels hideVisual showInteractionForceFields" />
    <DefaultPipeline depth="6" verbose="0" draw="0" />
    <BruteForceBroadPhase/>
    <BVHNarrowPhase/>
    <MinProximityIntersection name="Proximity" alarmDistance="0.3" contactDistance="0.2" />
    <DefaultContactManager name="Response" response="default" />

    <Node name="beam-withPointCollision">
        <EulerImplicitSolver rayleighStiffness="1.0" printLog="false"  rayleighMass="0.2" />
        <BTDLinearSolver template="BTDMatrix6d" printLog="false" verbose="false" />
        <MechanicalObject template="Rigid3d" name="DOFs" position="0 0 0 0 0 0 1  1 0 0 0 0 0 1  2 0 0 0 0 0 1  3 0 0 0 0 0 1  4 0 0 0 0 0 1  5 0 0 0 0 0 1  6 0 0 0 0 0 1  7 0 0 0 0 0 1" />
        <MeshTopology name="lines" lines="0 1 1 2 2 3 3 4 4 5 5 6 6 7" />

        <FixedConstraint name="FixedConstraint" indices="0" />
        <UniformMass vertexMass="1 1 0.01 0 0 0 0.1 0 0 0 0.1" printLog="false" />
        <BeamFEMForceField name="FEM" radius="0.1" youngModulus="20000000" poissonRatio="0.49"/>

    </Node>
</Node>

Now, I’d like to do the same with SofaPyton3:

import Sofa
# Choose in your script to activate or not the GUI
USE_GUI = True

def main():
    import SofaRuntime
    import Sofa.Gui
    SofaRuntime.importPlugin("SofaComponentAll")
    root = Sofa.Core.Node("root")
    createScene(root)
    Sofa.Simulation.init(root)

    if not USE_GUI:
        for iteration in range(10):
            Sofa.Simulation.animate(root, root.dt.value)
    else:
        Sofa.Gui.GUIManager.Init("myscene", "qglviewer")
        Sofa.Gui.GUIManager.createGUI(root, __file__)
        Sofa.Gui.GUIManager.SetDimension(1080, 1080)
        Sofa.Gui.GUIManager.MainLoop(root)
        Sofa.Gui.GUIManager.closeGUI()

def createScene(root):

    root.dt = 0.01
    root.name = 'root'
    root.gravity = [0.0, 0.0, -9.81]
    root.addObject('DefaultAnimationLoop')
    root.addObject("EulerImplicitSolver", rayleighStiffness=1.2, rayleighMass= 0.1)
    root.addObject("CGLinearSolver", tolerance=1e-12, threshold=1e-12, iterations=25)
    # root.addObject('BTDLinearSolver', template="BTDMatrix6d" , printLog="false" , verbose="false")
    # root.addObject("SparseLDLSolver")
    
    beam = root.addChild('Beam') 
    beam.addObject('MechanicalObject', template="Rigid3d", name="dofs", position = "0 0 0 0 0 0 1  1 0 0 0 0 0 1  2 0 0 0 0 0 1  3 0 0 0 0 0 1  4 0 0 0 0 0 1  5 0 0 0 0 0 1  6 0 0 0 0 0 1  7 0 0 0 0")
    beam.addObject('MeshTopology', name="lines" , lines="0 1 1 2 2 3 3 4 4 5 5 6 6 7")
    beam.addObject('FixedConstraint', name="FixedConstraint" , indices="0" )
    beam.addObject('BeamFEMForceField', name="FEM", poissonRatio=0.4, youngModulus=100000)
    beam.addObject('UniformMass', totalMass='1', printLog="False" )
    # beam.init()

    return root

if __name__ == '__main__':
    main()

Using python3 myscript.py , I can load the scene, but as soon as I click “animate”, I obtain the following errors:

---------------------------------------
Checking SOFA_ROOT and SOFAPYTHON3_ROOT
Using environment variable SOFA_ROOT: /home/louis/Documents/sofa/build/
Warning: environment variable SOFAPYTHON3_ROOT is empty. Trying to guess it.
Guessed SOFAPYTHON3_ROOT: /home/louis/Documents/sofa/build
---------------------------------------
[WARNING] [qt.conf.h] Cannot open file /home/louis/Documents/sofa/build/bin/qt.conf
[WARNING] [Sofa.Gui] Failed loading and/or customizing qt.conf from /home/louis/Documents/sofa/build/bin/qt.conf
qt_resource_data:

[INFO]    [PluginManager] Loaded plugin: /home/louis/Documents/sofa/build/lib/libSofaComponentAll.so
[WARNING] [Simulation] Default Visual Manager Loop will be used. Add DefaultVisualManagerLoop to the root node of scene file to remove this warning
[INFO]    [TopologyHandler] Topology: lines is not dynamic, topology engine on Data 'indices' won't be registered.
[INFO]    [TopologyHandler] Topology: lines is not dynamic, topology engine on Data 'beamsData' won't be registered.
[WARNING] [SofaEigen2Solver] SofaEigen2Solver is deprecated; Eigen classes are now located in Sofa.LinearAlgebra and SVDLinearSolver in SofaDenseSolver.You can remove SofaEigen2Solver from your scene, and if using SVDLinearSolver, please load SofaDenseSolver instead.
The constructor with a QGLFormat is deprecated, use the regular contructor instead.
[WARNING] [RealGUI] Global Bounding Box seems very small; Your viewer settings (based on the bbox) are likely invalid, switching to default value of [-1,-1,-1,1,1,1].This is caused by using component which does not implement properly the updateBBox function.You can remove this warning by manually forcing a value in the parameter bbox="minX minY minZ maxX maxY maxZ" in your root node 

[WARNING] [SofaViewer] Could not create file '/home/louis/Documents/sofa/src/share/textures/SOFA_logo.bmp'  
  Valid extensions: ["dds"]

GLib (gthread-posix.c): Unexpected error from C library during 'pthread_setspecific': Invalid argument.  Aborting.

########## SIG 6 - SIGABRT: usually caused by an abort() or assert() ##########
  sofa::helper::BackTrace::sig(int)
  gsignal
  abort
  g_private_set
  g_thread_self
  g_main_context_iteration
  QEventDispatcherGlib::processEvents(QFlags<QEventLoop::ProcessEventsFlag>)
  QEventLoop::exec(QFlags<QEventLoop::ProcessEventsFlag>)
  QCoreApplication::exec()
  sofa::gui::qt::RealGUI::mainLoop()
  sofa::gui::GUIManager::MainLoop(boost::intrusive_ptr<sofa::simulation::Node>, char const*)
  PyCFunction_Call
  _PyObject_MakeTpCall
  _PyEval_EvalFrameDefault
  _PyFunction_Vectorcall
  _PyEval_EvalFrameDefault
  _PyEval_EvalCodeWithName
  PyEval_EvalCode
  PyRun_SimpleFileExFlags
  Py_RunMain
  Py_BytesMain
  __libc_start_main
  _start
Aborted (core dumped)

Would you have an idea how to translate the .scn scene into a .py correctly please?

Also, once this will work, I would like to replace the line
beam.addObject('FixedConstraint', name="FixedConstraint" , indices="0" ) by a position and orientation assigned “dynamically ” to this same node (with indice “0”) at each time step. What is the best way ?

Lastly, how can I “read” the position and orientation of the last node (the 7th one) ?

Thanks for the support!

[Hugo]

Hey @dlouis

I apologize for the delay of my reply.

This sounds more like a python env/compilation issue. Which version of SOFA, SofaPython3 are you using? Is this the binary / a compiled version ?

Regarding your last point to assign dynamically a position, you can take a look at this post. Let us know if it helps.

Best wishes,

Hugo

[dlouis]

Hello @hugo

Thanks for the reply.
I tried again with SOFA binaries (v 21.12 ) and activated the associated SofaPython3 with the plugin manager, and nothing changes (same error).

Anyway, I think this is not a compilation issue, since I can run the examples located in plugins/SofaPython3/share/SofaPython3/examples without any problem (even when I click on animate, which is not the case with the script I showed you above). Hence, I think the problem comes indeed from my script, isn’t it ?

For the rest, I managed with another simple scene to assign position to a point, but I couldn’t try this with my own scene, obviously, since I cannot make it run.

Best
Louis

[CharlieD]

I tried your Python code you pasted above @dlouis on my own machine (SOFA v21.12, Python3.7, Win 10) and I didn’t get any glaring errors but I don’t know if I’m supposed to see anything. I admit I don’t completely understand what it is you are working on, but I don’t get those same C errors/issues. I’ve attached a screenshot of my PowerShell view of what I get when I run your code.

EDIT: Not sure if it’s just my computer/browser or it is the case for others, but I don’t see the image I attached. Here is a link to it.

[dlouis]

Hello @cdelor02 and @Hugo,

Sorry for the late reply, I was trying other stuff.

CharlieD, can you just launch the file, or can you also click on “animate” in the GUI, without having any problem ?
I can also execute the file with the same info/warnings as you, the problem only appears when I animate.

I have python 3.8.10 and SOFA 21.12 binaries, but as stated above, I don’t think it is a problem with my installation since other scenes work.

@Hugo do you have an idea ?

[CharlieD]

I am able to launch the file and click “Animate”, but the GUI view is still completely black so I cannot tell if anything is happening. Is it possible you are missing a package or something necessary for the GUI?

[Author]

Posts