Skip to content

Commit 878ddea

Browse files
epernodepernod
committed
Backup work on steerable catheter
1 parent c4cf360 commit 878ddea

4 files changed

Lines changed: 249 additions & 0 deletions

File tree

‎src/BeamAdapter/component/model/RodSteerableSection.cpp‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
/******************************************************************************
2+
* BeamAdapter plugin *
3+
* (c) 2006 Inria, University of Lille, CNRS *
4+
* *
5+
* This program is free software; you can redistribute it and/or modify it *
6+
* under the terms of the GNU Lesser General Public License as published by *
7+
* the Free Software Foundation; either version 2.1 of the License, or (at *
8+
* your option) any later version. *
9+
* *
10+
* This program is distributed in the hope that it will be useful, but WITHOUT *
11+
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
12+
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
13+
* for more details. *
14+
* *
15+
* You should have received a copy of the GNU Lesser General Public License *
16+
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
17+
*******************************************************************************
18+
* Authors: see Authors.md *
19+
* *
20+
* Contact information: contact@sofa-framework.org *
21+
******************************************************************************/
22+
#define SOFA_PLUGIN_BEAMADAPTER_RODSTEERABLESECTION_CPP
23+
24+
#include <BeamAdapter/component/model/RodSteerableSection.inl>
25+
#include <BeamAdapter/component/model/BaseRodSectionMaterial.inl>
26+
#include <sofa/core/ObjectFactory.h>
27+
#include <sofa/defaulttype/VecTypes.h>
28+
#include <sofa/defaulttype/RigidTypes.h>
29+
30+
namespace beamadapter
31+
{
32+
33+
template class SOFA_BEAMADAPTER_API RodSteerableSection<sofa::defaulttype::Rigid3Types>;
34+
35+
void registerRodSteerableSection(sofa::core::ObjectFactory* factory)
36+
{
37+
factory->registerObjects(sofa::core::ObjectRegistrationData("Class defining a rod steerable section, defining material and geometry parameters.")
38+
.add< RodSteerableSection<sofa::defaulttype::Rigid3Types> >());
39+
}
40+
41+
} // namespace beamadapter

‎src/BeamAdapter/component/model/RodSteerableSection.h‎

Lines changed: 90 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
/******************************************************************************
2+
* BeamAdapter plugin *
3+
* (c) 2006 Inria, University of Lille, CNRS *
4+
* *
5+
* This program is free software; you can redistribute it and/or modify it *
6+
* under the terms of the GNU Lesser General Public License as published by *
7+
* the Free Software Foundation; either version 2.1 of the License, or (at *
8+
* your option) any later version. *
9+
* *
10+
* This program is distributed in the hope that it will be useful, but WITHOUT *
11+
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
12+
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
13+
* for more details. *
14+
* *
15+
* You should have received a copy of the GNU Lesser General Public License *
16+
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
17+
*******************************************************************************
18+
* Authors: see Authors.md *
19+
* *
20+
* Contact information: contact@sofa-framework.org *
21+
******************************************************************************/
22+
#pragma once
23+
24+
#include <BeamAdapter/config.h>
25+
#include <BeamAdapter/component/model/BaseRodSectionMaterial.h>
26+
27+
namespace beamadapter
28+
{
29+
30+
using sofa::core::loader::MeshLoader;
31+
32+
/**
33+
* \class RodSteerableSection
34+
* \brief Specialization class of @sa BaseRodSectionMaterial describing a rod spire section.
35+
*
36+
* This class will describe a rod spire section using spire diameter and height between each spire. Length and mechanical
37+
* parameters are the same as @sa BaseRodSectionMaterial Data
38+
* Method @sa getRestTransformOnX will return the current position of the curviline abscisse along the spire.
39+
*/
40+
template <class DataTypes>
41+
class RodSteerableSection : public BaseRodSectionMaterial<DataTypes>
42+
{
43+
public:
44+
SOFA_CLASS(SOFA_TEMPLATE(RodSteerableSection, DataTypes), SOFA_TEMPLATE(BaseRodSectionMaterial, DataTypes));
45+
46+
using Real = typename DataTypes::Real;
47+
using Transform = typename sofa::defaulttype::SolidTypes<Real>::Transform;
48+
using Quat = sofa::type::Quat<Real>;
49+
50+
/// Default Constructor
51+
RodSteerableSection();
52+
53+
/// Override method to get the rest position of the beam. In this implementation, it will compute the current position given the spire parameters
54+
void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) override;
55+
56+
/// Function handling the event (listening needs to be true)
57+
void handleEvent(core::objectmodel::Event* ev) override;
58+
59+
protected:
60+
/// Internal method to init the section. Called by @sa BaseRodSectionMaterial::init() method
61+
bool initSection() override;
62+
63+
public:
64+
Data<Real> d_spireDiameter; ///< Data defining the diameter of the spire
65+
Data<Real> d_spireHeight; ///< Data defining the height between each spire
66+
67+
/// Boolean for bending
68+
Data<bool> d_activeBending;
69+
/// Boolean for unbending
70+
Data<bool> d_deactiveBending;
71+
/// Maximum angle that the catheter can reach
72+
Data<Real> d_angleMax;
73+
/// Minimum angle considering the catheter as flat
74+
Data<Real> d_flatAngle;
75+
/// Rate of bending
76+
Data<unsigned int> d_bendingRate;
77+
78+
protected:
79+
Real m_tipLength;
80+
Real m_currentAngleRadian;
81+
Real m_maxAngleRadian;
82+
Real m_maxUnbendingDiameter;
83+
Real m_incrementalAngleRadian;
84+
};
85+
86+
#if !defined(SOFA_PLUGIN_BEAMADAPTER_RODSTEERABLESECTION_CPP)
87+
extern template class SOFA_BEAMADAPTER_API RodSteerableSection<sofa::defaulttype::Rigid3Types>;
88+
#endif
89+
90+
} // namespace beamadapter

