Skip to content

Geometric stiffness in BeamLengthMapping #134

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 2 commits into
base: master
Choose a base branch
from
Draft

Geometric stiffness in BeamLengthMapping #134

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1070f26
Introduce example for BeamLengthMapping
alxbilger Feb 5, 2024
abdb755
Make BeamLengthMapping non-linear and implement buildGeometricStiffne…
alxbilger Feb 5, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
60 changes: 60 additions & 0 deletions examples/BeamLengthMapping.scn
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
<?xml version="1.0"?>
<Node name="root" dt="0.05" gravity="0 -9.81 0">
<Node name="Plugins">
<RequiredPlugin name="BeamAdapter"/>
<RequiredPlugin name="Sofa.Component.AnimationLoop"/> <!-- Needed to use components [FreeMotionAnimationLoop] -->
<RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Correction"/> <!-- Needed to use components [GenericConstraintCorrection] -->
<RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Model"/> <!-- Needed to use components [UniformLagrangianConstraint] -->
<RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Solver"/> <!-- Needed to use components [GenericConstraintSolver] -->
<RequiredPlugin name="Sofa.Component.IO.Mesh"/> <!-- Needed to use components [MeshOBJLoader] -->
<RequiredPlugin name="Sofa.Component.LinearSolver.Direct"/> <!-- Needed to use components [SparseLDLSolver] -->
<RequiredPlugin name="Sofa.Component.Mapping.Linear"/> <!-- Needed to use components [IdentityMapping] -->
<RequiredPlugin name="Sofa.Component.Mapping.NonLinear"/> <!-- Needed to use components [RigidMapping] -->
<RequiredPlugin name="Sofa.Component.Mass"/> <!-- Needed to use components [UniformMass] -->
<RequiredPlugin name="Sofa.Component.ODESolver.Backward"/> <!-- Needed to use components [EulerImplicitSolver] -->
<RequiredPlugin name="Sofa.Component.SolidMechanics.Spring"/> <!-- Needed to use components [RestShapeSpringsForceField] -->
<RequiredPlugin name="Sofa.Component.StateContainer"/> <!-- Needed to use components [MechanicalObject] -->
<RequiredPlugin name="Sofa.Component.Topology.Container.Dynamic"/> <!-- Needed to use components [EdgeSetTopologyContainer] -->
<RequiredPlugin name="Sofa.Component.Topology.Container.Grid"/> <!-- Needed to use components [CylinderGridTopology] -->
<RequiredPlugin name="Sofa.GL.Component.Rendering3D"/> <!-- Needed to use components [OglModel] -->
</Node>

<FreeMotionAnimationLoop/>
<GenericConstraintSolver maxIterations="1000" tolerance="1e-10"/>

<EulerImplicitSolver firstOrder="0" rayleighStiffness="0.0"/>
<SparseLDLSolver name="solver" template="CompressedRowSparseMatrix"/>
<GenericConstraintCorrection linearSolver="@solver"/>

<EdgeSetTopologyContainer edges="0 1 1 2 2 3 3 4" position="0 0 0 0.1 0 0 0.2 0 0 0.3 0 0 0.5 0 0"/>
<MechanicalObject template="Rigid3d" name="frames" showObject="1" showObjectScale="0.02"/>

<BeamInterpolation name="BeamInterpolation" printLog = "1" defaultYoungModulus="3e9" dofsAndBeamsAligned="false" straight="1" crossSectionShape="circular" radius="0.001" DOF0TransformNode0="0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 " DOF1TransformNode1=" 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 -0.1 0 0 0 0 0 1 "/>
<AdaptiveBeamForceFieldAndMass name="BeamForceField" computeMass="true" massDensity="1.24e3"/>
<RestShapeSpringsForceField name="anchor" points="0" stiffness="1e6" angularStiffness="1e5"/>

<UniformMass indices="4" totalMass="0.5" showAxisSizeFactor="0.2" vertexMass="0.5 0.008 0.0133 0.005 0.0050.005 0.0133 0.005 0.005 0.005 0.0133"/>

