[IRC] Fix things about xTips

examples/SingleBeamDeployment.scn

@@ -36,8 +36,8 @@
         <WireBeamInterpolation name="BeamInterpolation" WireRestShape="@../EdgeTopology/BeamRestShape" />
         <AdaptiveBeamForceFieldAndMass name="BeamForceField" interpolation="@BeamInterpolation"/>
         <InterventionalRadiologyController name="DeployController" template="Rigid3d" instruments="BeamInterpolation"
-                                    topology="@MeshLines" startingPos="0 0 0 0 0 0 1" xtip="0 0 0" printLog="1" 
-                                    rotationInstrument="0 0 0" step="0.5" speed="0.5" 
+                                    topology="@MeshLines" startingPos="0 0 0 0 0 0 1" xtip="0" printLog="1" 
+                                    rotationInstrument="0" step="0.5" speed="0.5" 
                                     listening="1" controlledInstrument="0"/>
         <FixedProjectiveConstraint name="FixedConstraint" indices="0" />
         <RestShapeSpringsForceField name="RestSPForceField" points="@DeployController.indexFirstNode" angularStiffness="1e8" stiffness="1e8"/> 

examples/SingleBeamDeploymentCollision.scn

@@ -54,8 +54,8 @@
         <WireBeamInterpolation name="BeamInterpolation" WireRestShape="@../EdgeTopology/BeamRestShape"/>
         <AdaptiveBeamForceFieldAndMass name="BeamForceField" interpolation="@BeamInterpolation"/>
         <InterventionalRadiologyController name="DeployController" template="Rigid3d" instruments="BeamInterpolation"
-                                    topology="@MeshLines" startingPos="0 0 0 0 0 0 1" xtip="0 0 0" printLog="1" 
-                                    rotationInstrument="0 0 0" step="5" speed="5" 
+                                    topology="@MeshLines" startingPos="0 0 0 0 0 0 1" xtip="0" printLog="1" 
+                                    rotationInstrument="0" step="5" speed="5" 
                                     listening="1" controlledInstrument="0"/>
         <LinearSolverConstraintCorrection wire_optimization="true" printLog="0" />
         <FixedProjectiveConstraint name="FixedConstraint" indices="0" />

examples/Tool_from_loader.scn

@@ -51,7 +51,7 @@
         <InterventionalRadiologyController name="IRController" template="Rigid3d" listening="true" controlledInstrument="0" 
             instruments="Interpol_GCatheter" printLog="0" 
             speed="10"  step="1" startingPos="0.0 0.0 0.0  0 0 0 1" 
-            rotationInstrument="0 0 0" xtip="0 0 0"  
+            rotationInstrument="0" xtip="0"  
             mainDirection="1 0 0"
         />
 

examples/python3/SingleBeamDeployment.py

@@ -41,8 +41,8 @@ def createScene(rootNode):
     BeamMechanics.addObject('WireBeamInterpolation', name='BeamInterpolation', WireRestShape='@../EdgeTopology/BeamRestShape', printLog=False)
     BeamMechanics.addObject('AdaptiveBeamForceFieldAndMass', name='BeamForceField', massDensity=0.00000155, interpolation='@BeamInterpolation')
     BeamMechanics.addObject('InterventionalRadiologyController', name='DeployController', template='Rigid3d', instruments='BeamInterpolation', 
-                                    topology="@MeshLines", startingPos=[0, 0, 0, 0, 0, 0, 1], xtip=[0, 0, 0], printLog=True, 
-                                    rotationInstrument=[0, 0, 0], step=0.5, speed=0.5, 
+                                    topology="@MeshLines", startingPos=[0, 0, 0, 0, 0, 0, 1], xtip=[0], printLog=True, 
+                                    rotationInstrument=[0], step=0.5, speed=0.5, 
                                     listening=True, controlledInstrument=0)
     BeamMechanics.addObject('FixedProjectiveConstraint', indices=0, name='FixedConstraint')
     BeamMechanics.addObject('RestShapeSpringsForceField', name="RestSPForceField", points='@DeployController.indexFirstNode', angularStiffness=1e8, stiffness=1e8)

