Skip to content

[tools] Create new articulated tool manager to simulate Pliers and scisors #16

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
epernod merged 17 commits into from
Oct 29, 2024
Merged

[tools] Create new articulated tool manager to simulate Pliers and scisors #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
25ac347
[tools] Clean PliersTool
epernod Dec 27, 2022
dcb3ecd
[tools] Move PliersToolManager into InteractionTools folder
epernod Dec 27, 2022
d823079
[tools] Start moving code from ArticulatedToolManager into dedicated …
epernod Dec 27, 2022
e6069a6
[tools] Continue moving code from ArticulatedToolManager into dedicat…
epernod Dec 28, 2022
7a69b9b
[src] Update Jaw models classes
epernod Dec 30, 2022
9597070
[scenes] Add example scene to use articulatedToolManager (wip)
epernod Dec 30, 2022
6935b94
[tools] Update ArticulatedToolManager with collision detection filter…
epernod Jul 30, 2024
45a8a3a
[tools] Start gathering more info in the jawsModel
epernod Jul 30, 2024
84f0f15
[tools] Move graspContact into BaseJawModel
epernod Jul 30, 2024
52a7063
[tools] Add stiffSpringFF
epernod Jul 30, 2024
d45fde6
[tools] Some code cleaning
epernod Jul 31, 2024
5f1e724
[tools] Fix stiffSpring, it should be placed under organs
epernod Jul 31, 2024
8cb6d3a
[tools] Update InteractionTool API to be called from outside. And som…
epernod Jul 31, 2024
356fabf
[tools] remove tool deactivation
epernod Aug 28, 2024
e11e59e
Fix compilation to match recent version of SOFA
epernod Aug 28, 2024
e209d19
small fix and backup scene structure to use the jaws
epernod Oct 28, 2024
ac32f43
Merge branch 'main' into pliers_tools
epernod Oct 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 14 additions & 4 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,12 +25,11 @@ set(USE_INFINYTOOLKIT_PLUGIN true CACHE BOOL "Use Interaction Tools plugin")
set(INFINYTOOLKIT_SRC_DIR src/InfinyToolkit)

