Controlling cable by SofaPython3 from python interpreter

[Abdelrahman]

Hello everyone

I’m trying to run one of the softrobotic example from conda environment without using any GUI like in my previous post

Running example with pygame

I’m trying to control the trunk example by changing one of the constraints and see the change of the end effector position like in the following code

from myTrunk import Trunk
from math import cos
from math import sin
from splib3.numerics import Vec3, Quat
from splib3.animation import animate, AnimationManager
from myController import TrunkController 
import Sofa
import Sofa.Gui
import Sofa.SofaGL
import SofaRuntime
import Sofa.Simulation
import os
import time

# This python script shows the functions to be implemented
# in order to create your Controller in python
class TrunkController(Sofa.Core.Controller):

    def __init__(self, *args, **kwargs):
        # These are needed (and the normal way to override from a python class)
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        print('Controller initialized')
        self.node = kwargs["node"]
        
        self.Simulation=self.node.getChild("Simulation")
        self.Trunk=self.Simulation.getChild("Trunk")
        self.Effector = self.Trunk.getChild('Effectors')
            
        
        # Get the cables from the parent Trunk
        self.cableL0=self.Trunk.getChild('cableL0')

        # you can access the object through its name
        self.cableL0Acutator = self.cableL0.cable

        self.endEffector = self.Effector.myEndEffector
        
        self.iteration = 0
        

    def onAnimateBeginEvent(self, eventType):
    	if self.iteration < 10:
    	    effectorPosition = self.endEffector.findData('position').value
    	    print(f'end effector position = {effectorPosition}')
    	    self.cableL0Acutator.findData('displacement').value += 5
    	    print(self.cableL0Acutator.findData('displacement').value)
    	    self.iteration += 1
    	elif self.iteration < 20:
    	    cableDisp = self.cableL0Acutator.findData('displacement').value
    	    print(f'Cable Displacement = {cableDisp}')
    	 
    	
    def onKeypressedEvent(self, event):
        pass

def main():
    # Make sure to load all SOFA libraries
    SofaRuntime.importPlugin("SofaBaseMechanics")
    SofaRuntime.importPlugin("SofaOpenglVisual")

    #Create the root node
    rootNode = Sofa.Core.Node("root")
    
    # Call the below 'createScene' function to create the scene graph
    createScene(rootNode)
    
    # to add the controller script
    rootNode.addObject(TrunkController(node=rootNode))
    
    # Once defined, initialization of the scene graph
    Sofa.Simulation.init(rootNode)

    # Run the simulation for 10 steps
    for iteration in range(30):
        print(f'Iteration #{iteration}')
        Sofa.Simulation.animate(rootNode, rootNode.dt.value)

    print("Simulation made 10 time steps. Done")

def createScene(rootNode):

    # Choose your resolution mode
    # 1- inverseMode=True, solve the inverse problem and control the end effectors
    # 2- inverseMode=False, solve the direct problem and set the cable displacements by hand
    inverseMode = False

    rootNode.addObject('RequiredPlugin', pluginName=['SoftRobots','SofaSparseSolver','SofaPreconditioner','SofaPython3','SofaConstraint',
                                                     'SofaImplicitOdeSolver','SofaLoader','SofaSimpleFem','SofaBoundaryCondition','SofaEngine',
                                                     'SofaOpenglVisual', 'SofaGeneralLoader', 'SofaBaseMechanics'])
    AnimationManager(rootNode)
    rootNode.addObject('VisualStyle', displayFlags='showBehavior')
    rootNode.gravity = [0., -9810., 0.]
    rootNode.dt = 0.01

    # For direct resolution, i.e direct control of the cable displacement	
    rootNode.addObject('GenericConstraintSolver', maxIterations=100, tolerance=1e-5)
    
    simulation = rootNode.addChild('Simulation')

    simulation.addObject('EulerImplicitSolver', name='odesolver', firstOrder=False, rayleighMass=0.1, rayleighStiffness=0.1)
    simulation.addObject('SparseLDLSolver', name='precond', template="CompressedRowSparseMatrixd")
    simulation.addObject('ShewchukPCGLinearSolver', name='linearSolver', iterations=500, tolerance=1.0e-18, preconditioners='precond')
    simulation.addObject('GenericConstraintCorrection', solverName='precond')

    trunk = Trunk(simulation, inverseMode=inverseMode)
    trunk.addVisualModel(color=[1., 1., 1., 0.8])
    trunk.fixExtremity()

if __name__ == '__main__':
    main()

and the output is as follows

