Gear/proportional mapping

[Alex-srj]

Hi guys,

I am trying to simulate a scene containing 2 gears. My first thought was to create 2 nodes and use a mapping between their MechanicalObjects ($\theta_2 = \theta_1r_1/r_2 = \theta_1r$, direction of rotation is not important at this stage). I looked through the available mappings and, correct me if i am wrong, the closest thing i found was IdentityMapping and SquareMapping. Since i am not completely sure on what functions i should and should not re-implement, i tried modifying the SquareMapping to create my own ProportionalMapping. The .h, .cpp, and .inl files are attached in ProportionalMapping.zip.

I added the files to the linear mapping folder of sofa, modified the CMakeLists.txt, recompiled the code, and tested it with the following scene:

#Required import for python
import Sofa.Core
import Sofa
import SofaRuntime
from Sofa import SofaLinearSolver
from scipy import sparse


def createScene(rootnode):
    rootnode.addObject("RequiredPlugin", pluginName=['Sofa.Component.Mass',
                                                     'Sofa.Component.MechanicalLoad',
                                                     'Sofa.Component.StateContainer',
                                                     'Sofa.Component.Visual',
                                                     'Sofa.Component.ODESolver.Backward',
                                                     'Sofa.Component.LinearSolver.Direct'])
    rootnode.addObject('VisualStyle')
    rootnode.addObject('DefaultAnimationLoop')
    rootnode.addObject("DefaultVisualManagerLoop")
    rootnode.addObject('EulerImplicitSolver')
    solver = rootnode.addObject('SparseLDLSolver', name='Solver', template='CompressedRowSparseMatrixMat3x3d')
    rootnode.gravity = [0, 0, -9.81]
    rootnode.dt = 0.005

    # Physical parameters (not very logical but to simplify the problem:D )
    r1 = 4.0
    m1 = 3.0
    I1 = m1 * r1 ** 2 / 2  # =24

    r = 2.0  # r1/r2

    r2 = r1 / r  # =2
    m2 = 5.0
    I2 = m2 * r2 ** 2 / 2  # =10

    disks = rootnode.addChild('Disks')

    disk1 = disks.addChild('Disk1')
    disk1.addObject("MechanicalObject", position=[0], template='Vec1')
    disk1.addObject("UniformMass", vertexMass=I1)
    disk1.addObject('ConstantForceField', forces=[1])

    disk2 = disks.addChild('Disk2')
    disk2.addObject("MechanicalObject", position=[0], template='Vec1')
    disk2.addObject("UniformMass", vertexMass=I2)
    disk2.addObject('ProportionalMapping', input=disk1.MechanicalObject.getLinkPath(), ratio=r)
    disk2.addObject('ConstantForceField', forces=[3])

    rootnode.addObject(MatrixAccessController('MatrixAccessor', name='matrixAccessor', linear_solver=solver))


class MatrixAccessController(Sofa.Core.Controller):


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

    def onAnimateEndEvent(self, event):
        system_matrix = self.linear_solver.A()
        rhs = self.linear_solver.b()
        solution = self.linear_solver.x()

        print('====================================')
        print('Global system matrix')
        print('====================================')
        print('dtype: ' + str(system_matrix.dtype))
        print('shape: ' + str(system_matrix.shape))
        print('ndim: ' + str(system_matrix.ndim))
        print('nnz: ' + str(system_matrix.nnz))
        print('matrix: \n' + str(system_matrix.toarray()))

        print('====================================')
        print('System right hand side')
        print('====================================')
        print('dtype: ' + str(rhs.dtype))
        print('shape: ' + str(rhs.shape))
        print('ndim: ' + str(rhs.ndim))
        print('rhs: ' + str(rhs))

        print('====================================')
        print('System solution')
        print('====================================')
        print('dtype: ' + str(solution.dtype))
        print('shape: ' + str(solution.shape))
        print('ndim: ' + str(solution.ndim))
        print('solution: ' + str(solution))

Even though the DOFs of both nodes are coupled with the desired ratio $r$, the result is incorrect. Mathematically speaking, the equations of motion of the disks are:

$$I_1\dot{v}_1 = \tau_1$$

$$I_2\dot{v}_2 = I_2r\dot{v}_1 = \tau_2$$

which gives:

$$(I_1 + I_2r)\dot{v}_1 = \tau_1 + \tau_2$$

I checked the output of MatrixAccessController to see the system matrix and the right hand side and got:

====================================
Global system matrix
====================================
dtype: float64
shape: (1, 1)
ndim: 2
nnz: 1
matrix: 
[[64.]]
====================================
System right hand side
====================================
dtype: float64
shape: (1,)
ndim: 1
rhs: [0.035]

$64$ corresponds to $I_1 + I_2r^2$ (tested for multiple values of r), but i am expecting $I_1 + I_2r$. I tried searching the mapping documentation but couldn't understand which function is contributing to the system matrix, so i am not sure if it is something missing in my mapping or an implementation error. As for the rhs, which corresponds to $dt f(x,t)$, it is implying that the force equals $0.035/0.005 = 7$ which i believe is $\tau_1 + \tau_2r$.

Any help would be appreciated. Thanks in advance !

[hugtalbot]

Hi @Alex-srj

Nice first steps : why have you started from the SquareMapping? A proportional mapping would be an Identity mapping just with a $r$ coefficient instead of 1 on its diagonal.
Let me summon @alxbilger if he sees something that I did not..

