diff --git a/CMakeLists.txt b/CMakeLists.txt index 151aa6c..e5db821 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -220,6 +220,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Component.SolidMechanics.FEM.E find_package(SofaPython3 QUIET) if(SofaPython3_FOUND) add_subdirectory(${SRC_DIR}/binding) + target_link_libraries(${PROJECT_NAME} PUBLIC SofaPython3::Plugin) # For the tests endif() ## Install rules for the library and headers; CMake package configurations files diff --git a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp index f76a696..89bae6d 100644 --- a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp +++ b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp @@ -91,12 +91,17 @@ using std::string; using sofa::type::Vec; using std::map; - namespace softrobotsinverse::solver { namespace { + +// QP solver options +#define QPOASES_OPT "qpOASES" +#define PROXQP_OPT "proxQP" +#define UNKNOWN_OPT "unknown" + template< typename TMultiVecId > void clearMultiVecId(BaseContext* ctx, const ConstraintParams* cParams, const TMultiVecId& vid) { @@ -130,7 +135,13 @@ QPInverseProblemSolver::QPInverseProblemSolver() , d_responseFriction(initData(&d_responseFriction, 0., "responseFriction", "Response friction for contact resolution")) - , d_qpSolver(initData(&d_qpSolver, "qpSolver", "QP solver implementation to be used")) + , d_qpSolver(initData(&d_qpSolver, +#if defined SOFTROBOTSINVERSE_ENABLE_PROXQP && !defined SOFTROBOTSINVERSE_ENABLE_QPOASES + {PROXQP_OPT , QPOASES_OPT}, // in that case, set proxQP as the default solver +#else + {QPOASES_OPT , PROXQP_OPT}, // let the user know about both options, even if only one is installed +#endif + "qpSolver", "QP solver implementation to be used.")) , d_epsilon(initData(&d_epsilon, 1e-3, "epsilon", "An energy term is added in the minimization process. \n" @@ -143,7 +154,7 @@ QPInverseProblemSolver::QPInverseProblemSolver() "If true, only for actuators." "Default value false.")) - , d_allowSliding(initData(&d_allowSliding,false,"allowSliding", + , d_allowSliding(initData(&d_allowSliding, false, "allowSliding", "In case of friction, this option enable/disable sliding contact.")) , d_graph(initData(&d_graph,"info","") ) @@ -158,20 +169,13 @@ QPInverseProblemSolver::QPInverseProblemSolver() , d_objective(initData(&d_objective, 250.0, "objective", "Erreur between the target and the end effector ")) - , m_lastCP(NULL) , m_CP1(nullptr) , m_CP2(nullptr) , m_CP3(nullptr) + , m_lastCP(nullptr) + , m_currentCP(nullptr) + , m_context(nullptr) { - sofa::helper::OptionsGroup qpSolvers{"qpOASES" , "proxQP"}; -#if defined SOFTROBOTSINVERSE_ENABLE_PROXQP && !defined SOFTROBOTSINVERSE_ENABLE_QPOASES - qpSolvers.setSelectedItem(QPSolverImpl::PROXQP); -#else - qpSolvers.setSelectedItem(QPSolverImpl::QPOASES); -#endif - - d_qpSolver.setValue(qpSolvers); - d_graph.setWidget("graph"); createProblems(); @@ -185,31 +189,52 @@ QPInverseProblemSolver::QPInverseProblemSolver() void QPInverseProblemSolver::createProblems() { - switch(d_qpSolver.getValue().getSelectedId()) + const auto qpSolver = sofa::helper::getReadAccessor(d_qpSolver); + + std::string selectedSolver = UNKNOWN_OPT; // Used to detect if the user has selected an uninstalled solver +#ifdef SOFTROBOTSINVERSE_ENABLE_PROXQP + if (qpSolver->getSelectedItem() == PROXQP_OPT) + selectedSolver = PROXQP_OPT; +#endif +#ifdef SOFTROBOTSINVERSE_ENABLE_QPOASES + if (qpSolver->getSelectedItem() == QPOASES_OPT) + selectedSolver = QPOASES_OPT; +#endif + + if(selectedSolver == UNKNOWN_OPT) { + if (d_qpSolver.hasDefaultValue()) + { + const auto defaultSolver = d_qpSolver.getDefaultValueString(); + msg_warning() << "Unknown or uninstalled specified solver: " << qpSolver->getSelectedItem() << ". Using the default solver instead: " << defaultSolver; + selectedSolver = defaultSolver; + } + } #ifdef SOFTROBOTSINVERSE_ENABLE_PROXQP - case QPSolverImpl::PROXQP : + if(selectedSolver == PROXQP_OPT) + { msg_info() << "Using proxQP solver"; m_CP1 = new module::QPInverseProblemProxQP(); m_CP2 = new module::QPInverseProblemProxQP(); m_CP3 = new module::QPInverseProblemProxQP(); - break; + } #endif #ifdef SOFTROBOTSINVERSE_ENABLE_QPOASES - case QPSolverImpl::QPOASES : + if(selectedSolver == QPOASES_OPT) + { msg_info() << "Using qpOASES solver"; m_CP1 = new module::QPInverseProblemQPOases(); m_CP2 = new module::QPInverseProblemQPOases(); m_CP3 = new module::QPInverseProblemQPOases(); - break; + } #endif - default : - msg_error() << "Unkown specified solver: " << d_qpSolver.getValue(); + + if (m_CP1 == nullptr) + { + msg_error() << "No installed qpSolver, the component cannot work."; sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - break; } - m_currentCP = m_CP1; } @@ -256,12 +281,12 @@ void QPInverseProblemSolver::init() VectorOperations vop(ExecParams::defaultInstance(), this->getContext()); { MultiVecDeriv lambda(&vop, m_lambdaId); - lambda.realloc(&vop,false,true); + lambda.realloc(&vop,false,true,sofa::core::VecIdProperties{"lambda", GetClass()->className}); m_lambdaId = lambda.id(); } { MultiVecDeriv dx(&vop, m_dxId); - dx.realloc(&vop,false,true); + dx.realloc(&vop,false,true,sofa::core::VecIdProperties{"constraint_dx", GetClass()->className}); m_dxId = dx.id(); } @@ -343,6 +368,9 @@ bool QPInverseProblemSolver::prepareStates(const ConstraintParams *cParams, Mult bool QPInverseProblemSolver::buildSystem(const ConstraintParams *cParams, MultiVecId res1, MultiVecId res2) { + if (sofa::core::objectmodel::BaseObject::d_componentState.getValue() == sofa::core::objectmodel::ComponentState::Invalid) + return false; + SOFA_UNUSED(res1); SOFA_UNUSED(res2); @@ -486,6 +514,8 @@ bool QPInverseProblemSolver::solveSystem(const ConstraintParams * cParams, MultiVecId res1, MultiVecId res2) { + if (sofa::core::objectmodel::BaseObject::d_componentState.getValue() == sofa::core::objectmodel::ComponentState::Invalid) + return false; SOFA_UNUSED(cParams); SOFA_UNUSED(res1); @@ -566,6 +596,9 @@ bool QPInverseProblemSolver::solveSystem(const ConstraintParams * cParams, void QPInverseProblemSolver::computeResidual(const ExecParams* eparam) { + if (sofa::core::objectmodel::BaseObject::d_componentState.getValue() == sofa::core::objectmodel::ComponentState::Invalid) + return; + for (unsigned int i=0; iconstOrder() == sofa::core::ConstraintOrder::POS_AND_VEL) diff --git a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h index 5409fbc..5799737 100644 --- a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h +++ b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h @@ -71,12 +71,6 @@ using sofa::core::MultiVecDerivId; class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl { public: - // List of availables QP solvers used to solve the inverse problem - enum QPSolverImpl - { - QPOASES = 0, - PROXQP - }; SOFA_CLASS(QPInverseProblemSolver, ConstraintSolverImpl); @@ -146,6 +140,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::componen sofa::Data d_maxIterations; sofa::Data d_tolerance; sofa::Data d_responseFriction; + sofa::Data d_qpSolver; sofa::Data d_epsilon; @@ -179,7 +174,6 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::componen virtual void createProblems(); void deleteProblems(); - private: void accumulateConstraint(const ConstraintParams *cParams, unsigned int & nbLinesTotal); void setConstraintProblemSize(const unsigned int &nbLinesTotal); diff --git a/tests/component/solver/QPInverseProblemImplTest.cpp b/tests/component/solver/QPInverseProblemImplTest.cpp index c8e3b52..8860fb8 100644 --- a/tests/component/solver/QPInverseProblemImplTest.cpp +++ b/tests/component/solver/QPInverseProblemImplTest.cpp @@ -1,31 +1,28 @@ -#include -using sofa::testing::BaseTest ; +#include #include using softrobotsinverse::solver::module::QPInverseProblemImpl ; -#include -using sofa::defaulttype::Vec3Types; - -using std::vector; - namespace softrobotsinverse::test { -template -struct QPInverseProblemImplTest : public BaseTest, QPInverseProblemImpl +struct QPInverseProblemImplTest : public ::testing::Test, QPInverseProblemImpl { typedef QPInverseProblemImpl ThisClass ; - typedef _DataTypes DataTypes; void solveInverseProblem(double &objective, sofa::type::vector &result, - sofa::type::vector &dual) override {} + sofa::type::vector &dual) override + { + SOFA_UNUSED(objective); + SOFA_UNUSED(result); + SOFA_UNUSED(dual); + } void isInTest() { - vector list; + std::vector list; list.push_back(1); list.push_back(3); @@ -146,35 +143,29 @@ struct QPInverseProblemImplTest : public BaseTest, QPInverseProblemImpl }; - -using ::testing::Types; -typedef Types DataTypes; - -TYPED_TEST_SUITE(QPInverseProblemImplTest, DataTypes); - -TYPED_TEST(QPInverseProblemImplTest, isInTest) { +TEST_F(QPInverseProblemImplTest, isInTest) { ASSERT_NO_THROW( this->isInTest() ); } -TYPED_TEST(QPInverseProblemImplTest, isCyclingOnPivotTest) { +TEST_F(QPInverseProblemImplTest, isCyclingOnPivotTest) { ASSERT_NO_THROW( this->isCyclingOnPivotTest() ); } -TYPED_TEST(QPInverseProblemImplTest, isCyclingOnSequenceTest) { +TEST_F(QPInverseProblemImplTest, isCyclingOnSequenceTest) { ASSERT_NO_THROW( this->isCyclingOnSequenceTest1() ); ASSERT_NO_THROW( this->isCyclingOnSequenceTest2() ); ASSERT_NO_THROW( this->isCyclingOnSequenceTest3() ); } -TYPED_TEST(QPInverseProblemImplTest, leaveSequenceUnchangedTest) { +TEST_F(QPInverseProblemImplTest, leaveSequenceUnchangedTest) { ASSERT_NO_THROW( this->leaveSequenceUnchangedTest() ); } -TYPED_TEST(QPInverseProblemImplTest, leaveCurrentSequenceUnchangedTest) { +TEST_F(QPInverseProblemImplTest, leaveCurrentSequenceUnchangedTest) { ASSERT_NO_THROW( this->leaveCurrentSequenceUnchangedTest() ); } -TYPED_TEST(QPInverseProblemImplTest, updateLambdaTest) { +TEST_F(QPInverseProblemImplTest, updateLambdaTest) { ASSERT_NO_THROW( this->updateLambdaTest() ); } diff --git a/tests/component/solver/QPInverseProblemSolverTest.cpp b/tests/component/solver/QPInverseProblemSolverTest.cpp index d475b22..a0df993 100644 --- a/tests/component/solver/QPInverseProblemSolverTest.cpp +++ b/tests/component/solver/QPInverseProblemSolverTest.cpp @@ -1,12 +1,11 @@ #include using std::string ; -#include -using sofa::testing::BaseTest ; + +#include #include #include #include - using sofa::helper::WriteAccessor ; using sofa::defaulttype::Vec3Types ; @@ -32,61 +31,32 @@ using std::stof; namespace sofa { -template -struct QPInverseProblemSolverTest : public BaseTest, - QPInverseProblemSolver +struct QPInverseProblemSolverTest : public ::testing::Test, QPInverseProblemSolver { typedef QPInverseProblemSolver ThisClass ; - typedef _DataTypes DataTypes; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::Real Real; - - - simulation::Node::SPtr m_root; ///< Root of the scene graph, created by the constructor an re-used in the tests - - - void normalTests() - { - Node::SPtr node = sofa::simulation::getSimulation()->createNewGraph("root"); - typename MechanicalObject::SPtr mecaobject = New >() ; - typename ThisClass::SPtr thisobject = New() ; - mecaobject->init() ; - - node->addObject(mecaobject) ; - node->addObject(thisobject) ; - - thisobject->setName("myname") ; - EXPECT_TRUE(thisobject->getName() == "myname") ; - - EXPECT_NO_THROW( thisobject->init() ) ; - - return ; - } + simulation::Node::SPtr m_root; ///< Root of the scene graph, created by the constructor an re-used in the tests - void doSetUp() override + void SetUp() { /// Load the scene string sceneName = "Finger.scn"; - string fileName = string(SOFTROBOTSINVERSE_TEST_DIR) + "/component/solver/scenes/" + sceneName; + m_root = sofa::simulation::node::load(fileName.c_str()); - m_root = core::objectmodel::SPtr_dynamic_cast( sofa::simulation::node::load(fileName.c_str())); - - /// Test if load has succeededls - simulation::SceneLoaderXML scene; - - if(!m_root || !scene.loadSucceed) + if(!m_root) ADD_FAILURE() << "Error while loading the scene: " << sceneName << std::endl; } + void normalTests() + { + EXPECT_NO_THROW(sofa::simulation::node::initRoot(m_root.get())); + return ; + } // Test the behavior of the algorithm void behaviorTests(const std::string& qpSolver) { - helper::system::TemporaryLocale locale(LC_NUMERIC, "C"); - sofa::simulation::node::initRoot(m_root.get()); int nbTimeStep = 10; @@ -146,15 +116,7 @@ struct QPInverseProblemSolverTest : public BaseTest, sofa::simulation::node::animate(m_root.get()); lambdaString = m_root->getChild("finger")->getChild("controlledPoints")->getObject("cable")->findData("force")->getValueString(); - // Don't know why proxQP has a very small primal residual here instead - if(qpSolver == "proxQP") - { - EXPECT_GE( stof(lambdaString.c_str()), -DBL_EPSILON); - } - else - { - EXPECT_GE( stof(lambdaString.c_str()), 0.); - } + EXPECT_GE( stof(lambdaString.c_str()), -DBL_EPSILON); // State2: Test lambda >= -20 m_root->getChild("goal")->getObject("goalMO")->findData("position")->read("-110 -10 7.5"); @@ -212,36 +174,28 @@ struct QPInverseProblemSolverTest : public BaseTest, }; - - -using ::testing::Types; -typedef Types DataTypes; - -TYPED_TEST_SUITE(QPInverseProblemSolverTest, DataTypes); - -TYPED_TEST(QPInverseProblemSolverTest, normalTests) { +TEST_F(QPInverseProblemSolverTest, normalTests) { ASSERT_NO_THROW( this->normalTests() ); } -#ifdef SOFTROBOTSINVERSE_ENABLE_QPOASES -TYPED_TEST(QPInverseProblemSolverTest, regressionTestsQpOASES) { +// We should always install at least one solver +// and the scene should not crash if we have selected an uninstalled solver + +TEST_F(QPInverseProblemSolverTest, regressionTestsQpOASES) { ASSERT_NO_THROW( this->regressionTests("qpOASES") ); } -TYPED_TEST(QPInverseProblemSolverTest, behaviorTestsQpOASES) { +TEST_F(QPInverseProblemSolverTest, behaviorTestsQpOASES) { ASSERT_NO_THROW( this->behaviorTests("qpOASES") ); } -#endif -#ifdef SOFTROBOTSINVERSE_ENABLE_PROXQP -TYPED_TEST(QPInverseProblemSolverTest, regressionTestsProxQP) { +TEST_F(QPInverseProblemSolverTest, regressionTestsProxQP) { ASSERT_NO_THROW( this->regressionTests("proxQP") ); } -TYPED_TEST(QPInverseProblemSolverTest, behaviorTestsProxQP) { +TEST_F(QPInverseProblemSolverTest, behaviorTestsProxQP) { ASSERT_NO_THROW( this->behaviorTests("proxQP") ); } -#endif } diff --git a/tests/component/solver/QPInverseProblemSolverWithContactTest.cpp b/tests/component/solver/QPInverseProblemSolverWithContactTest.cpp index 07dbb3d..a3f21de 100644 --- a/tests/component/solver/QPInverseProblemSolverWithContactTest.cpp +++ b/tests/component/solver/QPInverseProblemSolverWithContactTest.cpp @@ -5,25 +5,19 @@ using std::string ; #include #include -#include -using sofa::simulation::SceneLoaderPY ; +#include +#include +using sofapython3::SceneLoaderPY3 ; -#include +#include using sofa::simulation::Simulation ; +#include using sofa::simulation::Node ; -using sofa::simulation::setSimulation ; using sofa::core::objectmodel::New ; using sofa::core::objectmodel::BaseData ; -#include "component/solver/QPInverseProblemSolver.h" -using sofa::component::constraintset::QPInverseProblemSolver; - -using sofa::helper::vector; -using sofa::helper::rabs; -using std::stof; -using std::istringstream; - -#include +#include +using softrobotsinverse::solver::QPInverseProblemSolver; namespace sofa @@ -36,19 +30,16 @@ struct QPInverseProblemSolverWithContactTest : public ::testing::Test, QPInvers typedef QPInverseProblemSolver ThisClass ; simulation::Node::SPtr m_root; ///< Root of the scene graph, created by the constructor an re-used in the tests - simulation::Simulation* m_simulation; ///< created by the constructor an re-used in the tests - void SetUp() { - sofa::simulation::PythonEnvironment::Init(); + sofapython3::PythonEnvironment::Init(); // sofa init - static const string scenePath = string(SOFTROBOTS_TEST_DIR) + std::string("/component/solver/scenes/CubeOnFloor.pyscn"); - sofa::simulation::setSimulation(m_simulation = new sofa::simulation::graph::DAGSimulation()); - // load scene - m_root = simulation::getSimulation()->load(scenePath.c_str()); - simulation::getSimulation()->init(m_root.get()); + static const string scenePath = string(SOFTROBOTSINVERSE_TEST_DIR) + std::string("/component/solver/scenes/CubeOnFloor.pyscn"); + m_root = sofa::simulation::node::load(scenePath.c_str()); + + sofa::simulation::node::initRoot(m_root.get()); if(!m_root) ADD_FAILURE() << "Error while loading the scene: " << scenePath << std::endl; @@ -58,19 +49,18 @@ struct QPInverseProblemSolverWithContactTest : public ::testing::Test, QPInvers void testFallingCube() { helper::system::TemporaryLocale locale(LC_NUMERIC, "C"); - m_simulation->reset(m_root.get()); + sofa::simulation::node::reset(m_root.get()); int nbTimeStep = 10; m_root->getObject("QPInverseProblemSolver")->findData("epsilon")->read("0.0"); - m_root->getObject("CollisionResponse")->findData("responseParams")->read("mu=0.0"); + m_root->getObject("RuleBasedContactManager")->findData("responseParams")->read("mu=0.0"); m_root->getObject("QPInverseProblemSolver")->findData("responseFriction")->read("0.0"); - //The floor should stop the cube from falling (gravity allong -y direction) m_root->getChild("goal")->getObject("goalMO")->findData("position")->read("10 0 0"); for(int i=0; ianimate(m_root.get()); + sofa::simulation::node::animate(m_root.get()); string posString = m_root->getChild("model")->getChild("effector")->getObject("effectorPoint")->findData("position")->getValueString(); float x, y, z; @@ -82,18 +72,18 @@ struct QPInverseProblemSolverWithContactTest : public ::testing::Test, QPInvers void testFallingCubeWithFriction() { helper::system::TemporaryLocale locale(LC_NUMERIC, "C"); - m_simulation->reset(m_root.get()); + sofa::simulation::node::reset(m_root.get()); int nbTimeStep = 10; m_root->getObject("QPInverseProblemSolver")->findData("epsilon")->read("0.0"); - m_root->getObject("CollisionResponse")->findData("responseParams")->read("mu=0.6"); + m_root->getObject("RuleBasedContactManager")->findData("responseParams")->read("mu=0.6"); m_root->getObject("QPInverseProblemSolver")->findData("responseFriction")->read("0.6"); //The floor should stop the cube from falling (gravity allong -y direction) m_root->getChild("goal")->getObject("goalMO")->findData("position")->read("10 0 0"); for(int i=0; ianimate(m_root.get()); + sofa::simulation::node::animate(m_root.get()); string posString = m_root->getChild("model")->getChild("effector")->getObject("effectorPoint")->findData("position")->getValueString(); float x, y, z; @@ -111,32 +101,32 @@ struct QPInverseProblemSolverWithContactTest : public ::testing::Test, QPInvers //The force required to move the cube should be higher with friction m_root->getChild("goal")->getObject("goalMO")->findData("position")->read("11 0 0"); - m_root->getObject("CollisionResponse")->findData("responseParams")->read("mu=0.0"); + m_root->getObject("RuleBasedContactManager")->findData("responseParams")->read("mu=0.0"); m_root->getObject("QPInverseProblemSolver")->findData("responseFriction")->read("0.0"); for(int i=0; ianimate(m_root.get()); + sofa::simulation::node::animate(m_root.get()); string force1 = m_root->getChild("model")->getObject("act")->findData("force")->getValueString(); - m_simulation->reset(m_root.get()); + sofa::simulation::node::reset(m_root.get()); m_root->getChild("goal")->getObject("goalMO")->findData("position")->read("11 0 0"); - m_root->getObject("CollisionResponse")->findData("responseParams")->read("mu=0.5"); + m_root->getObject("RuleBasedContactManager")->findData("responseParams")->read("mu=0.5"); m_root->getObject("QPInverseProblemSolver")->findData("responseFriction")->read("0.5"); for(int i=0; ianimate(m_root.get()); + sofa::simulation::node::animate(m_root.get()); string force2 = m_root->getChild("model")->getObject("act")->findData("force")->getValueString(); - m_simulation->reset(m_root.get()); + sofa::simulation::node::reset(m_root.get()); m_root->getChild("goal")->getObject("goalMO")->findData("position")->read("11 0 0"); - m_root->getObject("CollisionResponse")->findData("responseParams")->read("mu=1."); + m_root->getObject("RuleBasedContactManager")->findData("responseParams")->read("mu=1."); m_root->getObject("QPInverseProblemSolver")->findData("responseFriction")->read("1."); for(int i=0; ianimate(m_root.get()); + sofa::simulation::node::animate(m_root.get()); string force3 = m_root->getChild("model")->getObject("act")->findData("force")->getValueString(); EXPECT_LE(stof(force1),stof(force2)); @@ -149,9 +139,9 @@ struct QPInverseProblemSolverWithContactTest : public ::testing::Test, QPInvers int nbTimeStep = 25; - m_simulation->reset(m_root.get()); + sofa::simulation::node::reset(m_root.get()); m_root->getObject("QPInverseProblemSolver")->findData("allowSliding")->read("1"); // Enable sliding - m_root->getObject("CollisionResponse")->findData("responseParams")->read("mu=0.8"); + m_root->getObject("RuleBasedContactManager")->findData("responseParams")->read("mu=0.8"); m_root->getObject("QPInverseProblemSolver")->findData("responseFriction")->read("0.8"); @@ -160,7 +150,7 @@ struct QPInverseProblemSolverWithContactTest : public ::testing::Test, QPInvers { position += 2./nbTimeStep*i; m_root->getChild("goal")->getObject("goalMO")->findData("position")->read(std::to_string(position)+" 0 0"); - m_simulation->animate(m_root.get()); + sofa::simulation::node::animate(m_root.get()); } string deltaString = m_root->getChild("model")->getChild("effector")->getObject("PositionEffector")->findData("delta")->getValueString(); @@ -168,15 +158,15 @@ struct QPInverseProblemSolverWithContactTest : public ::testing::Test, QPInvers getValueFromString(deltaString, x, y, z); double norm1 = sqrt(x*x+y*y+z*z); - m_simulation->reset(m_root.get()); + sofa::simulation::node::reset(m_root.get()); m_root->getObject("QPInverseProblemSolver")->findData("allowSliding")->read("0"); // Disable sliding m_root->getChild("goal")->getObject("goalMO")->findData("position")->read("12 0 0"); - m_root->getObject("CollisionResponse")->findData("responseParams")->read("mu=0.8"); + m_root->getObject("RuleBasedContactManager")->findData("responseParams")->read("mu=0.8"); m_root->getObject("QPInverseProblemSolver")->findData("responseFriction")->read("0.8"); for(int i=0; ianimate(m_root.get()); + sofa::simulation::node::animate(m_root.get()); deltaString = m_root->getChild("model")->getChild("effector")->getObject("PositionEffector")->findData("delta")->getValueString(); getValueFromString(deltaString, x, y, z); @@ -188,8 +178,8 @@ struct QPInverseProblemSolverWithContactTest : public ::testing::Test, QPInvers void getValueFromString(const string positionStr, float& x, float& y, float& z) { - istringstream iss(positionStr); - string xStr, yStr, zStr; + std::istringstream iss(positionStr); + std::string xStr, yStr, zStr; iss >> xStr; iss >> yStr; iss >> zStr; diff --git a/tests/component/solver/SolverTest.cmake b/tests/component/solver/SolverTest.cmake index b6db6fd..6f7911d 100644 --- a/tests/component/solver/SolverTest.cmake +++ b/tests/component/solver/SolverTest.cmake @@ -7,7 +7,7 @@ list(APPEND SOURCE_FILES ) -if(PLUGIN_SOFAPYTHON) +if(PLUGIN_SOFAPYTHON3) list(APPEND SOURCE_FILES component/solver/QPInverseProblemSolverWithContactTest.cpp ) diff --git a/tests/component/solver/scenes/CubeOnFloor.pyscn b/tests/component/solver/scenes/CubeOnFloor.pyscn index f7439bf..8963afa 100644 --- a/tests/component/solver/scenes/CubeOnFloor.pyscn +++ b/tests/component/solver/scenes/CubeOnFloor.pyscn @@ -2,79 +2,87 @@ import Sofa def createScene(rootNode): - rootNode.addObject('RequiredPlugin', pluginName='SoftRobots') - rootNode.addObject('RequiredPlugin', pluginName='SoftRobots.Inverse') - rootNode.addObject('RequiredPlugin', pluginName='SofaSparseSolver') - rootNode.addObject('VisualStyle', displayFlags='showWireframe showVisualModels showBehaviorModels showCollisionModels hideBoundingCollisionModels showForceFields showInteractionForceFields') - - rootNode.findData('gravity').value = [0, -9180, 0] - rootNode.findData('dt').value = 0.01 - - rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('QPInverseProblemSolver', epsilon=0, maxIterations=1000, tolerance=1e-5) - rootNode.addObject('CollisionPipeline') - rootNode.addObject('BruteForceBroadPhase') - rootNode.addObject('BVHNarrowPhase') - rootNode.addObject('CollisionResponse', response="FrictionContactConstraint", responseParams="mu=0.6") - rootNode.addObject('LocalMinDistance', alarmDistance=3, contactDistance=1) - - rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight") - - - ########################################## - # Effector goal for interactive control # - ########################################## - goal = rootNode.addChild('goal') - goal.addObject('MechanicalObject', name='goalMO', showObject=True, showObjectScale=5, drawMode=1, position=[10, 0, 0]) - - - ########################################## - # FEM Model # - ########################################## - model = rootNode.addChild('model') - model.addObject('EulerImplicitSolver') - model.addObject('SparseLDLSolver') - model.addObject('RegularGridTopology', name='loader', n=[6, 6, 6], min=[-10, -10, -10], max=[10, 10, 10]) - model.addObject('MechanicalObject', name='tetras', template='Vec3', showIndices=False, showIndicesScale=4e-5) - model.addObject('UniformMass', totalMass=0.005) - - model.addObject('ForcePointActuator', name="act", template='Vec3', showForce=True, visuScale=100, direction=[1, 0, 0], indices=[90, 126, 84, 120], minForce=0, maxForce=0.5) - - model.addObject('HexahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.49, youngModulus=600) - model.addObject('LinearSolverConstraintCorrection') - - - ########################################## - # Effector # - ########################################## - effector = model.addChild('effector') - effector.addObject('MechanicalObject', name="effectorPoint", position=[10, 0, 0]) - effector.addObject('PositionEffector', template='Vec3', indices=[0], effectorGoal=goal.goalMO.position.linkpath) - effector.addObject('BarycentricMapping', mapForces=False, mapMasses=False) - - - ########################################## - # Contact # - ########################################## - modelContact = model.addChild('contact') - modelContact.addObject('MeshObjLoader', name='loader', filename='mesh/cube.obj', scale=10) - modelContact.addObject('MeshTopology', src='@loader', name='topo') - modelContact.addObject('MechanicalObject') - modelContact.addObject('TriangleCollisionModel', group=1) - modelContact.addObject('LineCollisionModel', group=1) - modelContact.addObject('PointCollisionModel', group=1) - modelContact.addObject('BarycentricMapping') - - - ########################################## - # Contact # - ########################################## - floorContact = rootNode.addChild('floorContact') - floorContact.addObject('MeshObjLoader', name='loader', filename='mesh/square1.obj', scale=200, rotation=[90, 0, 180], translation=[100, -11, -100]) - floorContact.addObject('MeshTopology', src='@loader', name='topo') - floorContact.addObject('MechanicalObject') - floorContact.addObject('TriangleCollisionModel', group=2) - floorContact.addObject('LineCollisionModel', group=2) - floorContact.addObject('PointCollisionModel', group=2) - - return rootNode + rootNode.addObject('RequiredPlugin', pluginName='SoftRobots') + rootNode.addObject('RequiredPlugin', pluginName='SoftRobots.Inverse') + rootNode.addObject('RequiredPlugin', pluginName=['Sofa.Component.Visual', 'Sofa.GL.Component.Rendering3D', + 'Sofa.Component.Topology.Container.Grid', 'Sofa.Component.Topology.Container.Constant', + 'Sofa.Component.AnimationLoop', 'Sofa.Component.Collision.Response.Contact', + 'Sofa.Component.Collision.Detection.Algorithm', 'Sofa.Component.ODESolver.Backward', + 'Sofa.Component.LinearSolver.Direct', 'Sofa.Component.Mass', + 'Sofa.Component.Constraint.Lagrangian.Correction', 'Sofa.Component.Mapping.Linear', + 'Sofa.Component.IO.Mesh', 'Sofa.Component.Collision.Geometry', + 'Sofa.Component.Collision.Detection.Intersection',]) + + rootNode.addObject('VisualStyle', displayFlags='showWireframe showVisualModels showBehaviorModels showCollisionModels hideBoundingCollisionModels showForceFields showInteractionForceFields') + + rootNode.findData('gravity').value = [0, -9180, 0] + rootNode.findData('dt').value = 0.01 + + rootNode.addObject('FreeMotionAnimationLoop') + rootNode.addObject('QPInverseProblemSolver', epsilon=0, maxIterations=1000, tolerance=1e-5) + rootNode.addObject('CollisionPipeline') + rootNode.addObject('BruteForceBroadPhase') + rootNode.addObject('BVHNarrowPhase') + rootNode.addObject('RuleBasedContactManager', response="FrictionContactConstraint", responseParams="mu=0.6") + rootNode.addObject('LocalMinDistance', alarmDistance=3, contactDistance=1) + + rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight") + + + ########################################## + # Effector goal for interactive control # + ########################################## + goal = rootNode.addChild('goal') + goal.addObject('MechanicalObject', name='goalMO', showObject=True, showObjectScale=5, drawMode=1, position=[10, 0, 0]) + + + ########################################## + # FEM Model # + ########################################## + model = rootNode.addChild('model') + model.addObject('EulerImplicitSolver') + model.addObject('SparseLDLSolver', template="CompressedRowSparseMatrixd") + model.addObject('RegularGridTopology', name='loader', n=[6, 6, 6], min=[-10, -10, -10], max=[10, 10, 10]) + model.addObject('MechanicalObject', name='tetras', template='Vec3', showIndices=False, showIndicesScale=4e-5) + model.addObject('UniformMass', totalMass=0.005) + + model.addObject('ForcePointActuator', name="act", template='Vec3', showForce=True, visuScale=100, direction=[1, 0, 0], indices=[90, 126, 84, 120], minForce=0, maxForce=0.5) + + model.addObject('HexahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.49, youngModulus=600) + model.addObject('LinearSolverConstraintCorrection') + + + ########################################## + # Effector # + ########################################## + effector = model.addChild('effector') + effector.addObject('MechanicalObject', name="effectorPoint", position=[10, 0, 0]) + effector.addObject('PositionEffector', template='Vec3', indices=[0], effectorGoal=goal.goalMO.position.linkpath) + effector.addObject('BarycentricMapping', mapForces=False, mapMasses=False) + + + ########################################## + # Contact # + ########################################## + modelContact = model.addChild('contact') + modelContact.addObject('MeshOBJLoader', name='loader', filename='mesh/cube.obj', scale=10) + modelContact.addObject('MeshTopology', src='@loader', name='topo') + modelContact.addObject('MechanicalObject') + modelContact.addObject('TriangleCollisionModel', group=1) + modelContact.addObject('LineCollisionModel', group=1) + modelContact.addObject('PointCollisionModel', group=1) + modelContact.addObject('BarycentricMapping') + + + ########################################## + # Contact # + ########################################## + floorContact = rootNode.addChild('floorContact') + floorContact.addObject('MeshOBJLoader', name='loader', filename='mesh/square1.obj', scale=200, rotation=[90, 0, 180], translation=[100, -11, -100]) + floorContact.addObject('MeshTopology', src='@loader', name='topo') + floorContact.addObject('MechanicalObject') + floorContact.addObject('TriangleCollisionModel', group=2) + floorContact.addObject('LineCollisionModel', group=2) + floorContact.addObject('PointCollisionModel', group=2) + + return rootNode