Maze.py

import os
import pathlib
import sys

import Sofa
import Sofa.Core
from Sphere import Sphere
from stlib3.scene.contactheader import ContactHeader

sys.path.insert(0, str(pathlib.Path(__file__).parent.absolute())+"/../")
sys.path.insert(0, str(pathlib.Path(__file__).parent.absolute()))

path = os.path.dirname(os.path.abspath(__file__))
path_mesh = path + '/mesh/'


class Maze(Sofa.Prefab):
    prefabParameters = [
        {'name': 'translation', 'type': 'Vec3d', 'help': '', 'default': [-50.0, 0.0, 50.0]},
        {'name': 'rotation', 'type': 'Vec3d', 'help': '', 'default': [-90.0, 0.0, 0.0]}
    ]

    prefabData = [
        {'name': 'index', 'type': 'int', 'help': 'index of rigid to attach to', 'default': 0},
    ]

    def __init__(self, *args, **kwargs):
        Sofa.Prefab.__init__(self, *args, **kwargs)

    def init(self):
        self.addObject("MeshSTLLoader", name="loader", filename=path_mesh+"maze_4_coarse.stl",
                       translation=self.translation.value, rotation=self.rotation.value)
        self.addObject("MeshTopology", src='@loader')
        self.addObject("MechanicalObject", name='maze_mesh_mo')
        self.addObject("TriangleCollisionModel")
        self.addObject("LineCollisionModel")
        self.addObject("PointCollisionModel")


def createScene(rootNode):
    rootNode.gravity = [0., -9810., 0.]
    rootNode.dt = 0.01

    pluginList = ["Sofa.Component.AnimationLoop",
                  "Sofa.Component.Collision.Detection.Algorithm",
                  "Sofa.Component.Collision.Detection.Intersection",
                  "Sofa.Component.Collision.Geometry",
                  "Sofa.Component.Collision.Response.Contact",
                  "Sofa.Component.Constraint.Lagrangian.Correction",
                  "Sofa.Component.Constraint.Lagrangian.Solver",
                  "Sofa.Component.IO.Mesh", "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.LinearSolver.Iterative", "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.Spring",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Constant",
                  "Sofa.Component.Visual"]

    rootNode.addObject('RequiredPlugin', pluginName=pluginList)

    ContactHeader(rootNode, alarmDistance=15, contactDistance=0.5, frictionCoef=0)
    rootNode.addObject('VisualStyle', displayFlags=['showCollisionModels', 'showBehavior'])
    rootNode.addObject('DefaultVisualManagerLoop')

    effector = rootNode.addChild('Effector')
    effector.addObject('EulerImplicitSolver', firstOrder=True)
    effector.addObject('CGLinearSolver', iterations=100, threshold=1e-5, tolerance=1e-5)
    effector.addObject('MechanicalObject', template='Rigid3', name='goalMO', position=[0, 40, 0, 0, 0, 0, 1],
                       showObject=True, showObjectScale=10)
    effector.addObject('RestShapeSpringsForceField', points=0, angularStiffness=1e5, stiffness=1e5)
    effector.addObject('UncoupledConstraintCorrection', compliance='1e-10  1e-10  0 0 1e-10  0 1e-10 ')

    maze = effector.addChild(Maze())
    maze.addObject("RigidMapping", index=0)

    rootNode.addChild(Sphere(withSolver=True))

    return