PrecomputedConstraintCorrection make all the models in the scene dissapears

[BrunoB81HK]

Hi everyone,

I'm currently building a simulation and I came across this problem. I'm simulation a laparoscopy intervention with one or two instruments. In those simulations, only one instrument have a force feedback. For this, I have 2 different code fragment. When I'm using the code for one instrument, I use a PrecomputedConstraintCorrection. With it, the produced force is pretty good. In the code for two instruments, when I use a PrecomputedConstraintCorrection, all the models in the scene disappears at the first contact. To solve this issue, I'm using an UncoupledConstraintCorrection.

The problem is that, with an UncoupledConstraintCorrection, the computed forces are really noisy.

Is there any way that I can use a PrecomputedConstraintCorrection instead of an UncoupledConstraintCorrection?

Here is my current codes (the ConstraintCorrection I talk about is in Instruments/Trex):

One instrument

<Node name="Instruments">
    <!-- Load all the meshes -->
    <MeshObjLoader name="ShaftVisMesh" filename="../../mesh/Instrument/Shaft.obj"/>
    <MeshObjLoader name="LeftVisMesh" filename="../../mesh/Instrument/PokeL.obj"/>
    <MeshObjLoader name="RightVisMesh" filename="../../mesh/Instrument/PokeR.obj"/>
    <MeshObjLoader name="JawColMesh" filename="../../mesh/Instrument/JawCol.obj"/>

    <Node name="Trex">
        <EulerImplicitSolver rayleighMass="0.1" rayleighStiffness="0.01"/>
        <CGLinearSolver iterations="40" tolerance="1.0e-9" threshold="1.0e-15"/>
        <MechanicalObject name="Bati" template="Rigid3d" position="0 0 0 0 0 0 1" rest_position="@TrexDriver.trexPosition"/>
        <UniformMass totalMass="0.01"/>

        <!-- The force feedback component -->
        <LCPForceFeedback name="ForceFeedBack" template="Rigid3d" activate="1" forceCoef="10.0"/>

        <!-- Forcefield to link the Trex position with instrument base position -->
        <RestShapeSpringsForceField points="0" stiffness="1000" angularStiffness="1000000000"/>
        <PrecomputedConstraintCorrection recompute="true"/>

        <Node name="Grasper">
            <!-- Mechanical object containing the 2 articulations -->
            <MechanicalObject name="Articulations" template="Vec1d" position="0 0" rest_position="@TrexDriver.trexPliersAngles"/>

            <!-- Forcefield to link the desired joint angle to the jaws angle -->
            <RestShapeSpringsForceField points="0 1" stiffness="1000000 1000000"/>
            <UncoupledConstraintCorrection compliance="0.000001 0.000001"/>

            <Node name="Jaws">
                <MechanicalObject name="GrasperDOFs" template="Rigid3d" position="0 0 0 0 0 0 1  0 0 0 0 0 0 1  0 0 0 0 0 0 1"/>

                <!-- The point where the instrument grabs -->
                <Node name="GrabPoint">
                    <MechanicalObject position="-0.02 0 0 1 0 0 0"/>
                    <PointCollisionModel group="1" tags="TrexCol" contactStiffness="10.0"/>
                    <RigidMapping input="@.." index="0"/>

                </Node>

                <Node name="ShaftVisual">
                    <OglModel name="model" src="@/Instruments/ShaftVisMesh"/>
                    <RigidMapping input="@.." output="@model" index="0"/>

                </Node>

                <Node name="LeftVisual">
                    <OglModel name="model" src="@/Instruments/LeftVisMesh"/>
                    <RigidMapping input="@.." output="@model" index="1"/>

                </Node>

                <Node name="RightVisual">
                    <OglModel name="model" src="@/Instruments/RightVisMesh"/>
                    <RigidMapping input="@.." output="@model" index="2"/>

                </Node>

                <Node name="LeftCollision">
                    <MeshTopology src="@/Instruments/JawColMesh"/>
                    <MechanicalObject name="model" src="@/Instruments/JawColMesh"/>
                    <LineCollisionModel  moving="1" simulated="1" group="1"/>
                    <PointCollisionModel moving="1" simulated="1" group="1"/>
                    <RigidMapping input="@.." output="@model" index="1"/>

                </Node>

                <Node name="RightCollision">
                    <MeshTopology src="@/Instruments/JawColMesh"/>
                    <MechanicalObject name="model" src="@/Instruments/JawColMesh"/>
                    <LineCollisionModel  moving="1" simulated="1" group="1"/>
                    <PointCollisionModel moving="1" simulated="1" group="1"/>
                    <RigidMapping input="@.." output="@model" index="2"/>

                </Node>

                <ArticulatedSystemMapping input1="@../Articulations" output="@GrasperDOFs" input2="@../../Bati"/>

            </Node>

            <ArticulatedHierarchyContainer/>

            <!-- Articulations definitions -->
            <Node name="articulationCenters">
                <Node name="articulationCenterLeft">
                    <ArticulationCenter parentIndex="0" childIndex="1" posOnParent="0 0 0" posOnChild="0 0 0"/>

                    <Node name="articulations">
                        <Articulation translation="0" rotation="1" rotationAxis="0 0 1" articulationIndex="0"/>

                    </Node>

                </Node>

                <Node name="articulationCenterRight">
                    <ArticulationCenter parentIndex="0" childIndex="2" posOnParent="0 0 0" posOnChild="0 0 0"/>

                    <Node name="articulations">
                        <Articulation translation="0" rotation="1" rotationAxis="0 0 1" articulationIndex="1"/>

                    </Node>

                </Node>

            </Node>

        </Node>

    </Node>

