diff --git a/src/SoftRobots.Inverse/binding/Binding_QPInverseProblemSolver.cpp b/src/SoftRobots.Inverse/binding/Binding_QPInverseProblemSolver.cpp index 8424c8d..b386103 100644 --- a/src/SoftRobots.Inverse/binding/Binding_QPInverseProblemSolver.cpp +++ b/src/SoftRobots.Inverse/binding/Binding_QPInverseProblemSolver.cpp @@ -117,6 +117,12 @@ void moduleAddQPInverseProblemSolver(py::module &m) s.def(py::init<>()); s.def("solveSystem", &softrobotsinverse::solver::QPInverseProblemSolver::solveSystem); + + /// register the binding in the downcasting subsystem + sofapython3::PythonFactory::registerType([](sofa::core::objectmodel::Base* object) + { + return py::cast(dynamic_cast(object)); + }); } } diff --git a/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h b/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h index 775de99..b9b7fa5 100644 --- a/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h +++ b/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h @@ -113,12 +113,6 @@ class SOFA_SOFTROBOTS_INVERSE_API BarycentricCenterEffector : public PositionEff using PositionEffector::d_indices ; using PositionEffector::d_componentState ; - SOFA_ATTRIBUTE_DEPRECATED("v24.12", "v25.06", "Use d_useDirections instead.") - sofa::Data > d_axis; - - SOFA_ATTRIBUTE_DEPRECATED("v24.12", "v25.06", "Use d_effectorGoal instead.") - sofa::Data d_effectorGoalPosition; - sofa::Data d_drawBarycenter; sofa::Data d_barycenter; diff --git a/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.inl b/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.inl index a219be8..4a900ff 100644 --- a/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.inl +++ b/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.inl @@ -51,10 +51,6 @@ using sofa::core::objectmodel::ComponentState; template BarycentricCenterEffector::BarycentricCenterEffector(MechanicalState* object) : Inherit1(object) - , d_axis(initData(&d_axis, sofa::type::Vec<3,bool>(true,true,true), "axis", - "The parameter axis is of type Vec3 and allows to specify the directions in \n" - "which you want to solve the effector. If unspecified, the default \n" - "values are {true, true, true}).")) , d_drawBarycenter(initData(&d_drawBarycenter,false,"drawBarycenter", "If true, draw the barycenter" )) @@ -62,7 +58,6 @@ BarycentricCenterEffector::BarycentricCenterEffector(MechanicalState* , d_barycenter(initData(&d_barycenter,"barycenter", "Position of barycenter." )) { - d_axis.setDisplayed(false); d_indices.setDisplayed(false); // inherited from PositionModel but not used here d_barycenter.setReadOnly(true); } @@ -81,16 +76,6 @@ void BarycentricCenterEffector::init() indices.resize(1); Inherit1::init(); - - if (d_axis.isSet()) - { - msg_deprecated() << "The data axis is deprecated. To fix your scene please use useDirections instead. It will be remove in v25.06."; - auto useDirections = sofa::helper::getWriteAccessor(d_useDirections); - const auto& axis = sofa::helper::getReadAccessor(d_axis); - useDirections[0] = axis[0]; - useDirections[1] = axis[1]; - useDirections[2] = axis[2]; - } } template diff --git a/src/SoftRobots.Inverse/component/constraint/CableActuator.inl b/src/SoftRobots.Inverse/component/constraint/CableActuator.inl index 8c092be..948e8a4 100644 --- a/src/SoftRobots.Inverse/component/constraint/CableActuator.inl +++ b/src/SoftRobots.Inverse/component/constraint/CableActuator.inl @@ -61,13 +61,6 @@ CableActuator::CableActuator(MechanicalState* object) // These data from CableModel have no sense for actuator d_eqForce.setDisplayed(false); d_eqDisplacement.setDisplayed(false); - - // QP on only one value, we set dimension to one - m_lambdaInit.resize(1); - m_deltaMax.resize(1); - m_deltaMin.resize(1); - m_lambdaMax.resize(1); - m_lambdaMin.resize(1); } @@ -103,8 +96,10 @@ void CableActuator::reset() template void CableActuator::initData() { - d_displacement.setValue(0.0); + auto l = sofa::helper::getWriteAccessor(this->d_lambda); + l[0] = d_initForce.getValue(); d_force.setValue(d_initForce.getValue()); + if(d_initForce.isSet()) { m_hasLambdaInit = true; @@ -119,7 +114,7 @@ void CableActuator::initLimit() if(time < d_constrainAtTime.getValue()) return; - ReadAccessor> displacement = d_displacement; + ReadAccessor>> displacement = this->d_delta; ReadAccessor> maxDispVariation = d_maxDispVariation; ReadAccessor> maxPositiveDisplacement = d_maxPositiveDisplacement; ReadAccessor> maxNegativeDisplacement = d_maxNegativeDisplacement; @@ -157,10 +152,10 @@ void CableActuator::initLimit() { m_hasDeltaMax = true; m_hasDeltaMin = true; - if(rabs(m_deltaMin[0] - displacement) >= maxDispVariation || !d_maxNegativeDisplacement.isSet()) - m_deltaMin[0] = displacement - maxDispVariation; - if(rabs(m_deltaMax[0] - displacement) >= maxDispVariation || !d_maxPositiveDisplacement.isSet()) - m_deltaMax[0] = displacement + maxDispVariation; + if(rabs(m_deltaMin[0] - displacement[0]) >= maxDispVariation || !d_maxNegativeDisplacement.isSet()) + m_deltaMin[0] = displacement[0] - maxDispVariation; + if(rabs(m_deltaMax[0] - displacement[0]) >= maxDispVariation || !d_maxPositiveDisplacement.isSet()) + m_deltaMax[0] = displacement[0] + maxDispVariation; } } @@ -178,7 +173,7 @@ void CableActuator::updateLimit() return; } - ReadAccessor> displacement = d_displacement; + ReadAccessor>> displacement = this->d_delta; ReadAccessor> maxDispVariation = d_maxDispVariation; ReadAccessor> maxPositiveDisplacement = d_maxPositiveDisplacement; ReadAccessor> maxNegativeDisplacement = d_maxNegativeDisplacement; @@ -191,10 +186,10 @@ void CableActuator::updateLimit() if(d_maxDispVariation.isSet()) { - if(rabs(m_deltaMin[0] - displacement) >= maxDispVariation || !d_maxNegativeDisplacement.isSet()) - m_deltaMin[0] = displacement - maxDispVariation; - if(rabs(m_deltaMax[0] - displacement) >= maxDispVariation || !d_maxPositiveDisplacement.isSet()) - m_deltaMax[0] = displacement + maxDispVariation; + if(rabs(m_deltaMin[0] - displacement[0]) >= maxDispVariation || !d_maxNegativeDisplacement.isSet()) + m_deltaMin[0] = displacement[0] - maxDispVariation; + if(rabs(m_deltaMax[0] - displacement[0]) >= maxDispVariation || !d_maxPositiveDisplacement.isSet()) + m_deltaMax[0] = displacement[0] + maxDispVariation; } } @@ -202,14 +197,14 @@ void CableActuator::updateLimit() template void CableActuator::updateVisualization() { - ReadAccessor> displacement = d_displacement; - ReadAccessor> force = d_force; + ReadAccessor>> displacement = this->d_delta; + ReadAccessor>> force = this->d_lambda; ReadAccessor> maxPositiveDisplacement = d_maxPositiveDisplacement; ReadAccessor> maxForce = d_maxForce; if(d_maxPositiveDisplacement.isSet()) { - if(rabs(displacement-maxPositiveDisplacement) < 1e-5) + if(rabs(displacement[0]-maxPositiveDisplacement) < 1e-5) d_color.setValue(sofa::type::RGBAColor::red()); else d_color.setValue(m_color); @@ -217,7 +212,7 @@ void CableActuator::updateVisualization() if(d_maxForce.isSet()) { - if(rabs(force-maxForce) < 1e-5) + if(rabs(force[0]-maxForce) < 1e-5) d_color.setValue(sofa::type::RGBAColor::red()); else d_color.setValue(m_color); @@ -228,7 +223,13 @@ template void CableActuator::storeResults(sofa::type::vector &lambda, sofa::type::vector &delta) { + auto l = sofa::helper::getWriteAccessor(this->d_lambda); + auto d = sofa::helper::getWriteAccessor(this->d_delta); + + l[0] = lambda[0]; d_force.setValue(lambda[0]); + + d[0] = delta[0]; d_displacement.setValue(delta[0]); updateLimit(); diff --git a/src/SoftRobots.Inverse/component/constraint/CableEquality.inl b/src/SoftRobots.Inverse/component/constraint/CableEquality.inl index 16a1047..e9f667d 100644 --- a/src/SoftRobots.Inverse/component/constraint/CableEquality.inl +++ b/src/SoftRobots.Inverse/component/constraint/CableEquality.inl @@ -55,10 +55,6 @@ CableEquality::CableEquality(MechanicalState* object) d_maxPositiveDisplacement.setDisplayed(false); d_maxNegativeDisplacement.setDisplayed(false); d_maxDispVariation.setDisplayed(false); - - // QP on only one value, we set dimension to one - m_lambdaEqual.resize(1); - m_deltaEqual.resize(1); } @@ -85,8 +81,6 @@ void CableEquality::reinit() template void CableEquality::reset() { - d_displacement.setValue(0.0); - d_force.setValue(0.0); updateConstraint(); } diff --git a/src/SoftRobots.Inverse/component/constraint/CableSensor.inl b/src/SoftRobots.Inverse/component/constraint/CableSensor.inl index 185a30a..41e7f82 100644 --- a/src/SoftRobots.Inverse/component/constraint/CableSensor.inl +++ b/src/SoftRobots.Inverse/component/constraint/CableSensor.inl @@ -61,12 +61,12 @@ void CableSensor::setUpData() d_maxForce.setDisplayed(false); d_minForce.setDisplayed(false); d_eqForce.setDisplayed(false); - d_force.setDisplayed(false); + this->d_lambda.setDisplayed(false); d_maxPositiveDisplacement.setDisplayed(false); d_maxNegativeDisplacement.setDisplayed(false); d_maxDispVariation.setDisplayed(false); d_eqDisplacement.setDisplayed(false); - d_displacement.setDisplayed(false); + this->d_delta.setDisplayed(false); } diff --git a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h index 400b110..df3d327 100644 --- a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h +++ b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h @@ -104,7 +104,9 @@ class ForcePointActuator : public Actuator sofa::Data d_minForce; sofa::Data d_initForce; sofa::Data d_maxForceVariation; + SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v27.06", "Use d_lambda instead.") sofa::Data> d_force; + SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v27.06", "Use d_delta instead.") sofa::Data d_displacement; sofa::Data d_direction; sofa::Data d_energyWeight; diff --git a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl index 33dfc69..13814ee 100644 --- a/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl +++ b/src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl @@ -51,10 +51,10 @@ ForcePointActuator::ForcePointActuator(MechanicalState* object) "Index of the point of the model on which we want to apply the force")) , d_maxForce(initData(&d_maxForce, "maxForce", - "")) + "Maximum force.")) , d_minForce(initData(&d_minForce, "minForce", - "")) + "Minimum force.")) , d_initForce(initData(&d_initForce, Real(0.0), "initForce", "Initial force if any. Default is 0.")) @@ -62,12 +62,6 @@ ForcePointActuator::ForcePointActuator(MechanicalState* object) , d_maxForceVariation(initData(&d_maxForceVariation, "maxForceVariation", "Only available if the direction is set.")) - , d_force(initData(&d_force, "force", - "Warning: to get the actual force you should divide this value by dt.")) - - , d_displacement(initData(&d_displacement, Real(0.0), "displacement", - "")) - , d_direction(initData(&d_direction, "direction", "Direction of the force we want to apply. If d=[0,0,0], the direction \n" "will be optimized.")) @@ -78,10 +72,10 @@ ForcePointActuator::ForcePointActuator(MechanicalState* object) "transfered by this actuator. The default value used is the energyWeight defined in the inverse problem solver.")) , d_showForce(initData(&d_showForce, false, "showForce", - "")) + "Either to show the force or not.")) , d_visuScale(initData(&d_visuScale, Real(0.1), "visuScale", - "")) + "Visualization scale.")) { setUpData(); @@ -94,9 +88,6 @@ void ForcePointActuator::setUpData() d_epsilon.setOriginalData(&d_energyWeight); this->addAlias(&d_energyWeight, "penalty"); - d_force.setReadOnly(true); - d_displacement.setReadOnly(true); - d_showForce.setGroup("Visualization"); d_visuScale.setGroup("Visualization"); } @@ -135,6 +126,7 @@ template void ForcePointActuator::initData() { m_dim = (d_direction.getValue().norm()<1e-10)? Deriv::total_size: 1; + this->resizeConstraints(m_dim); if(d_energyWeight.isSet()) { @@ -150,12 +142,8 @@ void ForcePointActuator::initData() sofa::type::vector force; force.resize(m_dim, d_initForce.getValue()); + this->d_lambda.setValue(force); d_force.setValue(force); - - // QP on only one value, we set dimension to one - m_lambdaInit.resize(m_dim); - m_lambdaMax.resize(m_dim); - m_lambdaMin.resize(m_dim); } @@ -193,24 +181,26 @@ void ForcePointActuator::updateLimit() lambda = d_minForce.getValue(); } + auto lambda = sofa::helper::getReadAccessor(this->d_lambda); + if(d_maxForceVariation.isSet()) { if (m_dim>1) { for(unsigned int j=0; j= d_maxForceVariation.getValue() || !d_minForce.isSet()) - m_lambdaMin[j] = d_force.getValue()[j] - d_maxForceVariation.getValue(); - if(rabs(m_lambdaMax[j] - d_force.getValue()[j]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet()) - m_lambdaMax[j] = d_force.getValue()[j] + d_maxForceVariation.getValue(); + if(rabs(m_lambdaMin[j] - lambda[j]) >= d_maxForceVariation.getValue() || !d_minForce.isSet()) + m_lambdaMin[j] = lambda[j] - d_maxForceVariation.getValue(); + if(rabs(m_lambdaMax[j] - lambda[j]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet()) + m_lambdaMax[j] = lambda[j] + d_maxForceVariation.getValue(); } } else { - if(rabs(m_lambdaMin[0] - d_force.getValue()[0]) >= d_maxForceVariation.getValue() || !d_minForce.isSet()) - m_lambdaMin[0] = d_force.getValue()[0] - d_maxForceVariation.getValue(); - if(rabs(m_lambdaMax[0] - d_force.getValue()[0]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet()) - m_lambdaMax[0] = d_force.getValue()[0] + d_maxForceVariation.getValue(); + if(rabs(m_lambdaMin[0] - lambda[0]) >= d_maxForceVariation.getValue() || !d_minForce.isSet()) + m_lambdaMin[0] = lambda[0] - d_maxForceVariation.getValue(); + if(rabs(m_lambdaMax[0] - lambda[0]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet()) + m_lambdaMax[0] = lambda[0] + d_maxForceVariation.getValue(); } } } @@ -287,17 +277,27 @@ void ForcePointActuator::getConstraintViolation(const ConstraintParam template void ForcePointActuator::storeResults(vector &lambda, vector &delta) { + auto l = sofa::helper::getWriteAccessor(this->d_lambda); + auto d = sofa::helper::getWriteAccessor(this->d_delta); + WriteAccessor>> force = d_force; + d[0] = delta[0]; d_displacement.setValue(delta[0]); if(m_dim > 1) // No fixed direction { for(unsigned int j=0; j::draw(const VisualParams* vparams) ReadAccessor>> indices = sofa::helper::getReadAccessor(d_indices); ReadAccessor> visuScale = sofa::helper::getReadAccessor(d_visuScale); ReadAccessor > positions = m_state->readPositions(); - ReadAccessor>> force = d_force; + ReadAccessor>> force = this->d_lambda; Deriv direction = d_direction.getValue(); static const sofa::type::RGBAColor color(0,0,0.8,1); diff --git a/src/SoftRobots.Inverse/component/constraint/ForceSurfaceActuator.h b/src/SoftRobots.Inverse/component/constraint/ForceSurfaceActuator.h index dc4f09b..1b78afe 100644 --- a/src/SoftRobots.Inverse/component/constraint/ForceSurfaceActuator.h +++ b/src/SoftRobots.Inverse/component/constraint/ForceSurfaceActuator.h @@ -108,10 +108,10 @@ class ForceSurfaceActuator : public Actuator sofa::Data d_directions; sofa::Data d_updateNormals; - sofa::Data> d_triangles; - sofa::Data> d_quads; - sofa::Data> d_positions; - sofa::type::vector m_edges; + sofa::Data> d_triangles; + sofa::Data> d_quads; + sofa::Data> d_positions; + sofa::type::vector m_edges; sofa::Data d_maxForce; sofa::Data d_minForce; @@ -120,15 +120,17 @@ class ForceSurfaceActuator : public Actuator sofa::Data d_maxDisplacement; sofa::Data d_minDisplacement; - sofa::Data> d_force; - sofa::Data> d_displacement; + SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v27.06", "Use d_lambda instead.") + sofa::Data> d_force; + SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v27.06", "Use d_delta instead.") + sofa::Data> d_displacement; sofa::Data d_drawForce; sofa::Data d_drawSphere; sofa::Data d_drawSurface; sofa::Data d_visuScale; - bool m_useNormals{false}; + bool m_useNormals{false}; sofa::type::vector> m_pointsInSphereId; sofa::type::vector> m_trianglesInSpheresId; @@ -148,7 +150,6 @@ class ForceSurfaceActuator : public Actuator using Actuator::d_constraintIndex ; //////////////////////////////////////////////////////////////////////////// - void initLimit(); void initData(); void updateLimit(); diff --git a/src/SoftRobots.Inverse/component/constraint/ForceSurfaceActuator.inl b/src/SoftRobots.Inverse/component/constraint/ForceSurfaceActuator.inl index b09d57f..3ccdf33 100644 --- a/src/SoftRobots.Inverse/component/constraint/ForceSurfaceActuator.inl +++ b/src/SoftRobots.Inverse/component/constraint/ForceSurfaceActuator.inl @@ -91,12 +91,6 @@ ForceSurfaceActuator::ForceSurfaceActuator(MechanicalState* object) , d_minDisplacement(initData(&d_minDisplacement, "minDisplacement", "")) - , d_force(initData(&d_force, vector(0), "force", - "Warning: to get the actual force you should divide this value by dt.")) - - , d_displacement(initData(&d_displacement, vector(0), "displacement", - "")) - , d_drawForce(initData(&d_drawForce, false, "drawForces", "")) @@ -110,21 +104,10 @@ ForceSurfaceActuator::ForceSurfaceActuator(MechanicalState* object) "")) { - d_force.setReadOnly(true); - WriteAccessor>> force = d_force; - force.resize(1); - d_displacement.setReadOnly(true); - d_drawForce.setGroup("Visualization"); d_drawSphere.setGroup("Visualization"); d_drawSurface.setGroup("Visualization"); d_visuScale.setGroup("Visualization"); - - // QP on only one value, we set dimension to one - m_deltaMax.resize(1); - m_deltaMin.resize(1); - m_lambdaMax.resize(1); - m_lambdaMin.resize(1); } @@ -283,14 +266,15 @@ void ForceSurfaceActuator::initLimit() m_lambdaMin[0] = d_minForce.getValue(); } + auto force = sofa::helper::getReadAccessor(this->d_lambda); if(d_maxForceVariation.isSet()) { m_hasLambdaMax = true; m_hasLambdaMin = true; - if(rabs(m_lambdaMin[0] - d_force.getValue()[0]) >= d_maxForceVariation.getValue() || !d_minForce.isSet()) - m_lambdaMin[0] = d_force.getValue()[0] - d_maxForceVariation.getValue(); - if(rabs(m_lambdaMax[0] - d_force.getValue()[0]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet()) - m_lambdaMax[0] = d_force.getValue()[0] + d_maxForceVariation.getValue(); + if(rabs(m_lambdaMin[0] - force[0]) >= d_maxForceVariation.getValue() || !d_minForce.isSet()) + m_lambdaMin[0] = force[0] - d_maxForceVariation.getValue(); + if(rabs(m_lambdaMax[0] - force[0]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet()) + m_lambdaMax[0] = force[0] + d_maxForceVariation.getValue(); } if(d_maxDisplacement.isSet()) @@ -316,12 +300,13 @@ void ForceSurfaceActuator::updateLimit() if(d_minForce.isSet()) m_lambdaMin[0] = d_minForce.getValue(); + auto force = sofa::helper::getReadAccessor(this->d_lambda); if(d_maxForceVariation.isSet()) { - if(rabs(m_lambdaMin[0] - d_force.getValue()[0]) >= d_maxForceVariation.getValue() || !d_minForce.isSet()) - m_lambdaMin[0] = d_force.getValue()[0] - d_maxForceVariation.getValue(); - if(rabs(m_lambdaMax[0] - d_force.getValue()[0]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet()) - m_lambdaMax[0] = d_force.getValue()[0] + d_maxForceVariation.getValue(); + if(rabs(m_lambdaMin[0] - force[0]) >= d_maxForceVariation.getValue() || !d_minForce.isSet()) + m_lambdaMin[0] = force[0] - d_maxForceVariation.getValue(); + if(rabs(m_lambdaMax[0] - force[0]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet()) + m_lambdaMax[0] = force[0] + d_maxForceVariation.getValue(); } if(d_maxDisplacement.isSet()) @@ -338,7 +323,7 @@ void ForceSurfaceActuator::computeSurfaces() { ReadAccessor > > triangles = d_triangles; ReadAccessor > > quads = d_quads; - ReadAccessor > centers = d_centers; + ReadAccessor > centers = d_centers; computePointsInSpheres(); @@ -661,14 +646,20 @@ void ForceSurfaceActuator::storeResults(vector &lambda, vecto { WriteAccessor>> force = d_force; WriteAccessor>> displacement = d_displacement; + WriteAccessor>> l = this->d_lambda; + WriteAccessor>> d = this->d_delta; unsigned int nbForces = d_centers.getValue().size(); force.resize(nbForces); + l.resize(nbForces); displacement.resize(nbForces); + d.resize(nbForces); for(unsigned int i=0; i void ForceSurfaceActuator::updateCenter() { WriteAccessor>> centers = d_centers; - ReadAccessor>> displacement = d_displacement; + ReadAccessor>> displacement = this->d_delta; ReadAccessor>> directions = d_directions; int nbCenters = centers.size(); for(int i=0; i::draw(const VisualParams* vparams) template void ForceSurfaceActuator::drawForces(const VisualParams* vparams) { - if(m_state == nullptr || d_force.getValue().size()==0) + ReadAccessor>> force = this->d_lambda; + + if(m_state == nullptr || force.size() == 0) return; VecDeriv directions = d_directions.getValue(); ReadAccessor > positions = m_state->readPositions(); - ReadAccessor>> force = d_force; RGBAColor color(0,1,0,1); for(unsigned int i=0; i::drawSpheres(const VisualParams* vparams) template void ForceSurfaceActuator::drawSurfaces(const VisualParams* vparams) { - if(m_state == nullptr) return; diff --git a/src/SoftRobots.Inverse/component/constraint/JointActuator.h b/src/SoftRobots.Inverse/component/constraint/JointActuator.h index 02da0d9..89b15d2 100644 --- a/src/SoftRobots.Inverse/component/constraint/JointActuator.h +++ b/src/SoftRobots.Inverse/component/constraint/JointActuator.h @@ -106,7 +106,9 @@ class JointActuator : public Actuator sofa::Data d_maxAngle; sofa::Data d_minAngle; sofa::Data d_maxAngleVariation; + SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v27.06", "Use d_lambda instead.") sofa::Data d_effort; + SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v27.06", "Use d_delta instead.") sofa::Data d_angle; @@ -129,10 +131,12 @@ class JointActuator : public Actuator /// Bring inherited attributes and function in the current lookup context. /// otherwise any access to the base::attribute would require /// the "this->" approach. + using Actuator::d_lambda ; using Actuator::m_lambdaMax ; using Actuator::m_lambdaMin ; using Actuator::m_hasLambdaMax ; using Actuator::m_hasLambdaMin ; + using Actuator::d_delta ; using Actuator::m_deltaMax ; using Actuator::m_deltaMin ; using Actuator::m_hasDeltaMax ; @@ -141,11 +145,6 @@ class JointActuator : public Actuator using Actuator::d_componentState ; //////////////////////////////////////////////////////////////////////////// - -private: - - void setUpData(); - }; // Declares template as extern to avoid the code generation of the template for diff --git a/src/SoftRobots.Inverse/component/constraint/JointActuator.inl b/src/SoftRobots.Inverse/component/constraint/JointActuator.inl index b24f23f..9244c09 100644 --- a/src/SoftRobots.Inverse/component/constraint/JointActuator.inl +++ b/src/SoftRobots.Inverse/component/constraint/JointActuator.inl @@ -64,26 +64,9 @@ JointActuator::JointActuator(MechanicalState* object) , d_minAngle(initData(&d_minAngle, "minAngle", "In radian")) , d_maxAngleVariation(initData(&d_maxAngleVariation, "maxAngleVariation", "In radian")) - - , d_effort(initData(&d_effort, "effort", "Warning: to get the actual effort you should divide this value by dt.")) - - , d_angle(initData(&d_angle, "angle", "")) { - setUpData(); - - // QP on only one value, we set dimension to one - m_deltaMax.resize(1); - m_deltaMin.resize(1); - m_lambdaMax.resize(1); - m_lambdaMin.resize(1); -} - - -template -void JointActuator::setUpData() -{ - d_effort.setReadOnly(true); - d_angle.setReadOnly(true); + this->setLambdaName("effort"); + this->setDeltaName("angle"); } @@ -148,24 +131,27 @@ void JointActuator::initLimit() m_deltaMin[0] = d_minAngle.getValue(); } + auto effort = sofa::helper::getReadAccessor(d_lambda); + auto angle = sofa::helper::getReadAccessor(d_delta); + if(d_maxEffortVariation.isSet()) { m_hasLambdaMax = true; m_hasLambdaMin = true; - if(rabs(m_lambdaMin[0] - d_effort.getValue()) >= d_maxEffortVariation.getValue() || !d_minEffort.isSet()) - m_lambdaMin[0] = d_effort.getValue() - d_maxEffortVariation.getValue(); - if(rabs(m_lambdaMax[0] - d_effort.getValue()) >= d_maxEffortVariation.getValue() || !d_maxEffort.isSet()) - m_lambdaMax[0] = d_effort.getValue() + d_maxEffortVariation.getValue(); + if(rabs(m_lambdaMin[0] - effort[0]) >= d_maxEffortVariation.getValue() || !d_minEffort.isSet()) + m_lambdaMin[0] = effort[0] - d_maxEffortVariation.getValue(); + if(rabs(m_lambdaMax[0] - effort[0]) >= d_maxEffortVariation.getValue() || !d_maxEffort.isSet()) + m_lambdaMax[0] = effort[0] + d_maxEffortVariation.getValue(); } if(d_maxAngleVariation.isSet()) { m_hasDeltaMax = true; m_hasDeltaMin = true; - if(rabs(m_deltaMin[0] - d_angle.getValue()) >= d_maxAngleVariation.getValue() || !d_minAngle.isSet()) - m_deltaMin[0] = d_angle.getValue() - d_maxAngleVariation.getValue(); - if(rabs(m_deltaMax[0] - d_angle.getValue()) >= d_maxAngleVariation.getValue() || !d_maxAngle.isSet()) - m_deltaMax[0] = d_angle.getValue() + d_maxAngleVariation.getValue(); + if(rabs(m_deltaMin[0] - angle[0]) >= d_maxAngleVariation.getValue() || !d_minAngle.isSet()) + m_deltaMin[0] = angle[0] - d_maxAngleVariation.getValue(); + if(rabs(m_deltaMax[0] - angle[0]) >= d_maxAngleVariation.getValue() || !d_maxAngle.isSet()) + m_deltaMax[0] = angle[0] + d_maxAngleVariation.getValue(); } } @@ -173,6 +159,10 @@ void JointActuator::initLimit() template void JointActuator::initData() { + auto effort = sofa::helper::getWriteAccessor(d_lambda); + auto angle = sofa::helper::getWriteAccessor(d_delta); + effort[0] = d_initEffort.getValue(); + angle[0] = d_initAngle.getValue(); d_effort.setValue(d_initEffort.getValue()); d_angle.setValue(d_initAngle.getValue()); } @@ -193,20 +183,23 @@ void JointActuator::updateLimit() if(d_minAngle.isSet()) m_deltaMin[0] = d_minAngle.getValue(); + auto effort = sofa::helper::getReadAccessor(d_lambda); + auto angle = sofa::helper::getReadAccessor(d_delta); + if(d_maxEffortVariation.isSet()) { - if(rabs(m_lambdaMin[0] - d_effort.getValue()) >= d_maxEffortVariation.getValue() || !d_minEffort.isSet()) - m_lambdaMin[0] = d_effort.getValue() - d_maxEffortVariation.getValue(); - if(rabs(m_lambdaMax[0] - d_effort.getValue()) >= d_maxEffortVariation.getValue() || !d_maxEffort.isSet()) - m_lambdaMax[0] = d_effort.getValue() + d_maxEffortVariation.getValue(); + if(rabs(m_lambdaMin[0] - effort[0]) >= d_maxEffortVariation.getValue() || !d_minEffort.isSet()) + m_lambdaMin[0] = effort[0] - d_maxEffortVariation.getValue(); + if(rabs(m_lambdaMax[0] - effort[0]) >= d_maxEffortVariation.getValue() || !d_maxEffort.isSet()) + m_lambdaMax[0] = effort[0] + d_maxEffortVariation.getValue(); } if(d_maxAngleVariation.isSet()) { - if(rabs(m_deltaMin[0] - d_angle.getValue()) >= d_maxAngleVariation.getValue() || !d_minAngle.isSet()) - m_deltaMin[0] = d_angle.getValue() - d_maxAngleVariation.getValue(); - if(rabs(m_deltaMax[0] - d_angle.getValue()) >= d_maxAngleVariation.getValue() || !d_maxAngle.isSet()) - m_deltaMax[0] = d_angle.getValue() + d_maxAngleVariation.getValue(); + if(rabs(m_deltaMin[0] - angle[0]) >= d_maxAngleVariation.getValue() || !d_minAngle.isSet()) + m_deltaMin[0] = angle[0] - d_maxAngleVariation.getValue(); + if(rabs(m_deltaMax[0] - angle[0]) >= d_maxAngleVariation.getValue() || !d_maxAngle.isSet()) + m_deltaMax[0] = angle[0] + d_maxAngleVariation.getValue(); } } @@ -259,6 +252,11 @@ void JointActuator::storeResults(vector &lambda, vectorisComponentStateValid()) return ; + auto effort = sofa::helper::getWriteAccessor(d_lambda); + auto angle = sofa::helper::getWriteAccessor(d_delta); + effort[0] = lambda[0]; + angle[0] = delta[0]; + d_effort.setValue(lambda[0]); d_angle.setValue(delta[0]); updateLimit(); diff --git a/src/SoftRobots.Inverse/component/constraint/PositionEffector.h b/src/SoftRobots.Inverse/component/constraint/PositionEffector.h index 0c97cf4..85af634 100644 --- a/src/SoftRobots.Inverse/component/constraint/PositionEffector.h +++ b/src/SoftRobots.Inverse/component/constraint/PositionEffector.h @@ -68,7 +68,7 @@ class PositionEffector : public Effector, public softrobots::constrai const sofa::linearalgebra::BaseVector *Jdx) override; /////////////////////////////////////////////////////////////// - sofa::Data d_effectorGoal; + sofa::Data d_effectorGoal; void setTargetDefaultValue(); void resizeData(); diff --git a/src/SoftRobots.Inverse/component/constraint/PositionEquality.inl b/src/SoftRobots.Inverse/component/constraint/PositionEquality.inl index 7e3e666..6f1d425 100644 --- a/src/SoftRobots.Inverse/component/constraint/PositionEquality.inl +++ b/src/SoftRobots.Inverse/component/constraint/PositionEquality.inl @@ -54,8 +54,6 @@ PositionEquality::PositionEquality(MechanicalState* object) "Example of use: to allow the cable to reach an \n" "initial configuration before optimizing.")) { - // PositionEquality of dimension 1 - m_deltaEqual.resize(1); } diff --git a/src/SoftRobots.Inverse/component/constraint/SlidingActuator.h b/src/SoftRobots.Inverse/component/constraint/SlidingActuator.h index e917528..32e4366 100644 --- a/src/SoftRobots.Inverse/component/constraint/SlidingActuator.h +++ b/src/SoftRobots.Inverse/component/constraint/SlidingActuator.h @@ -135,8 +135,10 @@ class SlidingActuator : public Actuator sofa::Data d_indices; sofa::Data d_initForce; + SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v27.06", "Use d_lambda instead.") sofa::Data d_force; sofa::Data d_initDisplacement; + SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v27.06", "Use d_delta instead.") sofa::Data d_displacement; sofa::Data d_accumulateDisp; diff --git a/src/SoftRobots.Inverse/component/constraint/SlidingActuator.inl b/src/SoftRobots.Inverse/component/constraint/SlidingActuator.inl index bd4f51a..0d833e5 100644 --- a/src/SoftRobots.Inverse/component/constraint/SlidingActuator.inl +++ b/src/SoftRobots.Inverse/component/constraint/SlidingActuator.inl @@ -77,15 +77,9 @@ SlidingActuator::SlidingActuator(MechanicalState* object) , d_initForce(initData(&d_initForce, double(0.0), "initForce", "Initial force. Default is 0.")) - , d_force(initData(&d_force,double(0.0), "force", - "Output force. Warning: to get the actual force you should divide this value by dt.")) - , d_initDisplacement(initData(&d_initDisplacement, double(0.0), "initDisplacement", "Initial displacement. Default is 0.")) - , d_displacement(initData(&d_displacement,double(0.0), "displacement", - "Output displacement compared to the initial position.")) - , d_accumulateDisp(initData(&d_accumulateDisp, false, "accumulateDisp", "In case of relative displacement, accumulate the displacement.")) , d_showDirection(initData(&d_showDirection,false, "showDirection", @@ -106,17 +100,8 @@ SlidingActuator::SlidingActuator(MechanicalState* object) d_initForce.setGroup("Input"); d_initDisplacement.setGroup("Input"); - d_force.setGroup("Output"); - d_displacement.setGroup("Output"); - d_showDirection.setGroup("Visualization"); d_showVisuScale.setGroup("Visualization"); - - // QP on only one value, we set dimension to one - m_deltaMax.resize(1); - m_deltaMin.resize(1); - m_lambdaMax.resize(1); - m_lambdaMin.resize(1); } @@ -199,6 +184,11 @@ void SlidingActuator::initData() d_direction.setValue(direction); } + auto displacement = sofa::helper::getWriteAccessor(this->d_delta); + auto force = sofa::helper::getWriteAccessor(this->d_lambda); + + displacement[0] = d_initDisplacement.getValue(); + force[0] = d_initForce.getValue(); d_displacement.setValue(d_initDisplacement.getValue()); d_force.setValue(d_initForce.getValue()); } @@ -219,7 +209,7 @@ void SlidingActuator::checkIndicesRegardingState() template void SlidingActuator::initLimit() { - ReadAccessor> displacement = d_displacement; + ReadAccessor>> displacement = this->d_delta; ReadAccessor> maxDispVariation = d_maxDispVariation; ReadAccessor> maxPositiveDisplacement = d_maxPositiveDisplacement; ReadAccessor> maxNegativeDisplacement = d_maxNegativeDisplacement; @@ -254,10 +244,10 @@ void SlidingActuator::initLimit() { m_hasDeltaMax = true; m_hasDeltaMin = true; - if(rabs(m_deltaMin[0] - displacement) >= maxDispVariation || !d_maxNegativeDisplacement.isSet()) - m_deltaMin[0] = displacement - maxDispVariation; - if(rabs(m_deltaMax[0] - displacement) >= maxDispVariation || !d_maxPositiveDisplacement.isSet()) - m_deltaMax[0] = displacement + maxDispVariation; + if(rabs(m_deltaMin[0] - displacement[0]) >= maxDispVariation || !d_maxNegativeDisplacement.isSet()) + m_deltaMin[0] = displacement[0] - maxDispVariation; + if(rabs(m_deltaMax[0] - displacement[0]) >= maxDispVariation || !d_maxPositiveDisplacement.isSet()) + m_deltaMax[0] = displacement[0] + maxDispVariation; } } @@ -265,23 +255,23 @@ void SlidingActuator::initLimit() template void SlidingActuator::updateLimit() { - ReadAccessor> displacement = d_displacement; + ReadAccessor>> displacement = this->d_delta; ReadAccessor> maxDispVariation = d_maxDispVariation; ReadAccessor> maxPositiveDisplacement = d_maxPositiveDisplacement; ReadAccessor> maxNegativeDisplacement = d_maxNegativeDisplacement; if(d_maxPositiveDisplacement.isSet()) { - if (d_accumulateDisp.getValue() && displacement > 0) - m_deltaMax[0] = maxPositiveDisplacement - displacement; + if (d_accumulateDisp.getValue() && displacement[0] > 0) + m_deltaMax[0] = maxPositiveDisplacement - displacement[0]; else m_deltaMax[0] = maxPositiveDisplacement; } if(d_maxNegativeDisplacement.isSet()) { - if (d_accumulateDisp.getValue() && displacement < 0) - m_deltaMin[0] = -maxNegativeDisplacement - displacement; + if (d_accumulateDisp.getValue() && displacement[0] < 0) + m_deltaMin[0] = -maxNegativeDisplacement - displacement[0]; else m_deltaMin[0] = -maxNegativeDisplacement; } @@ -297,10 +287,10 @@ void SlidingActuator::updateLimit() } else { - if(rabs(m_deltaMin[0] - displacement) >= maxDispVariation || !d_maxNegativeDisplacement.isSet()) - m_deltaMin[0] = displacement - maxDispVariation; - if(rabs(m_deltaMax[0] - displacement) >= maxDispVariation || !d_maxPositiveDisplacement.isSet()) - m_deltaMax[0] = displacement + maxDispVariation; + if(rabs(m_deltaMin[0] - displacement[0]) >= maxDispVariation || !d_maxNegativeDisplacement.isSet()) + m_deltaMin[0] = displacement[0] - maxDispVariation; + if(rabs(m_deltaMax[0] - displacement[0]) >= maxDispVariation || !d_maxPositiveDisplacement.isSet()) + m_deltaMax[0] = displacement[0] + maxDispVariation; } } } @@ -378,16 +368,22 @@ void SlidingActuator::storeResults(sofa::type::vector &lambda { if(d_componentState.getValue() != ComponentState::Valid) return ; + + auto l = sofa::helper::getWriteAccessor(this->d_lambda); + l[0] = lambda[0]; d_force.setValue(lambda[0]); + auto d = sofa::helper::getWriteAccessor(this->d_delta); double& displacement = sofa::helper::getWriteAccessor(d_displacement); if(d_accumulateDisp.getValue()) { displacement += delta[0]; + d[0] += delta[0]; } else { displacement = delta[0]; + d[0] = delta[0]; } updateLimit(); diff --git a/src/SoftRobots.Inverse/component/constraint/SurfacePressureActuator.inl b/src/SoftRobots.Inverse/component/constraint/SurfacePressureActuator.inl index 4110402..55d6044 100644 --- a/src/SoftRobots.Inverse/component/constraint/SurfacePressureActuator.inl +++ b/src/SoftRobots.Inverse/component/constraint/SurfacePressureActuator.inl @@ -49,13 +49,6 @@ SurfacePressureActuator::SurfacePressureActuator(MechanicalState* obj // These data from SurfacePressureModel have no sense for actuator d_eqPressure.setDisplayed(false); d_eqVolumeGrowth.setDisplayed(false); - - // QP on only one value, we set dimension to one - m_lambdaInit.resize(1); - m_deltaMax.resize(1); - m_deltaMin.resize(1); - m_lambdaMax.resize(1); - m_lambdaMin.resize(1); } template @@ -91,6 +84,9 @@ template void SurfacePressureActuator::initData() { d_volumeGrowth.setValue(0.0); + + auto pressure = sofa::helper::getWriteAccessor(this->d_lambda); + pressure[0] = d_initPressure.getValue(); d_pressure.setValue(d_initPressure.getValue()); if(d_initPressure.isSet()){ @@ -150,7 +146,7 @@ void SurfacePressureActuator::initLimits() template void SurfacePressureActuator::updateLimits() { - sofa::helper::ReadAccessor> volumeGrowth = d_volumeGrowth; + sofa::helper::ReadAccessor>> volumeGrowth = this->d_delta; sofa::helper::ReadAccessor> maxVolumeGrowthVariation = d_maxVolumeGrowthVariation; sofa::helper::ReadAccessor> maxVolumeGrowth = d_maxVolumeGrowth; sofa::helper::ReadAccessor> minVolumeGrowth = d_minVolumeGrowth; @@ -163,10 +159,10 @@ void SurfacePressureActuator::updateLimits() if(d_maxVolumeGrowthVariation.isSet()) { - if(rabs(m_deltaMax[0] - volumeGrowth) >= maxVolumeGrowthVariation || !d_maxVolumeGrowth.isSet()) - m_deltaMax[0] = volumeGrowth + maxVolumeGrowthVariation; - if(rabs(m_deltaMin[0] + volumeGrowth) <= -maxVolumeGrowthVariation || !d_minVolumeGrowth.isSet()) - m_deltaMin[0] = volumeGrowth - maxVolumeGrowthVariation; + if(rabs(m_deltaMax[0] - volumeGrowth[0]) >= maxVolumeGrowthVariation || !d_maxVolumeGrowth.isSet()) + m_deltaMax[0] = volumeGrowth[0] + maxVolumeGrowthVariation; + if(rabs(m_deltaMin[0] + volumeGrowth[0]) <= -maxVolumeGrowthVariation || !d_minVolumeGrowth.isSet()) + m_deltaMin[0] = volumeGrowth[0] - maxVolumeGrowthVariation; } } @@ -174,6 +170,11 @@ template void SurfacePressureActuator::storeResults(sofa::type::vector &lambda, sofa::type::vector &delta) { + auto l = sofa::helper::getWriteAccessor(this->d_lambda); + auto d = sofa::helper::getWriteAccessor(this->d_delta); + l[0] = lambda[0]; + d[0] = delta[0]; + d_pressure.setValue(lambda[0]); d_volumeGrowth.setValue(delta[0]); diff --git a/src/SoftRobots.Inverse/component/constraint/SurfacePressureEquality.inl b/src/SoftRobots.Inverse/component/constraint/SurfacePressureEquality.inl index 1dfaf03..aad58a1 100644 --- a/src/SoftRobots.Inverse/component/constraint/SurfacePressureEquality.inl +++ b/src/SoftRobots.Inverse/component/constraint/SurfacePressureEquality.inl @@ -47,10 +47,6 @@ SurfacePressureEquality::SurfacePressureEquality(MechanicalState* obj d_minVolumeGrowth.setDisplayed(false); d_maxPressure.setDisplayed(false); d_minPressure.setDisplayed(false); - - // QP on only one value, we set dimension to one - m_lambdaEqual.resize(1); - m_deltaEqual.resize(1); } template diff --git a/src/SoftRobots.Inverse/component/constraint/VolumeEffector.inl b/src/SoftRobots.Inverse/component/constraint/VolumeEffector.inl index f099d73..c14b6ad 100644 --- a/src/SoftRobots.Inverse/component/constraint/VolumeEffector.inl +++ b/src/SoftRobots.Inverse/component/constraint/VolumeEffector.inl @@ -85,6 +85,8 @@ void VolumeEffector::getConstraintViolation(const ConstraintParams* c template void VolumeEffector::storeResults(sofa::type::vector &delta) { + auto d = sofa::helper::getWriteAccessor(this->d_delta); + d[0] = delta[0]; d_volumeGrowth.setValue(delta[0]); } diff --git a/src/SoftRobots.Inverse/component/constraint/YoungModulusActuator.h b/src/SoftRobots.Inverse/component/constraint/YoungModulusActuator.h index 3eef222..a10c96d 100644 --- a/src/SoftRobots.Inverse/component/constraint/YoungModulusActuator.h +++ b/src/SoftRobots.Inverse/component/constraint/YoungModulusActuator.h @@ -103,6 +103,7 @@ class YoungModulusActuator : public Actuator sofa::Data d_maxYoung; sofa::Data d_maxYoungVariationRatio; + SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v27.06", "Use d_lambda instead.") sofa::Data d_youngModulus; Real m_initialYoungModulus; @@ -125,6 +126,7 @@ class YoungModulusActuator : public Actuator /// Bring inherited attributes and function in the current lookup context. /// otherwise any access to the base::attribute would require /// the "this->" approach. + using Actuator::d_lambda ; using Actuator::m_state ; using Actuator::m_nbLines ; using Actuator::d_constraintIndex ; diff --git a/src/SoftRobots.Inverse/component/constraint/YoungModulusActuator.inl b/src/SoftRobots.Inverse/component/constraint/YoungModulusActuator.inl index 91a8b7f..9a58c93 100644 --- a/src/SoftRobots.Inverse/component/constraint/YoungModulusActuator.inl +++ b/src/SoftRobots.Inverse/component/constraint/YoungModulusActuator.inl @@ -54,19 +54,13 @@ YoungModulusActuator::YoungModulusActuator(MechanicalState* object) "Maximum variation of young / its actual value. \n" "If unspecified default value 1.0e1.")) - , d_youngModulus(initData(&d_youngModulus, (Real)0., "youngModulus", "Optimized Young modulus.")) - , l_forceField(initLink("forceField", "link to the force field")) , m_initialYoungModulus(0.0) , m_initError(false) , m_deltaYoungModulus(0.0) { - // QP on only one value, we set dimension to one - m_lambdaMax.resize(1); - m_lambdaMin.resize(1); - - d_youngModulus.setReadOnly(true); + this->setLambdaNameAndHelp("youngModulus", "Optimized Young modulus"); } @@ -103,21 +97,21 @@ void YoungModulusActuator::initLimit() template void YoungModulusActuator::updateLimit() { - const auto& youngModulus = sofa::helper::getReadAccessor(d_youngModulus); + const auto& youngModulus = sofa::helper::getReadAccessor(d_lambda); const auto& minYoung = sofa::helper::getReadAccessor(d_minYoung); const auto& maxYoung = sofa::helper::getReadAccessor(d_maxYoung); const auto& maxYoungVariationRatio = sofa::helper::getReadAccessor(d_maxYoungVariationRatio); - m_lambdaMin[0] =- (youngModulus - minYoung); - m_lambdaMax[0] = maxYoung - youngModulus; + m_lambdaMin[0] =- (youngModulus[0] - minYoung); + m_lambdaMax[0] = maxYoung - youngModulus[0]; - double youngMin = youngModulus - youngModulus * maxYoungVariationRatio; + double youngMin = youngModulus[0] - youngModulus[0] * maxYoungVariationRatio; if(youngMin >= minYoung) - m_lambdaMin[0] =- youngModulus * maxYoungVariationRatio; + m_lambdaMin[0] =- youngModulus[0] * maxYoungVariationRatio; - double youngMax = youngModulus + youngModulus * maxYoungVariationRatio; + double youngMax = youngModulus[0] + youngModulus[0] * maxYoungVariationRatio; if(youngMax <= maxYoung) - m_lambdaMax[0] = youngModulus * maxYoungVariationRatio; + m_lambdaMax[0] = youngModulus[0] * maxYoungVariationRatio; } @@ -134,6 +128,8 @@ void YoungModulusActuator::bwdInit() msg_info() << "Found force field named " << l_forceField->getName(); m_initialYoungModulus = l_forceField->d_youngModulus.getValue()[0]; + auto youngModulus = sofa::helper::getWriteAccessor(d_lambda); + youngModulus[0] = m_initialYoungModulus; d_youngModulus.setValue(m_initialYoungModulus); initLimit(); } @@ -152,6 +148,8 @@ void YoungModulusActuator::reset() if(d_componentState.getValue() == ComponentState::Invalid) return; + auto youngModulus = sofa::helper::getWriteAccessor(d_lambda); + youngModulus[0] = m_initialYoungModulus; d_youngModulus.setValue(m_initialYoungModulus); initLimit(); } @@ -179,6 +177,8 @@ void YoungModulusActuator::buildConstraintMatrix(const ConstraintPara MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex); const auto& youngModulus = l_forceField->d_youngModulus.getValue()[0]; + auto lambda = sofa::helper::getWriteAccessor(d_lambda); + lambda[0] = youngModulus; d_youngModulus.setValue(youngModulus); for (unsigned int j=0; j::storeResults(vector &lambda, vecto if(d_componentState.getValue() == ComponentState::Invalid) return; + auto l = sofa::helper::getWriteAccessor(d_lambda); + l[0] += Real(lambda[0]); Real youngModulus = sofa::helper::getWriteAccessor(d_youngModulus); youngModulus += Real(lambda[0]); diff --git a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp index 95e53bf..fa772a2 100644 --- a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp +++ b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp @@ -159,6 +159,8 @@ QPInverseProblemSolver::QPInverseProblemSolver() , d_objective(initData(&d_objective, 0.0, "objective", "Calculated optimal objective function value.")) + , d_mode(initData(&d_mode, sofa::helper::OptionsGroup{"inverse", "skipSolve"}, "mode", "Solver mode. In the `skipSolve` case, the component will skip the solving step.")) + , m_CP1(nullptr) , m_CP2(nullptr) , m_CP3(nullptr) @@ -496,12 +498,12 @@ bool QPInverseProblemSolver::solveSystem(const ConstraintParams * cParams, if(d_minContactForces.isSet()) m_currentCP->setMinContactForces(d_minContactForces.getValue()); if(d_maxContactForces.isSet()) m_currentCP->setMaxContactForces(d_maxContactForces.getValue()); - double objective; - int iterations; + double objective = 0.; + int iterations = 0; + if (d_mode.getValue().getSelectedId() == 0) { sofa::helper::ScopedAdvancedTimer("ConstraintsQP"); m_currentCP->solve(objective, iterations); - } module::QPInverseProblem::QPConstraintLists* qpCLists = m_currentCP->getQPConstraintLists(); diff --git a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h index 5ba881e..6f22ecb 100644 --- a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h +++ b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h @@ -156,6 +156,8 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::componen sofa::Data d_maxContactForces; sofa::Data d_objective; + sofa::Data d_mode; + protected: MultiVecDerivId m_lambdaId;