refactor add new component

CMakeLists.txt

@@ -37,6 +37,8 @@ set(HEADER_FILES
     ${SRC_ROOT_DIR}/mapping/RigidDistanceMapping.h
     ${SRC_ROOT_DIR}/mapping/RigidDistanceMapping.inl
     ${SRC_ROOT_DIR}/engine/PointsManager.h
+    ${SRC_ROOT_DIR}/mapping/GlobalToLocalCosseratMapping.h
+    ${SRC_ROOT_DIR}/mapping/GlobalToLocalCosseratMapping.inl
     ${SRC_ROOT_DIR}/engine/PointsManager.inl
     ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceField.h
     ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceField.inl
@@ -60,6 +62,7 @@ set(SOURCE_FILES
     ${SRC_ROOT_DIR}/mapping/DifferenceMultiMapping.cpp
     ${SRC_ROOT_DIR}/mapping/RigidDistanceMapping.cpp
     ${SRC_ROOT_DIR}/engine/PointsManager.cpp
+    ${SRC_ROOT_DIR}/mapping/GlobalToLocalCosseratMapping.cpp
     ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceField.cpp
     ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceFieldRigid.cpp
     ${SRC_ROOT_DIR}/forcefield/CosseratInternalActuation.cpp

src/Cosserat/config.h.in

@@ -50,7 +50,4 @@ namespace Cosserat
     SOFA_COSSERAT_API void init();
     constexpr const char *MODULE_NAME = "@PROJECT_NAME@";
     constexpr const char *MODULE_VERSION = "@PROJECT_VERSION@";
-
-
-
 }

src/Cosserat/forcefield/BeamHookeLawForceField.cpp

@@ -170,7 +170,7 @@ using namespace sofa::defaulttype;
 template class BeamHookeLawForceField<Vec3Types>;
 template class BeamHookeLawForceField<Vec6Types>;
 
-} // namespace sofa::component::forcefield
+} // namespace Cosserat::forcefield
 
 // namespace sofa
 namespace Cosserat

src/Cosserat/mapping/GlobalToLocalCosseratMapping.cpp

@@ -0,0 +1,49 @@
+/******************************************************************************
+*       SOFA, Simulation Open-Framework Architecture, development version     *
+*                (c) 2006-2019 INRIA, USTL, UJF, CNRS, MGH                    *
+*                                                                             *
+* This program is free software; you can redistribute it and/or modify it     *
+* under the terms of the GNU Lesser General Public License as published by    *
+* the Free Software Foundation; either version 2.1 of the License, or (at     *
+* your option) any later version.                                             *
+*                                                                             *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details.                                                           *
+*                                                                             *
+* You should have received a copy of the GNU Lesser General Public License    *
+* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: [email protected]                             *
+******************************************************************************/
+#define SOFA_COSSERAT_CPP_GlobalToLocalCosseratMapping
+#include <Cosserat/mapping/GlobalToLocalCosseratMapping.inl>
+
+#include <sofa/defaulttype/VecTypes.h>
+#include <sofa/defaulttype/RigidTypes.h>
+#include <sofa/core/ObjectFactory.h>
+
+namespace Cosserat::mapping
+{
+    using namespace sofa::defaulttype;
+template class SOFA_COSSERAT_API GlobalToLocalCosseratMapping< sofa::defaulttype::Rigid3Types,
+    sofa::defaulttype::Vec3Types >;
+} // namespace sofa::component::mapping
+
+namespace Cosserat
+{
+// Register in the Factory
+void registerGlobalToLocalCosseratMapping(sofa::core::ObjectFactory* factory)
+{
+  factory->registerObjects(sofa::core::ObjectRegistrationData(
+  "This component facilitates the creation of Cosserat Cables in SOFA simulations. It takes two mechanical"
+  "objects as inputs: the rigid base of the beam (with 6 degrees of freedom) and the local coordinates of the beam. Using "
+    "these inputs, the component computes and outputs the mechanical positions of the beam in global coordinates. "
+    "Like any mapping, it updates the positions and velocities of the outputs based on the inputs. "
+    "Additionally, forces applied to the outputs are propagated back to the inputs, ensuring bidirectional coupling.")
+    .add< mapping::GlobalToLocalCosseratMapping< sofa::defaulttype::Rigid3Types, sofa::defaulttype::Vec3Types>>(true));
+}
+}

src/Cosserat/mapping/GlobalToLocalCosseratMapping.h

