diff --git a/CMakeLists.txt b/CMakeLists.txt index dd366ad9..9835d97f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,6 +37,8 @@ set(HEADER_FILES ${SRC_ROOT_DIR}/engine/PointsManager.inl ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceField.h ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceField.inl + ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceFieldRigid.h + ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceFieldRigid.inl ${SRC_ROOT_DIR}/forcefield/CosseratInternalActuation.h ${SRC_ROOT_DIR}/forcefield/CosseratInternalActuation.inl ${SRC_ROOT_DIR}/constraint/CosseratSlidingConstraint.h @@ -56,6 +58,7 @@ set(SOURCE_FILES ${SRC_ROOT_DIR}/mapping/RigidDistanceMapping.cpp ${SRC_ROOT_DIR}/engine/PointsManager.cpp ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceField.cpp + ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceFieldRigid.cpp ${SRC_ROOT_DIR}/forcefield/CosseratInternalActuation.cpp ${SRC_ROOT_DIR}/constraint/CosseratSlidingConstraint.cpp ${SRC_ROOT_DIR}/mapping/LegendrePolynomialsMapping.cpp diff --git a/docs/testScene/tuto_1_6dofs.py b/docs/testScene/tuto_1_6dofs.py new file mode 100644 index 00000000..664ca37c --- /dev/null +++ b/docs/testScene/tuto_1_6dofs.py @@ -0,0 +1,67 @@ +# -*- coding: utf-8 -*- + +import Sofa + +stiffness_param = 1.e10 +beam_radius = 1. + + +def _add_rigid_base(p_node): + rigid_base_node = p_node.addChild('rigid_base') + rigid_base_node.addObject('MechanicalObject', template='Rigid3d', name="cosserat_base_mo", + position="0 0 0 0 0 0. 1", + showObject=1, showObjectScale='0.1') + rigid_base_node.addObject('RestShapeSpringsForceField', name='spring', stiffness=stiffness_param, + angularStiffness=stiffness_param, external_points="0", mstate="@cosserat_base_mo", + points="0", template="Rigid3d") + return rigid_base_node + + +def _add_cosserat_state(p_node, bending_states, list_sections_length, _radius=2.): + cosserat_coordinate_node = p_node.addChild('cosseratCoordinate') + cosserat_coordinate_node.addObject('MechanicalObject', template='Vec6d', name='cosserat_state', + position=bending_states) + cosserat_coordinate_node.addObject('BeamHookeLawForceFieldRigid', crossSectionShape='circular', + length=list_sections_length, radius=2., youngModulus=1.e4, + poissonRatio=0.4) + return cosserat_coordinate_node + + +def _add_cosserat_frame(p_node, _bending_node, framesF, _section_curv_abs, _frame_curv_abs, _radius, _beam_mass=0.0): + cosserat_in_Sofa_frame_node = p_node.addChild('cosserat_in_Sofa_frame_node') + + _bending_node.addChild(cosserat_in_Sofa_frame_node) + frames_mo = cosserat_in_Sofa_frame_node.addObject('MechanicalObject', template='Rigid3d', + name="FramesMO", position=framesF, showIndices=1, showObject=1, + showObjectScale=0.8) + + cosserat_in_Sofa_frame_node.addObject('UniformMass', totalMass=_beam_mass) + + cosserat_in_Sofa_frame_node.addObject('DiscreteCosseratMapping', curv_abs_input=_section_curv_abs, + curv_abs_output=_frame_curv_abs, name='cosseratMapping', + input1=_bending_node.cosserat_state.getLinkPath(), + input2=p_node.cosserat_base_mo.getLinkPath(), + output=frames_mo.getLinkPath(), debug=0, radius=_radius) + return cosserat_in_Sofa_frame_node + + +def createScene(root_node): + root_node.addObject('VisualStyle', displayFlags='showBehaviorModels showCollisionModels showMechanicalMappings') + root_node.gravity = [0, 0., 0] + # + base_node = _add_rigid_base(root_node) + + # + cos_nul_state = [0.0, 0.0, 0.0, 0., 0., 0.] # torsion, y_bending, z_bending + bending_states = [cos_nul_state, cos_nul_state, cos_nul_state] + list_sections_length = [10, 10, 10] + bending_node = _add_cosserat_state(root_node, bending_states, list_sections_length) + + section_curv_abs = [0, 10, 20, 30] + frames_curv_abs = [0, 10, 20, 30] + cosserat_G_frames = [[0., 0, 0, 0, 0, 0, 1], [10., 0, 0, 0, 0, 0, 1], [20., 0, 0, 0, 0, 0, 1], + [30., 0, 0, 0, 0, 0, 1]] + _add_cosserat_frame(base_node, bending_node, cosserat_G_frames, section_curv_abs, frames_curv_abs, + beam_radius) + + return root_node diff --git a/docs/testScene/tuto_2_6dofs.py b/docs/testScene/tuto_2_6dofs.py new file mode 100644 index 00000000..d7ec18c0 --- /dev/null +++ b/docs/testScene/tuto_2_6dofs.py @@ -0,0 +1,114 @@ +# -*- coding: utf-8 -*- +from useful.header import addHeader, addSolverNode, addVisual + +stiffness_param = 1.e10 +beam_radius = 1. + +nb_sections = 6 +nb_frames = 6 +beam_length = 30 + + +def _add_rigid_base(p_node): + rigid_base_node = p_node.addChild('rigid_base') + rigid_base_node.addObject('MechanicalObject', template='Rigid3d', name="cosserat_base_mo", + position="0 0 0 0 0 0. 1", + showObject=1, showObjectScale='0.1') + rigid_base_node.addObject('RestShapeSpringsForceField', name='spring', stiffness=stiffness_param, + angularStiffness=stiffness_param, external_points="0", mstate="@cosserat_base_mo", + points="0", template="Rigid3d") + return rigid_base_node + + +def _add_cosserat_state(p_node, bending_states, list_sections_length, _radius=2.): + cosserat_coordinate_node = p_node.addChild('cosseratCoordinate') + print(f' ===> bendind state : {bending_states}') + cosserat_coordinate_node.addObject('MechanicalObject', template='Vec6d', name='cosserat_state', + position=bending_states) + cosserat_coordinate_node.addObject('BeamHookeLawForceFieldRigid', crossSectionShape='circular', + length=list_sections_length, radius=2., youngModulus=1.e3, + poissonRatio=0.4) + return cosserat_coordinate_node + + +def _add_cosserat_frame(p_node, _bending_node, framesF, _section_curv_abs, _frame_curv_abs, _radius, _beam_mass=5): + cosserat_in_Sofa_frame_node = p_node.addChild('cosserat_in_Sofa_frame_node') + + _bending_node.addChild(cosserat_in_Sofa_frame_node) + frames_mo = cosserat_in_Sofa_frame_node.addObject('MechanicalObject', template='Rigid3d', + name="FramesMO", position=framesF, showIndices=0., showObject=0, + showObjectScale=0.8) + + cosserat_in_Sofa_frame_node.addObject('UniformMass', totalMass=_beam_mass) + + cosserat_in_Sofa_frame_node.addObject('DiscreteCosseratMapping', curv_abs_input=_section_curv_abs, + curv_abs_output=_frame_curv_abs, name='cosseratMapping', + input1=_bending_node.cosserat_state.getLinkPath(), + input2=p_node.cosserat_base_mo.getLinkPath(), + output=frames_mo.getLinkPath(), debug=1, radius=_radius) + return cosserat_in_Sofa_frame_node + + +def createScene(root_node): + root_node.addObject('VisualStyle', displayFlags='showBehaviorModels showCollisionModels showMechanicalMappings') +# root_node.gravity = [0, 0, 0] +# root_node.addObject('EulerImplicitSolver', firstOrder="0", rayleighStiffness="0.0", rayleighMass='0.0') +# root_node.addObject('SparseLDLSolver', name='solver') + + addHeader(root_node) + root_node.gravity = [0, -9.81, 0.] + + solver_node = addSolverNode(root_node, name="solver_node") + + # Add rigid base + base_node = _add_rigid_base(solver_node) + + # build beam geometry + + length_s = beam_length/float(nb_sections) + bending_states = [] + list_sections_length = [] + temp = 0. # where to start the base position + section_curv_abs = [0.] # section/segment curve abscissa + + for i in range(nb_sections): + bending_states.append([0, 0., 0., 0, 0., 0.]) # torsion, y-bending, z-bending, elongation, y-shear and z-shear + list_sections_length.append((((i + 1) * length_s) - i * length_s)) + temp += list_sections_length[i] + section_curv_abs.append(temp) +# bending_states[nb_sections-1] = [0, 0.0, 0.3, 0, 0., 0.] + bending_states[nb_sections-1] = [0., 0., 0., 0.2, 0., 0.] + + + print (f'The bending list : {bending_states}') + print (f'The section_curv_abs list : {section_curv_abs}') + + # call add cosserat state and force field + bending_node = _add_cosserat_state(solver_node, bending_states, list_sections_length) + + # comment : ??? + + length_f = beam_length/float(nb_frames) + cosserat_G_frames = [] + frames_curv_abs = [] + cable_position_f = [] # need sometimes for drawing segment + x, y, z = 0, 0, 0 + + for i in range(nb_frames+1): + sol = i * length_f + cosserat_G_frames.append([sol + x, y, z, 0, 0, 0, 1]) + cable_position_f.append([sol + x, y, z]) + frames_curv_abs.append(sol + x) + + cosserat_G_frames[nb_frames] = [beam_length + x, y, z, 0, 0, 0, 1] + cable_position_f[nb_frames] = [beam_length + x, y, z] + frames_curv_abs[nb_frames] = beam_length + x + + cosserat_frames = _add_cosserat_frame(base_node, bending_node, cosserat_G_frames, section_curv_abs, frames_curv_abs, + beam_radius) + + ## Add a force component to test the stretch +# applied_force =[0, -1.e8, 0, 0, 0, 0] +# const_force_node = cosserat_frames.addObject('ConstantForceField', name='constForce', showArrowSize=10, indices=nb_frames, forces=applied_force) + + return root_node diff --git a/src/Cosserat/CMakeLists.txt b/src/Cosserat/CMakeLists.txt index ceff3af7..dbcf73be 100644 --- a/src/Cosserat/CMakeLists.txt +++ b/src/Cosserat/CMakeLists.txt @@ -21,6 +21,8 @@ set(HEADER_FILES forcefield/BeamHookeLawForceField.h forcefield/BeamHookeLawForceField.inl + forcefield/BeamHookeLawForceFieldRigid.h + forcefield/BeamHookeLawForceFieldRigid.inl forcefield/CosseratInternalActuation.h forcefield/CosseratInternalActuation.inl forcefield/MyUniformVelocityDampingForceField.h @@ -46,6 +48,7 @@ set(SOURCE_FILES mapping/LegendrePolynomialsMapping.cpp forcefield/BeamHookeLawForceField.cpp + forcefield/BeamHookeLawForceFieldRigid.cpp forcefield/CosseratInternalActuation.cpp forcefield/MyUniformVelocityDampingForceField.cpp diff --git a/src/Cosserat/forcefield/BeamHookeLawForceField.cpp b/src/Cosserat/forcefield/BeamHookeLawForceField.cpp index 698e22cf..aa6b94e1 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceField.cpp +++ b/src/Cosserat/forcefield/BeamHookeLawForceField.cpp @@ -27,164 +27,25 @@ * Contact information: https://project.inria.fr/softrobot/contact/ * * * ******************************************************************************/ -#define SOFA_COSSERAT_BEAMHOOKELAWFORCEFIELD_CPP +#define SOFA_COSSERAT_CPP_BeamHookeLawForceField #include "BeamHookeLawForceField.inl" #include -using namespace sofa::defaulttype; - namespace sofa::component::forcefield { -template<> -void BeamHookeLawForceField::reinit() -{ - // Precompute and store values - Real Iy, Iz, J, A; - if ( d_crossSectionShape.getValue().getSelectedItem() == "rectangular") //rectangular cross-section - { - Real Ly = d_lengthY.getValue(); - Real Lz = d_lengthZ.getValue(); - - const Real LyLzLzLz = Ly * Lz * Lz * Lz; - const Real LzLyLyLy = Lz * Ly * Ly * Ly; - - Iy = LyLzLzLz / 12.0; - Iz = LzLyLyLy / 12.0; - J = Iy + Iz; - A = Ly * Lz; - - } - else //circular cross-section - { - msg_info() << "Cross section shape." << d_crossSectionShape.getValue().getSelectedItem() ; - - Real r = d_radius.getValue(); - Real rInner = d_innerRadius.getValue(); - const Real r4 = r * r * r * r; - const Real rInner4 = rInner * rInner * rInner * rInner; - - Iy = M_PI * (r4 - rInner4) / 4.0; - Iz = Iy; - J = Iy + Iz; - A = M_PI * (r * r - rInner4); - - } - m_crossSectionArea = A; - - if( d_useInertiaParams.getValue() ) - { - msg_info("BeamHookeLawForceField")<< "Pre-calculated inertia parameters are used for the computation of the stiffness matrix."; - m_K_section66[0][0] = d_GI.getValue(); - m_K_section66[1][1] = d_EIy.getValue(); - m_K_section66[2][2] = d_EIz.getValue(); - m_K_section66[3][3] = d_EA.getValue(); - m_K_section66[4][4] = d_GA.getValue(); - m_K_section66[5][5] = d_GA.getValue(); - } - else - { - //Pour du vec 6, on a _m_K =diag([G*J E*Iy E*Iz E*A G*As G*As]); % stifness matrix - Real E = d_youngModulus.getValue(); - Real G = E/(2.0*(1.0+d_poissonRatio.getValue())); - //Translational Stiffness (X, Y, Z directions): - // Gs * J(i): Shearing modulus times the second moment of the area (torsional stiffness). - // E * Js(i): Young's modulus times the second moment of the area (bending stiffness). - m_K_section66[0][0] = G*J; - m_K_section66[1][1] = E*Iy; - m_K_section66[2][2] = E*Iz; - //Rotational Stiffness (X, Y, Z directions): - //E * A: Young's modulus times the cross-sectional area (axial stiffness). - //Gs * A: Shearing modulus times the cross-sectional area (shearing stiffness). - m_K_section66[3][3] = E*A; - m_K_section66[4][4] = G*A; - m_K_section66[5][5] = G*A; - } -} - - -template<> -void BeamHookeLawForceField::addForce(const MechanicalParams* mparams, - DataVecDeriv& d_f, - const DataVecCoord& d_x, - const DataVecDeriv& d_v) -{ - SOFA_UNUSED(d_v); - SOFA_UNUSED(mparams); - - if(!this->getMState()) { - msg_info("BeamHookeLawForceField") << "No Mechanical State found, no force will be computed..." << "\n"; - compute_df=false; - return; - } - VecDeriv& f = *d_f.beginEdit(); - const VecCoord& x = d_x.getValue(); - // get the rest position (for non straight shape) - const VecCoord& x0 = this->mstate->read(VecCoordId::restPosition())->getValue(); - - f.resize(x.size()); - unsigned int sz = d_length.getValue().size(); - if(x.size()!= sz){ - msg_warning("BeamHookeLawForceField")<<" length : "<< sz <<"should have the same size as x... "<< x.size() <<"\n"; - compute_df = false; - return; - } - for (unsigned int i=0; i -void BeamHookeLawForceField::addDForce(const MechanicalParams* mparams, - DataVecDeriv& d_df , - const DataVecDeriv& d_dx) -{ - if (!compute_df) - return; - - WriteAccessor< DataVecDeriv > df = d_df; - ReadAccessor< DataVecDeriv > dx = d_dx; - Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); - - df.resize(dx.size()); - for (unsigned int i=0; i -void BeamHookeLawForceField::addKToMatrix(const MechanicalParams* mparams, - const MultiMatrixAccessor* matrix) -{ - MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate); - BaseMatrix* mat = mref.matrix; - unsigned int offset = mref.offset; - Real kFact = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); - - const VecCoord& pos = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - for (unsigned int n=0; nadd(offset + i + 6*n, offset + j + 6*n, -kFact * m_K_section66[i][j]*d_length.getValue()[n]); - - } -} - //////////////////////////////////////////// FACTORY ////////////////////////////////////////////// // Registering the component // see: http://wiki.sofa-framework.org/wiki/ObjectFactory // 1-RegisterObject("description") + .add<> : Register the component // 2-.add<>(true) : Set default template +using namespace sofa::defaulttype; +int BeamHookeLawForceFieldClass = core::RegisterObject("This component is used to compute internal stress for torsion (along x) and bending (y and z)") + .add >(true) -int BeamHookeLawForceFieldClass = core::RegisterObject("This component is used to compute internal stress for torsion " - "(along x) and bending (y and z)") - .add >(true) - .add >() - ; + ; ////////////////////////////////////////////////////////////////////////////////////////////////////// // Force template specialization for the most common sofa floating point related type. @@ -192,9 +53,6 @@ int BeamHookeLawForceFieldClass = core::RegisterObject("This component is used t // avoid the code generation of the template for each compilation unit. // see: http://www.stroustrup.com/C++11FAQ.html#extern-templates -template class SOFA_COSSERAT_API BeamHookeLawForceField; -template class SOFA_COSSERAT_API BeamHookeLawForceField; - - +template class BeamHookeLawForceField; -} // sofa::component::forcefield +} // forcefield diff --git a/src/Cosserat/forcefield/BeamHookeLawForceField.h b/src/Cosserat/forcefield/BeamHookeLawForceField.h index a59e6e9f..d9e086f7 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceField.h +++ b/src/Cosserat/forcefield/BeamHookeLawForceField.h @@ -45,8 +45,6 @@ namespace sofa::component::forcefield { using sofa::type::Vec ; -using type::Vec6; -using type::Vector3; using sofa::type::Mat ; using sofa::type::vector; using sofa::core::MechanicalParams; @@ -67,19 +65,18 @@ class BeamHookeLawForceField : public ForceField public : SOFA_CLASS(SOFA_TEMPLATE(BeamHookeLawForceField, DataTypes), SOFA_TEMPLATE(ForceField, DataTypes)); - typedef typename DataTypes::Real Real ; - typedef typename DataTypes::Coord Coord ; - typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; typedef typename DataTypes::VecCoord VecCoord; typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef Data DataVecCoord; typedef Data DataVecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename sofa::core::behavior::MechanicalState MechanicalState; - + typedef Vec<3, Real> Vec3; typedef Mat<3, 3, Real> Mat33; - typedef Mat<6, 6, Real> Mat66; + typedef CompressedRowSparseMatrix CSRMat33B66; typedef typename CompressedRowSparseMatrix::ColBlockConstIterator _3_3_ColBlockConstIterator; @@ -106,7 +103,7 @@ public : void addDForce(const MechanicalParams* mparams, DataVecDeriv& df , const DataVecDeriv& - dx ) override; + dx ) override; void addKToMatrix(const MechanicalParams* mparams, @@ -139,13 +136,10 @@ public : Data d_GI; Data d_GA; Data d_EA; - Data d_EIy; - Data d_EIz; - Data d_buildTorsion; + Data d_EI; bool compute_df; Mat33 m_K_section; - Mat66 m_K_section66; type::vector m_K_sectionList; /// Cross-section area @@ -158,16 +152,11 @@ private : /// Bring inherited attributes and function in the current lookup context. /// otherwise any access to the base::attribute would require /// the "this->" approach. -// using ForceField::getContext ; -// using ForceField::f_printLog ; + using ForceField::getContext ; + using ForceField::f_printLog ; //using ForceField::mstate ; //////////////////////////////////////////////////////////////////////////// }; -#if !defined(SOFA_COSSERAT_BEAMHOOKELAWFORCEFIELD_CPP) -extern template class SOFA_COSSERAT_API BeamHookeLawForceField< defaulttype::Vec3Types>; -extern template class SOFA_COSSERAT_API BeamHookeLawForceField< defaulttype::Vec6Types>; -#endif - -} // sofa::component::forcefield +} // forcefield diff --git a/src/Cosserat/forcefield/BeamHookeLawForceField.inl b/src/Cosserat/forcefield/BeamHookeLawForceField.inl index af020430..e6e6d508 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceField.inl +++ b/src/Cosserat/forcefield/BeamHookeLawForceField.inl @@ -34,272 +34,260 @@ #include #include #include // ?? -#include -#include -#include using sofa::core::behavior::MechanicalState ; using sofa::core::objectmodel::BaseContext ; using sofa::helper::ReadAccessor ; using sofa::helper::WriteAccessor ; using sofa::core::VecCoordId; + +#include using std::cout ; using std::endl ; +#include +#include + namespace sofa::component::forcefield { - using sofa::core::behavior::MultiMatrixAccessor ; - using sofa::core::behavior::BaseMechanicalState ; - using sofa::helper::WriteAccessor ; - - - template - BeamHookeLawForceField::BeamHookeLawForceField() - : Inherit1(), - d_crossSectionShape( initData(&d_crossSectionShape, {"circular","rectangular"}, - "crossSectionShape", - "shape of the cross-section. Can be: circular (tube with external radius being radius and internal radius being innerRadius ) or rectangular (lengthY and lengthZ) . Default is circular" )), - d_youngModulus( initData( &d_youngModulus, 1.0e9, "youngModulus", "Young Modulus describes the stiffness of the material")), - d_poissonRatio( initData( &d_poissonRatio, 0.45, "poissonRatio", "poisson Ratio describes the compressibility of the material")), - d_length( initData( &d_length, "length", "length of each beam")), - d_radius( initData( &d_radius, 1.0, "radius", "external radius of the cross section (if circular)")), - d_innerRadius( initData( &d_innerRadius, 0.0, "innerRadius", "internal radius of the cross section (if circular)")), - d_lengthY( initData( &d_lengthY, 1.0, "lengthY", "side length of the cross section along local y axis (if rectangular)")), - d_lengthZ( initData( &d_lengthZ, 1.0, "lengthZ", "side length of the cross section along local z axis (if rectangular)")), - d_variantSections(initData(&d_variantSections, false, "variantSections", "In case we have variant beam sections this has to be set to true")), - d_youngModulusList(initData(&d_youngModulusList, "youngModulusList", "The list of Young modulus in case we have sections with variable physical properties")), - d_poissonRatioList(initData(&d_poissonRatioList, "poissonRatioList", "The list of poisson's ratio in case we have sections with variable physical properties")), - d_useInertiaParams(initData(&d_useInertiaParams, false, "useInertiaParams", "If the inertia parameters are given by the user, there is no longer any need to use @d_youngModulus and @d_poissonRatio.")), - d_GI(initData(&d_GI, "GI", "The inertia parameter, GI")), - d_GA(initData(&d_GA, "GA", "The inertia parameter, GA")), - d_EA(initData(&d_EA, "EA", "The inertia parameter, EA")), - d_EIy(initData(&d_EIy, "EIy", "The inertia parameter, EIy")), - d_EIz(initData(&d_EIz, "EIz", "The inertia parameter, EIz")), - d_buildTorsion(initData(&d_buildTorsion, true,"build_torsion", "build torsion or the elongation of the beam ?")) - - { - compute_df=true; - } +using sofa::core::behavior::MultiMatrixAccessor ; +using sofa::core::behavior::BaseMechanicalState ; +using sofa::helper::WriteAccessor ; - template - BeamHookeLawForceField::~BeamHookeLawForceField() - {} - template - void BeamHookeLawForceField::init() - { - Inherit1::init(); +template +BeamHookeLawForceField::BeamHookeLawForceField() + : Inherit1(), + d_crossSectionShape( initData(&d_crossSectionShape, {"circular","rectangular"}, + "crossSectionShape", + "shape of the cross-section. Can be: circular (tube with external radius being radius and internal radius being innerRadius ) or rectangular (lengthY and lengthZ) . Default is circular" )), + d_youngModulus( initData( &d_youngModulus, 1.0e9, "youngModulus", "Young Modulus describes the stiffness of the material")), + d_poissonRatio( initData( &d_poissonRatio, 0.45, "poissonRatio", "poisson Ratio describes the compressibility of the material")), + d_length( initData( &d_length, "length", "length of each beam")), + d_radius( initData( &d_radius, 1.0, "radius", "external radius of the cross section (if circular)")), + d_innerRadius( initData( &d_innerRadius, 0.0, "innerRadius", "internal radius of the cross section (if circular)")), + d_lengthY( initData( &d_lengthY, 1.0, "lengthY", "side length of the cross section along local y axis (if rectangular)")), + d_lengthZ( initData( &d_lengthZ, 1.0, "lengthZ", "side length of the cross section along local z axis (if rectangular)")), + d_variantSections(initData(&d_variantSections, false, "variantSections", "In case we have variant beam sections this has to be set to true")), + d_youngModulusList(initData(&d_youngModulusList, "youngModulusList", "The list of Young modulus in case we have sections with variable physical properties")), + d_poissonRatioList(initData(&d_poissonRatioList, "poissonRatioList", "The list of poisson's ratio in case we have sections with variable physical properties")), + d_useInertiaParams(initData(&d_useInertiaParams, false, "useInertiaParams", "If the inertia parameters are given by the user, there is no longer any need to use @d_youngModulus and @d_poissonRatio.")), + d_GI(initData(&d_GI, "GI", "The inertia parameter, GI")), + d_GA(initData(&d_GA, "GA", "The inertia parameter, GA")), + d_EA(initData(&d_EA, "EA", "The inertia parameter, EA")), + d_EI(initData(&d_EI, "EI", "The inertia parameter, EI")) +{ + compute_df=true; +} - reinit(); - } +template +BeamHookeLawForceField::~BeamHookeLawForceField() +{} +template +void BeamHookeLawForceField::init() +{ + Inherit1::init(); + reinit(); +} - /*Cross-Section Properties Initialization: The reinit function begins by recalculating the properties +/*Cross-Section Properties Initialization: The reinit function begins by recalculating the properties related to the cross-section of the beams. It calculates the area moment of inertia (Iy and Iz), the polar moment of inertia (J), and the cross-sectional area (A). - These calculations depend on the chosen cross-section shape, either circular or rectangular. - The formulas used for these calculations are based on standard equations for these properties.*/ - template - void BeamHookeLawForceField::BeamHookeLawForceField::reinit() + These calculations depend on the chosen cross-section shape, either circular or rectangular. T + he formulas used for these calculations are based on standard equations for these properties.*/ +template +void BeamHookeLawForceField::reinit() +{ + // Precompute and store values + Real Iy, Iz, J, A; + if ( d_crossSectionShape.getValue().getSelectedItem() == "rectangular") //rectangular cross-section { - // Precompute and store values - Real Iy, Iz, J, A; - if ( d_crossSectionShape.getValue().getSelectedItem() == "rectangular") //rectangular cross-section - { - Real Ly = d_lengthY.getValue(); - Real Lz = d_lengthZ.getValue(); + Real Ly = d_lengthY.getValue(); + Real Lz = d_lengthZ.getValue(); - const Real LyLzLzLz = Ly * Lz * Lz * Lz; - const Real LzLyLyLy = Lz * Ly * Ly * Ly; + const Real LyLzLzLz = Ly * Lz * Lz * Lz; + const Real LzLyLyLy = Lz * Ly * Ly * Ly; - Iy = LyLzLzLz / 12.0; - Iz = LzLyLyLy / 12.0; - J = Iy + Iz; - A = Ly * Lz; + Iy = LyLzLzLz / 12.0; + Iz = LzLyLyLy / 12.0; + J = Iy + Iz; + A = Ly * Lz; - } - else //circular cross-section - { - msg_info() << "Cross section shape." << d_crossSectionShape.getValue().getSelectedItem() ; + } + else //circular cross-section + { + msg_info() << "Cross section shape." << d_crossSectionShape.getValue().getSelectedItem() ; - Real r = d_radius.getValue(); - Real rInner = d_innerRadius.getValue(); - const Real r4 = r * r * r * r; - const Real rInner4 = rInner * rInner * rInner * rInner; + Real r = d_radius.getValue(); + Real rInner = d_innerRadius.getValue(); + const Real r4 = r * r * r * r; + const Real rInner4 = rInner * rInner * rInner * rInner; - Iy = M_PI * (r4 - rInner4) / 4.0; - Iz = Iy; - J = Iy + Iz; - A = M_PI * (r * r - rInner4); + Iy = M_PI * (r4 - rInner4) / 4.0; + Iz = Iy; + J = Iy + Iz; + A = M_PI * (r * r - rInner4); - } - m_crossSectionArea = A; + } + m_crossSectionArea = A; + + if(!d_variantSections.getValue()) + { + if(!d_useInertiaParams.getValue()) + { + Real E = d_youngModulus.getValue(); + Real G = E/(2.0*(1.0+d_poissonRatio.getValue())); - if(d_variantSections.getValue()) + m_K_section[0][0] = G*J; + m_K_section[1][1] = E*Iy; + m_K_section[2][2] = E*Iz; + } + else { - /*If the d_variantSections flag is set to true, it implies that multi-section beams are used for + msg_info("BeamHookeLawForceField")<< "Pre-calculated inertia parameters are used for the computation " + "of the stiffness matrix."; + m_K_section[0][0] = d_GI.getValue(); + m_K_section[1][1] = d_EI.getValue(); + m_K_section[2][2] = d_EI.getValue(); + } + + }else { + /*If the d_variantSections flag is set to true, it implies that multi-section beams are used for the simulation. In this case, the code calculates and initializes a list of stiffness matrices (m_K_sectionList) for each section. The properties of each section, such as Young's modulus and Poisson's ratio, are specified in the d_youngModulusList and d_poissonRatioList data.*/ - msg_info("BeamHookeLawForceField")<< "Multi section beam are used for the simulation!"; - m_K_sectionList.clear(); + msg_info("BeamHookeLawForceField")<< "Multi section beam are used for the simulation!"; + m_K_sectionList.clear(); - const size_t szL = d_length.getValue().size(); + const size_t szL = d_length.getValue().size(); - if((szL != d_poissonRatioList.getValue().size())||(szL != d_youngModulusList.getValue().size())){ - msg_error("BeamHookeLawForceField")<< "Please the size of the data length, youngModulusList and " - "poissonRatioList should be the same !"; - return; - } + if((szL != d_poissonRatioList.getValue().size())||(szL != d_youngModulusList.getValue().size())){ + msg_error("BeamHookeLawForceField")<< "Please the size of the data length, youngModulusList and " + "poissonRatioList should be the same !"; + return; + } - /*Stiffness Matrix Initialization: Next, the code initializes the stiffness matrix m_K_section + /*Stiffness Matrix Initialization: Next, the code initializes the stiffness matrix m_K_section based on the properties of the cross-section and the material's Young's modulus (E) and Poisson's ratio. The stiffness matrix is essential for computing forces and simulating beam behavior.*/ - for(size_t k=0; k - void BeamHookeLawForceField::BeamHookeLawForceField::addForce(const MechanicalParams* mparams, - DataVecDeriv& d_f, - const DataVecCoord& d_x, - const DataVecDeriv& d_v) - { - SOFA_UNUSED(d_v); - SOFA_UNUSED(mparams); - - if(!this->getMState()) { - msg_info("BeamHookeLawForceField") << "No Mechanical State found, no force will be computed..." << "\n"; - compute_df=false; - return; - } - VecDeriv& f = *d_f.beginEdit(); - const VecCoord& x = d_x.getValue(); - // get the rest position (for non straight shape) - const VecCoord& x0 = this->mstate->read(VecCoordId::restPosition())->getValue(); - - f.resize(x.size()); - unsigned int sz = d_length.getValue().size(); - if(x.size()!= sz){ - msg_warning("BeamHookeLawForceField")<<" length : "<< sz <<"should have the same size as x... "<< x.size() <<"\n"; - compute_df = false; - return; - } - - if(!d_variantSections.getValue()) - for (unsigned int i=0; i +void BeamHookeLawForceField::addForce(const MechanicalParams* mparams, + DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecDeriv& d_v) +{ + SOFA_UNUSED(d_v); + SOFA_UNUSED(mparams); + if(!this->getMState()) { + msg_info("BeamHookeLawForceField") << "No Mechanical State found, no force will be computed..." << "\n"; + compute_df=false; + return; + } + VecDeriv& f = *d_f.beginEdit(); + const VecCoord& x = d_x.getValue(); + // get the rest position (for non straight shape) + const VecCoord& x0 = this->mstate->read(VecCoordId::restPosition())->getValue(); + + f.resize(x.size()); + unsigned int sz = d_length.getValue().size(); + if(x.size()!= sz){ + msg_warning("BeamHookeLawForceField")<<" length : "<< sz <<"should have the same size as x... "<< x.size() <<"\n"; + compute_df = false; + return; } - template - void BeamHookeLawForceField::addDForce(const MechanicalParams* mparams, - DataVecDeriv& d_df , const DataVecDeriv& d_dx) - { - if (!compute_df) - return; + if(!d_variantSections.getValue()) + for (unsigned int i=0; i df = d_df; - ReadAccessor< DataVecDeriv > dx = d_dx; - Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); +} - df.resize(dx.size()); - if(!d_variantSections.getValue()) - for (unsigned int i=0; i +void BeamHookeLawForceField::addDForce(const MechanicalParams* mparams, + DataVecDeriv& d_df , + const DataVecDeriv& d_dx) +{ + if (!compute_df) + return; + + WriteAccessor< DataVecDeriv > df = d_df; + ReadAccessor< DataVecDeriv > dx = d_dx; + Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); + + df.resize(dx.size()); + if(!d_variantSections.getValue()) + for (unsigned int i=0; i +double BeamHookeLawForceField::getPotentialEnergy(const MechanicalParams* mparams, + const DataVecCoord& d_x) const +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(d_x); - template - double BeamHookeLawForceField::getPotentialEnergy(const MechanicalParams* mparams, - const DataVecCoord& d_x) const - { - SOFA_UNUSED(mparams); - SOFA_UNUSED(d_x); + return 0.0; +} - return 0.0; - } +template +void BeamHookeLawForceField::addKToMatrix(const MechanicalParams* mparams, + const MultiMatrixAccessor* matrix) +{ + MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate); + BaseMatrix* mat = mref.matrix; + unsigned int offset = mref.offset; + Real kFact = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); - template - void BeamHookeLawForceField::addKToMatrix(const MechanicalParams* mparams, - const MultiMatrixAccessor* matrix) + const VecCoord& pos = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + for (unsigned int n=0; ngetMatrix(this->mstate); - BaseMatrix* mat = mref.matrix; - unsigned int offset = mref.offset; - Real kFact = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); - - const VecCoord& pos = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - for (unsigned int n=0; nadd(offset + i + 3*n, offset + j + 3*n, -kFact * m_K_section[i][j]*d_length.getValue()[n]); - else - for(unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j< 3; j++) - mat->add(offset + i + 3*n, offset + j + 3*n, -kFact * m_K_sectionList[n][i][j] * d_length.getValue()[n]); - } + if(!d_variantSections.getValue()) + for(unsigned int i = 0; i < 3; i++) + for (unsigned int j = 0; j< 3; j++) + mat->add(offset + i + 3*n, offset + j + 3*n, -kFact * m_K_section[i][j]*d_length.getValue()[n]); + else + for(unsigned int i = 0; i < 3; i++) + for (unsigned int j = 0; j< 3; j++) + mat->add(offset + i + 3*n, offset + j + 3*n, -kFact * m_K_sectionList[n][i][j] * d_length.getValue()[n]); } +} - template - typename BeamHookeLawForceField::Real BeamHookeLawForceField::getRadius() - { - return d_radius.getValue(); - } +template +typename BeamHookeLawForceField::Real BeamHookeLawForceField::getRadius() +{ + return d_radius.getValue(); +} } // forcefield diff --git a/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.cpp b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.cpp new file mode 100644 index 00000000..8671698f --- /dev/null +++ b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.cpp @@ -0,0 +1,63 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * +* * +* This library 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 library 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 library; if not, write to the Free Software Foundation, * +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * +******************************************************************************* +* Plugin SoftRobots v1.0 * +* * +* This plugin is also distributed under the GNU LGPL (Lesser General * +* Public License) license with the same conditions than SOFA. * +* * +* Contributors: Defrost team (INRIA, University of Lille, CNRS, * +* Ecole Centrale de Lille) * +* * +* Contact information: https://project.inria.fr/softrobot/contact/ * +* * +******************************************************************************/ +#define SOFA_COSSERAT_BeamHookeLawForceFieldRigid_CPP +#include "BeamHookeLawForceFieldRigid.inl" + +#include + +using namespace sofa::defaulttype; + +namespace sofa::component::forcefield +{ + + +//////////////////////////////////////////// FACTORY ////////////////////////////////////////////// +// Registering the component +// see: http://wiki.sofa-framework.org/wiki/ObjectFactory +// 1-RegisterObject("description") + .add<> : Register the component +// 2-.add<>(true) : Set default template + + +int BeamHookeLawForceFieldRigidClass = core::RegisterObject("This component is used to compute internal stress for torsion " + "(along x) and bending (y and z)") + .add >() + ; +////////////////////////////////////////////////////////////////////////////////////////////////////// + +// Force template specialization for the most common sofa floating point related type. +// This goes with the extern template declaration in the .h. Declaring extern template +// avoid the code generation of the template for each compilation unit. +// see: http://www.stroustrup.com/C++11FAQ.html#extern-templates + +template class SOFA_COSSERAT_API BeamHookeLawForceFieldRigid; + + + +} // sofa::component::forcefield diff --git a/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h new file mode 100644 index 00000000..89001a0c --- /dev/null +++ b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h @@ -0,0 +1,172 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * +* * +* This library 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 library 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 library; if not, write to the Free Software Foundation, * +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * +******************************************************************************* +* Plugin SoftRobots v1.0 * +* * +* This plugin is also distributed under the GNU LGPL (Lesser General * +* Public License) license with the same conditions than SOFA. * +* * +* Contributors: Defrost team (INRIA, University of Lille, CNRS, * +* Ecole Centrale de Lille) * +* * +* Contact information: https://project.inria.fr/softrobot/contact/ * +* * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::forcefield +{ + +using sofa::type::Vec ; +using type::Vec6; +using type::Vector3; +using sofa::type::Mat ; +using sofa::type::vector; +using sofa::core::MechanicalParams; +using sofa::linearalgebra::BaseMatrix; +using sofa::core::behavior::ForceField ; +using sofa::linearalgebra::CompressedRowSparseMatrix ; +using sofa::core::behavior::MultiMatrixAccessor ; + +using sofa::helper::OptionsGroup; + +/** + * This component is used to compute the Hooke's law on a beam computed on strain / stress + * Only bending and torsion strain / stress are considered here +*/ +template +class BeamHookeLawForceFieldRigid : public ForceField +{ +public : + SOFA_CLASS(SOFA_TEMPLATE(BeamHookeLawForceFieldRigid, DataTypes), SOFA_TEMPLATE(ForceField, DataTypes)); + + typedef typename DataTypes::Real Real ; + typedef typename DataTypes::Coord Coord ; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename sofa::core::behavior::MechanicalState MechanicalState; + + typedef Mat<3, 3, Real> Mat33; + typedef Mat<6, 6, Real> Mat66; + typedef CompressedRowSparseMatrix CSRMat33B66; + + typedef typename CompressedRowSparseMatrix::ColBlockConstIterator _3_3_ColBlockConstIterator; + typedef typename CompressedRowSparseMatrix::RowBlockConstIterator _3_3_RowBlockConstIterator; + typedef typename CompressedRowSparseMatrix::BlockConstAccessor _3_3_BlockConstAccessor; + typedef typename CompressedRowSparseMatrix::BlockAccessor _3_3_BlockAccessor; + + +public : + BeamHookeLawForceFieldRigid(); + virtual ~BeamHookeLawForceFieldRigid(); + + ////////////////////////// Inherited from BaseObject ///////////////////////// + void init() override; + void reinit() override; + /////////////////////////////////////////////////////////////////////////// + + ////////////////////////// Inherited from ForceField ///////////////////////// + void addForce(const MechanicalParams* mparams, + DataVecDeriv& f , + const DataVecCoord& x , + const DataVecDeriv& v) override; + + void addDForce(const MechanicalParams* mparams, + DataVecDeriv& df , + const DataVecDeriv& + dx ) override; + + + void addKToMatrix(const MechanicalParams* mparams, + const MultiMatrixAccessor* matrix) override; + + double getPotentialEnergy(const MechanicalParams* mparams, + const DataVecCoord& x) const override; + //////////////////////////////////////////////////////////////////////////// + + Real getRadius(); + +protected: + Data d_crossSectionShape; + Data d_youngModulus; /// youngModulus + Data d_poissonRatio; /// poissonRatio + Data> d_length ; /// length of each beam + + /// Circular Cross Section + Data d_radius; + Data d_innerRadius; + /// Rectangular Cross Section + Data d_lengthY; + Data d_lengthZ; + //In case we have a beam with different properties per section + Data d_variantSections; /// bool to identify different beams sections + Data> d_youngModulusList; /// youngModulus + Data> d_poissonRatioList; /// poissonRatio + /// If the inertia parameters are given by the user, there is no longer any need to use YG. + Data d_useInertiaParams; + Data d_GI; + Data d_GA; + Data d_EA; + Data d_EIy; + Data d_EIz; + Data d_buildTorsion; + + bool compute_df; + Mat33 m_K_section; + Mat66 m_K_section66; + type::vector m_K_sectionList; + + /// Cross-section area + Real m_crossSectionArea; + +private : + + ////////////////////////// Inherited attributes //////////////////////////// + /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html + /// Bring inherited attributes and function in the current lookup context. + /// otherwise any access to the base::attribute would require + /// the "this->" approach. +// using ForceField::getContext ; +// using ForceField::f_printLog ; + //using ForceField::mstate ; + //////////////////////////////////////////////////////////////////////////// +}; + +#if !defined(SOFA_COSSERAT_BeamHookeLawForceFieldRigid_CPP) +extern template class SOFA_COSSERAT_API BeamHookeLawForceFieldRigid< defaulttype::Vec6Types>; +#endif + + +} // sofa::component::forcefield diff --git a/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.inl b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.inl new file mode 100644 index 00000000..8ca552a0 --- /dev/null +++ b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.inl @@ -0,0 +1,250 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * +* * +* This library 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 library 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 library; if not, write to the Free Software Foundation, * +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * +******************************************************************************* +* Plugin SoftRobots v1.0 * +* * +* This plugin is also distributed under the GNU LGPL (Lesser General * +* Public License) license with the same conditions than SOFA. * +* * +* Contributors: Defrost team (INRIA, University of Lille, CNRS, * +* Ecole Centrale de Lille) * +* * +* Contact information: https://project.inria.fr/softrobot/contact/ * +* * +******************************************************************************/ +#pragma once +#include "BeamHookeLawForceFieldRigid.h" + +#include +#include +#include +#include // ?? +#include +#include +#include + +using sofa::core::behavior::MechanicalState ; +using sofa::core::objectmodel::BaseContext ; +using sofa::helper::ReadAccessor ; +using sofa::helper::WriteAccessor ; +using sofa::core::VecCoordId; +using std::cout ; +using std::endl ; + +namespace sofa::component::forcefield +{ + using sofa::core::behavior::MultiMatrixAccessor ; + using sofa::core::behavior::BaseMechanicalState ; + using sofa::helper::WriteAccessor ; + + + template + BeamHookeLawForceFieldRigid::BeamHookeLawForceFieldRigid() + : Inherit1(), + d_crossSectionShape( initData(&d_crossSectionShape, {"circular","rectangular"}, + "crossSectionShape", + "shape of the cross-section. Can be: circular (tube with external radius being radius and internal radius being innerRadius ) or rectangular (lengthY and lengthZ) . Default is circular" )), + d_youngModulus( initData( &d_youngModulus, 1.0e9, "youngModulus", "Young Modulus describes the stiffness of the material")), + d_poissonRatio( initData( &d_poissonRatio, 0.45, "poissonRatio", "poisson Ratio describes the compressibility of the material")), + d_length( initData( &d_length, "length", "length of each beam")), + d_radius( initData( &d_radius, 1.0, "radius", "external radius of the cross section (if circular)")), + d_innerRadius( initData( &d_innerRadius, 0.0, "innerRadius", "internal radius of the cross section (if circular)")), + d_lengthY( initData( &d_lengthY, 1.0, "lengthY", "side length of the cross section along local y axis (if rectangular)")), + d_lengthZ( initData( &d_lengthZ, 1.0, "lengthZ", "side length of the cross section along local z axis (if rectangular)")), + d_variantSections(initData(&d_variantSections, false, "variantSections", "In case we have variant beam sections this has to be set to true")), + d_youngModulusList(initData(&d_youngModulusList, "youngModulusList", "The list of Young modulus in case we have sections with variable physical properties")), + d_poissonRatioList(initData(&d_poissonRatioList, "poissonRatioList", "The list of poisson's ratio in case we have sections with variable physical properties")), + d_useInertiaParams(initData(&d_useInertiaParams, false, "useInertiaParams", "If the inertia parameters are given by the user, there is no longer any need to use @d_youngModulus and @d_poissonRatio.")), + d_GI(initData(&d_GI, "GI", "The inertia parameter, GI")), + d_GA(initData(&d_GA, "GA", "The inertia parameter, GA")), + d_EA(initData(&d_EA, "EA", "The inertia parameter, EA")), + d_EIy(initData(&d_EIy, "EIy", "The inertia parameter, EIy")), + d_EIz(initData(&d_EIz, "EIz", "The inertia parameter, EIz")), + d_buildTorsion(initData(&d_buildTorsion, true,"build_torsion", "build torsion or the elongation of the beam ?")) + + { + compute_df=true; + } + + template + BeamHookeLawForceFieldRigid::~BeamHookeLawForceFieldRigid() + {} + + template + void BeamHookeLawForceFieldRigid::init() + { + Inherit1::init(); + reinit(); + } + + /*Cross-Section Properties Initialization: The reinit function begins by recalculating the properties + related to the cross-section of the beams. It calculates the area moment of inertia (Iy and Iz), + the polar moment of inertia (J), and the cross-sectional area (A). + These calculations depend on the chosen cross-section shape, either circular or rectangular. + The formulas used for these calculations are based on standard equations for these properties.*/ + template + void BeamHookeLawForceFieldRigid::reinit() + { + // Precompute and store values + Real Iy, Iz, J, A; + if ( d_crossSectionShape.getValue().getSelectedItem() == "rectangular") //rectangular cross-section + { + Real Ly = d_lengthY.getValue(); + Real Lz = d_lengthZ.getValue(); + + const Real LyLzLzLz = Ly * Lz * Lz * Lz; + const Real LzLyLyLy = Lz * Ly * Ly * Ly; + + Iy = LyLzLzLz / 12.0; + Iz = LzLyLyLy / 12.0; + J = Iy + Iz; + A = Ly * Lz; + + } + else //circular cross-section + { + msg_info() << "Cross section shape." << d_crossSectionShape.getValue().getSelectedItem() ; + + Real r = d_radius.getValue(); + Real rInner = d_innerRadius.getValue(); + const Real r4 = r * r * r * r; + const Real rInner4 = rInner * rInner * rInner * rInner; + + Iy = M_PI * (r4 - rInner4) / 4.0; + Iz = Iy; + J = Iy + Iz; + A = M_PI * (r * r - rInner4); + + } + m_crossSectionArea = A; + + if( d_useInertiaParams.getValue() ) + { + msg_info("BeamHookeLawForceFieldRigid")<< "Pre-calculated inertia parameters are used for the computation of the stiffness matrix."; + m_K_section66[0][0] = d_GI.getValue(); + m_K_section66[1][1] = d_EIy.getValue(); + m_K_section66[2][2] = d_EIz.getValue(); + m_K_section66[3][3] = d_EA.getValue(); + m_K_section66[4][4] = d_GA.getValue(); + m_K_section66[5][5] = d_GA.getValue(); + } + else + { + //Pour du vec 6, on a _m_K =diag([G*J E*Iy E*Iz E*A G*As G*As]); % stifness matrix + Real E = d_youngModulus.getValue(); + Real G = E/(2.0*(1.0+d_poissonRatio.getValue())); + //Translational Stiffness (X, Y, Z directions): + // Gs * J(i): Shearing modulus times the second moment of the area (torsional stiffness). + // E * Js(i): Young's modulus times the second moment of the area (bending stiffness). + m_K_section66[0][0] = G*J; + m_K_section66[1][1] = E*Iy; + m_K_section66[2][2] = E*Iz; + //Rotational Stiffness (X, Y, Z directions): + //E * A: Young's modulus times the cross-sectional area (axial stiffness). + //Gs * A: Shearing modulus times the cross-sectional area (shearing stiffness). + m_K_section66[3][3] = E*A; + m_K_section66[4][4] = G*A; + m_K_section66[5][5] = G*A; + } + } + + template + void BeamHookeLawForceFieldRigid::addForce(const MechanicalParams* mparams, + DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecDeriv& d_v) + { + SOFA_UNUSED(d_v); + SOFA_UNUSED(mparams); + + if(!this->getMState()) { + msg_info("BeamHookeLawForceField") << "No Mechanical State found, no force will be computed..." << "\n"; + compute_df=false; + return; + } + VecDeriv& f = *d_f.beginEdit(); + const VecCoord& x = d_x.getValue(); + // get the rest position (for non straight shape) + const VecCoord& x0 = this->mstate->read(VecCoordId::restPosition())->getValue(); + + f.resize(x.size()); + unsigned int sz = d_length.getValue().size(); + if(x.size()!= sz){ + msg_warning("BeamHookeLawForceField")<<" length : "<< sz <<"should have the same size as x... "<< x.size() <<"\n"; + compute_df = false; + return; + } + for (unsigned int i=0; i + void BeamHookeLawForceFieldRigid::addDForce(const MechanicalParams* mparams, + DataVecDeriv& d_df , + const DataVecDeriv& d_dx) + { + if (!compute_df) + return; + + WriteAccessor< DataVecDeriv > df = d_df; + ReadAccessor< DataVecDeriv > dx = d_dx; + Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); + + df.resize(dx.size()); + for (unsigned int i=0; i + void BeamHookeLawForceFieldRigid::addKToMatrix(const MechanicalParams* mparams, + const MultiMatrixAccessor* matrix) + { + MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate); + BaseMatrix* mat = mref.matrix; + unsigned int offset = mref.offset; + Real kFact = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); + + const VecCoord& pos = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + for (unsigned int n=0; nadd(offset + i + 6*n, offset + j + 6*n, -kFact * m_K_section66[i][j]*d_length.getValue()[n]); + + } + } + + template + double BeamHookeLawForceFieldRigid::getPotentialEnergy(const MechanicalParams* mparams, + const DataVecCoord& d_x) const + { + SOFA_UNUSED(mparams); + SOFA_UNUSED(d_x); + + return 0.0; + } + template + typename BeamHookeLawForceFieldRigid::Real BeamHookeLawForceFieldRigid::getRadius() + { + return d_radius.getValue(); + } + +} // forcefield diff --git a/src/Cosserat/mapping/BaseCosserat.cpp b/src/Cosserat/mapping/BaseCosserat.cpp index 887ba9c0..8071197c 100644 --- a/src/Cosserat/mapping/BaseCosserat.cpp +++ b/src/Cosserat/mapping/BaseCosserat.cpp @@ -34,6 +34,8 @@ template<> BaseCosserat::se3 BaseCosserat::build_Xi_hat(const Coord1& strain_i){ se3 Xi; + msg_info("BaseCosserat")<<" ===========> Build Xi Hat rigid is called "; + Xi[0][1] = -strain_i(2); Xi[0][2] = strain_i[1]; Xi[1][2] = -strain_i[0]; @@ -43,9 +45,18 @@ BaseCosserat::se3 BaseCosserat::computeExponentialSE3(co if(theta <= std::numeric_limits::epsilon()){ g_X_n = I4 + curv_abs_x_n*Xi_hat_n; }else { +// se3 Xi_hat_n_2 = Xi_hat_n * Xi_hat_n; +// se3 Xi_hat_n_3 = Xi_hat_n_2 * Xi_hat_n; +// SReal costheta = std::cos(theta); +// SReal sintheta = std::cos(theta); +// SReal theta2 = std::pow(theta,2); +// SReal theta3 = theta2 * theta; +// g_X_n = I4 + curv_abs_x_n*Xi_hat_n + ((1.-costheta)/(theta2))*Xi_hat_n_2 +((theta-sintheta)/theta3)*Xi_hat_n_3; double scalar1= (1.0 - std::cos(curv_abs_x_n*theta))/std::pow(theta,2); double scalar2 = (curv_abs_x_n*theta - std::sin(curv_abs_x_n*theta))/std::pow(theta,3); g_X_n = I4 + curv_abs_x_n*Xi_hat_n + scalar1*Xi_hat_n*Xi_hat_n + scalar2*Xi_hat_n*Xi_hat_n*Xi_hat_n ; } + if(d_debug.getValue()) + msg_info("BaseCosserat: ")<< "matrix g_X : "<< g_X_n; - // msg_info("BaseCosserat: ")<< "matrix g_X : "<< g_X; type::Mat3x3 M; g_X_n.getsub(0,0,M); //get the rotation matrix diff --git a/src/Cosserat/mapping/BaseCosserat.h b/src/Cosserat/mapping/BaseCosserat.h index 11f98415..d6eb293b 100644 --- a/src/Cosserat/mapping/BaseCosserat.h +++ b/src/Cosserat/mapping/BaseCosserat.h @@ -235,6 +235,7 @@ class BaseCosserat : public virtual sofa::core::objectmodel::BaseObject Xi[2][0] = -Xi(0,2); Xi[2][1] = -Xi(1,2); + //@TODO: Why this , if q = 0 ???? Xi[0][3] = 1.0; return Xi; } diff --git a/src/Cosserat/mapping/BaseCosserat.inl b/src/Cosserat/mapping/BaseCosserat.inl index 9f42b514..f58711d3 100644 --- a/src/Cosserat/mapping/BaseCosserat.inl +++ b/src/Cosserat/mapping/BaseCosserat.inl @@ -101,7 +101,8 @@ void BaseCosserat::computeExponentialSE3(const double & curv_a _g_X = I4 + curv_abs_x_n*Xi_hat_n + scalar1*Xi_hat_n*Xi_hat_n + scalar2*Xi_hat_n*Xi_hat_n*Xi_hat_n ; } - // msg_info("BaseCosserat: ")<< "matrix g_X : "<< g_X; + if(d_debug.getValue()) + msg_info("BaseCosserat: ")<< "matrix _g_X : "<< _g_X; type::Mat3x3 M; _g_X.getsub(0,0,M); //get the rotation matrix @@ -129,12 +130,12 @@ void BaseCosserat::update_ExponentialSE3(const In1VecCoord & i const Coord1 strain_n = inDeform[m_indicesVectors[i]-1]; //Cosserat reduce coordinates (strain) the size varies from 1 to 6 // The distance between the frame and the closest beam node toward the base const SReal curv_abs_x = m_framesLengthVectors[i]; // curv_abs_x = frame_curv_abs - L_(n-1) - computeExponentialSE3(curv_abs_x, strain_n,g_X_frame_i); + computeExponentialSE3(curv_abs_x, strain_n, g_X_frame_i); m_framesExponentialSE3Vectors.push_back(g_X_frame_i); if(d_debug.getValue()){ - msg_info("BaseCosserat:")<< "__________________________________________"; - msg_info("BaseCosserat:")<< "x :"<< curv_abs_x << "; k :"<< strain_n; + msg_info("BaseCosserat:")<< "_________________"<:: applyJ( const type::vector& dataVecIn1Vel, const type::vector& dataVecIn2Vel) { + if(d_debug.getValue()) + std::cout<< " ########## ApplyJ R Function ########"<< std::endl; + if(dataVecOutVel.empty() || dataVecIn1Vel.empty() ||dataVecIn2Vel.empty() ) return; const In1VecDeriv& in1_vel = dataVecIn1Vel[0]->getValue(); @@ -84,18 +87,17 @@ void DiscreteCosseratMapping:: applyJ( if(d_debug.getValue()) std::cout<< "Node velocity : "<< i << " = " << eta_node_i<< std::endl; } - const OutVecCoord& out = m_toModel->read(core::ConstVecCoordId::position())->getValue(); auto sz = curv_abs_frames.size(); out_vel.resize(sz); + for (unsigned int i = 0 ; i < sz; i++) { Transform Trans = m_framesExponentialSE3Vectors[i].inversed(); Tangent Adjoint; Adjoint.clear(); this->computeAdjoint(Trans, Adjoint); - - type::Vec6 frame_Xi_dot; - for (unsigned int u =0; u<6; u++) - frame_Xi_dot(i) = in1_vel[i-1][u]; + type::Vec6 frame_Xi_dot = in1_vel[m_indicesVectors[i]-1]; +// for (unsigned int u =0; u<6; u++) +// frame_Xi_dot(i) = in1_vel[m_indicesVectors[i]-1][u]; Vec6 eta_frame_i = Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i]-1] + m_framesTangExpVectors[i] * frame_Xi_dot ); // eta @@ -103,7 +105,6 @@ void DiscreteCosseratMapping:: applyJ( Tangent Proj = this->build_projector(T); out_vel[i] = Proj * eta_frame_i; - if(d_debug.getValue()) std::cout<< "Frame velocity : "<< i << " = " << eta_frame_i<< std::endl; } @@ -122,6 +123,8 @@ void DiscreteCosseratMapping:: applyJT( if(dataVecOut1Force.empty() || dataVecInForce.empty() || dataVecOut2Force.empty()) return; + if(d_debug.getValue()) + std::cout<< " ########## ApplyJT force R Function ########"<< std::endl; const OutVecDeriv& in = dataVecInForce[0]->getValue(); In1VecDeriv& out1 = *dataVecOut1Force[0]->beginEdit(); @@ -210,6 +213,9 @@ void DiscreteCosseratMapping::applyJT( if(dataMatOut1Const.empty() || dataMatOut2Const.empty() || dataMatInConst.empty() ) return; + if(d_debug.getValue()) + std::cout<< " ########## ApplyJT constraint R Function ########"<< std::endl; + //We need only one input In model and input Root model (if present) In1MatrixDeriv& out1 = *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space (reduced coordinate) In2MatrixDeriv& out2 = *dataMatOut2Const[0]->beginEdit(); // constraints on the reference frame (base frame) diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index 5e2047d2..14250cd4 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -96,7 +96,12 @@ void DiscreteCosseratMapping::reinit() } if(d_debug.getValue()) - msg_info("DiscreteCosseratMapping")<< " m_vecTransform : "<< m_vecTransform; + { + msg_info("DiscreteCosseratMapping")<< " =================" + "============================ "; + msg_info("DiscreteCosseratMapping")<< " m_vecTransform : "<< m_vecTransform; + } + this->initialize(); } @@ -110,6 +115,8 @@ void DiscreteCosseratMapping::apply( if(dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) return; + if(d_debug.getValue()) + std::cout<< " ########## Apply Function ########"<< std::endl; ///Do Apply //We need only one input In model and input Root model (if present) const In1VecCoord& in1 = dataVecIn1Pos[0]->getValue(); @@ -133,6 +140,9 @@ void DiscreteCosseratMapping::apply( } frame *= m_framesExponentialSE3Vectors[i]; //frame*gX(x) + if(d_debug.getValue()) + std::cout<< "Frame : "<< i << " = " << frame<< std::endl; + type::Vec3 v = frame.getOrigin(); type::Quat q = frame.getOrientation(); out[i] = OutCoord(v,q); @@ -199,6 +209,8 @@ void DiscreteCosseratMapping:: applyJ( if(dataVecOutVel.empty() || dataVecIn1Vel.empty() ||dataVecIn2Vel.empty() ) return; + if(d_debug.getValue()) + std::cout<< " ########## ApplyJ Function ########"<< std::endl; const In1VecDeriv& in1_vel = dataVecIn1Vel[0]->getValue(); const In2VecDeriv& in2_vel = dataVecIn2Vel[0]->getValue(); OutVecDeriv& out_vel = *dataVecOutVel[0]->beginEdit(); @@ -282,6 +294,8 @@ void DiscreteCosseratMapping:: applyJT( if(dataVecOut1Force.empty() || dataVecInForce.empty() || dataVecOut2Force.empty()) return; + if(d_debug.getValue()) + std::cout<< " ########## ApplyJT force Function ########"<< std::endl; const OutVecDeriv& in = dataVecInForce[0]->getValue(); In1VecDeriv& out1 = *dataVecOut1Force[0]->beginEdit(); @@ -371,6 +385,8 @@ void DiscreteCosseratMapping::applyJT( if(dataMatOut1Const.empty() || dataMatOut2Const.empty() || dataMatInConst.empty() ) return; + if(d_debug.getValue()) + std::cout<< " ########## ApplyJT Constraint Function ########"<< std::endl; //We need only one input In model and input Root model (if present) In1MatrixDeriv& out1 = *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space (reduced coordinate) In2MatrixDeriv& out2 = *dataMatOut2Const[0]->beginEdit(); // constraints on the reference frame (base frame)