<Node name="lengthConstraint">
<MechanicalObject template="Vec1d" name="mappedDOFs" position="0.1 0.1 0.1 0.1"/>
<BeamLengthMapping name="beamLMap" geometricStiffness="Exact"/>
<UniformLagrangianConstraint template="Vec1d" iterative="false"/>
</Node>

<Node name="beamVisual">
<CylinderGridTopology name='topo' center='0 0 0' axis='1 0 0' radius="0.001" length="0.4" n='3 3 16' computeHexaList='0' computeQuadList='0' drawHexahedra='0'/>
<MechanicalObject template='Vec3' name='visual'/>

<AdaptiveBeamMapping interpolation='@../BeamInterpolation' input='@../frames' output='@./visual' useCurvAbs='1'/>
<Node name="ogl">
<OglModel color='0 0 1 0.2'/>
<IdentityMapping/>
</Node>
</Node>

<Node name="cubeVisual">
<MeshOBJLoader filename='mesh/cube.obj' scale='0.1' name='loader'/>
<OglModel src='@loader'/>
<RigidMapping index='4'/>
</Node>
</Node>
8 changes: 3 additions & 5 deletions src/BeamAdapter/component/mapping/BeamLengthMapping.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@
#include <BeamAdapter/config.h>
#include <BeamAdapter/component/BeamInterpolation.h>
#include <BeamAdapter/component/controller/AdaptiveBeamController.h>
#include <sofa/component/mapping/nonlinear/NonLinearMappingData.h>

#include <sofa/linearalgebra/EigenSparseMatrix.h>