[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaBaseMechanics.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaOpenglVisual.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaLoader.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaMiscCollision.so
[INFO]    [SofaPython3]  Initializing with python version 3.8.5 (default, Sep  4 2020, 07:30:14) 
[GCC 7.3.0]
[INFO]    [SofaPython3] Shared library name is 'libpython3.8m.so'
[INFO]    [SofaPython3] Added '/home/abdelrahman/sofa/plugins/SoftRobots/python' to sys.path
[INFO]    [SofaPython3] Added '/home/abdelrahman/sofa/build/master/lib/python3/site-packages' to sys.path
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaPython3.so
[WARNING] [DefaultPipeline(DefaultPipeline)] A NarrowPhase component is required to compute collision detection and was not found in the current scene
(sofa2) abdelrahman@abdelrahman-ROG-Strix:~/MyRLProject/SimpleExample$ cd ../MainwSim/
(sofa2) abdelrahman@abdelrahman-ROG-Strix:~/MyRLProject/MainwSim$ python mainSimwController.py 
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaBaseMechanics.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaOpenglVisual.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSTLIB.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSoftRobots.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaSparseSolver.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaPreconditioner.so
[INFO]    [SofaPython3]  Initializing with python version 3.8.5 (default, Sep  4 2020, 07:30:14) 
[GCC 7.3.0]
[INFO]    [SofaPython3] Shared library name is 'libpython3.8m.so'
[INFO]    [SofaPython3] Added '/home/abdelrahman/sofa/plugins/SoftRobots/python' to sys.path
[INFO]    [SofaPython3] Added '/home/abdelrahman/sofa/build/master/lib/python3/site-packages' to sys.path
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaPython3.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaConstraint.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaImplicitOdeSolver.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaLoader.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaSimpleFem.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaBoundaryCondition.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaEngine.so
[INFO]    [PluginManager] Loaded plugin: /home/abdelrahman/sofa/build/master/lib/libSofaGeneralLoader.so
Controller initialized
[INFO]    [TopologyHandler] Topology: container is not dynamic, topology engine on Data 'indices' won't be registered.
[INFO]    [TopologyHandler] Topology: container is not dynamic, topology engine on Data 'texcoords' won't be registered.
Iteration #0
end effector position before cable displacemenet= [[ 4.4408921e-16 -4.4408921e-16  1.9500000e+02]]
Cable Displacement = 5.0
end effector position after cable displacemenet= [[ 4.4408921e-16 -4.4408921e-16  1.9500000e+02]]
Iteration #1
end effector position before cable displacemenet= [[ 3.20902334e-03 -1.06605636e+00  1.94998940e+02]]
Cable Displacement = 10.0
end effector position after cable displacemenet= [[ 3.20902334e-03 -1.06605636e+00  1.94998940e+02]]
Iteration #2
end effector position before cable displacemenet= [[ 1.13290293e-02 -3.35092081e+00  1.94978125e+02]]
Cable Displacement = 15.0
end effector position after cable displacemenet= [[ 1.13290293e-02 -3.35092081e+00  1.94978125e+02]]
Iteration #3
end effector position before cable displacemenet= [[ 2.31395251e-02 -6.97783922e+00  1.94891736e+02]]
Cable Displacement = 20.0
end effector position after cable displacemenet= [[ 2.31395251e-02 -6.97783922e+00  1.94891736e+02]]
Iteration #4
end effector position before cable displacemenet= [[ 3.54498358e-02 -1.20149975e+01  1.94663253e+02]]
Cable Displacement = 25.0
end effector position after cable displacemenet= [[ 3.54498358e-02 -1.20149975e+01  1.94663253e+02]]
Iteration #5
end effector position before cable displacemenet= [[ 4.39851308e-02 -1.84780184e+01  1.94179759e+02]]
Cable Displacement = 30.0
end effector position after cable displacemenet= [[ 4.39851308e-02 -1.84780184e+01  1.94179759e+02]]
Iteration #6
end effector position before cable displacemenet= [[ 4.39899067e-02 -2.63342583e+01  1.93289655e+02]]
Cable Displacement = 35.0
end effector position after cable displacemenet= [[ 4.39899067e-02 -2.63342583e+01  1.93289655e+02]]
Iteration #7
end effector position before cable displacemenet= [[ 3.06714950e-02 -3.55028398e+01  1.91806260e+02]]
Cable Displacement = 40.0
end effector position after cable displacemenet= [[ 3.06714950e-02 -3.55028398e+01  1.91806260e+02]]
Iteration #8
end effector position before cable displacemenet= [[-4.10606202e-04 -4.58508222e+01  1.89517832e+02]]
Cable Displacement = 45.0
end effector position after cable displacemenet= [[-4.10606202e-04 -4.58508222e+01  1.89517832e+02]]
Iteration #9
end effector position before cable displacemenet= [[-5.29346937e-02 -5.71878681e+01  1.86203949e+02]]
Cable Displacement = 50.0
end effector position after cable displacemenet= [[-5.29346937e-02 -5.71878681e+01  1.86203949e+02]]
Iteration #10
Cable Displacement = 50.0
Iteration #11
Cable Displacement = 50.0
Iteration #12
Cable Displacement = 50.0
Iteration #13
Cable Displacement = 50.0
Iteration #14
Cable Displacement = 50.0
Iteration #15
Cable Displacement = 50.0
Iteration #16
Cable Displacement = 50.0
Iteration #17
Cable Displacement = 50.0
Iteration #18
Cable Displacement = 50.0
Iteration #19
Cable Displacement = 50.0

as you can see in the output that the effector position is affected only by the gravity as I have tried to run it without changing the cable displacement and got the same result. When I run this example inside sofa program it works fine with slight change in the controller as I control the cable displacement through the keyboard and when releasing the keyboard keys the cable displacement returns to zero but here in the code running from the python interpreter it doesn’t.

Can anyone tell me what am doing wrong as I have been stuck in this for a week now.
Thanks and sorry for the long post and the noob question

[Hugo]

Hey @abdelrahman2010

The fact that you get different results between SOFA from a python environment and SOFA from runSofa necessarily comes from your main() function (since this function is called when working from python and not called from runSofa).

Indeed, you can notice that the main() function is adding
rootNode.addObject(TrunkController(node=rootNode))

despite the TrunkController is also defined in the createScene() function.
Could you try to remove this line?

Best

Hugo

[Author]

Posts