Add motion replay controller component to the INFINYTOOLKIT

CMakeLists.txt

@@ -58,6 +58,13 @@ set(HEADER_FILES
     ${INFINYTOOLKIT_SRC_DIR}/MeshTools/GridBarycentersPositions.h
 
     ${INFINYTOOLKIT_SRC_DIR}/BruteForceFeedback.h
+
+    ## Replay motion controller
+    ${INFINYTOOLKIT_SRC_DIR}/MotionReplayController/MotionReplayController.h
+    ${INFINYTOOLKIT_SRC_DIR}/MotionReplayController/MotionReplayController.inl
+
+
+
 )
 
 set(SOURCE_FILES
@@ -91,6 +98,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

examples/MotionReplayController.scn

@@ -0,0 +1,53 @@
+<?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="InfinyToolkit"/> <!-- Needed to use MotionReplayController -->
+
+    <VisualStyle displayFlags="showVisual showWireframe" />
+
+
+    <!-- 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>
+
+        <!-- Motion Replay Controller -->
+        <MotionReplayController
+            name="motionReplay"
+            motionFile="grid_states.csv"
+            dt="0.02"
+        />
+
+    </Node>
+
+</Node>

src/InfinyToolkit/MotionReplayController/MotionReplayController.cpp

@@ -0,0 +1,46 @@
+/*****************************************************************************
+ *                 - 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                             *
+ ****************************************************************************/
+
+#define SOFA_COMPONENT_MOTIONREPLAYCONTROLLER_CPP
+
+#include <sofa/core/ObjectFactory.h>
+
+#include <InfinyToolkit/MotionReplayController/MotionReplayController.h>
+#include <InfinyToolkit/MotionReplayController/MotionReplayController.inl>
+
+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 >()
+    );
+}
+
+
+} // namespace sofa::infinytoolkit

src/InfinyToolkit/MotionReplayController/MotionReplayController.h

@@ -0,0 +1,77 @@
+/*****************************************************************************
+ *                 - 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/config.h>
+
+#include <sofa/component/controller/Controller.h>
+
+#include <sofa/type/Vec.h>
+#include <sofa/defaulttype/VecTypes.h>
+#include <sofa/core/objectmodel/Data.h>
+#include <sofa/core/objectmodel/Event.h>
+#include <sofa/core/objectmodel/DataFileName.h>
+
+#include <vector>
+#include <string>
+
+namespace sofa::infinytoolkit
+{
+
+class MotionReplayController
+    : public sofa::component::controller::Controller
+{
+public:
+
+    SOFA_CLASS(MotionReplayController,
+                sofa::component::controller::Controller);
+
+    MotionReplayController();
+    ~MotionReplayController() override = default;
+
+
+   using Coord = sofa::type::Vec3d;
+   using VecCoord = std::vector<Coord>;
+
+
+
+   void init() override;
+   void handleEvent(sofa::core::objectmodel::Event* event) override;
+
+private:
+
+    sofa::core::objectmodel::DataFileName d_motionFile; /// CSV file containing the frames
+    sofa::core::objectmodel::Data<double> d_dt; /// Simulation time-step
+
+    sofa::core::behavior::MechanicalState<sofa::defaulttype::Vec3dTypes>* mGridState{nullptr}; ///Controlled grid
+
+    std::vector<VecCoord> frames;
+
+    size_t currentIndex{0};
+
+    void loadMotion();
+};
+
+
+} // namespace sofa::infinytoolkit

src/InfinyToolkit/MotionReplayController/MotionReplayController.inl

@@ -0,0 +1,168 @@
+/*****************************************************************************
+ *                 - 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/core/behavior/MechanicalState.h>
+#include <sofa/component/statecontainer/MechanicalObject.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>
+
+namespace sofa::infinytoolkit
+{
+
+MotionReplayController::MotionReplayController()
+    : d_motionFile(initData(&d_motionFile, "motionFile",
+        "Path to CSV motion file, where each row contains one frame."))
+    , d_dt(initData(&d_dt, 0.02, "dt", "Time step of the SOFA scene"))
+    {
+    }
+
+void MotionReplayController::init()
+    {
+        mGridState = this->getContext()->get<sofa::core::behavior::MechanicalState<sofa::defaulttype::Vec3dTypes>>();
+
+    if (!mGridState)
+    {
+        msg_error() << "[MotionReplay] MechanicalState is null!";
+        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;
+
+       // Always loop like Python version
+       if (currentIndex >= frames.size())
+           currentIndex = 0;
+
+       auto positions =  mGridState->writePositions();
+
+       if (positions.size() != frames[currentIndex].size())
+       {
+           msg_error() << "[MotionReplay] Frame size mismatch: "
+               << "MO points = " << positions.size()
+               << ", frame points = " << frames[currentIndex].size();
+           return;
+       }
+
+       for (size_t i = 0; i < positions.size(); ++i)
+       {
+           positions[i] = Coord(
+               frames[currentIndex][i][0],
+               frames[currentIndex][i][1],
+               frames[currentIndex][i][2]);           
+       }
+
+       ++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 = mGridState->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

src/InfinyToolkit/initInfinyToolkit.cpp

@@ -58,6 +58,8 @@ extern void registerNeedleTracker(sofa::core::ObjectFactory* factory);
 extern void registerPliersPositionsMapper(sofa::core::ObjectFactory* factory);
 extern void registerRotationEngine(sofa::core::ObjectFactory* factory);
 
+// Heart Motion Replayer
+extern void registerMotionReplayController(sofa::core::ObjectFactory* factory);
 
 
 
@@ -145,6 +147,9 @@ void registerObjects(sofa::core::ObjectFactory* factory)
     registerNeedleTracker(factory);
     registerPliersPositionsMapper(factory);
     registerRotationEngine(factory);
+
+    // Heart Motion Replayer
+    registerMotionReplayController(factory);
 }
 
 } // namespace sofa::component