‎src/BeamAdapter/component/model/RodSteerableSection.inl‎

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
/******************************************************************************
2+
* BeamAdapter plugin *
3+
* (c) 2006 Inria, University of Lille, CNRS *
4+
* *
5+
* This program is free software; you can redistribute it and/or modify it *
6+
* under the terms of the GNU Lesser General Public License as published by *
7+
* the Free Software Foundation; either version 2.1 of the License, or (at *
8+
* your option) any later version. *
9+
* *
10+
* This program is distributed in the hope that it will be useful, but WITHOUT *
11+
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
12+
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
13+
* for more details. *
14+
* *
15+
* You should have received a copy of the GNU Lesser General Public License *
16+
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
17+
*******************************************************************************
18+
* Authors: see Authors.md *
19+
* *
20+
* Contact information: contact@sofa-framework.org *
21+
******************************************************************************/
22+
#pragma once
23+
24+
#include <BeamAdapter/component/model/RodSteerableSection.h>
25+
#include <BeamAdapter/component/model/BaseRodSectionMaterial.inl>
26+
#include <sofa/core/objectmodel/KeypressedEvent.h>
27+
#include <sofa/core/objectmodel/BaseComponent.h>
28+
29+
namespace beamadapter
30+
{
31+
32+
template <class DataTypes>
33+
RodSteerableSection<DataTypes>::RodSteerableSection()
34+
: BaseRodSectionMaterial<DataTypes>()
35+
, d_spireDiameter(initData(&d_spireDiameter, (Real)0.1, "spireDiameter", "diameter of the spire"))
36+
, d_spireHeight(initData(&d_spireHeight, (Real)0.01, "spireHeight", "height between each spire"))
37+
, d_activeBending(initData(&d_activeBending, (bool)false, "activeBending", "Boolean activating the bending of the steerable catheter"))
38+
, d_deactiveBending(initData(&d_deactiveBending, (bool)false, "deactiveBending", "Boolean deactivating the bending of the steerable catheter"))
39+
, d_angleMax(initData(&d_angleMax, (Real)180.0, "angleMax", "Maximum angle that the catheter can reach \n (in degree [0-360])"))
40+
, d_flatAngle(initData(&d_flatAngle, (Real)1.0, "flatAngle", "Angle below which we consider the catheter as flat/n (Can't be zero)"))
41+
, d_bendingRate(initData(&d_bendingRate, (unsigned int)10, "bendingRate", "Nb of step needed to reach the maximum bending angle /n (the lower, the faster)"))
42+
43+
{
44+
this->f_listening.setValue(true);
45+
}
46+
47+
48+
template <class DataTypes>
49+
bool RodSteerableSection<DataTypes>::initSection()
50+
{
51+
const auto length = this->d_length.getValue();
52+
if (length <= Real(0.0))
53+
{
54+
msg_error() << "Length is 0 (or negative), check if d_length has been given or well computed.";
55+
return false;
56+
}
57+
58+
return true;
59+
}
60+
61+
62+
template <class DataTypes>
63+
void RodSteerableSection<DataTypes>::getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start)
64+
{
65+
Real projetedLength = d_spireDiameter.getValue() * M_PI;
66+
Real lengthSpire = sqrt(d_spireHeight.getValue() * d_spireHeight.getValue() + projetedLength * projetedLength);
67+
// angle in the z direction
68+
Real phi = atan(d_spireHeight.getValue() / projetedLength);
69+
70+
Quat Qphi;
71+
Qphi.axisToQuat(type::Vec3(0, 0, 1), phi);
72+
73+
// spire angle (if theta=2*PI, there is a complete spire between startx and x)
74+
Real lengthCurve = x_used - x_start;
75+
Real numSpire = lengthCurve / lengthSpire;
76+
Real theta = 2 * M_PI * numSpire;
77+
78+
// computation of the Quat
79+
Quat Qtheta;
80+
Qtheta.axisToQuat(type::Vec3(0, 1, 0), theta);
81+
Quat newSpireQuat = Qtheta * Qphi;
82+
83+
// computation of the position
84+
Real radius = d_spireDiameter.getValue() / 2.0;
85+
type::Vec3 PosEndCurve(radius * sin(theta), numSpire * d_spireHeight.getValue(), radius * (cos(theta) - 1));
86+
type::Vec3 SpirePos = PosEndCurve + type::Vec3(x_start, 0, 0);
87+
88+
global_H_local.set(SpirePos, newSpireQuat);
89+
}
90+
91+
template <class DataTypes>
92+
void RodSteerableSection<DataTypes>::handleEvent(core::objectmodel::Event* event)
93+
{
94+
if (sofa::core::objectmodel::KeypressedEvent::checkEventType(event))
95+
{
96+
sofa::core::objectmodel::KeypressedEvent* ke = static_cast<sofa::core::objectmodel::KeypressedEvent*>(event);
97+
//msg_info() << "HapticEmulator handleEvent gets character '" << ke->getKey() << "'. ";
98+
99+
if (ke->getKey() == '8')
100+
{
101+
std::cout << "RodSteerableSection<DataTypes>::handleEvent + key pressed" << std::endl;
102+
auto val = d_spireDiameter.getValue() + 1.0f;
103+
//m_currentAngleRadian += m_incrementalAngleRadian;
104+
d_spireDiameter.setValue(val);
105+
}
106+
else if (ke->getKey() == '9')
107+
{
108+
std::cout << "RodSteerableSection<DataTypes>::handleEvent - key pressed" << std::endl;
109+
auto val = d_spireDiameter.getValue() - 1.0f;
110+
d_spireDiameter.setValue(val);
111+
}
112+
113+
}
114+
}
115+
116+
} // namespace beamadapter

‎src/BeamAdapter/initBeamAdapter.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@ extern void registerBeamLengthMapping(sofa::core::ObjectFactory* factory);
4343
extern void registerMultiAdaptiveBeamMapping(sofa::core::ObjectFactory* factory);
4444
extern void registerRodMeshSection(sofa::core::ObjectFactory* factory);
4545
extern void registerRodSpireSection(sofa::core::ObjectFactory* factory);
46+
extern void registerRodSteerableSection(sofa::core::ObjectFactory* factory);
4647
extern void registerRodStraightSection(sofa::core::ObjectFactory* factory);
4748
extern void registerBeamProjectionDifferenceMultiMapping(sofa::core::ObjectFactory* factory);
4849

@@ -112,6 +113,7 @@ void registerObjects(sofa::core::ObjectFactory* factory)
112113
registerMultiAdaptiveBeamMapping(factory);
113114
registerRodMeshSection(factory);
114115
registerRodSpireSection(factory);
116+
registerRodSteerableSection(factory);
115117
registerRodStraightSection(factory);
116118
registerBeamProjectionDifferenceMultiMapping(factory);
117119
}

0 commit comments

Comments
 (0)