</Node>

Two instruments

<Node name="Instruments">
    <!-- Load all the meshes -->
    <MeshObjLoader name="ShaftVisMesh" filename="../../mesh/Instrument/Shaft.obj"/>
    <MeshObjLoader name="LeftVisMesh" filename="../../mesh/Instrument/PokeL.obj"/>
    <MeshObjLoader name="RightVisMesh" filename="../../mesh/Instrument/PokeR.obj"/>
    <MeshObjLoader name="JawColMesh" filename="../../mesh/Instrument/JawCol.obj"/>

    <Node name="Trex">
        <EulerImplicitSolver rayleighMass="0.1" rayleighStiffness="0.01"/>
        <CGLinearSolver iterations="40" tolerance="1.0e-9" threshold="1.0e-15"/>
        <MechanicalObject name="Bati" template="Rigid3d" position="-0.02 -0.05 0.15 0 0 0 1" rest_position="@TrexDriver.trexPosition"/>
        <UniformMass totalMass="0.01"/>

        <!-- The force feedback component -->
        <LCPForceFeedback name="ForceFeedBack" template="Rigid3d" activate="1" forceCoef="10.0"/>

        <!-- Forcefield to link the Trex position with instrument base position -->
        <RestShapeSpringsForceField points="0" stiffness="1000" angularStiffness="1000000000"/>
        <UncoupledConstraintCorrection compliance="0.0001  0.000000001 0 0 0.000000001 0 0.000000001"/>

        <Node name="Grasper">
            <!-- Mechanical object containing the 2 articulations -->
            <MechanicalObject name="Articulations" template="Vec1d" position="0 0" rest_position="@TrexDriver.trexPliersAngles"/>

            <!-- Forcefield to link the desired joint angle to the jaws angle -->
            <RestShapeSpringsForceField points="0 1" stiffness="1000000 1000000"/>
            <UncoupledConstraintCorrection compliance="0.000001 0.000001"/>

            <Node name="Jaws">
                <MechanicalObject name="GrasperDOFs" template="Rigid3d" position="0 0 0 0 0 0 1  0 0 0 0 0 0 1  0 0 0 0 0 0 1"/>

                <!-- The point where the instrument grabs -->
                <Node name="GrabPoint">
                    <MechanicalObject position="-0.02 0 0 1 0 0 0"/>
                    <PointCollisionModel group="1" tags="TrexCol" contactStiffness="10.0"/>
                    <RigidMapping input="@.." index="0"/>

                </Node>

                <Node name="ShaftVisual">
                    <OglModel name="model" src="@/Instruments/ShaftVisMesh"/>
                    <RigidMapping input="@.." output="@model" index="0"/>

                </Node>

                <Node name="LeftVisual">
                    <OglModel name="model" src="@/Instruments/LeftVisMesh"/>
                    <RigidMapping input="@.." output="@model" index="1"/>

                </Node>

                <Node name="RightVisual">
                    <OglModel name="model" src="@/Instruments/RightVisMesh"/>
                    <RigidMapping input="@.." output="@model" index="2"/>

                </Node>

                <Node name="LeftCollision">
                    <MeshTopology src="@/Instruments/JawColMesh"/>
                    <MechanicalObject name="model" src="@/Instruments/JawColMesh"/>
                    <LineCollisionModel  moving="1" simulated="1" group="1"/>
                    <PointCollisionModel moving="1" simulated="1" group="1"/>
                    <RigidMapping input="@.." output="@model" index="1"/>

                </Node>

                <Node name="RightCollision">
                    <MeshTopology src="@/Instruments/JawColMesh"/>
                    <MechanicalObject name="model" src="@/Instruments/JawColMesh"/>
                    <LineCollisionModel  moving="1" simulated="1" group="1"/>
                    <PointCollisionModel moving="1" simulated="1" group="1"/>
                    <RigidMapping input="@.." output="@model" index="2"/>

                </Node>

                <ArticulatedSystemMapping input1="@../Articulations" output="@GrasperDOFs" input2="@../../Bati"/>

            </Node>

            <ArticulatedHierarchyContainer/>

            <!-- Articulations definitions -->
            <Node name="articulationCenters">
                <Node name="articulationCenterLeft">
                    <ArticulationCenter parentIndex="0" childIndex="1" posOnParent="0 0 0" posOnChild="0 0 0"/>

                    <Node name="articulations">
                        <Articulation translation="0" rotation="1" rotationAxis="0 0 1" articulationIndex="0"/>

                    </Node>

                </Node>

                <Node name="articulationCenterRight">
                    <ArticulationCenter parentIndex="0" childIndex="2" posOnParent="0 0 0" posOnChild="0 0 0"/>

                    <Node name="articulations">
                        <Articulation translation="0" rotation="1" rotationAxis="0 0 1" articulationIndex="1"/>

                    </Node>

                </Node>

            </Node>

        </Node>

    </Node>

    <Node name="Dummy">
        <EulerImplicitSolver rayleighMass="0.1" rayleighStiffness="0.01"/>
        <CGLinearSolver iterations="40" tolerance="1.0e-9" threshold="1.0e-15"/>
        <MechanicalObject name="Bati" template="Rigid3d" position="-0.02 0.05 0.15 0 0 0 1" rest_position="@TrexDriver.dummyPosition"/>
        <UniformMass totalMass="0.01"/>

        <!-- Forcefield to link the Dummy position with instrument base position -->
        <RestShapeSpringsForceField points="0" stiffness="1000" angularStiffness="1000000000"/>
        <UncoupledConstraintCorrection compliance="0.0001  0.000000001 0 0 0.000000001 0 0.000000001"/>

        <Node name="Grasper">
            <!-- Mechanical object containing the 2 articulations -->
            <MechanicalObject name="Articulations" template="Vec1d" position="0 0" rest_position="@TrexDriver.dummyPliersAngles"/>

            <!-- Forcefield to link the desired joint angle to the jaws angle -->
            <RestShapeSpringsForceField points="0 1" stiffness="1000000 1000000"/>
            <UncoupledConstraintCorrection compliance="0.000001 0.000001"/>

            <Node name="Jaws">
                <MechanicalObject name="GrasperDOFs" template="Rigid3d" position="0 0 0 0 0 0 1  0 0 0 0 0 0 1  0 0 0 0 0 0 1"/>

                <!-- The point where the instrument grabs -->
                <Node name="GrabPoint">
                    <MechanicalObject position="-0.02 0 0 1 0 0 0"/>
                    <PointCollisionModel group="2" tags="DummyCol" contactStiffness="10.0"/>
                    <RigidMapping input="@.." index="0"/>

                </Node>

                <Node name="ShaftVisual">
                    <OglModel name="model" src="@/Instruments/ShaftVisMesh"/>
                    <RigidMapping input="@.." output="@model" index="0"/>

                </Node>

                <Node name="LeftVisual">
                    <OglModel name="model" src="@/Instruments/LeftVisMesh"/>
                    <RigidMapping input="@.." output="@model" index="1"/>

                </Node>

                <Node name="RightVisual">
                    <OglModel name="model" src="@/Instruments/RightVisMesh"/>
                    <RigidMapping input="@.." output="@model" index="2"/>

                </Node>

                <Node name="LeftCollision">
                    <MeshTopology src="@/Instruments/JawColMesh"/>
                    <MechanicalObject name="model" src="@/Instruments/JawColMesh"/>
                    <LineCollisionModel  moving="1" simulated="1" group="2"/>
                    <PointCollisionModel moving="1" simulated="1" group="2"/>
                    <RigidMapping input="@.." output="@model" index="1"/>

                </Node>

                <Node name="RightCollision">
                    <MeshTopology src="@/Instruments/JawColMesh"/>
                    <MechanicalObject name="model" src="@/Instruments/JawColMesh"/>
                    <LineCollisionModel  moving="1" simulated="1" group="2"/>
                    <PointCollisionModel moving="1" simulated="1" group="2"/>
                    <RigidMapping input="@.." output="@model" index="2"/>

                </Node>

                <ArticulatedSystemMapping input1="@../Articulations" output="@GrasperDOFs" input2="@../../Bati"/>

            </Node>

            <ArticulatedHierarchyContainer/>

            <!-- Articulations definitions -->
            <Node name="articulationCenters">
                <Node name="articulationCenterLeft">
                    <ArticulationCenter parentIndex="0" childIndex="1" posOnParent="0 0 0" posOnChild="0 0 0"/>

                    <Node name="articulations">
                        <Articulation translation="0" rotation="1" rotationAxis="0 0 1" articulationIndex="0"/>

                    </Node>

                </Node>

                <Node name="articulationCenterRight">
                    <ArticulationCenter parentIndex="0" childIndex="2" posOnParent="0 0 0" posOnChild="0 0 0"/>

                    <Node name="articulations">
                        <Articulation translation="0" rotation="1" rotationAxis="0 0 1" articulationIndex="1"/>

                    </Node>

                </Node>

            </Node>

        </Node>

    </Node>

