From a931c269e56590f74f0e987266177337f6858204 Mon Sep 17 00:00:00 2001 From: EulalieCoevoet Date: Fri, 19 Dec 2025 15:01:49 +0100 Subject: [PATCH 1/4] [solver-constraint] rename epsilon by energyWeight --- .../BarycentricCenterEffector_Rigid.py | 2 +- .../constraint/CableEquality/CableEquality.py | 2 +- .../constraint/ForcePointActuator/Tissues.py | 2 +- .../ForceSurfaceActuator.py | 2 +- .../constraint/JointActuator/JointActuator.py | 2 +- .../SlidingActuator/actuatedBeam.py | 2 +- .../SurfacePressureEquality.py | 2 +- .../component/constraint/ForcePointActuator.h | 8 ++-- .../constraint/ForcePointActuator.inl | 24 +++++++--- .../solver/QPInverseProblemSolver.cpp | 48 +++++++++---------- .../component/solver/QPInverseProblemSolver.h | 8 ++-- .../solver/modules/QPInverseProblem.cpp | 2 +- .../solver/modules/QPInverseProblem.h | 4 +- .../solver/modules/QPInverseProblemImpl.cpp | 28 ++++++----- .../solver/modules/QPInverseProblemImpl.h | 2 +- .../modules/QPInverseProblemQPOases.cpp | 6 +-- .../solver/QPInverseProblemSolverTest.cpp | 2 +- .../QPInverseProblemSolverWithContactTest.cpp | 4 +- .../component/solver/scenes/CubeOnFloor.pyscn | 2 +- 19 files changed, 84 insertions(+), 68 deletions(-) diff --git a/examples/sofapython3/component/constraint/BarycentricCenterEffector/BarycentricCenterEffector_Rigid.py b/examples/sofapython3/component/constraint/BarycentricCenterEffector/BarycentricCenterEffector_Rigid.py index e7e09c4..24f0820 100644 --- a/examples/sofapython3/component/constraint/BarycentricCenterEffector/BarycentricCenterEffector_Rigid.py +++ b/examples/sofapython3/component/constraint/BarycentricCenterEffector/BarycentricCenterEffector_Rigid.py @@ -21,7 +21,7 @@ def createScene(rootNode): rootNode.findData('dt').value = 0.01 rootNode.findData('gravity').value = [0, -9810, 0] rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('QPInverseProblemSolver', epsilon=1e-3, tolerance=1e-8, maxIterations=2500) + rootNode.addObject('QPInverseProblemSolver', energyWeight=1e-3, tolerance=1e-8, maxIterations=2500) ########################################## # goal diff --git a/examples/sofapython3/component/constraint/CableEquality/CableEquality.py b/examples/sofapython3/component/constraint/CableEquality/CableEquality.py index 6c1c322..e7b8119 100644 --- a/examples/sofapython3/component/constraint/CableEquality/CableEquality.py +++ b/examples/sofapython3/component/constraint/CableEquality/CableEquality.py @@ -15,7 +15,7 @@ def createScene(rootNode): rootNode.findData('gravity').value = [0, 0, 0] rootNode.findData('dt').value = 0.02 rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('QPInverseProblemSolver', printLog=0, epsilon=1e-3, maxIterations=1000, tolerance=1e-4) + rootNode.addObject('QPInverseProblemSolver', printLog=0, energyWeight=1e-3, maxIterations=1000, tolerance=1e-4) VolumetricMeshPath = MeshesPath + 'Accordeon_Volumetric.vtk' SurfaceMeshPath = MeshesPath + 'Accordeon_Surface.stl' diff --git a/examples/sofapython3/component/constraint/ForcePointActuator/Tissues.py b/examples/sofapython3/component/constraint/ForcePointActuator/Tissues.py index c6763c3..02f0131 100644 --- a/examples/sofapython3/component/constraint/ForcePointActuator/Tissues.py +++ b/examples/sofapython3/component/constraint/ForcePointActuator/Tissues.py @@ -48,7 +48,7 @@ def createScene(rootNode): rootNode.dt = 0.02 rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('QPInverseProblemSolver', printLog=False, epsilon=1e-3, tolerance=1e-4, maxIterations=2500) + rootNode.addObject('QPInverseProblemSolver', printLog=False, energyWeight=1e-3, tolerance=1e-4, maxIterations=2500) rootNode.addObject('CollisionPipeline') rootNode.addObject('BruteForceBroadPhase') rootNode.addObject('BVHNarrowPhase') diff --git a/examples/sofapython3/component/constraint/ForceSurfaceActuator/ForceSurfaceActuator.py b/examples/sofapython3/component/constraint/ForceSurfaceActuator/ForceSurfaceActuator.py index 6bc7813..e395429 100644 --- a/examples/sofapython3/component/constraint/ForceSurfaceActuator/ForceSurfaceActuator.py +++ b/examples/sofapython3/component/constraint/ForceSurfaceActuator/ForceSurfaceActuator.py @@ -21,7 +21,7 @@ def createScene(rootNode): rootNode.findData('dt').value = 0.02 rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('QPInverseProblemSolver', epsilon=1e-3, tolerance=1e-2, maxIterations=500) + rootNode.addObject('QPInverseProblemSolver', energyWeight=1e-3, tolerance=1e-2, maxIterations=500) ########################################## # Effector goal for interactive control # diff --git a/examples/sofapython3/component/constraint/JointActuator/JointActuator.py b/examples/sofapython3/component/constraint/JointActuator/JointActuator.py index d21e81c..9f0dc14 100644 --- a/examples/sofapython3/component/constraint/JointActuator/JointActuator.py +++ b/examples/sofapython3/component/constraint/JointActuator/JointActuator.py @@ -34,7 +34,7 @@ def createScene(rootNode): rootNode.addObject('VisualStyle', displayFlags='showBehaviorModels') rootNode.addObject('BackgroundSetting', color=[0, 0.168627, 0.211765, 1.]) rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('QPInverseProblemSolver', epsilon=0) + rootNode.addObject('QPInverseProblemSolver', energyWeight=0) # Target position of the end effector goal = rootNode.addChild('Goal') diff --git a/examples/sofapython3/component/constraint/SlidingActuator/actuatedBeam.py b/examples/sofapython3/component/constraint/SlidingActuator/actuatedBeam.py index 857c878..813b2f0 100644 --- a/examples/sofapython3/component/constraint/SlidingActuator/actuatedBeam.py +++ b/examples/sofapython3/component/constraint/SlidingActuator/actuatedBeam.py @@ -35,7 +35,7 @@ def createScene(rootNode): 'showForceFields showInteractionForceFields') rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('QPInverseProblemSolver', epsilon=0.001, printLog=False, tolerance=1e-10, maxIterations=1000) + rootNode.addObject('QPInverseProblemSolver', energyWeight=0.001, printLog=False, tolerance=1e-10, maxIterations=1000) ######################################### # Goal for end effector # diff --git a/examples/sofapython3/component/constraint/SurfacePressureEquality/SurfacePressureEquality.py b/examples/sofapython3/component/constraint/SurfacePressureEquality/SurfacePressureEquality.py index db9eba8..c078c9b 100644 --- a/examples/sofapython3/component/constraint/SurfacePressureEquality/SurfacePressureEquality.py +++ b/examples/sofapython3/component/constraint/SurfacePressureEquality/SurfacePressureEquality.py @@ -15,7 +15,7 @@ def createScene(rootNode): rootNode.findData('gravity').value = [0, 0, 0] rootNode.findData('dt').value = 0.02 rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('QPInverseProblemSolver', printLog=0, epsilon=1e-1, maxIterations=1000, tolerance=1e-5) + rootNode.addObject('QPInverseProblemSolver', printLog=0, energyWeight=1e-1, maxIterations=1000, tolerance=1e-5) rootNode.addObject('BackgroundSetting', color=[1, 1, 1, 1]) rootNode.addObject('LightManager') diff --git a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h index 38c8ca6..5afd156 100644 --- a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h +++ b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h @@ -105,7 +105,9 @@ class ForcePointActuator : public Actuator sofa::Data> d_force; sofa::Data d_displacement; sofa::Data d_direction; - sofa::Data d_epsilon; + sofa::Data d_energyWeight; + SOFA_ATTRIBUTE_DEPRECATED("v25.12", "v26.12", "Use d_energyWeight instead.") + sofa::Data d_penalty; // Used to deprecate the name of the data from the UI sofa::Data d_showForce; sofa::Data d_visuScale; @@ -134,11 +136,11 @@ class ForcePointActuator : public Actuator using Actuator::m_lambdaMax ; using Actuator::m_lambdaMin ; using Actuator::m_lambdaInit ; - using Actuator::m_epsilon ; + using Actuator::m_energyWeight ; using Actuator::m_hasLambdaMax ; using Actuator::m_hasLambdaMin ; using Actuator::m_hasLambdaInit ; - using Actuator::m_hasEpsilon ; + using Actuator::m_hasEnergyWeight ; using Actuator::m_nbLines ; //////////////////////////////////////////////////////////////////////////// diff --git a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl index 0f803d7..4ee8e0b 100644 --- a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl +++ b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl @@ -72,9 +72,12 @@ ForcePointActuator::ForcePointActuator(MechanicalState* object) "Direction of the force we want to apply. If d=[0,0,0], the direction \n" "will be optimized.")) - , d_epsilon(initData(&d_epsilon, Real(1e-3), "penalty", - "Use this value to prioritize the constraint. 0 means no limitation on the energy \n" - "transfered by this actuator. Default is 1e-3.")) + , d_energyWeight(initData(&d_energyWeight, Real(1e-3), "energyWeight", + "An energy term is added in the minimization process. \n" + "Specify the energy weight of the constraint. 0 means no limitation on the energy \n" + "transfered by this actuator. The default value used is the energyWeight defined in the inverse problem solver.")) + + , d_penalty(initData(&d_penalty, Real(1e-3), "penalty", "")) , d_showForce(initData(&d_showForce, false, "showForce", "")) @@ -90,6 +93,8 @@ ForcePointActuator::ForcePointActuator(MechanicalState* object) template void ForcePointActuator::setUpData() { + d_penalty.setDisplayed(false); + d_force.setReadOnly(true); d_displacement.setReadOnly(true); @@ -115,6 +120,13 @@ void ForcePointActuator::init() "To remove this error message fix your scene possibly by " "adding a MechanicalObject." ; + if (d_penalty.isSet()) + { + msg_deprecated() << "The data penalty is deprecated. To fix your scene please use energyWeight instead. It will be removed in v26.12."; + const auto& penalty = sofa::helper::getReadAccessor(d_penalty); + d_energyWeight.setValue(penalty); + } + initData(); initLimit(); } @@ -132,10 +144,10 @@ void ForcePointActuator::initData() { m_dim = (d_direction.getValue().norm()<1e-10)? Deriv::total_size: 1; - if(d_epsilon.isSet()) + if(d_energyWeight.isSet()) { - m_hasEpsilon = true; - m_epsilon = d_epsilon.getValue(); + m_hasEnergyWeight = true; + m_energyWeight = d_energyWeight.getValue(); } if(d_initForce.isSet()) diff --git a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp index f76a696..4b826b8 100644 --- a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp +++ b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp @@ -132,16 +132,19 @@ QPInverseProblemSolver::QPInverseProblemSolver() , d_qpSolver(initData(&d_qpSolver, "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" - "Epsilon has to be chosen sufficiently small so that the deformation \n" - "energy does not disrupt the quality of the effector positioning. " - "Default value 1e-3.")) + , d_energyWeight(initData(&d_energyWeight, 1e-3, "energyWeight", + "An energy term is added in the minimization process. \n" + "This is the weight of this term. \n" + "It has to be chosen sufficiently small so that the deformation \n" + "energy does not disrupt the quality of the effector positioning. " + "Default value 1e-3.")) + + , d_epsilon(initData(&d_epsilon, 1e-3, "epsilon", "")) , d_actuatorsOnly(initData(&d_actuatorsOnly, false, "actuatorsOnly", - "An energy term is added in the minimization process. \n" - "If true, only for actuators." - "Default value false.")) + "An energy term is added in the minimization process. \n" + "If true, add it only for actuators." + "Default value false.")) , d_allowSliding(initData(&d_allowSliding,false,"allowSliding", "In case of friction, this option enable/disable sliding contact.")) @@ -156,12 +159,12 @@ QPInverseProblemSolver::QPInverseProblemSolver() "If set, will contraints the sum of contact forces \n" "to be lesser or equal to the given value.") ) - , d_objective(initData(&d_objective, 250.0, "objective", "Erreur between the target and the end effector ")) + , d_objective(initData(&d_objective, 0.0, "objective", "Calculated optimal objective function value.")) - , m_lastCP(NULL) , m_CP1(nullptr) , m_CP2(nullptr) , m_CP3(nullptr) + , m_lastCP(nullptr) { sofa::helper::OptionsGroup qpSolvers{"qpOASES" , "proxQP"}; #if defined SOFTROBOTSINVERSE_ENABLE_PROXQP && !defined SOFTROBOTSINVERSE_ENABLE_QPOASES @@ -170,6 +173,8 @@ QPInverseProblemSolver::QPInverseProblemSolver() qpSolvers.setSelectedItem(QPSolverImpl::QPOASES); #endif + d_objective.setReadOnly(true); + d_qpSolver.setValue(qpSolvers); d_graph.setWidget("graph"); @@ -181,6 +186,14 @@ QPInverseProblemSolver::QPInverseProblemSolver() deleteProblems(); createProblems(); }); + + d_epsilon.setDisplayed(false); + if (d_epsilon.isSet()) + { + msg_deprecated() << "The data epsilon is deprecated. To fix your scene please use energyWeight instead. It will be removed in v26.12."; + const auto& epsilon = sofa::helper::getReadAccessor(d_epsilon); + d_energyWeight.setValue(epsilon); + } } void QPInverseProblemSolver::createProblems() @@ -469,19 +482,6 @@ inline void QPInverseProblemSolver::buildCompliance(const ConstraintParams *cPar AdvancedTimer::stepEnd("Get Compliance"); } -void QPInverseProblemSolver::rebuildSystem(double massFactor, double forceFactor) -{ - d_graph.beginEdit()->clear(); - d_graph.endEdit(); - - //rebuildConstraintCorrectionSystem() - for (unsigned int i=0; irebuildSystem(massFactor, forceFactor); - } -} - bool QPInverseProblemSolver::solveSystem(const ConstraintParams * cParams, MultiVecId res1, MultiVecId res2) @@ -494,7 +494,7 @@ bool QPInverseProblemSolver::solveSystem(const ConstraintParams * cParams, double time = getContext()->getTime(); m_currentCP->setTime(time); - m_currentCP->setEpsilon(d_epsilon.getValue()); + m_currentCP->setEnergyWeight(d_energyWeight.getValue()); m_currentCP->setEnergyActuatorsOnly(d_actuatorsOnly.getValue()); m_currentCP->setTolerance(d_tolerance.getValue()); m_currentCP->setMaxIterations(d_maxIterations.getValue()); diff --git a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h index 5409fbc..2af8902 100644 --- a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h +++ b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h @@ -102,9 +102,6 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::componen MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; - void rebuildSystem(double massFactor, - double forceFactor) override; - bool solveSystem(const ConstraintParams* cParams, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; @@ -148,7 +145,10 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::componen sofa::Data d_responseFriction; sofa::Data d_qpSolver; - sofa::Data d_epsilon; + sofa::Data d_energyWeight; + SOFA_ATTRIBUTE_DEPRECATED("v25.12", "v26.12", "Use d_energyWeight instead.") + sofa::Data d_epsilon; // Used to deprecate the name of the data from the UI + sofa::Data d_actuatorsOnly; sofa::Data d_allowSliding; sofa::Data > > d_graph; diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.cpp b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.cpp index 156a8c2..4f2f975 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.cpp +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.cpp @@ -56,7 +56,7 @@ using sofa::core::objectmodel::Base; QPInverseProblem::QPInverseProblem() - : m_epsilon(1e-3) + : m_ernergyWeight(1e-3) , m_tolerance(1e-5) , m_maxIteration(200) , m_largestQNormVariation(0.0) diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.h b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.h index 0f6577c..644d267 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.h +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.h @@ -100,7 +100,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblem : public sofa::component::con void solve(double& objective, int& iterations); void setTime(const double& time) {m_time = time;} - void setEpsilon(const double& epsilon) {m_epsilon = epsilon;} + void setEnergyWeight(const double& energyWeight) {m_ernergyWeight = energyWeight;} void setEnergyActuatorsOnly(const bool& actuatorsOnly) {m_actuatorsOnly = actuatorsOnly;} void setTolerance(const double& tolerance) {m_tolerance = tolerance;} void setMaxIterations(const int& maxIt) {m_maxIteration = maxIt;} @@ -127,7 +127,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblem : public sofa::component::con QPSystem* m_qpSystem; QPConstraintLists* m_qpCLists; - double m_epsilon; + double m_ernergyWeight; bool m_actuatorsOnly; double m_tolerance; int m_maxIteration; diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp index 4fec6a2..62a0054 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp @@ -109,9 +109,11 @@ void QPInverseProblemImpl::init(){ } /// Build system -void QPInverseProblemImpl:: computeEnergyWeight(double& weight) + + +void QPInverseProblemImpl:: computeEnergyScalingFactor(double& scalingFactor) { - weight = 0.0; + scalingFactor = 0.0; unsigned int nbActuators = m_qpCLists->actuatorRowIds.size(); unsigned int nbEquality = m_qpCLists->equalityRowIds.size(); @@ -148,7 +150,7 @@ void QPInverseProblemImpl:: computeEnergyWeight(double& weight) } if(normW > 1e-14) - weight = normQ/normW; + scalingFactor = normQ/normW; } @@ -198,9 +200,9 @@ void QPInverseProblemImpl::buildQPMatrices() m_qpSystem->c[j] += m_qpSystem->W[m_qpCLists->effectorRowIds[i]][acIds[j]]*m_qpSystem->dFree[m_qpCLists->effectorRowIds[i]]; } - // Add energy term to Q+=eps*||Q||/||Waa||*Waa, eps is set by user - double weight = 0.; - computeEnergyWeight(weight); // compute ||Q||/||Waa|| + // Add energy term to Q+=eps*||Q||/||Waa||*Waa, eps (the weight of this term) is set by user + double scalingFactor = 0.; + computeEnergyScalingFactor(scalingFactor); // compute ||Q||/||Waa|| int actuatorsId = 0; int equalityId = 0; unsigned int actuatorsNbLines = 0; @@ -226,10 +228,10 @@ void QPInverseProblemImpl::buildQPMatrices() } for(unsigned int j=0; jhasEpsilon() && j==k) // energy of a specific actuator - m_qpSystem->Q[k][j] += ac->getEpsilon()*weight*m_qpSystem->W[acIds[k]][acIds[j]]; + if(ac->hasEnergyWeight() && j==k) // energy of a specific actuator + m_qpSystem->Q[k][j] += ac->getEnergyWeight()*scalingFactor*m_qpSystem->W[acIds[k]][acIds[j]]; else - m_qpSystem->Q[k][j] += m_epsilon*weight*m_qpSystem->W[acIds[k]][acIds[j]]; + m_qpSystem->Q[k][j] += m_ernergyWeight*scalingFactor*m_qpSystem->W[acIds[k]][acIds[j]]; } } else if (khasEpsilon() && j==k) // energy of a specific actuator - m_qpSystem->Q[k][j] += ac->getEpsilon()*weight*m_qpSystem->W[acIds[k]][acIds[j]]; + if(ac->hasEnergyWeight() && j==k) // energy of a specific actuator + m_qpSystem->Q[k][j] += ac->getEnergyWeight()*scalingFactor*m_qpSystem->W[acIds[k]][acIds[j]]; else - m_qpSystem->Q[k][j] += m_epsilon*weight*m_qpSystem->W[acIds[k]][acIds[j]]; + m_qpSystem->Q[k][j] += m_ernergyWeight*scalingFactor*m_qpSystem->W[acIds[k]][acIds[j]]; } } else // contacts { for(unsigned int j=0; jQ[k][j] += m_epsilon*weight*m_qpSystem->W[acIds[k]][acIds[j]]; + m_qpSystem->Q[k][j] += m_ernergyWeight*scalingFactor*m_qpSystem->W[acIds[k]][acIds[j]]; } } diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.h b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.h index 9e2fc64..7641e19 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.h +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.h @@ -82,7 +82,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemImpl : public QPInverseProblem vector &result, vector &dual) = 0; - void computeEnergyWeight(double& weight); + void computeEnergyScalingFactor(double& weight); void buildQPMatrices(); void solveWithContact(vector& result, double &objective, int &iterations); diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.cpp b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.cpp index 5a349e5..5cd5c26 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.cpp +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.cpp @@ -48,8 +48,8 @@ QPInverseProblemQPOases::QPInverseProblemQPOases() : } void QPInverseProblemQPOases::solveInverseProblem(double& objective, - vector &result, - vector &dual) + vector &result, + vector &dual) { m_qpSystem->previousResult = result; @@ -213,4 +213,4 @@ void QPInverseProblemQPOases::updateQPMatrices(real_t * Q, real_t * c, real_t * } // namespace -#endif \ No newline at end of file +#endif diff --git a/tests/component/solver/QPInverseProblemSolverTest.cpp b/tests/component/solver/QPInverseProblemSolverTest.cpp index d475b22..38a1972 100644 --- a/tests/component/solver/QPInverseProblemSolverTest.cpp +++ b/tests/component/solver/QPInverseProblemSolverTest.cpp @@ -92,7 +92,7 @@ struct QPInverseProblemSolverTest : public BaseTest, int nbTimeStep = 10; string deltaString; double tolerance = 1e-10; - m_root->getObject("QPInverseProblemSolver")->findData("epsilon")->read("0.0"); + m_root->getObject("QPInverseProblemSolver")->findData("energyWeight")->read("0.0"); m_root->getObject("QPInverseProblemSolver")->findData("qpSolver")->read(qpSolver); // Test inverse resolution (effector == target) diff --git a/tests/component/solver/QPInverseProblemSolverWithContactTest.cpp b/tests/component/solver/QPInverseProblemSolverWithContactTest.cpp index 07dbb3d..297ef09 100644 --- a/tests/component/solver/QPInverseProblemSolverWithContactTest.cpp +++ b/tests/component/solver/QPInverseProblemSolverWithContactTest.cpp @@ -61,7 +61,7 @@ struct QPInverseProblemSolverWithContactTest : public ::testing::Test, QPInvers m_simulation->reset(m_root.get()); int nbTimeStep = 10; - m_root->getObject("QPInverseProblemSolver")->findData("epsilon")->read("0.0"); + m_root->getObject("QPInverseProblemSolver")->findData("energyWeight")->read("0.0"); m_root->getObject("CollisionResponse")->findData("responseParams")->read("mu=0.0"); m_root->getObject("QPInverseProblemSolver")->findData("responseFriction")->read("0.0"); @@ -85,7 +85,7 @@ struct QPInverseProblemSolverWithContactTest : public ::testing::Test, QPInvers m_simulation->reset(m_root.get()); int nbTimeStep = 10; - m_root->getObject("QPInverseProblemSolver")->findData("epsilon")->read("0.0"); + m_root->getObject("QPInverseProblemSolver")->findData("energyWeight")->read("0.0"); m_root->getObject("CollisionResponse")->findData("responseParams")->read("mu=0.6"); m_root->getObject("QPInverseProblemSolver")->findData("responseFriction")->read("0.6"); diff --git a/tests/component/solver/scenes/CubeOnFloor.pyscn b/tests/component/solver/scenes/CubeOnFloor.pyscn index f7439bf..ed2776a 100644 --- a/tests/component/solver/scenes/CubeOnFloor.pyscn +++ b/tests/component/solver/scenes/CubeOnFloor.pyscn @@ -11,7 +11,7 @@ def createScene(rootNode): rootNode.findData('dt').value = 0.01 rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('QPInverseProblemSolver', epsilon=0, maxIterations=1000, tolerance=1e-5) + rootNode.addObject('QPInverseProblemSolver', energyWeight=0, maxIterations=1000, tolerance=1e-5) rootNode.addObject('CollisionPipeline') rootNode.addObject('BruteForceBroadPhase') rootNode.addObject('BVHNarrowPhase') From 9d6410fdf3aaa41ffa86c3a719f4a652f1c38298 Mon Sep 17 00:00:00 2001 From: EulalieCoevoet Date: Sun, 8 Feb 2026 11:49:33 +0100 Subject: [PATCH 2/4] [extlibs] fixes qpOASES libs conflict (#79) * [extlibs] fixes qpOASES libs conflict * forgotten files --- .../include/qpOASES/Matrices.hpp | 32 +++++++++---------- .../include/qpOASES/SQProblemSchur.hpp | 16 +++++----- extlibs/qpOASES-3.2.0/src/BLASReplacement.cpp | 20 ++++++------ .../qpOASES-3.2.0/src/LAPACKReplacement.cpp | 22 ++++++------- 4 files changed, 45 insertions(+), 45 deletions(-) diff --git a/extlibs/qpOASES-3.2.0/include/qpOASES/Matrices.hpp b/extlibs/qpOASES-3.2.0/include/qpOASES/Matrices.hpp index ae06407..653a3a4 100644 --- a/extlibs/qpOASES-3.2.0/include/qpOASES/Matrices.hpp +++ b/extlibs/qpOASES-3.2.0/include/qpOASES/Matrices.hpp @@ -45,24 +45,24 @@ #ifdef __USE_SINGLE_PRECISION__ /** Macro for calling level 3 BLAS operation in single precision. */ - #define GEMM sgemm_ + #define GEMM qpoases_sgemm_ /** Macro for calling level 3 BLAS operation in single precision. */ - #define SYR ssyr_ + #define SYR qpoases_ssyr_ /** Macro for calling level 3 BLAS operation in single precision. */ - #define SYR2 ssyr2_ + #define SYR2 qpoases_ssyr2_ /** Macro for calling level 3 BLAS operation in single precision. */ - #define POTRF spotrf_ + #define POTRF qpoases_spotrf_ #else /** Macro for calling level 3 BLAS operation in double precision. */ - #define GEMM dgemm_ + #define GEMM qpoases_dgemm_ /** Macro for calling level 3 BLAS operation in double precision. */ - #define SYR dsyr_ + #define SYR qpoases_dsyr_ /** Macro for calling level 3 BLAS operation in double precision. */ - #define SYR2 dsyr2_ + #define SYR2 qpoases_dsyr2_ /** Macro for calling level 3 BLAS operation in double precision. */ - #define POTRF dpotrf_ + #define POTRF qpoases_dpotrf_ #endif /* __USE_SINGLE_PRECISION__ */ @@ -70,32 +70,32 @@ extern "C" { /** Performs one of the matrix-matrix operation in double precision. */ - void dgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, + void qpoases_dgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, const double*, const double*, const unsigned long*, const double*, const unsigned long*, const double*, double*, const unsigned long* ); /** Performs one of the matrix-matrix operation in single precision. */ - void sgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, + void qpoases_sgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, const float*, const float*, const unsigned long*, const float*, const unsigned long*, const float*, float*, const unsigned long* ); /** Performs a symmetric rank 1 operation in double precision. */ - void dsyr_ ( const char *, const unsigned long *, const double *, const double *, + void qpoases_dsyr_ ( const char *, const unsigned long *, const double *, const double *, const unsigned long *, double *, const unsigned long *); /** Performs a symmetric rank 1 operation in single precision. */ - void ssyr_ ( const char *, const unsigned long *, const float *, const float *, + void qpoases_ssyr_ ( const char *, const unsigned long *, const float *, const float *, const unsigned long *, float *, const unsigned long *); /** Performs a symmetric rank 2 operation in double precision. */ - void dsyr2_ ( const char *, const unsigned long *, const double *, const double *, + void qpoases_dsyr2_ ( const char *, const unsigned long *, const double *, const double *, const unsigned long *, const double *, const unsigned long *, double *, const unsigned long *); /** Performs a symmetric rank 2 operation in single precision. */ - void ssyr2_ ( const char *, const unsigned long *, const float *, const float *, + void qpoases_ssyr2_ ( const char *, const unsigned long *, const float *, const float *, const unsigned long *, const float *, const unsigned long *, float *, const unsigned long *); /** Calculates the Cholesky factorization of a real symmetric positive definite matrix in double precision. */ - void dpotrf_ ( const char *, const unsigned long *, double *, const unsigned long *, long * ); + void qpoases_dpotrf_ ( const char *, const unsigned long *, double *, const unsigned long *, long * ); /** Calculates the Cholesky factorization of a real symmetric positive definite matrix in single precision. */ - void spotrf_ ( const char *, const unsigned long *, float *, const unsigned long *, long * ); + void qpoases_spotrf_ ( const char *, const unsigned long *, float *, const unsigned long *, long * ); } diff --git a/extlibs/qpOASES-3.2.0/include/qpOASES/SQProblemSchur.hpp b/extlibs/qpOASES-3.2.0/include/qpOASES/SQProblemSchur.hpp index 7f09695..f2bd007 100644 --- a/extlibs/qpOASES-3.2.0/include/qpOASES/SQProblemSchur.hpp +++ b/extlibs/qpOASES-3.2.0/include/qpOASES/SQProblemSchur.hpp @@ -49,9 +49,9 @@ /** Macro for calling level 3 BLAS operation in single precision. */ //#define ORMQR sormqr_ /** Macro for calling level 3 BLAS operation in single precision. */ - #define TRTRS strtrs_ + #define TRTRS qpoases_strtrs_ /** Macro for calling level 3 BLAS operation in single precision. */ - #define TRCON strcon_ + #define TRCON qpoases_strcon_ #else @@ -60,9 +60,9 @@ /** Macro for calling level 3 BLAS operation in double precision. */ //#define ORMQR dormqr_ /** Macro for calling level 3 BLAS operation in double precision. */ - #define TRTRS dtrtrs_ + #define TRTRS qpoases_dtrtrs_ /** Macro for calling level 3 BLAS operation in double precision. */ - #define TRCON dtrcon_ + #define TRCON qpoases_dtrcon_ #endif /* __USE_SINGLE_PRECISION__ */ @@ -84,17 +84,17 @@ extern "C" { //float *WORK, const unsigned long *LWORK, int *INFO ); /** Solve a triangular system (double precision) */ - void dtrtrs_( const char *UPLO, const char *TRANS, const char *DIAG, const unsigned long *N, const unsigned long *NRHS, + void qpoases_dtrtrs_( const char *UPLO, const char *TRANS, const char *DIAG, const unsigned long *N, const unsigned long *NRHS, double *A, const unsigned long *LDA, double *B, const unsigned long *LDB, long *INFO ); /** Solve a triangular system (single precision) */ - void strtrs_( const char *UPLO, const char *TRANS, const char *DIAG, const unsigned long *N, const unsigned long *NRHS, + void qpoases_strtrs_( const char *UPLO, const char *TRANS, const char *DIAG, const unsigned long *N, const unsigned long *NRHS, float *A, const unsigned long *LDA, float *B, const unsigned long *LDB, long *INFO ); /** Estimate the reciprocal of the condition number of a triangular matrix in double precision */ - void dtrcon_( const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, double *A, const unsigned long *LDA, + void qpoases_dtrcon_( const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, double *A, const unsigned long *LDA, double *RCOND, double *WORK, const unsigned long *IWORK, long *INFO ); /** Estimate the reciprocal of the condition number of a triangular matrix in single precision */ - void strcon_( const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, float *A, const unsigned long *LDA, + void qpoases_strcon_( const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, float *A, const unsigned long *LDA, float *RCOND, float *WORK, const unsigned long *IWORK, long *INFO ); } diff --git a/extlibs/qpOASES-3.2.0/src/BLASReplacement.cpp b/extlibs/qpOASES-3.2.0/src/BLASReplacement.cpp index 745fd55..a650efe 100644 --- a/extlibs/qpOASES-3.2.0/src/BLASReplacement.cpp +++ b/extlibs/qpOASES-3.2.0/src/BLASReplacement.cpp @@ -35,11 +35,11 @@ #include -extern "C" void dgemm_( const char *TRANSA, const char *TRANSB, - const unsigned long *M, const unsigned long *N, const unsigned long *K, - const double *ALPHA, const double *A, const unsigned long *LDA, const double *B, const unsigned long *LDB, - const double *BETA, double *C, const unsigned long *LDC - ) +extern "C" void qpoases_dgemm_( const char *TRANSA, const char *TRANSB, + const unsigned long *M, const unsigned long *N, const unsigned long *K, + const double *ALPHA, const double *A, const unsigned long *LDA, const double *B, const unsigned long *LDB, + const double *BETA, double *C, const unsigned long *LDC + ) { unsigned long i, j, k; @@ -90,11 +90,11 @@ extern "C" void dgemm_( const char *TRANSA, const char *TRANSB, C[j+(*LDC)*k] += *ALPHA * A[i+(*LDA)*j] * B[i+(*LDB)*k]; } -extern "C" void sgemm_( const char *TRANSA, const char *TRANSB, - const unsigned long *M, const unsigned long *N, const unsigned long *K, - const float *ALPHA, const float *A, const unsigned long *LDA, const float *B, const unsigned long *LDB, - const float *BETA, float *C, const unsigned long *LDC - ) +extern "C" void qpoases_sgemm_( const char *TRANSA, const char *TRANSB, + const unsigned long *M, const unsigned long *N, const unsigned long *K, + const float *ALPHA, const float *A, const unsigned long *LDA, const float *B, const unsigned long *LDB, + const float *BETA, float *C, const unsigned long *LDC + ) { unsigned long i, j, k; diff --git a/extlibs/qpOASES-3.2.0/src/LAPACKReplacement.cpp b/extlibs/qpOASES-3.2.0/src/LAPACKReplacement.cpp index 61a4a98..2b38e94 100644 --- a/extlibs/qpOASES-3.2.0/src/LAPACKReplacement.cpp +++ b/extlibs/qpOASES-3.2.0/src/LAPACKReplacement.cpp @@ -35,9 +35,9 @@ #include -extern "C" void dpotrf_( const char *uplo, const unsigned long *_n, double *a, - const unsigned long *_lda, long *info - ) +extern "C" void qpoases_dpotrf_( const char *uplo, const unsigned long *_n, double *a, + const unsigned long *_lda, long *info + ) { double sum; long i, j, k; @@ -77,7 +77,7 @@ extern "C" void dpotrf_( const char *uplo, const unsigned long *_n, double *a, } -extern "C" void spotrf_( const char *uplo, const unsigned long *_n, float *a, +extern "C" void qpoases_spotrf_( const char *uplo, const unsigned long *_n, float *a, const unsigned long *_lda, long *info ) { @@ -118,15 +118,15 @@ extern "C" void spotrf_( const char *uplo, const unsigned long *_n, float *a, *info = 0; } -extern "C" void dtrtrs_( const char *UPLO, const char *TRANS, const char *DIAG, - const unsigned long *N, const unsigned long *NRHS, - double *A, const unsigned long *LDA, double *B, const unsigned long *LDB, long *INFO - ) +extern "C" void qpoases_dtrtrs_( const char *UPLO, const char *TRANS, const char *DIAG, + const unsigned long *N, const unsigned long *NRHS, + double *A, const unsigned long *LDA, double *B, const unsigned long *LDB, long *INFO + ) { ; /* Dummy. If SQProblemSchur is to be used, system LAPACK must be used */ } -extern "C" void strtrs_( const char *UPLO, const char *TRANS, const char *DIAG, +extern "C" void qpoases_strtrs_( const char *UPLO, const char *TRANS, const char *DIAG, const unsigned long *N, const unsigned long *NRHS, float *A, const unsigned long *LDA, float *B, const unsigned long *LDB, long *INFO ) @@ -134,7 +134,7 @@ extern "C" void strtrs_( const char *UPLO, const char *TRANS, const char *DIAG, ; /* Dummy. If SQProblemSchur is to be used, system LAPACK must be used */ } -extern "C" void dtrcon_( const char *NORM, const char *UPLO, const char *DIAG, +extern "C" void qpoases_dtrcon_( const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, double *A, const unsigned long *LDA, double *RCOND, double *WORK, const unsigned long *IWORK, long *INFO ) @@ -142,7 +142,7 @@ extern "C" void dtrcon_( const char *NORM, const char *UPLO, const char *DIAG, ; /* Dummy. If SQProblemSchur is to be used, system LAPACK must be used */ } -extern "C" void strcon_( const char *NORM, const char *UPLO, const char *DIAG, +extern "C" void qpoases_strcon_( const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, float *A, const unsigned long *LDA, float *RCOND, float *WORK, const unsigned long *IWORK, long *INFO ) From 567791be7086905e99035fae0ebf9d116d20e2a5 Mon Sep 17 00:00:00 2001 From: Olivier Roussel Date: Thu, 5 Mar 2026 10:54:57 +0100 Subject: [PATCH 3/4] Update Conda CI to devel conda packages (#80) * update Conda CI to new devel conda packages * fix typo & cosms * debugging * fix step order * add required pybind11 for softrobots * add required cxxopts for softrobots * fix python interpreter path for windows * fix win compiler * dear windows bash --- .github/workflows/ci-conda.yml | 120 ++++++++++++++++++++------------- 1 file changed, 75 insertions(+), 45 deletions(-) diff --git a/.github/workflows/ci-conda.yml b/.github/workflows/ci-conda.yml index fa487a8..4dd9330 100644 --- a/.github/workflows/ci-conda.yml +++ b/.github/workflows/ci-conda.yml @@ -1,4 +1,4 @@ -name: CI - Linux/OSX/Windows - Conda +name: CI - Conda on: workflow_dispatch: @@ -7,13 +7,13 @@ on: jobs: build-with-conda-and-test: - name: "[conda:${{ matrix.os }}:py${{ matrix.python_version }}:${{ matrix.qp_solver }}]" + name: "${{ matrix.os }} with SOFA master from Conda devel [QPSolver = ${{ matrix.qp_solver }}]" runs-on: ${{ matrix.os }} strategy: fail-fast: false matrix: - os: [ubuntu-22.04, macos-13, macos-14, windows-2022] - python_version: ["3.13"] + os: [ubuntu-latest, macos-latest, windows-latest, ubuntu-24.04-arm] + python_version: ["3.12"] build_type: ["Release"] qp_solver: ["qpOases", "proxQP"] @@ -28,68 +28,93 @@ jobs: channels: conda-forge conda-remove-defaults: "true" - - name: Checkout source code - uses: actions/checkout@v4 - - - name: Install SOFA from nightly packages [Conda] + - name: Install SOFA and SofaPython3 from devel packages shell: bash -l {0} run: | - conda install sofa-devel sofa-python3 sofa-stlib sofa-softrobots -c sofa-framework-nightly + conda install sofa-devel sofa-python3 -c https://prefix.dev/sofa-framework-devel - - name: Install compilation environment [Conda / Linux] - if: contains(matrix.os, 'ubuntu') + - name: Install compilation environment shell: bash -l {0} run: | - conda install cmake compilers make ninja - conda install mesa-libgl-devel-cos7-x86_64 + conda install cmake cxx-compiler ninja pkg-config pybind11 cxxopts - - name: Install compilation environment [Conda / macOS] - if: contains(matrix.os, 'macos') + - name: Checkout SOFA SoftRobots + uses: actions/checkout@v4 + with: + repository: SofaDefrost/SoftRobots + ref: master + path: SoftRobots + + - name: Configure SoftRobots [Linux / macOS] + if: contains(matrix.os, 'ubuntu') || contains(matrix.os, 'macos') shell: bash -l {0} run: | - conda install cmake compilers make ninja + cd SoftRobots + mkdir build + cd build + cmake .. -GNinja \ + -DCMAKE_INSTALL_PREFIX=${CONDA_PREFIX} \ + -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + -DPython_EXECUTABLE:PATH=${CONDA_PREFIX}/bin/python \ + -DSOFTROBOTS_BUILD_TESTS:BOOL=OFF - - name: Install compilation environment [Conda / Windows] + - name: Configure SoftRobots [Windows] if: contains(matrix.os, 'windows') + # It's better to use CMD to have all VS variables setup + shell: cmd /C CALL {0} + run: | + cd SoftRobots + mkdir build + cd build + cmake .. -GNinja ^ + -DCMAKE_INSTALL_PREFIX=%CONDA_PREFIX%/Library ^ + -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} ^ + -DPython_EXECUTABLE:PATH=%CONDA_PREFIX%/python.exe ^ + -DSOFTROBOTS_BUILD_TESTS:BOOL=OFF + + - name: Build & install SoftRobots [Linux / macOS] + if: contains(matrix.os, 'ubuntu') || contains(matrix.os, 'macos') shell: bash -l {0} run: | - conda install cmake vs2022_win-64 + cd SoftRobots/build + cmake --build . --config ${{ matrix.build_type }} -v -j 2 + cmake --install . --config ${{ matrix.build_type }} - - name: Install dependencies [Conda] - shell: bash -l {0} + - name: Build & install SoftRobots [Windows] + if: contains(matrix.os, 'windows') + # It's better to use CMD to have all VS variables setup + shell: cmd /C CALL {0} run: | - conda install eigen libboost-headers pybind11 cxxopts + cd SoftRobots/build + cmake --build . --config ${{ matrix.build_type }} -v -j 2 + cmake --install . --config ${{ matrix.build_type }} - - name: Install QP solver dependencies [Conda / proxQP] + - name: Checkout source code + uses: actions/checkout@v4 + + - name: Install QP solver dependencies [proxQP] if: contains(matrix.qp_solver, 'proxQP') shell: bash -l {0} run: | conda install proxsuite echo "CMAKE_OPTS_QPSOLVER=-DSOFTROBOTSINVERSE_ENABLE_PROXQP=ON -DSOFTROBOTSINVERSE_ENABLE_QPOASES=OFF" >> "$GITHUB_ENV" - - name: Install QP solver dependencies [Conda / qpOases & not osx-arm64] - if: contains(matrix.qp_solver, 'qpOases') && !contains(matrix.os, 'macos-14') + - name: Install QP solver dependencies [qpOases] + if: contains(matrix.qp_solver, 'qpOases') shell: bash -l {0} run: | conda install qpoases echo "CMAKE_OPTS_QPSOLVER=-DSOFTROBOTSINVERSE_ENABLE_PROXQP=OFF -DSOFTROBOTSINVERSE_ENABLE_QPOASES=ON" >> "$GITHUB_ENV" - # Use embedded qpOases version as its conda package for python>3.10 under osx-arm64 platform is not supported anymore - - name: Install QP solver dependencies [Conda / qpOases & osx-arm64] - if: contains(matrix.qp_solver, 'qpOases') && contains(matrix.os, 'macos-14') - shell: bash -l {0} - run: | - echo "CMAKE_OPTS_QPSOLVER=-DSOFTROBOTSINVERSE_ENABLE_PROXQP=OFF -DSOFTROBOTSINVERSE_ENABLE_QPOASES=ON" >> "$GITHUB_ENV" - - - name: Print environment [Conda] + - name: Print environment shell: bash -l {0} run: | conda info conda list env - - name: Configure [Conda / Linux & macOS] - if: contains(matrix.os, 'macos') || contains(matrix.os, 'ubuntu') + - name: Configure SoftRobots.Inverse [Linux / macOS] + if: contains(matrix.os, 'ubuntu') || contains(matrix.os, 'macos') shell: bash -l {0} run: | mkdir build @@ -100,32 +125,37 @@ jobs: -DPython_EXECUTABLE:PATH=${CONDA_PREFIX}/bin/python \ -DSOFTROBOTSINVERSE_BUILD_TESTS:BOOL=ON - - name: Configure [Conda / Windows] + - name: Configure SoftRobots.Inverse [Windows] if: contains(matrix.os, 'windows') - shell: bash -l {0} + # It's better to use CMD to have all VS variables setup + shell: cmd /C CALL {0} run: | mkdir build cd build - cmake .. -G"Visual Studio 17 2022" -T "v143" $CMAKE_OPTS_QPSOLVER \ - -DCMAKE_GENERATOR_PLATFORM=x64 \ - -DCMAKE_INSTALL_PREFIX=${CONDA_PREFIX} \ - -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ - -DPython_EXECUTABLE:PATH=${CONDA_PREFIX}/python.exe \ + cmake .. -GNinja %CMAKE_OPTS_QPSOLVER% ^ + -DCMAKE_INSTALL_PREFIX=%CONDA_PREFIX% ^ + -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} ^ + -DPython_EXECUTABLE:PATH=%CONDA_PREFIX%/python.exe ^ -DSOFTROBOTSINVERSE_BUILD_TESTS:BOOL=ON - - name: Build [Conda] + - name: Build && install SoftRobots.Inverse [Linux / macOS] + if: contains(matrix.os, 'ubuntu') || contains(matrix.os, 'macos') shell: bash -l {0} run: | cd build cmake --build . --config ${{ matrix.build_type }} -v -j 2 + cmake --install . --config ${{ matrix.build_type }} - - name: Install [Conda] - shell: bash -l {0} + - name: Build && install SoftRobots.Inverse [Windows] + if: contains(matrix.os, 'windows') + # It's better to use CMD to have all VS variables setup + shell: cmd /C CALL {0} run: | cd build + cmake --build . --config ${{ matrix.build_type }} -v -j 2 cmake --install . --config ${{ matrix.build_type }} - - name: Test [Conda] + - name: Test SoftRobots.Inverse shell: bash -l {0} run: | cd build From 9fa298209b1821b84804270ef82d97f10a405796 Mon Sep 17 00:00:00 2001 From: EulalieCoevoet Date: Tue, 10 Mar 2026 17:40:00 +0100 Subject: [PATCH 4/4] this is the way to manage the lifecycle --- .../component/constraint/ForcePointActuator.h | 4 +++- .../component/constraint/ForcePointActuator.inl | 12 ++---------- .../component/solver/QPInverseProblemSolver.cpp | 11 ++--------- .../component/solver/QPInverseProblemSolver.h | 4 +++- 4 files changed, 10 insertions(+), 21 deletions(-) diff --git a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h index 5afd156..a697449 100644 --- a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h +++ b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h @@ -33,6 +33,8 @@ #include +#include + namespace softrobotsinverse::constraint { using softrobotsinverse::behavior::Actuator; @@ -107,7 +109,7 @@ class ForcePointActuator : public Actuator sofa::Data d_direction; sofa::Data d_energyWeight; SOFA_ATTRIBUTE_DEPRECATED("v25.12", "v26.12", "Use d_energyWeight instead.") - sofa::Data d_penalty; // Used to deprecate the name of the data from the UI + sofa::core::objectmodel::lifecycle::RenamedData d_epsilon; sofa::Data d_showForce; sofa::Data d_visuScale; diff --git a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl index 4ee8e0b..33dfc69 100644 --- a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl +++ b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl @@ -77,8 +77,6 @@ ForcePointActuator::ForcePointActuator(MechanicalState* object) "Specify the energy weight of the constraint. 0 means no limitation on the energy \n" "transfered by this actuator. The default value used is the energyWeight defined in the inverse problem solver.")) - , d_penalty(initData(&d_penalty, Real(1e-3), "penalty", "")) - , d_showForce(initData(&d_showForce, false, "showForce", "")) @@ -93,7 +91,8 @@ ForcePointActuator::ForcePointActuator(MechanicalState* object) template void ForcePointActuator::setUpData() { - d_penalty.setDisplayed(false); + d_epsilon.setOriginalData(&d_energyWeight); + this->addAlias(&d_energyWeight, "penalty"); d_force.setReadOnly(true); d_displacement.setReadOnly(true); @@ -120,13 +119,6 @@ void ForcePointActuator::init() "To remove this error message fix your scene possibly by " "adding a MechanicalObject." ; - if (d_penalty.isSet()) - { - msg_deprecated() << "The data penalty is deprecated. To fix your scene please use energyWeight instead. It will be removed in v26.12."; - const auto& penalty = sofa::helper::getReadAccessor(d_penalty); - d_energyWeight.setValue(penalty); - } - initData(); initLimit(); } diff --git a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp index 4b826b8..e8d57d0 100644 --- a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp +++ b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp @@ -139,8 +139,6 @@ QPInverseProblemSolver::QPInverseProblemSolver() "energy does not disrupt the quality of the effector positioning. " "Default value 1e-3.")) - , d_epsilon(initData(&d_epsilon, 1e-3, "epsilon", "")) - , d_actuatorsOnly(initData(&d_actuatorsOnly, false, "actuatorsOnly", "An energy term is added in the minimization process. \n" "If true, add it only for actuators." @@ -187,13 +185,8 @@ QPInverseProblemSolver::QPInverseProblemSolver() createProblems(); }); - d_epsilon.setDisplayed(false); - if (d_epsilon.isSet()) - { - msg_deprecated() << "The data epsilon is deprecated. To fix your scene please use energyWeight instead. It will be removed in v26.12."; - const auto& epsilon = sofa::helper::getReadAccessor(d_epsilon); - d_energyWeight.setValue(epsilon); - } + d_epsilon.setOriginalData(&d_energyWeight); + addAlias(&d_energyWeight, "epsilon"); } void QPInverseProblemSolver::createProblems() diff --git a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h index 2af8902..87c0c24 100644 --- a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h +++ b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h @@ -43,6 +43,8 @@ #include #include +#include + using sofa::core::objectmodel::KeypressedEvent ; namespace softrobotsinverse::solver @@ -147,7 +149,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::componen sofa::Data d_energyWeight; SOFA_ATTRIBUTE_DEPRECATED("v25.12", "v26.12", "Use d_energyWeight instead.") - sofa::Data d_epsilon; // Used to deprecate the name of the data from the UI + sofa::core::objectmodel::lifecycle::RenamedData d_epsilon; sofa::Data d_actuatorsOnly; sofa::Data d_allowSliding;