@@ -0,0 +1,131 @@
+/******************************************************************************
+ *       SOFA, Simulation Open-Framework Architecture, development version     *
+ *                (c) 2006-2019 INRIA, USTL, UJF, CNRS, MGH                    *
+ *                                                                             *
+ * This program is free software; you can redistribute it and/or modify it     *
+ * under the terms of the GNU Lesser General Public License as published by    *
+ * the Free Software Foundation; either version 2.1 of the License, or (at     *
+ * your option) any later version.                                             *
+ *                                                                             *
+ * This program is distributed in the hope that it will be useful, but WITHOUT *
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+ * for more details.                                                           *
+ *                                                                             *
+ * You should have received a copy of the GNU Lesser General Public License    *
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+ *******************************************************************************
+ * Authors: The SOFA Team and external contributors (see Authors.txt)          *
+ *                                                                             *
+ * Contact information: [email protected]                             *
+ ******************************************************************************/
+#pragma once
+#include <Cosserat/config.h>
+#include <Cosserat/mapping/BaseCosseratMapping.h>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <cmath>
+#include <sofa/core/BaseMapping.h>
+#include <sofa/core/Mapping.h>
+#include <sofa/core/State.h>
+#include <sofa/helper/ColorMap.h>
+#include <vector>
+
+namespace Cosserat::mapping {
+namespace {
+using Mat3x6 = sofa::type::Mat<3, 6, SReal>;
+using Mat6x3 = sofa::type::Mat<6, 3, SReal>;
+using sofa::Data;
+using sofa::type::Mat4x4;
+using sofa::type::Mat6x6;
+using sofa::type::Quat;
+using sofa::type::Vec3;
+using sofa::type::Vec6;
+} // namespace
+
+template <class TIn, class TOut>
+class GlobalToLocalCosseratMapping : public sofa::core::Mapping<TIn, TOut> {
+public:
+  SOFA_CLASS(SOFA_TEMPLATE2(GlobalToLocalCosseratMapping, TIn, TOut),
+             SOFA_TEMPLATE2(sofa::core::Mapping, TIn, TOut));
+
+  typedef TIn In;
+  typedef TOut Out;
+  typedef In InDataTypes;
+  typedef Out OutDataTypes;
+  typedef typename InDataTypes::VecCoord InVecCoord;
+  typedef typename InDataTypes::VecDeriv InVecDeriv;
+  typedef typename InDataTypes::Coord InCoord;
+  typedef typename InDataTypes::Deriv InDeriv;
+  typedef typename InDataTypes::Real Real;
+  typedef typename InDataTypes::MatrixDeriv InMatrixDeriv;
+
+  typedef typename OutDataTypes::VecCoord OutVecCoord;
+  typedef typename OutDataTypes::VecDeriv OutVecDeriv;
+  typedef typename OutDataTypes::MatrixDeriv OutMatrixDeriv;
+  typedef typename OutDataTypes::Coord OutCoord;
+  typedef typename OutDataTypes::Deriv OutDeriv;
+  typedef typename OutDataTypes::Real OutReal;
+
+  /// @name Data Fields
+  /// @{
+  Data<int> d_deformationAxis;
+  Data<SReal> d_max;
+  Data<SReal> d_min;
+  Data<SReal> d_radius;
+  Data<bool> d_drawMapBeam;
+  Data<sofa::type::RGBAColor> d_color;
+  Data<vector<int>> d_index;
+  Data<unsigned int> d_baseIndex;
+  Data<vector<double>> d_curv_abs_section;
+  /// @}
+
+  struct StrainResult {
+    OutCoord strain;
+    Eigen::Matrix4d transformMatrix;
+  };
+
+  using Inherit1::fromModel;
+  using Inherit1::toModel;
+
+  /**
+   * Calcule la déformation à partir d'un quaternion
+   * @param frame Vecteur contenant la position (0-2) et le quaternion (3-6)
+   * @param curvAbs Abscisse courbe
+   * @param gXp Matrice précédente
+   * @return Structure contenant le vecteur de déformation et la matrice de
+   * transformation
+   */
+  StrainResult getStrainFromQuat(const InCoord &frame, const double curvAbs,
+                                 const Eigen::Matrix4d &gXp);
+  vector<StrainResult> m_strainResults;
+
+public:
+  void init() override;
+  void apply(const sofa::core::MechanicalParams *mparams,
+             Data<OutVecCoord> &out, const Data<InVecCoord> &in) override;
+  void applyJ(const sofa::core::MechanicalParams *mparams,
+              Data<OutVecDeriv> &out, const Data<InVecDeriv> &in) override;
+  void applyJT(const sofa::core::MechanicalParams *mparams,
+               Data<InVecDeriv> &outForce,
+               const Data<OutVecDeriv> &inForce) override;
+  void applyJT(const sofa::core::ConstraintParams *cparams,
+               Data<InMatrixDeriv> &out,
+               const Data<OutMatrixDeriv> &in) override;
+
+  void computeLogarithm(const double &x, const Mat4x4 &gX, Mat4x4 &log_gX);
+  void computeLogarithm(const double &x, const InCoord &gX, Mat4x4 &log_gX);
+
+protected:
+  sofa::helper::ColorMap m_colorMap;
+
+  GlobalToLocalCosseratMapping();
+  ~GlobalToLocalCosseratMapping() override = default;
+};
+
+#if !defined(SOFA_COSSERAT_CPP_GlobalToLocalCosseratMapping)
+extern template class SOFA_COSSERAT_API GlobalToLocalCosseratMapping<
+    sofa::defaulttype::Rigid3Types, sofa::defaulttype::Vec3Types>;
+#endif
+
+} // namespace Cosserat::mapping