[alxbilger]

The matrix A that is retrieved from your linear solver is computed as:

$$ A = M + k K $$

• $M$ is the mass matrix
• $K$ is the stiffness matrix
• $k = -dt^2$

2 components can contributes to $K$ in your scene:

• ConstantForceField, but it does not because the force is linear, so its derivative is null
• ProportionalMapping, but it does not because the Hessian matrix is null (mapping is linear)

So $K=0$.

In the end, $A=M=M_1 + J^T M_2 J$. In your case, $J = r$. With $M_1=I_1$ and $M_2=I_2$, it leads to $A=I_1+r^2 I_2$. It is what you observe.

IMO, the implementation of ProportionalMapping is correct. The unit test passes (it tests also the derivatives, no only the proportionality relationship). And the result corresponds to what is expected (regarding how SOFA works).

[Alex-srj]

Can you please give more details why its $+ J^TM_2J$ ? And i imagine for the force in the rhs its $+ J^Tf_2$? And if possible in which functions are these calculations done?

[alxbilger]

For any mapping function $f$:

$$ x_2 = f(x_1) $$

$f$ is computed in the function apply.

$$ v_2 = \frac{d x_2}{dt} = \frac{df(x_1)}{dt}=\frac{df(x_1)}{dx_1} \frac{dx_1}{dt} = J v1 $$

$J$ is computed in the function applyJ.

$$ a_2 = \frac{dJv_1}{dt} = J \frac{dv_1}{dt} + \frac{dJ}{dt} v_1 = J a_1 $$

So, we can compute:

$$ f_2 = M_2 J a_1 $$

But $f_2$ is a mapped force, so it needs to be projected in the main DoFs space. The projection is $J^T f_2 = J^T M_2 J a_1$

$J_T$ is computed in applyJT.

[Alex-srj]

Thank you! So in my case, i needed to use an identity matrix in applyJT(mparams, ...) instead of the Jacobian to get f_1 + f_2 and the square root of the Jacobian in applyJT(cparams, ...) instead of the Jacobian to get $A = I_1 + rI_2$.

Just tested it and it seems to work according to the equations i wrote in the original post. However, the unit test fails:

[----------] 1 test from ProportionalMappingTest/0, where TypeParam = sofa::component::mapping::linear::ProportionalMapping<sofa::defaulttype::StdVectorTypes<sofa::type::Vec<1u, double>, sofa::type::Vec<1u, double>, double>, sofa::defaulttype::StdVectorTypes<sofa::type::Vec<1u, double>, sofa::type::Vec<1u, double>, double> >
[ RUN      ] ProportionalMappingTest/0.testDifferentRatio
[WARNING] [MappingTest] applyJT on matrices is not tested
[WARNING] [Node(root)] An animation loop is required, but has not been found. Add an animation loop in the root of your scene to fix this warning. The list of available animation loop components is: [DefaultAnimationLoop]. A component of type DefaultAnimationLoop will be automatically added for you.
/home/user/sofa/src/Sofa/Component/Mapping/Testing/src/sofa/component/mapping/testing/MappingTestCreation.h:335: Failure
Failed
applyJT test failed, difference should be less than 6.6613381477509392e-14 (1.4246757817569542)
jfc = 2.1370136726354314 1.9115761807707956 -1.1897971191395993 0.59143776129532488
 fp = 0.71233789087847721 0.63719206025693187 -0.39659903971319976 0.19714592043177495

/home/user/sofa/src/Sofa/Component/Mapping/Testing/src/sofa/component/mapping/testing/MappingTestCreation.h:460: Failure
Failed
Failed Seed number = 0

/home/user/sofa/src/Sofa/Component/Mapping/Linear/tests/ProportionalMapping_test.cpp:87: Failure
Value of: this->test()
  Actual: false
Expected: true
[  FAILED  ] ProportionalMappingTest/0.testDifferentRatio, where TypeParam = sofa::component::mapping::linear::ProportionalMapping<sofa::defaulttype::StdVectorTypes<sofa::type::Vec<1u, double>, sofa::type::Vec<1u, double>, double>, sofa::defaulttype::StdVectorTypes<sofa::type::Vec<1u, double>, sofa::type::Vec<1u, double>, double> > (0 ms)
[----------] 1 test from ProportionalMappingTest/0 (0 ms total)

[----------] Global test environment tear-down
[==========] 5 tests from 4 test suites ran. (3 ms total)
[  PASSED  ] 4 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] ProportionalMappingTest/0.testDifferentRatio, where TypeParam = sofa::component::mapping::linear::ProportionalMapping<sofa::defaulttype::StdVectorTypes<sofa::type::Vec<1u, double>, sofa::type::Vec<1u, double>, double>, sofa::defaulttype::StdVectorTypes<sofa::type::Vec<1u, double>, sofa::type::Vec<1u, double>, double> >

 1 FAILED TEST

Is it a problem?

[hugtalbot]

Any update since 2 weeks @Alex-srj ?
Since we see you being active do not hesitate to join us in Lille on Nov 26

[github-actions[bot]]

⚠️ ⚠️ ⚠️
@Alex-srj
Feedback has been given to you by the project reviewers, however we have not received a response from you. Without further news in the coming weeks, this discussion will be automatically closed in order to keep this forum clean and fresh 🌱 Thank you for your understanding