SofaPython3/examples/access_constraint_matrix.py at 0b1fb3f9e4f30b73d7c3927346dac040856aaa50 · bakpaul/SofaPython3 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
# Required import for python
import Sofa
import SofaRuntime
import numpy as np


# Function called when the scene graph is being created
def createScene(root):
    root.addObject("RequiredPlugin", pluginName=["Sofa.Component.AnimationLoop",
                                                 "Sofa.Component.Constraint.Lagrangian.Correction",
                                                 "Sofa.Component.Constraint.Lagrangian.Model",
                                                 "Sofa.Component.Constraint.Lagrangian.Solver",
                                                 "Sofa.Component.Constraint.Projective",
                                                 "Sofa.Component.IO.Mesh",
                                                 "Sofa.Component.LinearSolver.Direct",
                                                 "Sofa.Component.Mapping.MappedMatrix",
                                                 "Sofa.Component.Mass",
                                                 "Sofa.Component.ODESolver.Backward",
                                                 "Sofa.Component.Topology.Container.Dynamic",
                                                 "Sofa.Component.Mapping.NonLinear",
                                                 "Sofa.Component.StateContainer"
                                                 ])

    root.addObject("FreeMotionAnimationLoop", solveVelocityConstraintFirst=True)
    root.addObject("BlockGaussSeidelConstraintSolver", tolerance=1e-9, maxIterations=1000)
    root.addObject("StringMeshCreator", name="loader", resolution="20")

    root.addObject("EulerImplicitSolver")
    root.addObject("EigenSimplicialLLT", template='CompressedRowSparseMatrixMat3x3d')
    root.addObject("GenericConstraintCorrection")

    root.addObject("EdgeSetTopologyContainer", name="topo", position="@loader.position", edges="@loader.edges")
    mechanical_object = root.addObject("MechanicalObject", name="defoDOF", template="Vec3d")
    root.addObject("EdgeSetGeometryAlgorithms", drawEdges=True)
    root.addObject("FixedProjectiveConstraint", indices=[0])
    root.addObject("DiagonalMass", name="mass", totalMass="1e-3")

    ext = root.addChild("extensionsNode")
    ext.addObject("MechanicalObject", template="Vec1d", name="extensionsDOF")
    ext.addObject("DistanceMapping", name="distanceMapping", topology="@topo")
    ext.addObject("UniformLagrangianConstraint", template="Vec1d", iterative=True)

    root.addObject(MatrixAccessController('MatrixAccessor', name='matrixAccessor', mechanical_object=mechanical_object))


class MatrixAccessController(Sofa.Core.Controller):

    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.mechanical_object = kwargs.get("mechanical_object")

    def onBuildConstraintSystemEndEvent(self, event):
        constraint_matrix = self.mechanical_object.constraint.value

        print(self.mechanical_object.constraint.linkpath)

        print('====================================')
        print('Constraint matrix in constraint space')
        print('====================================')
        print('dtype: ' + str(constraint_matrix.dtype))
        print('shape: ' + str(constraint_matrix.shape))
        print('ndim: ' + str(constraint_matrix.ndim))

        print(constraint_matrix)