set(HEADER_FILES
${INFINYTOOLKIT_SRC_DIR}/config.h.in
${INFINYTOOLKIT_SRC_DIR}/config.h.in
${INFINYTOOLKIT_SRC_DIR}/CollisionDetectionDisplay.h
${INFINYTOOLKIT_SRC_DIR}/RotationEngine.h
${INFINYTOOLKIT_SRC_DIR}/PliersToolManager.h
${INFINYTOOLKIT_SRC_DIR}/PliersPositionsMapper.h
${INFINYTOOLKIT_SRC_DIR}/HapticEmulator.h
${INFINYTOOLKIT_SRC_DIR}/HapticEmulator.h
${INFINYTOOLKIT_SRC_DIR}/NeedleTracker.h
${INFINYTOOLKIT_SRC_DIR}/MiddleForceField.h
${INFINYTOOLKIT_SRC_DIR}/MiddleForceField.inl
Expand All @@ -44,6 +43,12 @@ set(HEADER_FILES
${INFINYTOOLKIT_SRC_DIR}/CarvingTools/SurfaceCarvingPerformer.h
${INFINYTOOLKIT_SRC_DIR}/CarvingTools/HapticCarvingManager.h

## Interaction tools sections
${INFINYTOOLKIT_SRC_DIR}/InteractionTools/ArticulatedToolManager.h
${INFINYTOOLKIT_SRC_DIR}/InteractionTools/BaseJawModel.h
${INFINYTOOLKIT_SRC_DIR}/InteractionTools/GrasperJawModel.h
${INFINYTOOLKIT_SRC_DIR}/InteractionTools/ScissorJawModel.h

## Mesh tools sections
${INFINYTOOLKIT_SRC_DIR}/MeshTools/NearestTexcoordsMap.h

Expand All @@ -53,7 +58,6 @@ set(HEADER_FILES
set(SOURCE_FILES
${INFINYTOOLKIT_SRC_DIR}/CollisionDetectionDisplay.cpp
${INFINYTOOLKIT_SRC_DIR}/RotationEngine.cpp
${INFINYTOOLKIT_SRC_DIR}/PliersToolManager.cpp
${INFINYTOOLKIT_SRC_DIR}/PliersPositionsMapper.cpp
${INFINYTOOLKIT_SRC_DIR}/HapticEmulator.cpp
${INFINYTOOLKIT_SRC_DIR}/NeedleTracker.cpp
Expand All @@ -69,6 +73,12 @@ set(SOURCE_FILES
${INFINYTOOLKIT_SRC_DIR}/CarvingTools/SurfaceCarvingPerformer.cpp
${INFINYTOOLKIT_SRC_DIR}/CarvingTools/HapticCarvingManager.cpp

## Interaction tools sections
${INFINYTOOLKIT_SRC_DIR}/InteractionTools/ArticulatedToolManager.cpp
${INFINYTOOLKIT_SRC_DIR}/InteractionTools/BaseJawModel.cpp
${INFINYTOOLKIT_SRC_DIR}/InteractionTools/GrasperJawModel.cpp
${INFINYTOOLKIT_SRC_DIR}/InteractionTools/ScissorJawModel.cpp

## Mesh tools sections
${INFINYTOOLKIT_SRC_DIR}/MeshTools/NearestTexcoordsMap.cpp

Expand Down
127 changes: 127 additions & 0 deletions examples/ArticulatedToolManager.scn
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,127 @@
<?xml version="1.0" ?>
<Node name="root" dt="0.05" gravity="0 -9 0" >
<VisualStyle displayFlags="hideVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields" />

<Node name="RequiredPlugins">
<RequiredPlugin name="Geomagic"/> <!-- Needed to use components [GeomagicDriver] -->
<RequiredPlugin name="Sofa.Component.AnimationLoop"/> <!-- Needed to use components [FreeMotionAnimationLoop] -->
<RequiredPlugin name="Sofa.Component.Collision.Detection.Algorithm"/> <!-- Needed to use components [BVHNarrowPhase, BruteForceBroadPhase, DefaultPipeline] -->
<RequiredPlugin name="Sofa.Component.Collision.Detection.Intersection"/> <!-- Needed to use components [LocalMinDistance] -->
<RequiredPlugin name="Sofa.Component.Collision.Geometry"/> <!-- Needed to use components [LineCollisionModel, PointCollisionModel, TriangleCollisionModel] -->
<RequiredPlugin name="Sofa.Component.Collision.Response.Contact"/> <!-- Needed to use components [DefaultContactManager] -->
<RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Correction"/> <!-- Needed to use components [LinearSolverConstraintCorrection] -->
<RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Solver"/> <!-- Needed to use components [LCPConstraintSolver] -->
<RequiredPlugin name="Sofa.Component.Constraint.Projective"/> <!-- Needed to use components [FixedConstraint] -->
<RequiredPlugin name="Sofa.Component.Engine.Select"/> <!-- Needed to use components [BoxROI] -->
<RequiredPlugin name="Sofa.Component.Haptics"/> <!-- Needed to use components [LCPForceFeedback] -->
<RequiredPlugin name="Sofa.Component.IO.Mesh"/> <!-- Needed to use components [MeshOBJLoader] -->
<RequiredPlugin name="Sofa.Component.LinearSolver.Direct"/> <!-- Needed to use components [SparseLDLSolver] -->
<RequiredPlugin name="Sofa.Component.Mapping.Linear"/> <!-- Needed to use components [IdentityMapping] -->
<RequiredPlugin name="Sofa.Component.Mapping.NonLinear"/> <!-- Needed to use components [RigidMapping] -->
<RequiredPlugin name="Sofa.Component.Mass"/> <!-- Needed to use components [DiagonalMass, UniformMass] -->
<RequiredPlugin name="Sofa.Component.ODESolver.Backward"/> <!-- Needed to use components [EulerImplicitSolver] -->
<RequiredPlugin name="Sofa.Component.SolidMechanics.FEM.Elastic"/> <!-- Needed to use components [TetrahedralCorotationalFEMForceField] -->
<RequiredPlugin name="Sofa.Component.SolidMechanics.Spring"/> <!-- Needed to use components [RestShapeSpringsForceField] -->
<RequiredPlugin name="Sofa.Component.StateContainer"/> <!-- Needed to use components [MechanicalObject] -->
<RequiredPlugin name="Sofa.Component.Topology.Container.Constant"/> <!-- Needed to use components [MeshTopology] -->
<RequiredPlugin name="Sofa.Component.Topology.Container.Dynamic"/> <!-- Needed to use components [TetrahedronSetGeometryAlgorithms, TetrahedronSetTopologyContainer, TetrahedronSetTopologyModifier, TriangleSetGeometryAlgorithms, TriangleSetTopologyContainer, TriangleSetTopologyModifier] -->
<RequiredPlugin name="Sofa.Component.Topology.Container.Grid"/> <!-- Needed to use components [RegularGridTopology] -->
<RequiredPlugin name="Sofa.Component.Topology.Mapping"/> <!-- Needed to use components [Hexa2TetraTopologicalMapping, Tetra2TriangleTopologicalMapping] -->
<RequiredPlugin name="Sofa.Component.Visual"/> <!-- Needed to use components [VisualStyle] -->
<RequiredPlugin name="Sofa.GL.Component.Rendering3D"/> <!-- Needed to use components [OglModel] -->
<RequiredPlugin name="InfinyToolkit" />
</Node>

<DefaultVisualManagerLoop />
<FreeMotionAnimationLoop/>
<LCPConstraintSolver tolerance="0.001" maxIt="1000"/>

<DefaultPipeline name="pipeline" depth="6" verbose="0"/>
<BruteForceBroadPhase/>
<BVHNarrowPhase/>
<DefaultContactManager name="response" response="FrictionContactConstraint" />
<MinProximityIntersection name="Proximity" alarmDistance="1.0" contactDistance="0.1" />

<GeomagicDriver name="GeomagicDevice" deviceName="Default Device" scale="1" drawDeviceFrame="1"
positionBase="0 0 0" orientationBase="0 0.707 0 -0.707" drawDevice="0"/>

<ArticulatedToolManager name="toolManager" jawModel1="@Instrument/JawUp_collision/jawUp" jawModel2="@Instrument/JawDown_collision/jawDown" />

<Node name="ToolController">
<MechanicalObject template="Rigid3" name="GrasperDOFs" position="@GeomagicDevice.positionDevice"/>
</Node>






<Node name="Instrument" >
<EulerImplicitSolver name="ODE solver" rayleighStiffness="0.05" rayleighMass="1.0" />
<SparseLDLSolver template="CompressedRowSparseMatrixMat3x3d"/>

<MechanicalObject name="instrumentState" template="Rigid3" tags="GeomagicDevice" position="0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1"/>
<UniformMass name="mass" totalMass="0.1" />

<RestShapeSpringsForceField stiffness='100000000000000 100000000000000 100000000000000' angularStiffness='100000000000000 100000000000000 100000000000000'
external_rest_shape='@../ToolController/DOFs' points='0' external_points='0' />
<LCPForceFeedback activate="true" forceCoef="0.001"/>
<LinearSolverConstraintCorrection/>

<Node name="CollisionModel" >
<MeshOBJLoader filename="./mesh/grasper_shaft_collision.obj" name="loader"/>
<MeshTopology src="@loader" name="InstrumentCollisionModel" />
<MechanicalObject src="@loader" name="instrumentCollisionState" dy="0" rz="0" dz="0" tags="toolPosition"/>
<TriangleCollisionModel name="triangleCol" tags="toolCollision_shaft"/>
<LineCollisionModel name="instrumentLine" tags="toolCollision_shaft"/>
<PointCollisionModel name="pointCol" tags="toolCollision_shaft"/>

<RigidMapping input="@.." output="@instrumentCollisionState" index="0"/>
</Node>

<Node name="Shaft_visu" >
<MeshOBJLoader name="meshLoader_1" filename="./mesh/grasper_shaft.obj" handleSeams="1" />
<OglModel name="InstrumentVisualModel" src="@meshLoader_1"/>
<RigidMapping input="@.." output="@InstrumentVisualModel" index="0"/>
</Node>

<Node name="JawUp_collision" >
<MeshOBJLoader filename="./mesh/grasper_jaws_up_collision.obj" name="loader"/>
<MeshTopology src="@loader" name="InstrumentCollisionModel" />
<MechanicalObject src="@loader" name="instrumentCollisionState" ry="0" rz="0" dy="0" />

<GrasperJawModel name="jawUp" jawModel="@instrumentCollisionState"/>

<TriangleCollisionModel name="triangleCol" tags="toolCollision_jaws"/>
<LineCollisionModel name="instrumentLine" tags="toolCollision_jaws"/>
<PointCollisionModel name="pointCol" tags="toolCollision_jaws"/>
<RigidMapping input="@.." output="@instrumentCollisionState" index="1"/>
</Node>

<Node name="JawUp_visu" >
<MeshOBJLoader name="meshLoader_1" filename="./mesh/grasper_jaws_up.obj" handleSeams="1" />
<OglModel name="InstrumentVisualModel" src="@meshLoader_1"/>
<RigidMapping input="@.." output="@InstrumentVisualModel" index="1"/>
</Node>

<Node name="JawDown_collision" >
<MeshOBJLoader filename="./mesh/grasper_jaws_down_collision.obj" name="loader"/>
<MeshTopology src="@loader" name="InstrumentCollisionModel" />
<MechanicalObject src="@loader" name="instrumentCollisionState" ry="0" rz="0" dy="0" />

<GrasperJawModel name="jawDown" jawModel="@instrumentCollisionState"/>

<TriangleCollisionModel name="triangleCol" tags="toolCollision_jaws"/>
<LineCollisionModel name="instrumentLine" tags="toolCollision_jaws"/>
<PointCollisionModel name="pointCol" tags="toolCollision_jaws"/>
<RigidMapping input="@.." output="@instrumentCollisionState" index="2"/>
</Node>

<Node name="JawDown_visu" >
<MeshOBJLoader name="meshLoader_1" filename="./mesh/grasper_jaws_down.obj" handleSeams="1" />
<OglModel name="InstrumentVisualModel" src="@meshLoader_1"/>
<RigidMapping input="@.." output="@InstrumentVisualModel" index="2"/>
</Node>
</Node>

</Node>
106 changes: 106 additions & 0 deletions examples/VirtualTools.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,106 @@
<Node name="Group">
<Node name="Jaws_up_controller_right">
<MeshOBJLoader name="loader" filename="mesh/tools/grasper_up_jaw_collision_light.obj" scale3d="1 1 1" translation="0 0 0" handleSeams="0"/>
<MeshTopology name="Jaws_up_right_mesh" src="@loader" />
<MechanicalObject name="dofs" src="@loader" />

</Node>

<Node name="Jaws_down_controller_right">
<MeshOBJLoader name="loader" filename="mesh/tools/grasper_low_jaw_collision_light.obj" scale3d="1 1 1" translation="0 0 0" handleSeams="0"/>
<MeshTopology name="Jaws_down_right_mesh" src="@loader" />
<MechanicalObject name="dofs" src="@loader" />
</Node>



<Node name="Jaws_up_controller_left">
<MeshOBJLoader name="loader" filename="mesh/tools/grasper_up_jaw_collision_light.obj" scale3d="1 1 1" translation="0 0 0" handleSeams="0"/>
<MeshTopology name="Jaws_up_left_mesh" src="@loader" />
<MechanicalObject name="dofs" src="@loader" />
</Node>

<Node name="Jaws_down_controller_left">
<MeshOBJLoader name="loader" filename="mesh/tools/grasper_low_jaw_collision_light.obj" scale3d="1 1 1" translation="0 0 0" handleSeams="0"/>
<MeshTopology name="Jaws_down_left_mesh" src="@loader" />
<MechanicalObject name="dofs" src="@loader" />
</Node>


<Node name="Jaws_up_right" >
<EulerImplicitSolver name="ODE solver" rayleighStiffness="0.01" rayleighMass="0.0" />
<SparseLDLSolver template="CompressedRowSparseMatrixMat3x3d"/>

<MeshOBJLoader name="loader" filename="mesh/tools/grasper_up_jaw_collision_light.obj" scale3d="1 1 1" translation="0 0 0" handleSeams="0"/>
<MechanicalObject name="instrumentState" src="@loader" />
<MeshTopology name="InstrumentCollisionModel" src="@loader"/>
<UniformMass name="mass" totalMass="1" />

<RestShapeSpringsForceField stiffness='1e8' angularStiffness='1e8' external_rest_shape='@../Jaws_up_controller_right/dofs' />
<LinearSolverConstraintCorrection/>


<SphereCollisionModel name="sphereColUpRight" radius="0.01" simulated="1" moving="1" group="3" />

<GrasperJawModel name="grasperJawUpRight" jawController="@../Jaws_up_controller_right/dofs" jawDofs="@instrumentState" jawCollision="@sphereColUpRight" stiffness="1000"/>
</Node>


<Node name="Jaws_down_right" >
<EulerImplicitSolver name="ODE solver" rayleighStiffness="0.01" rayleighMass="0.0" />
<SparseLDLSolver template="CompressedRowSparseMatrixMat3x3d"/>

<MeshOBJLoader name="loader" filename="mesh/tools/grasper_low_jaw_collision_light.obj" scale3d="1 1 1" translation="0 0 0" handleSeams="0"/>
<MechanicalObject name="instrumentState" src="@loader" />

<UniformMass name="mass" totalMass="1" />

<RestShapeSpringsForceField stiffness='1e8' angularStiffness='1e8' external_rest_shape='@../Jaws_down_controller_right/dofs' />
<LinearSolverConstraintCorrection/>

<SphereCollisionModel name="sphereColDownRight" radius="0.01" simulated="1" moving="1" group="3" />

<GrasperJawModel name="grasperJawDownRight" jawController="@../Jaws_down_controller_right/dofs" jawDofs="@instrumentState" jawCollision="@sphereColDownRight" stiffness="1000"/>
</Node>





<Node name="Jaws_up_left" >
<EulerImplicitSolver name="ODE solver" rayleighStiffness="0.01" rayleighMass="0.0" />
<SparseLDLSolver template="CompressedRowSparseMatrixMat3x3d"/>

<MeshOBJLoader name="loader" filename="mesh/tools/grasper_up_jaw_collision_light.obj" scale3d="1 1 1" translation="0 0 0" handleSeams="0"/>
<MechanicalObject name="instrumentState" src="@loader" />
<MeshTopology name="InstrumentCollisionModel" src="@loader"/>
<UniformMass name="mass" totalMass="1" />

<RestShapeSpringsForceField stiffness='1e8' angularStiffness='1e8' external_rest_shape='@../Jaws_up_controller_left/dofs' />
<LinearSolverConstraintCorrection/>

<SphereCollisionModel name="sphereColUpLeft" radius="0.01" simulated="1" moving="1" group="3"/>
<GrasperJawModel name="grasperJawUpLeft" jawController="@../Jaws_up_controller_left/dofs" jawDofs="@instrumentState" jawCollision="@sphereColUpLeft" stiffness="1000"/>
</Node>


<Node name="Jaws_down_left" >
<EulerImplicitSolver name="ODE solver" rayleighStiffness="0.01" rayleighMass="0.0" />
<SparseLDLSolver template="CompressedRowSparseMatrixMat3x3d"/>

<MeshOBJLoader name="loader" filename="mesh/tools/grasper_low_jaw_collision_light.obj" scale3d="1 1 1" translation="0 0 0" handleSeams="0"/>
<MechanicalObject name="instrumentState" src="@loader" />
<MeshTopology name="InstrumentCollisionModel" src="@loader"/>
<UniformMass name="mass" totalMass="1" />

<RestShapeSpringsForceField stiffness='1e8' angularStiffness='1e8' external_rest_shape='@../Jaws_down_controller_left/dofs' />
<LinearSolverConstraintCorrection/>

<SphereCollisionModel name="sphereColDownLeft" radius="0.01" simulated="1" moving="1" group="3"/>
<GrasperJawModel name="grasperJawDownLeft" jawController="@../Jaws_down_controller_left/dofs" jawDofs="@instrumentState" jawCollision="@sphereColDownLeft" stiffness="1000"/>
</Node>

<ArticulatedToolManager name="Grasper_right_manager" jawModel1="@Jaws_up_right/grasperJawUpRight" jawModel2="@Jaws_down_right/grasperJawDownRight" targetModel="@Organs/dofs" printLog="0" drawContacts="0"/>
<ArticulatedToolManager name="Grasper_left_manager" jawModel1="@Jaws_up_left/grasperJawUpLeft" jawModel2="@Jaws_down_left/grasperJawDownLeft" targetModel="@Organs/dofs" printLog="0" drawContacts="0"/>

</Node>
Loading