Skip to content

Commit b9ed8d8

Browse files
rmolazemepernod
rmolazem
and
epernod
authored
[Controller] Add MotionReplayController component to load deformation of a CSV file, add an option for extra motion - Fixes LNROBO-34 (#41)
* Add the new component files to the CMakeLists of InfinyToolkit. * Add all the files for the motion controller. * Uncomment the full definitions of the functions. * Add the register function of the motion replay controller to the InfinyToolkit source file. * Simplifying the motion replay controller class by inheriting from Controller class. * Calling registration function of the motion controller in the registerObjects function. * Fixing a small bug in the constructor and enabling f_listening at init. * Define the motion file as DataFileName. * Modify the lines according to the change in the header file. * Add a simple example for MotionReplayController. * Add the CSV file needed for the example scene. * Add Sofa.Component.Engine.Select to the find_package and target_link_libraries. * Get the fixed indices from the scene, to make them static while breathing. * Fix some specific points while breathing. * Refactor MotionReplayController to use SingleLink for grid and BoxROI. * Modify the scene to use SingleLink for grid and BoxROI. * Applying suggestions. * Remove the inl file. * Apply changes suggested by the last review plus small modifications for generalizing the offset motion. * Update the example scene file according to the new modifications. * Fix controller initialization by removing the the engine output from init function. * When the DVF time step differs the scene time step log a message. * Fixing a small typo in the constructor of the controller. * Remove the message if the fixedIndices was empty. * Make the fixed points to be fixed either by the animation or the offset motion. * a few cleanup * remove static cast --------- Co-authored-by: epernod <erik.pernod@gmail.com>
1 parent 4166b84 commit b9ed8d8

6 files changed

Lines changed: 551 additions & 0 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,8 @@ find_package(Sofa.Component.Collision.Geometry REQUIRED)
3030
find_package(Sofa.Component.Controller REQUIRED)
3131
find_package(Sofa.Component.Haptics REQUIRED)
3232
find_package(Sofa.GUI.Component REQUIRED)
33+
find_package(Sofa.Component.Engine.Select REQUIRED)
34+
3335

3436
set(USE_INFINYTOOLKIT_PLUGIN true CACHE BOOL "Use Interaction Tools plugin")
3537

@@ -69,6 +71,12 @@ set(HEADER_FILES
6971
${INFINYTOOLKIT_SRC_DIR}/MeshTools/GridBarycentersPositions.h
7072

7173
${INFINYTOOLKIT_SRC_DIR}/BruteForceFeedback.h
74+
75+
## Replay motion controller
76+
${INFINYTOOLKIT_SRC_DIR}/MotionReplayController/MotionReplayController.h
77+
78+
79+
7280
)
7381

7482
set(SOURCE_FILES
@@ -102,6 +110,11 @@ set(SOURCE_FILES
102110
${INFINYTOOLKIT_SRC_DIR}/MeshTools/GridBarycentersPositions.cpp
103111

104112
${INFINYTOOLKIT_SRC_DIR}/BruteForceFeedback.cpp
113+
114+
## Replay motion controller
115+
${INFINYTOOLKIT_SRC_DIR}/MotionReplayController/MotionReplayController.cpp
116+
117+
105118
)
106119

107120
# Add component for carving using refinement mesh
@@ -140,6 +153,7 @@ target_link_libraries(${PROJECT_NAME}
140153
Sofa.Component.Controller
141154
Sofa.Component.Haptics
142155
Sofa.GUI.Component
156+
Sofa.Component.Engine.Select
143157
)
144158

145159

‎examples/MotionReplayController.scn‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
<?xml version="1.0" ?>
2+
<Node name="root" dt="0.02" gravity="0 0 0">
3+
<!-- Required Plugins -->
4+
<RequiredPlugin name="Sofa.Component.StateContainer"/> <!-- Needed to use components [MechanicalObject] -->
5+
<RequiredPlugin name="Sofa.Component.Topology.Container.Grid"/> <!-- Needed to use components [RegularGridTopology] -->
6+
<RequiredPlugin name="Sofa.Component.Visual"/> <!-- Needed to use components [VisualStyle] -->
7+
<RequiredPlugin name="Sofa.GL.Component.Rendering3D"/> <!-- Needed to use components [OglModel] -->
8+
<RequiredPlugin name="Sofa.Component.Mapping.Linear"/> <!-- Needed to use components [IdentityMapping] -->
9+
<RequiredPlugin name="Sofa.Component.Constraint.Projective"/> <!-- Needed to use components [FixedProjectiveConstraint] -->
10+
<RequiredPlugin name="Sofa.Component.Engine.Select"/> <!-- Needed to use components [BoxROI] -->
11+
<RequiredPlugin name="InfinyToolkit"/> <!-- Needed to use MotionReplayController -->
12+
13+
<VisualStyle displayFlags="showVisual showWireframe showBehavior"/>
14+
15+
16+
<!-- Animation and Visual Loops -->
17+
<DefaultAnimationLoop/>
18+
<DefaultVisualManagerLoop />
19+
20+
21+
<!-- Patient Grid Node -->
22+
<Node name="PatientGrid">
23+
24+
<!-- Regular Grid Topology -->
25+
<RegularGridTopology
26+
name="grid"
27+
nx="10" ny="10" nz="10"
28+
xmin="-8" xmax="12"
29+
ymin="-6" ymax="14"
30+
zmin="-10" zmax="10"
31+
/>
32+
33+
<!-- Mechanical Object linked to the grid -->
34+
<MechanicalObject
35+
name="mechanicalDofs"
36+
template="Vec3d"
37+
position="@grid.position"
38+
listening="true"
39+
/>
40+
41+
<Node name="VisuGrid">
42+
<OglModel color="green" src="@../mechanicalDofs" name="Visual" />
43+
<IdentityMapping input="@../" output="@Visual"/>
44+
</Node>
45+
46+
47+
<!-- Fixed indices-->
48+
<BoxROI name="TopROI"
49+
drawBoxes="0"
50+
box="-3.0 -5.0 -4.0 1.5 -3.5 2.0" />
51+
52+
<FixedProjectiveConstraint indices="@TopROI.indices" />
53+
54+
<!-- Motion Replay Controller -->
55+
<MotionReplayController
56+
name="motionReplay"
57+
motionFile="grid_states.csv"
58+
gridState="@mechanicalDofs"
59+
fixedIndices = "@TopROI.indices"
60+
/>
61+
62+
</Node>
63+
64+
</Node>

‎examples/grid_states.csv‎

Lines changed: 150 additions & 0 deletions
Large diffs are not rendered by default.

‎src/InfinyToolkit/MotionReplayController/MotionReplayController.cpp‎

Lines changed: 234 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,234 @@
1+
/*****************************************************************************
2+
* - Copyright (C) - 2020 - InfinyTech3D - *
3+
* *
4+
* This file is part of the InfinyToolkit plugin for the SOFA framework *
5+
* *
6+
* Commercial License Usage: *
7+
* Licensees holding valid commercial license from InfinyTech3D may use this *
8+
* file in accordance with the commercial license agreement provided with *
9+
* the Software or, alternatively, in accordance with the terms contained in *
10+
* a written agreement between you and InfinyTech3D. For further information *
11+
* on the licensing terms and conditions, contact: contact@infinytech3d.com *
12+
* *
13+
* GNU General Public License Usage: *
14+
* Alternatively, this file may be used under the terms of the GNU General *
15+
* Public License version 3. The licenses are as published by the Free *
16+
* Software Foundation and appearing in the file LICENSE.GPL3 included in *
17+
* the packaging of this file. Please review the following information to *
18+
* ensure the GNU General Public License requirements will be met: *
19+
* https://www.gnu.org/licenses/gpl-3.0.html. *
20+
* *
21+
* Authors: see Authors.txt *
22+
* Further information: https://infinytech3d.com *
23+
****************************************************************************/
24+
#pragma once
25+
26+
#include <InfinyToolkit/MotionReplayController/MotionReplayController.h>
27+
#include <sofa/component/statecontainer/MechanicalObject.h>
28+
#include <sofa/core/ObjectFactory.h>
29+
#include <sofa/core/objectmodel/Context.h>
30+
#include <sofa/core/objectmodel/DataFileName.cpp>
31+
#include <sofa/helper/logging/Messaging.h>
32+
#include <sofa/simulation/AnimateBeginEvent.h>
33+
34+
#include <fstream>
35+
#include <sstream>
36+
#include <string>
37+
#include <unordered_set>
38+
39+
40+
namespace sofa::infinytoolkit
41+
{
42+
43+
using namespace sofa::defaulttype;
44+
45+
void registerMotionReplayController(sofa::core::ObjectFactory* factory)
46+
{
47+
factory->registerObjects(
48+
sofa::core::ObjectRegistrationData("Motion replay controller to induce the heart motion.")
49+
.add< MotionReplayController >()
50+
);
51+
}
52+
53+
MotionReplayController::MotionReplayController()
54+
:l_gridState(initLink("gridState", "Link to the grid control."))
55+
, d_fixedIndices(initData(&d_fixedIndices, "fixedIndices", "Indices of the nodes that should be fixed."))
56+
, d_motionFile(initData(&d_motionFile, "motionFile",
57+
"Path to CSV motion file, where each row contains one frame."))
58+
, d_dvfTimeStep(initData(&d_dvfTimeStep, 0.02,
59+
"dvfTimeStep", " Time step used to record the DVF."))
60+
, d_displacementAmplitude(initData(&d_displacementAmplitude, 1.0, "displacementAmplitude", "Amplitude for extra motion."))
61+
, d_displacementAxis(initData(&d_displacementAxis, 1, "displacementAxis", " Axis along which the extra motion is applied: 0=X, 1=Y, 2=Z."))
62+
, d_infinyLoop(initData(&d_infinyLoop, true, "motionLoop", "Replay motion infinitely."))
63+
64+
{
65+
66+
}
67+
68+
void MotionReplayController::init()
69+
{
70+
// Resolve MechanicalState
71+
if (l_gridState.get() == nullptr)
72+
{
73+
msg_error() << "Error no target grid found!";
74+
this->d_componentState.setValue(
75+
sofa::core::objectmodel::ComponentState::Invalid);
76+
return;
77+
78+
}
79+
80+
int axis = d_displacementAxis.getValue();
81+
82+
if (axis < 0 || axis > 2)
83+
{
84+
msg_warning() << "Invalid motion axis: ", axis, ". Valid values are 0=X, 1=Y, 2=Z.";
85+
return;
86+
}
87+
88+
double sceneDt = this->getContext()->getDt();
89+
double dvfDt = d_dvfTimeStep.getValue();
90+
91+
if (sceneDt != dvfDt)
92+
{
93+
msg_warning() << "[MotionReplay] Scene time step (" << sceneDt
94+
<< ") differs from DVF time step (" << dvfDt
95+
<< "). For accurate motion replay, set the scene time step equal to the DVF time step.";
96+
return;
97+
}
98+
99+
this->f_listening.setValue(true);
100+
101+
loadMotion();
102+
103+
}
104+
105+
106+
void MotionReplayController::handleEvent(sofa::core::objectmodel::Event* event)
107+
{
108+
109+
if (!sofa::simulation::AnimateBeginEvent::checkEventType(event))
110+
return;
111+
112+
if (frames.empty())
113+
return;
114+
115+
if (currentIndex >= frames.size())
116+
{
117+
if (d_infinyLoop.getValue())
118+
currentIndex = 0;
119+
else
120+
return;
121+
}
122+
123+
124+
auto positions = l_gridState->writePositions();
125+
126+
if (positions.size() != frames[currentIndex].size())
127+
{
128+
msg_error() << "[MotionReplay] Frame size mismatch: "
129+
<< "MO points = " << positions.size()
130+
<< ", frame points = " << frames[currentIndex].size();
131+
return;
132+
}
133+
134+
double offset = 0.0;
135+
auto amplitudeOffset = d_displacementAmplitude.getValue();
136+
137+
if (amplitudeOffset)
138+
{
139+
double frequency = 0.1; // Hz
140+
double t = this->getContext()->getTime();
141+
offset = amplitudeOffset * sin(2.0 * M_PI * frequency * t);
142+
}
143+
144+
const auto& fixedIndices = d_fixedIndices.getValue();
145+
for (sofa::Index i = 0; i < positions.size(); ++i)
146+
{
147+
auto it = std::find(fixedIndices.begin(), fixedIndices.end(), i);
148+
if (it != fixedIndices.end())
149+
continue;
150+
151+
// Copy frame position
152+
positions[i][0] = frames[currentIndex][i][0];
153+
positions[i][1] = frames[currentIndex][i][1];
154+
positions[i][2] = frames[currentIndex][i][2];
155+
156+
// Apply displacement if needed
157+
if (offset != 0.0)
158+
{
159+
int axis = d_displacementAxis.getValue(); // 0=X, 1=Y, 2=Z
160+
positions[i][axis] += offset;
161+
}
162+
}
163+
164+
++currentIndex;
165+
}
166+
167+
void MotionReplayController::loadMotion()
168+
{
169+
frames.clear();
170+
currentIndex = 0;
171+
172+
const std::string filename = d_motionFile.getFullPath();
173+
174+
if (filename.empty())
175+
{
176+
msg_error() << "[MotionReplay] motionFile not specified!";
177+
return;
178+
}
179+
180+
// Open file stream
181+
std::ifstream file(filename);
182+
if (!file.is_open())
183+
{
184+
msg_error() << "[MotionReplay] Cannot open file: " << filename;
185+
return;
186+
}
187+
188+
size_t numPoints = l_gridState->getSize();
189+
190+
std::string line;
191+
size_t lineNumber = 0;
192+
while (std::getline(file, line))
193+
{
194+
++lineNumber;
195+
std::stringstream ss(line);
196+
std::string value;
197+
198+
std::vector<double> values;
199+
while (std::getline(ss, value, ','))
200+
{
201+
values.push_back(std::stod(value));
202+
}
203+
204+
if (values.size() != numPoints * 3)
205+
{
206+
msg_error() << "[MotionReplay] Line " << lineNumber
207+
<< ": expected " << numPoints * 3
208+
<< " values, got " << values.size();
209+
frames.clear();
210+
return;
211+
}
212+
213+
VecCoord frame;
214+
frame.reserve(numPoints);
215+
216+
for (size_t i = 0; i < numPoints; ++i)
217+
{
218+
Coord c;
219+
c[0] = values[3 * i + 0];
220+
c[1] = values[3 * i + 1];
221+
c[2] = values[3 * i + 2];
222+
frame.push_back(c);
223+
}
224+
225+
frames.push_back(std::move(frame));
226+
}
227+
228+
msg_info() << "[MotionReplay] Loaded " << frames.size()
229+
<< " frames from " << filename;
230+
}
231+
232+
233+
234+
} // namespace sofa::infinytoolkit

0 commit comments

Comments
 (0)