Skip to content
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

[Mass] Fix rigid uniform mass #4939

Open
wants to merge 8 commits into
base: master
Choose a base branch
from
Open

[Mass] Fix rigid uniform mass #4939

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
08ce046
Add unit test to test rotation of inertia matrix for rigid objects
bakpaul Aug 22, 2024
ab7333a
Apply suggestions from code review
bakpaul Aug 26, 2024
016890b
Wrong test set...
bakpaul Aug 26, 2024
ddb3a64
Use accessors instead of data
bakpaul Aug 26, 2024
e53483c
Fix inertia matrix rotation for rigid by rotating the inertia matrix …
bakpaul Aug 26, 2024
2618d83
Add centrifugal unit test
bakpaul Aug 28, 2024
d758e85
Merge branch 'add_unit_test_rigid_rotation' into fix_rigid_uniform_mass
bakpaul Aug 28, 2024
c8e3a08
Merge branch 'master' into fix_rigid_uniform_mass
bakpaul Sep 30, 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
4 changes: 2 additions & 2 deletions Sofa/Component/Mass/src/sofa/component/mass/UniformMass.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -330,9 +330,9 @@ Vec6 UniformMass<RigidTypes>::getMomentumRigid3DImpl( const MechanicalParams*,
ReadAccessor<DataVecDeriv> v = d_v;
ReadAccessor<DataVecCoord> x = d_x;
const ReadAccessor<Data<SetIndexArray > > indices = d_indices;
const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue();

Real m = d_vertexMass.getValue().mass;
const typename MassType::Mat3x3& I = d_vertexMass.getValue().inertiaMassMatrix;

type::Vec6d momentum;

Expand All @@ -341,7 +341,7 @@ Vec6 UniformMass<RigidTypes>::getMomentumRigid3DImpl( const MechanicalParams*,
typename RigidTypes::Vec3 linearMomentum = m*v[index].getLinear();
for( int j=0 ; j< 3 ; ++j ) momentum[j] += linearMomentum[j];

typename RigidTypes::Vec3 angularMomentum = cross( x[index].getCenter(), linearMomentum ) + ( I * v[index].getAngular() );
typename RigidTypes::Vec3 angularMomentum = cross( x[index].getCenter(), linearMomentum ) + ( d_vertexMass.getValue().rotate(pos[index].getOrientation()).inertiaMassMatrix * v[index].getAngular() );
for( int j=0 ; j< 3 ; ++j ) momentum[3+j] += angularMomentum[j];
}

Expand Down
62 changes: 50 additions & 12 deletions Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -400,6 +400,7 @@ void UniformMass<DataTypes>::addMDx ( const core::MechanicalParams*,
{
helper::WriteAccessor<DataVecDeriv> res = vres;
helper::ReadAccessor<DataVecDeriv> dx = vdx;
const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue();

WriteAccessor<Data<SetIndexArray > > indices = d_indices;

Expand All @@ -408,7 +409,13 @@ void UniformMass<DataTypes>::addMDx ( const core::MechanicalParams*,
m *= typename DataTypes::Real(factor);

for (const auto i : indices)
res[i] += dx[i] * m;
{
if constexpr (std::is_same<defaulttype::Rigid3Types, DataTypes>::value)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

add test using trait isRigid

res[i] += dx[i] * m.rotate(pos[i].getOrientation());
else
res[i] += dx[i] * m;

}
}


Expand All @@ -419,12 +426,18 @@ void UniformMass<DataTypes>::accFromF ( const core::MechanicalParams*,
{
WriteOnlyAccessor<DataVecDeriv> a = va;
ReadAccessor<DataVecDeriv> f = vf;
const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue();

const ReadAccessor<Data<SetIndexArray > > indices = d_indices;

MassType m = d_vertexMass.getValue();
for (const auto i : indices)
a[i] = f[i] / m;
{
if constexpr (std::is_same<defaulttype::Rigid3Types, DataTypes>::value)
a[i] = f[i] / m.rotate(pos[i].getOrientation());
else
a[i] = f[i] / m;
}
}


Expand Down Expand Up @@ -473,23 +486,24 @@ void UniformMass<DataTypes>::addForce ( const core::MechanicalParams*, DataVecDe
return;

helper::WriteAccessor<DataVecDeriv> f = vf;
const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue();

// weight
const SReal* g = getContext()->getGravity().ptr();
Deriv theGravity;
DataTypes::set
( theGravity, g[0], g[1], g[2] );
const MassType& m = d_vertexMass.getValue();
Deriv mg = theGravity * m;

dmsg_info() <<" addForce, mg = "<<d_vertexMass<<" * "<<theGravity<<" = "<<mg;

const ReadAccessor<Data<SetIndexArray > > indices = d_indices;

// add weight and inertia force
for (const auto i : indices)
{
f[i] += mg;
if constexpr (std::is_same<defaulttype::Rigid3Types, DataTypes>::value)
f[i] += theGravity*m.rotate(pos[i].getOrientation());
else
f[i] += theGravity*m;
}
}

Expand All @@ -501,12 +515,18 @@ SReal UniformMass<DataTypes>::getKineticEnergy ( const MechanicalParams* params,

ReadAccessor<DataVecDeriv> v = d_v;
const ReadAccessor<Data<SetIndexArray > > indices = d_indices;
const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue();

SReal e = 0;
const MassType& m = d_vertexMass.getValue();

for (const auto i : indices)
e += v[i] * m * v[i];
{
if constexpr (std::is_same<defaulttype::Rigid3Types, DataTypes>::value)
e += v[i] * m.rotate(pos[i].getOrientation()) * v[i];
else
e += v[i] * m * v[i];
}

return e/2;
}
Expand All @@ -518,6 +538,7 @@ SReal UniformMass<DataTypes>::getPotentialEnergy ( const MechanicalParams* param
SOFA_UNUSED(params);
ReadAccessor<DataVecCoord> x = d_x;
const ReadAccessor<Data<SetIndexArray > > indices = d_indices;
const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue();

SReal e = 0;
const MassType& m = d_vertexMass.getValue();
Expand All @@ -526,10 +547,14 @@ SReal UniformMass<DataTypes>::getPotentialEnergy ( const MechanicalParams* param
Deriv gravity;
DataTypes::set(gravity, g[0], g[1], g[2]);

Deriv mg = gravity * m;

for (const auto i : indices)
e -= mg * x[i];
{
if constexpr (std::is_same<defaulttype::Rigid3Types, DataTypes>::value)
e -= gravity * m.rotate(pos[i].getOrientation())* x[i];
else
e -= gravity * m * x[i];
}

return e;
}
Expand Down Expand Up @@ -558,6 +583,7 @@ template <class DataTypes>
void UniformMass<DataTypes>::addMToMatrix (sofa::linearalgebra::BaseMatrix * mat, SReal mFact, unsigned int &offset)
{
const MassType& m = d_vertexMass.getValue();
const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue();

static constexpr auto N = Deriv::total_size;

Expand All @@ -566,22 +592,30 @@ void UniformMass<DataTypes>::addMToMatrix (sofa::linearalgebra::BaseMatrix * mat
const ReadAccessor<Data<SetIndexArray > > indices = d_indices;
for (auto id : *indices)
{
calc ( mat, m, int(offset + N * id), mFact);
if constexpr (std::is_same<defaulttype::Rigid3Types, DataTypes>::value)
calc( mat, m.rotate(pos[id].getOrientation()), int(offset + N * id), mFact);
else
calc( mat, m, int(offset + N * id), mFact);
}
}

template <class DataTypes>
void UniformMass<DataTypes>::buildMassMatrix(sofa::core::behavior::MassMatrixAccumulator* matrices)
{
const MassType& m = d_vertexMass.getValue();
const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue();

static constexpr auto N = Deriv::total_size;

AddMToMatrixFunctor<Deriv,MassType, core::behavior::MassMatrixAccumulator> calc;

const ReadAccessor<Data<SetIndexArray > > indices = d_indices;
for (const auto index : indices)
{
calc( matrices, m, N * index, 1.);
if constexpr (std::is_same<defaulttype::Rigid3Types, DataTypes>::value)
calc( matrices, m.rotate(pos[index].getOrientation()), N * index, 1.);
else
calc( matrices, m, N * index, 1.);
}
}

Expand All @@ -598,13 +632,17 @@ void UniformMass<DataTypes>::getElementMass (sofa::Index index ,
BaseMatrix *m ) const
{
SOFA_UNUSED(index);
const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue();

static const BaseMatrix::Index dimension = BaseMatrix::Index(DataTypeInfo<Deriv>::size());
if ( m->rowSize() != dimension || m->colSize() != dimension )
m->resize ( dimension, dimension );

m->clear();
AddMToMatrixFunctor<Deriv,MassType, linearalgebra::BaseMatrix>() ( m, d_vertexMass.getValue(), 0, 1 );
if constexpr (std::is_same<defaulttype::Rigid3Types, DataTypes>::value)
AddMToMatrixFunctor<Deriv,MassType, linearalgebra::BaseMatrix>() ( m, d_vertexMass.getValue().rotate(pos[index].getOrientation()), 0, 1 );
else
AddMToMatrixFunctor<Deriv,MassType, linearalgebra::BaseMatrix>() ( m, d_vertexMass.getValue(), 0, 1 );
}


Expand Down
160 changes: 160 additions & 0 deletions Sofa/Component/Mass/tests/UniformMass_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@ using sofa::testing::BaseTest;
using testing::Types;

#include <sofa/core/ExecParams.h>
#include <sofa/core/VecId.h>

template <class TDataTypes, class TMassTypes>
struct TemplateTypes
Expand Down Expand Up @@ -412,6 +413,151 @@ struct UniformMassTest : public BaseTest
EXPECT_TRUE(todo == false) ;
}


static Node::SPtr generateRigidScene()
{
static const string scene =
"<?xml version='1.0' ?>"
"<Node name='root' dt='0.01' gravity='0 0 0'>"
" <RequiredPlugin name='Sofa.Component.Mass'/>"
" <RequiredPlugin name='Sofa.Component.StateContainer'/>"
" <RequiredPlugin name='Sofa.Component.Topology.Container.Grid'/>"
" <RequiredPlugin name='Sofa.Component.Visual'/>"
" <RequiredPlugin name='Sofa.Component.ODESolver.Backward'/>"
" <RequiredPlugin name='Sofa.Component.LinearSolver.Direct'/>"
" <RequiredPlugin name='Sofa.Component.Engine.Select'/>"
" <RequiredPlugin name='Sofa.Component.Constraint.Projective'/>"
" <RequiredPlugin name='Sofa.Component.SolidMechanics.FEM.Elastic'/>"
" <RequiredPlugin name='Sofa.Component.MechanicalLoad'/>"
" <DefaultAnimationLoop />"
" <EulerImplicitSolver rayleighStiffness='0.' rayleighMass='0.0'/>"
" <SparseLDLSolver template='CompressedRowSparseMatrixd'/>"
" <Node name='Aligned' >"
" <MechanicalObject name='Mstate1' template='Rigid3' position='0 0 0 0 0 0 1' showObject='true' showObjectScale='0.1'/>"
" <UniformMass name='mass' vertexMass='300 0.0158 [0.0427 0.0 0.0 0.0 0.0427 0.0 0.0 0.0 0.00375]'/>"
" <ConstantForceField name='ConstantForceField1' forces='0 0 0 0 0 0'/>"
" </Node>"
" <Node name='Rotated' >"
" <MechanicalObject name='Mstate2' template='Rigid3' position='1 0 0 0 0 0 1' showObject='true' showObjectScale='0.1'/>"
" <UniformMass name='mass' vertexMass='300 0.0158 [0.0427 0.0 0.0 0.0 0.0427 0.0 0.0 0.0 0.00375]'/>"
" <ConstantForceField name='ConstantForceField2' forces='0 0 0 0 0 0'/>"
" </Node>"
"</Node>";



Node::SPtr root = SceneLoaderXML::loadFromMemory("loadWithNoParam", scene.c_str());
sofa::simulation::node::initRoot(root.get());

return root;
}

void nonIdentityInertiaMatrix_DifferentRotationDirection()
{
Node::SPtr root = generateRigidScene();
Rigid3Types::VecDeriv* CF1_force = reinterpret_cast<Rigid3Types::VecDeriv*>(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr());
Rigid3Types::VecDeriv* CF2_force = reinterpret_cast<Rigid3Types::VecDeriv*>(root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->beginEditVoidPtr());

(*CF1_force)[0][5] = 1.0;
(*CF2_force)[0][3] = 1.0;

root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->endEditVoidPtr();
root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->endEditVoidPtr();

auto mstate1 = root->getChild("Aligned")->getNodeObject<MechanicalObject<Rigid3Types>>();
auto mstate2 = root->getChild("Rotated")->getNodeObject<MechanicalObject<Rigid3Types>>();

sofa::simulation::node::animate(root.get(),50);

//Because the inertia is smaller along z, we expect different velocity after some times with a ratio equivalent to the inverse ratio between the inertia
EXPECT_GT(mstate2->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][3],0.0);
EXPECT_NEAR(mstate1->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][5] /
mstate2->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][3],
0.0427/0.00375, 1.0e-5 );


}

void nonIdentityInertiaMatrix_RotationOfOneRigid()
{
Node::SPtr root = generateRigidScene();

sofa::simulation::node::animate(root.get(),1);


auto mstate1 = root->getChild("Aligned")->getNodeObject<MechanicalObject<Rigid3Types>>();
auto mstate2 = root->getChild("Rotated")->getNodeObject<MechanicalObject<Rigid3Types>>();

//Rotate the rigid
mstate2->write(sofa::core::VecCoordId::position())->setValue({Rigid3Types::Coord(Rigid3Types::Coord::Pos(1,0,0),Rigid3Types::Coord::Rot (0.707106781,0,0.707106781,0))});

Rigid3Types::VecDeriv* CF1_force = reinterpret_cast<Rigid3Types::VecDeriv*>(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr());
Rigid3Types::VecDeriv* CF2_force = reinterpret_cast<Rigid3Types::VecDeriv*>(root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->beginEditVoidPtr());

//With rotated state, we now apply rotation along the Z axis of both rigids, this should result in the same acceleration if the inertia matrix is also rotated
(*CF1_force)[0][5] = 1.0;
(*CF2_force)[0][3] = 1.0;

root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->endEditVoidPtr();
root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->endEditVoidPtr();

sofa::simulation::node::animate(root.get(),50);
EXPECT_GT(mstate1->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][5],0.0);
EXPECT_GT(mstate2->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][3],0.0);
EXPECT_NEAR(mstate1->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][5] /
mstate2->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][3],
1.0, 1.0e-5 );
}