examples/python3/SingleBeamDeploymentCollision.py

@@ -49,8 +49,8 @@ def createScene(rootNode):
     BeamMechanics.addObject('WireBeamInterpolation', name='BeamInterpolation', WireRestShape='@../EdgeTopology/BeamRestShape', printLog=False)
     BeamMechanics.addObject('AdaptiveBeamForceFieldAndMass', name='BeamForceField', massDensity=0.00000155, interpolation='@BeamInterpolation')
     BeamMechanics.addObject('InterventionalRadiologyController', name='DeployController', template='Rigid3d', instruments='BeamInterpolation', 
-                                    topology="@MeshLines", startingPos=[0, 0, 0, 0, 0, 0, 1], xtip=[0, 0, 0], printLog=True, 
-                                    rotationInstrument=[0, 0, 0], step=5., speed=5., 
+                                    topology="@MeshLines", startingPos=[0, 0, 0, 0, 0, 0, 1], xtip=[0], printLog=True, 
+                                    rotationInstrument=[0], step=5., speed=5., 
                                     listening=True, controlledInstrument=0)
     BeamMechanics.addObject('LinearSolverConstraintCorrection', wire_optimization='true', printLog=False)
     BeamMechanics.addObject('FixedProjectiveConstraint', indices=0, name='FixedConstraint')

src/BeamAdapter/component/controller/InterventionalRadiologyController.h

@@ -86,7 +86,6 @@ class InterventionalRadiologyController : public sofa::component::controller::Me
 
     ////////////////////// Inherited from BaseObject ///////////////////////////////////////////////
     virtual void init() override ;
-    virtual void bwdInit() override ;
     ////////////////////////////////////////////////////////////////////////////////////////////////
 
 

src/BeamAdapter/component/controller/InterventionalRadiologyController.inl

@@ -43,6 +43,7 @@
 
 
 #include <BeamAdapter/component/controller/InterventionalRadiologyController.h>
+#include <ranges>
 
 
 namespace beamadapter
@@ -128,6 +129,7 @@ void InterventionalRadiologyController<DataTypes>::init()
     {
         msg_info() << m_instrumentsList.size() << " instrument(s) found (WireBeamInterpolation)";
     }
+    const auto numberOfInstruments = m_instrumentsList.size();
 
      m_activatedPointsBuf.clear();
 
@@ -143,15 +145,44 @@ void InterventionalRadiologyController<DataTypes>::init()
         loadMotionData(d_motionFilename.getValue());
     }
 
-    auto x_instr_tip = sofa::helper::getWriteOnlyAccessor(d_xTip);
-    x_instr_tip.resize(m_instrumentsList.size());
-
-    auto angle_Instrument = sofa::helper::getWriteOnlyAccessor(d_rotationInstrument);
-    angle_Instrument.resize(m_instrumentsList.size());
-
-    for(unsigned int i=0; i<m_instrumentsList.size(); i++)
-        m_instrumentsList[i]->setControlled(true);
+    auto checkData = [&](auto& data)
+    {
+        if(data.isSet())
+        {
+            const auto& values = data.getValue();
+            const auto valueSize = values.size();
+
+            if(valueSize != numberOfInstruments)
+            {
+                msg_warning() << "Discrepancy for " << data.getName()  << " value: it manages " << valueSize << " tools, but there are " << numberOfInstruments << " defined in instrumentPathList.";
+                if(valueSize > numberOfInstruments)
+                {
+                    msg_warning() << "The superfluous values will be ignored.";
+                }
+                else
+                {
+                    msg_warning() << "The missing values will be set as zero.";
+                }
+            }
+        }
+    };
+
+    {
+        checkData(d_xTip);
+        auto xTip = sofa::helper::getWriteOnlyAccessor(d_xTip);
+        xTip.resize(numberOfInstruments);
+    }
+    {
+        checkData(d_rotationInstrument);
+        auto angle_Instrument = sofa::helper::getWriteOnlyAccessor(d_rotationInstrument);
+        angle_Instrument.resize(numberOfInstruments);
+    }
 