Expand Down Expand Up @@ -102,7 +103,7 @@ using sofa::core::topology::TopologyContainer ;
* https://www.sofa-framework.org/community/doc/programming-with-sofa/create-your-component/
*/
template <class TIn, class TOut>
class BeamLengthMapping : public Mapping<TIn, TOut>
class BeamLengthMapping : public Mapping<TIn, TOut>, public nonlinear::NonLinearMappingData<true>
{
public:
SOFA_CLASS(SOFA_TEMPLATE2(BeamLengthMapping,TIn,TOut),
Expand Down Expand Up @@ -155,9 +156,6 @@ class BeamLengthMapping : public Mapping<TIn, TOut>
SingleLink<BeamLengthMapping<TIn, TOut>,
BInterpolation, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> l_adaptativebeamInterpolation;

Data< unsigned > d_geometricStiffness; ///< how to compute geometric stiffness (0->no GS, 1->exact GS, 2->stabilized GS)


BeamLengthMapping(State< In >* from=NULL,
State< Out >* to=NULL,
BeamInterpolation< TIn >* interpolation=NULL) ;
Expand Down Expand Up @@ -187,7 +185,7 @@ class BeamLengthMapping : public Mapping<TIn, TOut>
// interface of baseMapping.h
virtual void updateK( const MechanicalParams* /*mparams*/, core::ConstMultiVecDerivId /*outForce*/ ) override;
const linearalgebra::BaseMatrix* getK() override;

void buildGeometricStiffnessMatrix(sofa::core::GeometricStiffnessMatrix* matrices) override;



Expand Down
133 changes: 129 additions & 4 deletions src/BeamAdapter/component/mapping/BeamLengthMapping.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
#include <sofa/core/MechanicalParams.h>
#include <sofa/helper/ScopedAdvancedTimer.h>
#include <iomanip>
#include <sofa/core/BaseLocalMappingMatrix.h>


namespace sofa
Expand Down Expand Up @@ -74,8 +75,6 @@ BeamLengthMapping<TIn,TOut>::BeamLengthMapping(State< In >* from, State< Out >*
BeamInterpolation< TIn >* interpolation)
: Inherit(from, to)
, l_adaptativebeamInterpolation(initLink("interpolation", "Path to the Interpolation component on scene"), interpolation)
, d_geometricStiffness(initData(&d_geometricStiffness, 2u, "geometricStiffness", "0 -> no GS, 1 -> exact GS, 2 -> stabilized GS (default)"))

{


Expand Down Expand Up @@ -431,7 +430,7 @@ void BeamLengthMapping< TIn, TOut>::applyJT(const core::ConstraintParams* cparam
template <class TIn, class TOut>
void BeamLengthMapping< TIn, TOut>::applyDJT(const MechanicalParams* mparams, core::MultiVecDerivId parentDfId, core::ConstMultiVecDerivId childDfId)
{
const unsigned& geometricStiffness = d_geometricStiffness.getValue();
const unsigned& geometricStiffness = this->d_geometricStiffness.getValue().getSelectedId();
if( !geometricStiffness ) return;

const SReal kfactor = mparams->kFactor();
Expand Down Expand Up @@ -591,7 +590,7 @@ void BeamLengthMapping< TIn, TOut>::applyDJT(const MechanicalParams* mparams, co
template <class TIn, class TOut>
void BeamLengthMapping<TIn, TOut>::updateK(const core::MechanicalParams* mparams, core::ConstMultiVecDerivId childForceId )
{
const unsigned& geometricStiffness = d_geometricStiffness.getValue();
const unsigned& geometricStiffness = this->d_geometricStiffness.getValue().getSelectedId();
if( !geometricStiffness ) { K_geom.resize(0,0); return; }
//helper::ReadAccessor<Data<VecDeriv> > childForce( *childForceId[(const core::State<TOut>*)this->getToModels()[0]].read() );

Expand Down Expand Up @@ -775,6 +774,132 @@ const sofa::linearalgebra::BaseMatrix* BeamLengthMapping<TIn, TOut>::getK()
return &K_geom;
}

template <class TIn, class TOut>
void BeamLengthMapping<TIn, TOut>::buildGeometricStiffnessMatrix(
sofa::core::GeometricStiffnessMatrix* matrices)
{
const unsigned& geometricStiffness = d_geometricStiffness.getValue().getSelectedId();
if( !geometricStiffness )
{
return;
}

const Data<InVecCoord>& dataInX = *this->getFromModel()->read(VecCoordId::position());
const InVecCoord& x_in = dataInX.getValue();

const auto childForce = this->toModel->readTotalForces();
const auto dJdx = matrices->getMappingDerivativeIn(this->fromModel).withRespectToPositionsIn(this->fromModel);

const unsigned int s = l_adaptativebeamInterpolation->getNumBeams();

for (unsigned int i = 0; i < s; i++)
{
const Deriv& force_i = childForce[i];

// force in compression (>0) can lead to negative eigen values in geometric stiffness
// this results in a undefinite implicit matrix that causes instabilies
// if stabilized GS (geometricStiffness==2) -> keep only force in extension
if (force_i[0] < 0 || geometricStiffness == 1)
{
//1. get the indices of the Dofs of the beam a
unsigned int IdxNode[2];
l_adaptativebeamInterpolation->getNodeIndices(i,IdxNode[0],IdxNode[1]);

//2. get the force on the mapped dof
Real childF = force_i[0];

//3. get the spline points
Vec<3, InReal> P0,P1,P2,P3;
l_adaptativebeamInterpolation->getSplinePoints(i, x_in , P0, P1, P2, P3);

////////////////////////////
//4. compute the equivalent stiffness on the spline control point (apply DJt on spline map)
Mat<4,4,Mat3> K;
computeDJtSpline(childF, P0,P1,P2,P3, K);

//- K must be transfered from the control points to the DOFs
// - get the transformation of the DOFs
Transform DOF0Global_H_local0, DOF1Global_H_local1;
l_adaptativebeamInterpolation->getDOFtoLocalTransformInGlobalFrame(i, DOF0Global_H_local0, DOF1Global_H_local1, x_in);

// - create a matrix of the lever in the global frame
Real L = l_adaptativebeamInterpolation->getLength(i);
// - rotate the levers in the global frame
Vec3 lev(-L/3.0,0.0,0.0);
Vec3 Lev00_global = -DOF0Global_H_local0.getOrigin();
Vec3 Lev01_global = DOF0Global_H_local0.getOrientation().rotate(lev) - DOF0Global_H_local0.getOrigin();
lev[0]=L/3;
Vec3 Lev12_global = DOF1Global_H_local1.getOrientation().rotate(lev) - DOF1Global_H_local1.getOrigin();
Vec3 Lev13_global = -DOF1Global_H_local1.getOrigin();

// create matrices:
Mat3 Lev00_mat, Lev01_mat, Lev12_mat,Lev13_mat ;
createCrossMatrix(Lev01_global, Lev01_mat);
createCrossMatrix(Lev12_global, Lev12_mat);
createCrossMatrix(Lev00_global, Lev00_mat);
createCrossMatrix(Lev13_global, Lev13_mat);

Mat<4,4,Mat3> SplineP_J_DOFs;
SplineP_J_DOFs[0][0].identity(); SplineP_J_DOFs[0][1] = Lev00_mat; SplineP_J_DOFs[0][2].clear(); SplineP_J_DOFs[0][3].clear();
SplineP_J_DOFs[1][0].identity(); SplineP_J_DOFs[1][1] = Lev01_mat; SplineP_J_DOFs[1][2].clear(); SplineP_J_DOFs[1][3].clear();
SplineP_J_DOFs[2][0].clear(); SplineP_J_DOFs[2][1].clear(); SplineP_J_DOFs[2][2].identity(); SplineP_J_DOFs[2][3] = Lev12_mat;
SplineP_J_DOFs[3][0].clear(); SplineP_J_DOFs[3][1].clear(); SplineP_J_DOFs[3][2].identity(); SplineP_J_DOFs[3][3] = Lev13_mat;

Mat<4,4,Mat3> Result;
for (unsigned int l=0; l<4;l++)// block lines
{
for (unsigned int c=0; c<4;c++) // block columns
{
Result[l][c].clear();
for (unsigned int j=0; j<4;j++)
{
for (unsigned int k=0; k<4;k++)
{
Result[l][c] += SplineP_J_DOFs[j][l].transposed() * K[j][k] * SplineP_J_DOFs[k][c];
}
}

}
}

// 5. compute the equivalent stiffness on the rigid rotation of control point (apply DJt on rigid at fixed forces on spline)
// -compute the equivalent forces on the spline control point (apply Jt on spline map)

Vec3 F0, F1, F2, F3;
computeJtSpline(childF, P0,P1,P2,P3, F0, F1, F2, F3);
Mat3 F0_mat, F1_mat, F2_mat, F3_mat;
createCrossMatrix(F0, F0_mat);
createCrossMatrix(F1, F1_mat);
createCrossMatrix(F2, F2_mat);
createCrossMatrix(F3, F3_mat);

// 6. assembly
for (unsigned j = 0; j < 2; j++)
{
for (unsigned int k = 0; k < 2; k++)
{
for (unsigned int l = 0; l < 3; l++)
{
for (unsigned int c = 0; c < 3; c++)
{
// translation translation
dJdx(Nin*IdxNode[j]+l, Nin*IdxNode[k]+c ) += Result[2*j ][2*k ][l][c];
// translation rotation
dJdx(Nin*IdxNode[j]+l , Nin*IdxNode[k]+c+3 ) += Result[2*j ][2*k+1][l][c];
// rotation translation
dJdx(Nin*IdxNode[j]+l+3, Nin*IdxNode[k]+c ) += Result[2*j+1][2*k ][l][c];
// rotation rotation
dJdx(Nin*IdxNode[j]+l+3, Nin*IdxNode[k]+c+3 ) += Result[2*j+1][2*k+1][l][c];
}
}

}

}
}
}
}


template <class TIn, class TOut>
void BeamLengthMapping<TIn, TOut>::computeJSpline(Real &dlength, const Vec3& P0, const Vec3& P1, const Vec3& P2, const Vec3& P3,
Expand Down