[Controller] Add MotionReplayController component to load deformation of a CSV file, add an option for extra motion - Fixes LNROBO-34

CMakeLists.txt

@@ -19,6 +19,8 @@ find_package(Sofa.Component.Collision.Geometry REQUIRED)
 find_package(Sofa.Component.Controller REQUIRED)
 find_package(Sofa.Component.Haptics REQUIRED)
 find_package(Sofa.GUI.Component REQUIRED)
+find_package(Sofa.Component.Engine.Select REQUIRED)
+
 
 set(USE_INFINYTOOLKIT_PLUGIN true CACHE BOOL "Use Interaction Tools plugin")
 
@@ -58,6 +60,12 @@ set(HEADER_FILES
     ${INFINYTOOLKIT_SRC_DIR}/MeshTools/GridBarycentersPositions.h
 
     ${INFINYTOOLKIT_SRC_DIR}/BruteForceFeedback.h
+
+    ## Replay motion controller
+    ${INFINYTOOLKIT_SRC_DIR}/MotionReplayController/MotionReplayController.h
+
+
+
 )
 
 set(SOURCE_FILES
@@ -91,6 +99,11 @@ set(SOURCE_FILES
     ${INFINYTOOLKIT_SRC_DIR}/MeshTools/GridBarycentersPositions.cpp
 
     ${INFINYTOOLKIT_SRC_DIR}/BruteForceFeedback.cpp
+
+    ## Replay motion controller
+    ${INFINYTOOLKIT_SRC_DIR}/MotionReplayController/MotionReplayController.cpp
+
+
 )
 
 # Add component for carving using refinement mesh
@@ -129,6 +142,7 @@ target_link_libraries(${PROJECT_NAME}
     Sofa.Component.Controller 
     Sofa.Component.Haptics
     Sofa.GUI.Component
+    Sofa.Component.Engine.Select
 )
 
 

examples/MotionReplayController.scn

@@ -0,0 +1,64 @@
+<?xml version="1.0" ?>
+<Node name="root" dt="0.02" gravity="0 0 0">
+    <!-- Required Plugins -->
+    <RequiredPlugin name="Sofa.Component.StateContainer"/> <!-- Needed to use components [MechanicalObject] -->
+    <RequiredPlugin name="Sofa.Component.Topology.Container.Grid"/> <!-- Needed to use components [RegularGridTopology] -->
+    <RequiredPlugin name="Sofa.Component.Visual"/> <!-- Needed to use components [VisualStyle] -->
+    <RequiredPlugin name="Sofa.GL.Component.Rendering3D"/> <!-- Needed to use components [OglModel] -->
+    <RequiredPlugin name="Sofa.Component.Mapping.Linear"/> <!-- Needed to use components [IdentityMapping] -->
+    <RequiredPlugin name="Sofa.Component.Constraint.Projective"/> <!-- Needed to use components [FixedProjectiveConstraint] -->
+    <RequiredPlugin name="Sofa.Component.Engine.Select"/> <!-- Needed to use components [BoxROI] -->
+    <RequiredPlugin name="InfinyToolkit"/> <!-- Needed to use MotionReplayController -->
+
+    <VisualStyle displayFlags="showVisual showWireframe showBehavior"/>
+
+
+    <!-- Animation and Visual Loops -->
+    <DefaultAnimationLoop/>
+    <DefaultVisualManagerLoop />
+
+
+    <!-- Patient Grid Node -->
+    <Node name="PatientGrid">
+
+        <!-- Regular Grid Topology -->
+        <RegularGridTopology
+            name="grid"
+            nx="10" ny="10" nz="10"
+            xmin="-8" xmax="12"
+            ymin="-6" ymax="14"
+            zmin="-10" zmax="10"
+        />
+
+        <!-- Mechanical Object linked to the grid -->
+        <MechanicalObject
+            name="mechanicalDofs"
+            template="Vec3d"
+            position="@grid.position"
+            listening="true"
+        />
+
+        <Node name="VisuGrid">
+            <OglModel color="green" src="@../mechanicalDofs" name="Visual" />
+            <IdentityMapping input="@../" output="@Visual"/>
+        </Node>
+
+
+        <!-- Fixed indices-->
+        <BoxROI name="TopROI"
+        drawBoxes="0"
+        box="-3.0 -5.0 -4.0  1.5 -3.5 2.0" />
+
+        <FixedProjectiveConstraint indices="@TopROI.indices" />
+
+        <!-- Motion Replay Controller -->
+        <MotionReplayController
+            name="motionReplay"
+            motionFile="grid_states.csv"
+            gridState="@mechanicalDofs"
+            fixedIndices = "@TopROI.indices"
+        />
+
+    </Node>
+
+</Node>

src/InfinyToolkit/MotionReplayController/MotionReplayController.cpp

@@ -0,0 +1,240 @@
+/*****************************************************************************
+ *                 - Copyright (C) - 2020 - InfinyTech3D -                   *
+ *                                                                           *
+ * This file is part of the InfinyToolkit plugin for the SOFA framework      *
+ *                                                                           *
+ * Commercial License Usage:                                                 *
+ * Licensees holding valid commercial license from InfinyTech3D may use this *
+ * file in accordance with the commercial license agreement provided with    *
+ * the Software or, alternatively, in accordance with the terms contained in *
+ * a written agreement between you and InfinyTech3D. For further information *
+ * on the licensing terms and conditions, contact: contact@infinytech3d.com  *
+ *                                                                           *
+ * GNU General Public License Usage:                                         *
+ * Alternatively, this file may be used under the terms of the GNU General   *
+ * Public License version 3. The licenses are as published by the Free       *
+ * Software Foundation and appearing in the file LICENSE.GPL3 included in    *
+ * the packaging of this file. Please review the following information to    *
+ * ensure the GNU General Public License requirements will be met:           *
+ * https://www.gnu.org/licenses/gpl-3.0.html.                                *
+ *                                                                           *
+ * Authors: see Authors.txt                                                  *
+ * Further information: https://infinytech3d.com                             *
+ ****************************************************************************/
+#pragma once
+
+#include <InfinyToolkit/MotionReplayController/MotionReplayController.h>
+#include <sofa/component/statecontainer/MechanicalObject.h>
+#include <sofa/core/ObjectFactory.h>
+#include <sofa/core/objectmodel/Context.h>
+#include <sofa/core/objectmodel/DataFileName.cpp>
+#include <sofa/helper/logging/Messaging.h>
+#include <sofa/simulation/AnimateBeginEvent.h>
+
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <unordered_set>
+
+
+namespace sofa::infinytoolkit
+{
+
+    using namespace sofa::defaulttype;
+
+    void registerMotionReplayController(sofa::core::ObjectFactory* factory)
+    {
+        factory->registerObjects(
+            sofa::core::ObjectRegistrationData("Motion replay controller to induce the heart motion.")
+            .add< MotionReplayController >()
+        );
+    }
+
+    MotionReplayController::MotionReplayController()
+        :l_gridState(initLink("gridState", "Link to the grid control."))
+        , d_fixedIndices(initData(&d_fixedIndices, "fixedIndices", "Indices of the nodes that should be fixed."))
+        , d_motionFile(initData(&d_motionFile, "motionFile",
+            "Path to CSV motion file, where each row contains one frame."))
+        , d_dvfTimeStep(initData(&d_dvfTimeStep, 0.02,
+            "dvfTimeStep", " Time step used to record the DVF."))
+        , d_displacementAmplitude(initData(&d_displacementAmplitude, 1.0, "displacementAmplitude", "Amplitude for extra motion."))
+        , d_displacementAxis(initData(&d_displacementAxis, 1, "displacementAxis", " Axis along which the extra motion is applied: 0=X, 1=Y, 2=Z."))
+        , d_infinyLoop(initData(&d_infinyLoop, true, "motionLoop", "Replay motion infinitely."))
+
+    {
+
+    }
+
+    void MotionReplayController::init()
+    {
+        // Resolve MechanicalState
+        if (l_gridState.get() == nullptr)
+        {
+            msg_error() << "Error no target grid found!";
+            this->d_componentState.setValue(
+                sofa::core::objectmodel::ComponentState::Invalid);
+            return;
+
+        }
+
+        int axis = d_displacementAxis.getValue();
+
+        if (axis < 0 || axis > 2)
+        {
+            msg_warning() << "Invalid motion axis: ", axis, ". Valid values are 0=X, 1=Y, 2=Z.";
+            return; 
+        }
+
+        double sceneDt = this->getContext()->getDt();
+        double dvfDt = d_dvfTimeStep.getValue();
+
+        if (sceneDt != dvfDt)
+        {
+            msg_warning() << "[MotionReplay] Scene time step (" << sceneDt
+                << ") differs from DVF time step (" << dvfDt
+                << "). For accurate motion replay, set the scene time step equal to the DVF time step.";
+            return;
+        }
+
+        this->f_listening.setValue(true);
+
+        loadMotion();
+
+    }
+
+
+    void MotionReplayController::handleEvent(sofa::core::objectmodel::Event* event)
+    {
+
+        if (!sofa::simulation::AnimateBeginEvent::checkEventType(event))
+            return;
+
+        if (frames.empty())
+            return;
+
+        if (currentIndex >= frames.size())
+        {
+            if (d_infinyLoop.getValue())
+                currentIndex = 0;
+            else
+                return;
+        }
+
+
+        auto positions = l_gridState->writePositions();
+
+        if (positions.size() != frames[currentIndex].size())
+        {
+            msg_error() << "[MotionReplay] Frame size mismatch: "
+                << "MO points = " << positions.size()
+                << ", frame points = " << frames[currentIndex].size();
+            return;
+        }
+
+        double offset = 0.0;
+        auto amplitudeOffset = d_displacementAmplitude.getValue();
+
+        if (amplitudeOffset)
+        {
+            double frequency = 0.1;  // Hz
+            double t = this->getContext()->getTime();
+            offset = amplitudeOffset * sin(2.0 * M_PI * frequency * t);
+        }
+
+        const auto& fixedIndices = d_fixedIndices.getValue();
+
+        if (fixedIndices.empty())
+        {
+            msg_warning() << "No fixed indices provided.";
+            return;
+        }
+
+        std::unordered_set<unsigned int> fixedSet(
+            fixedIndices.begin(),
+            fixedIndices.end()
+        );
+
+        for (size_t i = 0; i < positions.size(); ++i)
+        {
+            positions[i][0] = frames[currentIndex][i][0];
+            positions[i][1] = frames[currentIndex][i][1];
+            positions[i][2] = frames[currentIndex][i][2];
+
+            if (fixedSet.find(static_cast<unsigned int>(i)) == fixedSet.end())
+            {
+                int axis = d_displacementAxis.getValue();  // 0=X, 1=Y, 2=Z
+                positions[i][1] += offset;
+            }
+        }
+
+     ++currentIndex;
+    }
+
+    void MotionReplayController::loadMotion()
+    {
+        frames.clear();
+        currentIndex = 0;
+
+        const std::string filename = d_motionFile.getFullPath();
+
+        if (filename.empty())
+        {
+            msg_error() << "[MotionReplay] motionFile not specified!";
+            return;
+        }
+
+        // Open file stream
+        std::ifstream file(filename);
+        if (!file.is_open())
+        {
+            msg_error() << "[MotionReplay] Cannot open file: " << filename;
+            return;
+        }
+
+        size_t numPoints = l_gridState->getSize();
+
+        std::string line;
+        size_t lineNumber = 0;
+        while (std::getline(file, line))
+        {
+            ++lineNumber;
+            std::stringstream ss(line);
+            std::string value;
+
+            std::vector<double> values;
+            while (std::getline(ss, value, ','))
+            {
+                values.push_back(std::stod(value));
+            }
+
+            if (values.size() != numPoints * 3)
+            {
+                msg_error() << "[MotionReplay] Line " << lineNumber
+                    << ": expected " << numPoints * 3
+                    << " values, got " << values.size();
+                frames.clear();
+                return;
+            }
+
+            VecCoord frame;
+            frame.reserve(numPoints);
+
+            for (size_t i = 0; i < numPoints; ++i)
+            {
+                Coord c;
+                c[0] = values[3 * i + 0];
+                c[1] = values[3 * i + 1];
+                c[2] = values[3 * i + 2];
+                frame.push_back(c);
+            }
+
+            frames.push_back(std::move(frame));
+        }
+
+        msg_info() << "[MotionReplay] Loaded " << frames.size()
+            << " frames from " << filename;
+    }
+
+
+
+} // namespace sofa::infinytoolkit