</Node>

Using

SOFA v21.06
Ubuntu 20.04

[hugtalbot]

Thanks a lot @BrunoB81HK for reporting your issue.
Choosing by default a full construction of the compliance matrix (precomputed in your case using the PrecomputedConstraintCorrection) is a very good practice.

However, I am not familiar working with two haptic devices but @epernod is. Erik how do you handle such two-haptic-device force feedback? Some vague memory tells me you faced this topic as well ..

[hugtalbot]

@BrunoB81HK ?

[BrunoB81HK]

Hi,

Sorry for the late answer.

I'm not sure to understand where are the 2 haptic devices in your scene?

They are defined in the Trex and the Dummy nodes. The driver is defined higher in the scene. It's an object that come from a custom plugin and his name is TrexDriver. It is use to get some values such as @TrexDriver.trexPosition, @TrexDriver.dummyPosition, @TrexDriver.trexPliersAngles and @TrexDriver.dummyPliersAngles.

What kind of tool are we talking about?

If by tool, you mean the device. It's a custom haptic device that we developed. It's using a new actuation technology.

If you mean the tool in the simulation. It's a grasper looking like this:

Are you using the precomputed constraint correction for the tool or the object to interact with?

I'm currently trying to use it on both, but your reply suggest it may not be a good idea?