void nonIdentityInertiaMatrix_CentrifugalForces()
{
Node::SPtr root = generateRigidScene();

sofa::simulation::node::animate(root.get(), 1);

constexpr sofa::type::Vec3 zAxis(0, 0, 1);
auto * mstate1 = root->getChild("Aligned")->getNodeObject<MechanicalObject<Rigid3Types>>();
const auto * DataPos = mstate1->read(sofa::core::ConstVecId::position());
const auto * DataVel = mstate1->read(sofa::core::ConstVecDerivId::velocity());

Rigid3Types::VecDeriv* CF1_force = reinterpret_cast<Rigid3Types::VecDeriv*>(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr());

//We apply two different rotation, one exactly normal to z and one slightly along z
(*CF1_force)[0][3] = 1.0;
(*CF1_force)[0][5] = 0.1;

root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->endEditVoidPtr();

sofa::simulation::node::animate(root.get(),100);

//After rotating for some time, the centrifugal forces should have made the Z axis (along which most of the mass is located) normal to the axis of rotation
sofa::type::Vec3 Vel1 = DataVel->getValue()[0].getVOrientation();
sofa::type::Vec3 ori1_z = DataPos->getValue()[0].getOrientation().rotate(zAxis);
EXPECT_GT(norm(Vel1),0.0);
EXPECT_NEAR(dot(Vel1/norm(Vel1),ori1_z), 0.0, 1.0e-5 );

//To make sure the first try wasn't a fluke, we continue the rotation a bit and we check again
sofa::simulation::node::animate(root.get(),5);
Vel1 = DataVel->getValue()[0].getVOrientation();
ori1_z = DataPos->getValue()[0].getOrientation().rotate(zAxis);
EXPECT_GT(norm(Vel1),0.0);
EXPECT_NEAR(dot(Vel1/norm(Vel1),ori1_z), 0.0, 1.0e-5 );

//and again
sofa::simulation::node::animate(root.get(),5);
Vel1 = DataVel->getValue()[0].getVOrientation();
ori1_z = DataPos->getValue()[0].getOrientation().rotate(zAxis);
EXPECT_GT(norm(Vel1),0.0);
EXPECT_NEAR(dot(Vel1/norm(Vel1),ori1_z), 0.0, 1.0e-5 );

//and one final time
sofa::simulation::node::animate(root.get(),5);
Vel1 = DataVel->getValue()[0].getVOrientation();
ori1_z = DataPos->getValue()[0].getOrientation().rotate(zAxis);
EXPECT_GT(norm(Vel1),0.0);
EXPECT_NEAR(dot(Vel1/norm(Vel1),ori1_z), 0.0, 1.0e-5 );
}

};


Expand Down Expand Up @@ -493,3 +639,17 @@ TYPED_TEST(UniformMassTest, checkRigidAttribute) {
TYPED_TEST(UniformMassTest, reinitTest) {
//ASSERT_NO_THROW(this->reinitTest()) ;
}

TYPED_TEST(UniformMassTest, nonIdentityInertiaMatrix_DifferentRotationDirection){
EXPECT_NO_THROW(this->nonIdentityInertiaMatrix_DifferentRotationDirection());
}

TYPED_TEST(UniformMassTest, nonIdentityInertiaMatrix_RotationOfOneRigid){
EXPECT_NO_THROW(this->nonIdentityInertiaMatrix_RotationOfOneRigid());
}

TYPED_TEST(UniformMassTest, nonIdentityInertiaMatrix_CentrifugalForces){
EXPECT_NO_THROW(this->nonIdentityInertiaMatrix_CentrifugalForces());
}


Loading
Loading