src/Cosserat/mapping/GlobalToLocalCosseratMapping.inl

@@ -0,0 +1,173 @@
+/******************************************************************************
+ *       SOFA, Simulation Open-Framework Architecture, development version     *
+ *                (c) 2006-2019 INRIA, USTL, UJF, CNRS, MGH                    *
+ *                                                                             *
+ * This program is free software; you can redistribute it and/or modify it     *
+ * under the terms of the GNU Lesser General Public License as published by    *
+ * the Free Software Foundation; either version 2.1 of the License, or (at     *
+ * your option) any later version.                                             *
+ *                                                                             *
+ * This program is distributed in the hope that it will be useful, but WITHOUT *
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+ * for more details.                                                           *
+ *                                                                             *
+ * You should have received a copy of the GNU Lesser General Public License    *
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+ *******************************************************************************
+ * Authors: The SOFA Team and external contributors (see Authors.txt)          *
+ *                                                                             *
+ * Contact information: [email protected]                             *
+ ******************************************************************************/
+#pragma once
+#include <Cosserat/mapping/GlobalToLocalCosseratMapping.h>
+
+#include <sofa/core/Mapping.inl>
+#include <sofa/core/behavior/MechanicalState.h>
+#include <sofa/core/objectmodel/BaseContext.h>
+#include <sofa/core/visual/VisualParams.h>
+#include <sofa/gl/template.h>
+#include <sofa/helper/AdvancedTimer.h>
+#include <sofa/helper/logging/Message.h>
+#include <sofa/helper/visual/DrawTool.h>
+#include <unsupported/Eigen/MatrixFunctions>
+#include <sofa/type/Quat.h>
+#include <string>
+
+namespace Cosserat::mapping {
+
+using sofa::core::objectmodel::BaseContext;
+using sofa::defaulttype::SolidTypes;
+using sofa::helper::AdvancedTimer;
+using sofa::helper::WriteAccessor;
+using sofa::type::RGBAColor;
+template <class TIn, class TOut>
+GlobalToLocalCosseratMapping<TIn, TOut>::GlobalToLocalCosseratMapping():d_deformationAxis(
+    initData(&d_deformationAxis, (int)1, "deformationAxis",
+      "the axis in which we want to show the deformation.\n")),
+    d_max(initData(&d_max, (SReal)1.0e-2, "max",
+                       "the maximum of the deformation.\n")),
+    d_min(initData(&d_min, (SReal)0.0, "min",
+                   "the minimum of the deformation.\n")),
+    d_radius(
+        initData(&d_radius, (SReal)0.05, "radius",
+                 "the axis in which we want to show the deformation.\n")),
+        d_color(initData(&d_color,
+        sofa::type::RGBAColor(40 / 255.0, 104 / 255.0, 137 / 255.0, 0.8),
+        "color", "The default beam color")),
+    d_curv_abs_section(
+        initData(&d_curv_abs_section, "curv_abs_section",
+                 "The curvilinear abscissa of the section")) {
+
+  }
+
+  template <class TIn, class TOut>
+  GlobalToLocalCosseratMapping<TIn, TOut>::StrainResult GlobalToLocalCosseratMapping<TIn,
+  TOut>::getStrainFromQuat(const InCoord& frame,
+                              const double curvAbs, const Eigen::Matrix4d& gXp)
+  {
+    StrainResult result;
+    //result.strain.resize(3);
+
+  // Initialisation de la matrice de transformation
+  Eigen::Matrix4d gX = Eigen::Matrix4d::Zero();
+
+  // Construction du quaternion à partir des composantes du frame
+  // En python nous avons l'habitude du x,y,z,w mais ici nous avons w,x,y,z
+  Eigen::Quaterniond q(frame[6], frame[3], frame[4], frame[5]); // w, x, y, z
+  q.normalize();
+
+  // Conversion du quaternion en matrice de rotation
+  gX.block<3,3>(0,0) = q.toRotationMatrix();
+
+  // Ajout de la translation
+  gX.block<3,1>(0,3) = Eigen::Vector3d(frame[0], frame[1], frame[2]);
+  gX(3,3) = 1.0;
+
+  if (curvAbs <= 0.0) {
+    // Cas où l'abscisse curv est nulle ou négative
+    result.strain = OutCoord(0.0, 0.0, 0.0);
+  } else {
+    Eigen::Matrix4d gXpInv = gXp.inverse();
+    Eigen::Matrix4d product = gXpInv * gX;
+
+    // Calcul du logarithme matriciel
+    //@todo : vérifier si ce calcul de log correspond à celui calculé en python
+    Eigen::Matrix4d xiHat = product.log().eval() / curvAbs;
+
+    // Extraction des composantes de déformation
+    result.strain[0] = xiHat(2,1);  // κ1
+    result.strain[1] = xiHat(0,2);  // κ2
+    result.strain[2] = xiHat(1,0);  // κ3
+  }
+
+  result.transformMatrix = gX;
+  return result;
+}
+
+  template <class TIn, class TOut>
+  void GlobalToLocalCosseratMapping<TIn, TOut>::init() {
+
+  if(fromModel.empty())
+  {
+    msg_error() << "input1 not found" ;
+    return;
+  }
+
+  if(toModel.empty())
+  {
+    msg_error() << "output missing" ;
+    return;
+  }
+  // Get initial frame state
+  //auto xfromData =
+  //    m_global_frames->read(sofa::core::ConstVecCoordId::position());
+  //const vector<OutCoord> xfrom = xfromData->getValue();
+  Inherit1::init();
+}
+
+  template <class TIn, class TOut>
+  void GlobalToLocalCosseratMapping<TIn, TOut>::apply(const sofa::core::MechanicalParams *mparams,
+        Data< OutVecCoord>& out, const Data< InVecCoord>& in){
+      // Get initial frame state
+    m_strainResults.clear();
+
+    const auto &local_in = in.getValue();
+    auto &local_out = *out.beginEdit();
+
+    auto curvAbs = d_curv_abs_section.getValue();
+    auto frames_size = local_in.size();
+    local_out.resize(frames_size - 1);
+
+    for (auto i = 0; i < frames_size; i++) {
+      InCoord frame = local_in[i];
+      Eigen::Matrix4d gXp = Eigen::Matrix4d::Identity();
+      std::vector<double> strain;
+
+      auto strain_struct = getStrainFromQuat(frame, curvAbs[i], gXp);
+      local_out[i] = strain_struct.strain;
+
+      m_strainResults.push_back(strain_struct);
+    }
+    out.endEdit();
+  }
+
+    template <class TIn, class TOut>
+    void GlobalToLocalCosseratMapping<TIn, TOut>::applyJ(const sofa::core::MechanicalParams *mparams,
+    Data< OutVecDeriv >& out, const Data< InVecDeriv >& in) {
+      // Get initial frame state
+  }
+  template <class TIn, class TOut>
+    void GlobalToLocalCosseratMapping<TIn, TOut>::applyJT(const sofa::core::MechanicalParams *mparams, Data< InVecDeriv >& outForce,
+      const Data< OutVecDeriv >& inForce) {
+      // Get initial frame state
+  }
+
+  template <class TIn, class TOut>
+  void GlobalToLocalCosseratMapping<TIn, TOut>::applyJT(const sofa::core::ConstraintParams *cparams, Data< InMatrixDeriv >& out,
+      const Data< OutMatrixDeriv >& in) {
+
+  }
+
+
+} // namespace Cosserat::mapping