---

If I end up using an UncoupledConstraintCorrection, how can I compute/choose the right compliance matrix? Is there documentation on this?

[epernod]

Hi,
ok this looks very nice. I would love to hear more about the device you build yourself.
Anyway, I'm sorry I lost the point here. Is your problem happening when you use 2 tools instead of 1 or only when you change UncoupledConstraintCorrection by PrecomputedConstraintCorrection ?

As I said, the problem with precomputed is that it is an approximation based on the rest position of the object. I'm not an expert in constraint correction but for a rigid device that move, I think is it not appropriate.
On the other hand Uncoupled is also an approximation and will apply the same constraint to your tool if you get collide with jelly or steel.

Therefor if you want an accurate solution I recommend to use LinearSolverConstraintCorrection but you need to use direct solver such as SparseLDLSolver .
You can look at this example of the Geomagic interacting with deformable cubes:
https://github.com/sofa-framework/sofa/blob/master/applications/plugins/Geomagic/scenes/Geomagic-DeformableCubes.scn

Let me know if this help.
Best regards,

[BrunoB81HK]

Hi,

I could gladly take the time to tell you more about our device if you want.

Is your problem happening when you use 2 tools instead of 1 or only when you change UncoupledConstraintCorrection by PrecomputedConstraintCorrection?

When I use only one instrument with PrecomputedConstraintCorrection, everything is fine. The problem arise when I'm trying to use a PrecomputedConstraintCorrection for 2 instruments at the same time. There is no problem if I'm using an UncoupledConstraintCorrection for 2 instruments at the same time. As you said in you post, maybe I'm just not using the apropriate constraint correction.

Therefor if you want an accurate solution I recommend to use LinearSolverConstraintCorrection but you need to use direct solver such as SparseLDLSolver.

I'll try this this week and give you some feedback on it.

Thanks a lot for your help!

[BrunoB81HK]

Hi @epernod,

From what I've been able to test today, the proposition to use a LinearSolverConstraintCorrection with a SparseLDLSolver works like a charm!

thanks a lot for your help!