Skip to content

Commit

Permalink
update the branche cosseratExtension and fix some bugs
Browse files Browse the repository at this point in the history
  • Loading branch information
@adagolodjo
adagolodjo committed Dec 4, 2023
1 parent e2a755e commit b357920
Show file tree
Hide file tree
Showing 15 changed files with 948 additions and 398 deletions.
3 changes: 3 additions & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
67 changes: 67 additions & 0 deletions docs/testScene/tuto_1_6dofs.py
View file
Original file line number Diff line number Diff line change
@@ -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
114 changes: 114 additions & 0 deletions docs/testScene/tuto_2_6dofs.py
View file
Original file line number Diff line number Diff line change
@@ -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
3 changes: 3 additions & 0 deletions src/Cosserat/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -46,6 +48,7 @@ set(SOURCE_FILES
mapping/LegendrePolynomialsMapping.cpp

forcefield/BeamHookeLawForceField.cpp
forcefield/BeamHookeLawForceFieldRigid.cpp
forcefield/CosseratInternalActuation.cpp
forcefield/MyUniformVelocityDampingForceField.cpp

Expand Down
156 changes: 7 additions & 149 deletions src/Cosserat/forcefield/BeamHookeLawForceField.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,174 +27,32 @@
* Contact information: https://project.inria.fr/softrobot/contact/ *
* *
******************************************************************************/
#define SOFA_COSSERAT_BEAMHOOKELAWFORCEFIELD_CPP
#define SOFA_COSSERAT_CPP_BeamHookeLawForceField
#include "BeamHookeLawForceField.inl"

#include <sofa/core/ObjectFactory.h>

using namespace sofa::defaulttype;

namespace sofa::component::forcefield
{

template<>
void BeamHookeLawForceField<defaulttype::Vec6fTypes>::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<defaulttype::Vec6fTypes>::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<x.size(); i++)
f[i] -= (m_K_section66 * (x[i] - x0[i])) * d_length.getValue()[i];

d_f.endEdit();

}

template<>
void BeamHookeLawForceField<defaulttype::Vec6fTypes>::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<dx.size(); i++)
df[i] -= (m_K_section66 * dx[i])*kFactor* d_length.getValue()[i];
}

template<>
void BeamHookeLawForceField<defaulttype::Vec6Types>::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; n<pos.size(); n++)
{
for(unsigned int i = 0; i < 6; i++)
for (unsigned int j = 0; j< 6; j++)
mat->add(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<BeamHookeLawForceField<Vec3Types> >(true)

int BeamHookeLawForceFieldClass = core::RegisterObject("This component is used to compute internal stress for torsion "
"(along x) and bending (y and z)")
.add<BeamHookeLawForceField<Vec3Types> >(true)
.add<BeamHookeLawForceField<Vec6Types> >()
;
;
//////////////////////////////////////////////////////////////////////////////////////////////////////

// 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 BeamHookeLawForceField<Vec3Types>;
template class SOFA_COSSERAT_API BeamHookeLawForceField<Vec6Types>;


template class BeamHookeLawForceField<Vec3Types>;

} // sofa::component::forcefield
} // forcefield
Loading

0 comments on commit b357920

Please sign in to comment.