moved to task.update(x9 to task.update().

ADVRHumanoids · Mar 25, 2024 · fa9b051 · fa9b051
1 parent 63479ea
commit fa9b051
Show file tree

Hide file tree

Showing 109 changed files with 450 additions and 529 deletions.
diff --git a/include/OpenSoT/SubTask.h b/include/OpenSoT/SubTask.h
@@ -63,7 +63,7 @@
 
         /** Updates the A, b, Aeq, beq, Aineq, b*Bound matrices
             @param x variable state at the current step (input) */
-        virtual void _update(const Eigen::VectorXd &x);
+        virtual void _update();
 
         static const std::string _SUBTASK_SEPARATION_;
 

diff --git a/include/OpenSoT/Task.h b/include/OpenSoT/Task.h
@@ -112,9 +112,8 @@
          */
         std::vector<bool> _active_joints_mask;
 
-        /** Updates the A, b, Aeq, beq, Aineq, b*Bound matrices
-            @param x variable state at the current step (input) */
-        virtual void _update(const Vector_type &x) = 0;
+        /** Updates the A, b, Aeq, beq, Aineq, b*Bound matrices */
+        virtual void _update() = 0;
 
         struct istrue //predicate
         {
@@ -372,14 +371,13 @@
             @return the number of rows of A */
         virtual const unsigned int getTaskSize() const { return _A.rows(); }
 
-        /** Updates the A, b, Aeq, beq, Aineq, b*Bound matrices
-            @param x variable state at the current step (input) */
-        void update(const Vector_type &x) {
+        /** Updates the A, b, Aeq, beq, Aineq, b*Bound matrices */
+        void update() {
 
 
             for(typename std::list< ConstraintPtr >::iterator i = this->getConstraints().begin();
                 i != this->getConstraints().end(); ++i) (*i)->update();
-            this->_update(x);
+            this->_update();
 
             if(!_is_active){
                 _A_last_active = _A;

diff --git a/include/OpenSoT/solvers/l1HQP.h b/include/OpenSoT/solvers/l1HQP.h
@@ -32,7 +32,7 @@ namespace solvers {
         priority_constraint(const std::string& id,
                             const OpenSoT::tasks::GenericLPTask::Ptr high_priority_task,
                             const OpenSoT::tasks::GenericLPTask::Ptr low_priority_task);
-        void update(const Eigen::VectorXd& x);
+        void update();
     private:
         std::weak_ptr<OpenSoT::tasks::GenericLPTask> _high_task;
         std::weak_ptr<OpenSoT::tasks::GenericLPTask> _low_task;
@@ -48,7 +48,7 @@ namespace solvers {
         constraint_helper(std::string id, OpenSoT::constraints::Aggregated::ConstraintPtr constraints,
                           const AffineHelper& x);
 
-        void update(const Eigen::VectorXd& x);
+        void update();
     private:
         OpenSoT::constraints::Aggregated::ConstraintPtr _constraints;
         AffineHelper _constraint;
@@ -79,7 +79,7 @@ namespace solvers {
         task_to_constraint_helper(std::string id, OpenSoT::tasks::Aggregated::TaskPtr& task,
                                   const AffineHelper& x, const AffineHelper& t);
 
-        void update(const Eigen::VectorXd& x);
+        void update();
     private:
         OpenSoT::tasks::Aggregated::TaskPtr& _task;
         AffineHelper _constraint;

diff --git a/include/OpenSoT/tasks/Aggregated.h b/include/OpenSoT/tasks/Aggregated.h
@@ -145,7 +145,7 @@ using namespace OpenSoT::utils;
 
             ~Aggregated();
 
-            void _update(const Eigen::VectorXd &x);
+            void _update();
 
 
             /**

diff --git a/include/OpenSoT/tasks/GenericLPTask.h b/include/OpenSoT/tasks/GenericLPTask.h
@@ -37,7 +37,7 @@ class GenericLPTask: public Task<Eigen::MatrixXd, Eigen::VectorXd> {
 
     ~GenericLPTask();
 
-    virtual void _update(const Eigen::VectorXd &x);
+    virtual void _update();
 
     /**
      * @brief setc update the c of the task

diff --git a/include/OpenSoT/tasks/GenericTask.h b/include/OpenSoT/tasks/GenericTask.h
@@ -40,7 +40,7 @@ class GenericTask: public Task<Eigen::MatrixXd, Eigen::VectorXd> {
 
     ~GenericTask();
 
-    virtual void _update(const Eigen::VectorXd &x);
+    virtual void _update();
 
     /**
      * @brief setA update the A matrix of the task

diff --git a/include/OpenSoT/tasks/MinimizeVariable.h b/include/OpenSoT/tasks/MinimizeVariable.h
@@ -37,7 +37,7 @@ class MinimizeVariable : public Task<Eigen::MatrixXd, Eigen::VectorXd> {
 
     bool setReference(const Eigen::VectorXd& ref);
 
-    virtual void _update(const Eigen::VectorXd& x);
+    virtual void _update();
 
     void getReference(Eigen::VectorXd& ref);
 

diff --git a/include/OpenSoT/tasks/acceleration/AngularMomentum.h b/include/OpenSoT/tasks/acceleration/AngularMomentum.h
@@ -54,7 +54,7 @@ namespace OpenSoT {
            Eigen::MatrixXd _Mom;
            Eigen::Matrix3d _K;
 
-           void _update(const Eigen::VectorXd& x);
+           void _update();
 
            std::string _base_link;
            std::string _distal_link;

diff --git a/include/OpenSoT/tasks/acceleration/Cartesian.h b/include/OpenSoT/tasks/acceleration/Cartesian.h
@@ -145,7 +145,7 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
 
         bool reset();
 
-        virtual void _update(const Eigen::VectorXd& x);
+        virtual void _update();
 
         virtual void _log(XBot::MatLogger2::Ptr logger);
 

diff --git a/include/OpenSoT/tasks/acceleration/CoM.h b/include/OpenSoT/tasks/acceleration/CoM.h
@@ -93,7 +93,7 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
 
         bool reset() override;
 
-        virtual void _update(const Eigen::VectorXd& x);
+        virtual void _update();
 
         virtual void _log(XBot::MatLogger2::Ptr logger);
 

diff --git a/include/OpenSoT/tasks/acceleration/Contact.h b/include/OpenSoT/tasks/acceleration/Contact.h
@@ -46,7 +46,7 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
                  );
 
 
-        virtual void _update(const Eigen::VectorXd& x);
+        virtual void _update();
 
         virtual void _log(XBot::MatLogger2::Ptr logger);
 

diff --git a/include/OpenSoT/tasks/acceleration/DynamicFeasibility.h b/include/OpenSoT/tasks/acceleration/DynamicFeasibility.h
@@ -63,7 +63,7 @@ namespace OpenSoT {
             Eigen::VectorXd checkTask(const Eigen::VectorXd& x);
 
         private:
-                virtual void _update(const Eigen::VectorXd& x);
+                virtual void _update();
 
                 const XBot::ModelInterface& _robot;
                 AffineHelper _qddot;

diff --git a/include/OpenSoT/tasks/acceleration/MinJointVel.h b/include/OpenSoT/tasks/acceleration/MinJointVel.h
@@ -40,7 +40,7 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
         OpenSoT::tasks::acceleration::Postural::Ptr _postural;
         Eigen::MatrixXd I;
 
-        virtual void _update(const Eigen::VectorXd& x);
+        virtual void _update();
 
 
     };

diff --git a/include/OpenSoT/tasks/acceleration/Postural.h b/include/OpenSoT/tasks/acceleration/Postural.h
@@ -47,7 +47,7 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
         void setGainType(GainType type);
         GainType getGainType() const;
 
-        virtual void _update(const Eigen::VectorXd& x);
+        virtual void _update();
 
         /**
          * @brief setReference sets a new reference for the postural actuated part.

diff --git a/include/OpenSoT/tasks/floating_base/Contact.h b/include/OpenSoT/tasks/floating_base/Contact.h
@@ -45,7 +45,7 @@ namespace OpenSoT{
                         const Eigen::MatrixXd& contact_matrix = Eigen::MatrixXd::Identity(6,6), 
                         const Eigen::Affine3d& desired_contact_pose = Eigen::Affine3d::Identity() );
                 ~Contact();
-                virtual void _update(const Eigen::VectorXd& x);
+                virtual void _update();
 
                 void setLinkInContact(const std::string link_in_contact);
                 const std::string& getLinkInContact() const;

diff --git a/include/OpenSoT/tasks/floating_base/IMU.h b/include/OpenSoT/tasks/floating_base/IMU.h
@@ -41,7 +41,7 @@ namespace OpenSoT{
                 IMU(XBot::ModelInterface& robot, XBot::ImuSensor::ConstPtr imu);
                 ~IMU();
 
-                void _update(const Eigen::VectorXd& x);
+                void _update();
 
             private:
                 Eigen::MatrixXd _J;

diff --git a/include/OpenSoT/tasks/force/Cartesian.h b/include/OpenSoT/tasks/force/Cartesian.h
@@ -94,7 +94,7 @@ namespace OpenSoT { namespace tasks { namespace force {
 
     static const std::string world_name;
 
-    virtual void _update(const Eigen::VectorXd& x);
+    virtual void _update();
     virtual void _log(XBot::MatLogger2::Ptr logger);
 
     const XBot::ModelInterface& _robot;

diff --git a/include/OpenSoT/tasks/force/CoM.h b/include/OpenSoT/tasks/force/CoM.h
@@ -52,7 +52,6 @@
 
                 virtual void _log(XBot::MatLogger2::Ptr logger);
 
-                AffineHelper _wrenches;
                 AffineHelper _com_task;
 
                 XBot::ModelInterface& _robot;
@@ -108,14 +107,7 @@
                 Eigen::Vector3d velocityError;
                 Eigen::Vector3d angularMomentumError;
 
-                /**
-                 * @brief CoM
-                 * @param x the initial configuration of the robot
-                 * @param robot the robot model, with floating base link set on the support foot
-                 */
-                CoM(const Eigen::VectorXd& x, 
-                    std::vector<std::string>& links_in_contact,
-                    XBot::ModelInterface& robot);
+
 
 
                 CoM(std::vector<AffineHelper> wrenches,
@@ -124,7 +116,7 @@
 
                 ~CoM();
 
-                void _update(const Eigen::VectorXd& x);
+                void _update();
 
 
                 void setLinearReference(const Eigen::Vector3d& desiredPosition);

diff --git a/include/OpenSoT/tasks/force/FloatingBase.h b/include/OpenSoT/tasks/force/FloatingBase.h
@@ -56,7 +56,7 @@ namespace OpenSoT {
             void setEnabledContacts(const std::vector<bool>& enabled_contacts);
 
             private:
-            virtual void _update(const Eigen::VectorXd& x);
+            virtual void _update();
             virtual void _log(XBot::MatLogger2::Ptr logger);
 
             std::vector<std::string> _contact_links;

diff --git a/include/OpenSoT/tasks/force/Force.h b/include/OpenSoT/tasks/force/Force.h
@@ -57,7 +57,7 @@ namespace OpenSoT {
             const std::string& getDistalLink() const;
             const std::string& getBaseLink() const;
         protected:
-            virtual void _update(const Eigen::VectorXd& x);    
+            virtual void _update();
         private:
             std::string _distal_link, _base_link;
             OpenSoT::tasks::MinimizeVariable::Ptr _min_var;
@@ -80,7 +80,7 @@ namespace OpenSoT {
         private:
             std::map<std::string, Wrench::Ptr> wrench_tasks;
             OpenSoT::tasks::Aggregated::Ptr _aggregated_task;
-            virtual void _update(const Eigen::VectorXd& x);
+            virtual void _update();
 
 
         };

diff --git a/include/OpenSoT/tasks/velocity/AngularMomentum.h b/include/OpenSoT/tasks/velocity/AngularMomentum.h
@@ -48,7 +48,7 @@ namespace OpenSoT {
 
            Eigen::MatrixXd _Momentum;
 
-           void _update(const Eigen::VectorXd& x);
+           void _update();
 
            std::string _base_link;
            std::string _distal_link;

diff --git a/include/OpenSoT/tasks/velocity/Cartesian.h b/include/OpenSoT/tasks/velocity/Cartesian.h
@@ -111,7 +111,7 @@
 
                 ~Cartesian();
 
-                virtual void _update(const Eigen::VectorXd& x);
+                virtual void _update();
 
                 /**
                  * @brief setReference sets a new reference for the Cartesian task.

diff --git a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h
@@ -421,7 +421,7 @@ namespace OpenSoT {
            Eigen::Vector6d _deadzone;
            std::vector<double> _tmp;
 
-           void _update(const Eigen::VectorXd& x);
+           void _update();
 
            SecondOrderFilterArray<double> _filter;
 

diff --git a/include/OpenSoT/tasks/velocity/CoM.h b/include/OpenSoT/tasks/velocity/CoM.h
@@ -69,7 +69,7 @@
 
                 ~CoM();
 
-                virtual void _update(const Eigen::VectorXd& x);
+                virtual void _update();
 
                 /**
                  * @brief setReference sets a new reference for the CoM task.

diff --git a/include/OpenSoT/tasks/velocity/Contact.h b/include/OpenSoT/tasks/velocity/Contact.h
@@ -42,7 +42,7 @@ class Contact : public Task <Eigen::MatrixXd, Eigen::VectorXd> {
     ~Contact();
 
 
-    void _update(const Eigen::VectorXd& x);
+    void _update();
 
     const std::string& getLinkName() const;
 

diff --git a/include/OpenSoT/tasks/velocity/Gaze.h b/include/OpenSoT/tasks/velocity/Gaze.h
@@ -88,7 +88,7 @@ class Gaze: public OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>
 
     /** Updates the A, b, Aeq, beq, Aineq, b*Bound matrices
         @param x variable state at the current step (input) */
-    virtual void _update(const Eigen::VectorXd &x);
+    virtual void _update();
 
     /**
      * @brief getActiveJointsMask return a vector of length NumberOfDOFs.

diff --git a/include/OpenSoT/tasks/velocity/JointAdmittance.h b/include/OpenSoT/tasks/velocity/JointAdmittance.h
@@ -232,7 +232,7 @@ namespace OpenSoT {
            XBot::ModelInterface& _robot;
            XBot::ModelInterface& _model;
 
-           void _update(const Eigen::VectorXd& x);
+           void _update();
 
            SecondOrderFilter<Eigen::VectorXd> _filter;
 

diff --git a/include/OpenSoT/tasks/velocity/LinearMomentum.h b/include/OpenSoT/tasks/velocity/LinearMomentum.h
@@ -50,7 +50,7 @@ namespace OpenSoT {
 
            Eigen::MatrixXd _Momentum;
 
-           void _update(const Eigen::VectorXd& x);
+           void _update();
 
         public:
            /**

diff --git a/include/OpenSoT/tasks/velocity/Manipulability.h b/include/OpenSoT/tasks/velocity/Manipulability.h
@@ -47,7 +47,7 @@ namespace OpenSoT {
 
                 ~Manipulability();
 
-                void _update(const Eigen::VectorXd& x);
+                void _update();
 
                 /**
                  * @brief ComputeManipulabilityIndex
@@ -74,7 +74,7 @@ namespace OpenSoT {
                 {
                     if(lambda >= 0.0){
                         _lambda = lambda;
-                        this->_update(Eigen::VectorXd(0));
+                        this->_update();
                     }
                 }
 
@@ -134,7 +134,7 @@ namespace OpenSoT {
                         _robot->update();
 
 
-                        _CartesianTask->update(q);
+                        _CartesianTask->update();
 
                         return computeManipulabilityIndex();
                     }

diff --git a/include/OpenSoT/tasks/velocity/MinimumEffort.h b/include/OpenSoT/tasks/velocity/MinimumEffort.h
@@ -107,7 +107,7 @@
                  * computeEffort() function will take into account the updated posture of the robot.
                  * @param x the actual posture of the robot
                  */
-                void _update(const Eigen::VectorXd& x);
+                void _update();
 
                 /**
                  * @brief computeEffort
@@ -134,7 +134,7 @@
                 {
                     if(lambda >= 0.0){
                         _lambda = lambda;
-                        this->_update(Eigen::VectorXd(0));
+                        this->_update();
                     }
                 }
             };

diff --git a/include/OpenSoT/tasks/velocity/Postural.h b/include/OpenSoT/tasks/velocity/Postural.h
@@ -48,7 +48,7 @@
 
                 ~Postural();
 
-                virtual void _update(const Eigen::VectorXd& x);
+                virtual void _update();
 
                 /**
                  * @brief setReference sets a new reference for the Postural task.