+    for(auto* instrument : m_instrumentsList)
+    {
+        instrument->setControlled(true);
+    }
+
     if (!l_fixedConstraint)
     {
         typename FixedProjectiveConstraint<DataTypes>::SPtr fixedConstraint{};
@@ -176,49 +207,49 @@ void InterventionalRadiologyController<DataTypes>::init()
 
     m_nodeCurvAbs.clear();
     m_idInstrumentCurvAbsTable.clear();
+
+    // initiliaze current curvAbs
+    // if there are to be deployed at start (xtip set) then it should reflect that.
+    // it should always start with zero (origin)
     m_nodeCurvAbs.push_back(0.0);
-    type::vector<int> listInit;
-
-    for(unsigned int i=0; i<m_instrumentsList.size(); i++)
-        listInit.push_back(int(i));
-
-    m_idInstrumentCurvAbsTable.push_back(listInit);
-
-    Inherit::init();
-
-    sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid);
-}
-
-template<class DataTypes>
-void InterventionalRadiologyController<DataTypes>::loadMotionData(std::string filename)
-{
-    if (!helper::system::DataRepository.findFile(filename))
+    const auto& xTips = d_xTip.getValue();
+    for(const auto xTip : xTips | std::views::reverse)
     {
-        msg_error() << "File " << filename << " not found.";
-        return;
+        if(xTip > 0.0)
+        {
+            m_nodeCurvAbs.push_back(xTip);
+        }
     }
-    std::ifstream file(filename.c_str());
-
-    std::string line;
-    Vec3 result;
-    while( std::getline(file,line) )
+
+    // initialize curvAbsNode <-> instrument ID table
+    // i,e identify for each "simulated" node the corresponding tool(s)
+    // if xtip not initialized then only the 0 (origin) node is considered, and has all the tools
+    // and as before, if they are deployed at start (xtip set), then we need to take it into account
+    for(sofa::Index i = 0 ; i < m_nodeCurvAbs.size(); i++)
     {
-        if (line.empty())
-            continue;
-        std::istringstream values(line);
-        values >> result[0] >> result[1] >> result[2];
-        result[0] /= 1000;
-        m_sensorMotionData.push_back(result);
+        type::vector<int> listTool;
+        for(sofa::Index id = 0 ; id < numberOfInstruments ; id++)
+        {
+            const auto xTip = xTips[id];
+
+            if(xTip >= m_nodeCurvAbs[i])
+            {
+                listTool.push_back(id);
+            }
+        }
+
+        m_idInstrumentCurvAbsTable.push_back(listTool);
     }
 
-    file.close();
-}
+
+    type::vector<int> listInit;
+    for(unsigned int i=0; i<numberOfInstruments; i++)
+        listInit.push_back(int(i));
 
+    m_idInstrumentCurvAbsTable.push_back(listInit);
 
-template <class DataTypes>
-void InterventionalRadiologyController<DataTypes>::bwdInit()
-{
-    // assign the starting pos to each point of the Mechanical State
+    Inherit::init();
+
     Coord stPos =d_startingPos.getValue();
     stPos.getOrientation().normalize();
     d_startingPos.setValue(stPos);
@@ -257,6 +288,30 @@ void InterventionalRadiologyController<DataTypes>::bwdInit()
     sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid);
 }
 
+template<class DataTypes>
+void InterventionalRadiologyController<DataTypes>::loadMotionData(std::string filename)
+{
+    if (!helper::system::DataRepository.findFile(filename))
+    {
+        msg_error() << "File " << filename << " not found.";
+        return;
+    }
+    std::ifstream file(filename.c_str());
+
+    std::string line;
+    Vec3 result;
+    while( std::getline(file,line) )
+    {
+        if (line.empty())
+            continue;
+        std::istringstream values(line);
+        values >> result[0] >> result[1] >> result[2];
+        result[0] /= 1000;
+        m_sensorMotionData.push_back(result);
+    }
+
+    file.close();
+}
 
 /*!
  * \todo fix the mouse event with better controls