diff --git a/src/SoftRobots/component/behavior/SoftRobotsConstraint.h b/src/SoftRobots/component/behavior/SoftRobotsConstraint.h index b6f06d60..1a4534c7 100644 --- a/src/SoftRobots/component/behavior/SoftRobotsConstraint.h +++ b/src/SoftRobots/component/behavior/SoftRobotsConstraint.h @@ -71,12 +71,7 @@ class SoftRobotsConstraint : public softrobots::behavior::SoftRobotsBaseConstrai ///< if false, the constraint does nothing virtual bool isActive() const; - /// Construct the constraint violations vector of each constraint - /// - /// \param resV is the result vector that contains the whole constraint violations - /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - void getConstraintViolation(const sofa::core::ConstraintParams* cParams, - BaseVector *resV) override; + /// Construct the constraint violations vector of each constraint /// @@ -90,16 +85,6 @@ class SoftRobotsConstraint : public softrobots::behavior::SoftRobotsBaseConstrai BaseVector *resV, const BaseVector *Jdx) = 0; - /// Construct the Jacobian Matrix - /// - /// \param cId is the result constraint sparse matrix Id - /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it - /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - /// - /// Warning: is constraint matrix is built with the current position. Free configuration available in cParams - void buildConstraintMatrix(const sofa::core::ConstraintParams* cParams, - sofa::core::MultiMatrixDerivId cId, - unsigned int &cIndex) override; /// Construct the Jacobian Matrix /// @@ -126,6 +111,23 @@ class SoftRobotsConstraint : public softrobots::behavior::SoftRobotsBaseConstrai sofa::core::behavior::MechanicalState *m_state { nullptr }; ///< Associated mechanical state + /// Construct the constraint violations vector of each constraint + /// + /// \param resV is the result vector that contains the whole constraint violations + /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) + void doGetConstraintViolation(const sofa::core::ConstraintParams* cParams, + BaseVector *resV) override; + + /// Construct the Jacobian Matrix + /// + /// \param cId is the result constraint sparse matrix Id + /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it + /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) + /// + /// Warning: is constraint matrix is built with the current position. Free configuration available in cParams + void doBuildConstraintMatrix(const sofa::core::ConstraintParams* cParams, + sofa::core::MultiMatrixDerivId cId, + unsigned int &cIndex) override; private: void storeLambda(const sofa::core::ConstraintParams* cParams, sofa::Data& resId, const sofa::Data& jacobian, const sofa::linearalgebra::BaseVector* lambda); }; diff --git a/src/SoftRobots/component/behavior/SoftRobotsConstraint.inl b/src/SoftRobots/component/behavior/SoftRobotsConstraint.inl index de137dcb..d1893e2b 100644 --- a/src/SoftRobots/component/behavior/SoftRobotsConstraint.inl +++ b/src/SoftRobots/component/behavior/SoftRobotsConstraint.inl @@ -76,7 +76,7 @@ void SoftRobotsConstraint::init() template -void SoftRobotsConstraint::getConstraintViolation(const sofa::core::ConstraintParams* cParams, +void SoftRobotsConstraint::doGetConstraintViolation(const sofa::core::ConstraintParams* cParams, BaseVector *resV) { if (cParams) @@ -107,7 +107,7 @@ void SoftRobotsConstraint::getConstraintViolation(const sofa::core::C template -void SoftRobotsConstraint::buildConstraintMatrix(const sofa::core::ConstraintParams* cParams, +void SoftRobotsConstraint::doBuildConstraintMatrix(const sofa::core::ConstraintParams* cParams, sofa::core::MultiMatrixDerivId cId, unsigned int &cIndex) {