From fa9b05129c5af37d49e3774b370c2e64f09a8c4a Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Mon, 25 Mar 2024 23:57:41 +0100 Subject: [PATCH] moved to task.update(x9 to task.update(). --- include/OpenSoT/SubTask.h | 2 +- include/OpenSoT/Task.h | 12 ++- include/OpenSoT/solvers/l1HQP.h | 6 +- include/OpenSoT/tasks/Aggregated.h | 2 +- include/OpenSoT/tasks/GenericLPTask.h | 2 +- include/OpenSoT/tasks/GenericTask.h | 2 +- include/OpenSoT/tasks/MinimizeVariable.h | 2 +- .../tasks/acceleration/AngularMomentum.h | 2 +- .../OpenSoT/tasks/acceleration/Cartesian.h | 2 +- include/OpenSoT/tasks/acceleration/CoM.h | 2 +- include/OpenSoT/tasks/acceleration/Contact.h | 2 +- .../tasks/acceleration/DynamicFeasibility.h | 2 +- .../OpenSoT/tasks/acceleration/MinJointVel.h | 2 +- include/OpenSoT/tasks/acceleration/Postural.h | 2 +- include/OpenSoT/tasks/floating_base/Contact.h | 2 +- include/OpenSoT/tasks/floating_base/IMU.h | 2 +- include/OpenSoT/tasks/force/Cartesian.h | 2 +- include/OpenSoT/tasks/force/CoM.h | 12 +-- include/OpenSoT/tasks/force/FloatingBase.h | 2 +- include/OpenSoT/tasks/force/Force.h | 4 +- .../OpenSoT/tasks/velocity/AngularMomentum.h | 2 +- include/OpenSoT/tasks/velocity/Cartesian.h | 2 +- .../tasks/velocity/CartesianAdmittance.h | 2 +- include/OpenSoT/tasks/velocity/CoM.h | 2 +- include/OpenSoT/tasks/velocity/Contact.h | 2 +- include/OpenSoT/tasks/velocity/Gaze.h | 2 +- .../OpenSoT/tasks/velocity/JointAdmittance.h | 2 +- .../OpenSoT/tasks/velocity/LinearMomentum.h | 2 +- .../OpenSoT/tasks/velocity/Manipulability.h | 6 +- .../OpenSoT/tasks/velocity/MinimumEffort.h | 4 +- include/OpenSoT/tasks/velocity/Postural.h | 2 +- include/OpenSoT/tasks/velocity/PureRolling.h | 6 +- include/OpenSoT/utils/AffineUtils.h | 2 +- include/OpenSoT/utils/AutoStack.h | 2 +- src/constraints/TaskToConstraint.cpp | 2 +- .../velocity/CartesianPositionConstraint.cpp | 4 +- src/solvers/l1HQP.cpp | 14 +-- src/solvers/nHQP.cpp | 2 +- src/tasks/Aggregated.cpp | 4 +- src/tasks/GenericLPTask.cpp | 12 +-- src/tasks/GenericTask.cpp | 8 +- src/tasks/MinimizeVariable.cpp | 2 +- src/tasks/SubTask.cpp | 4 +- src/tasks/acceleration/AngularMomentum.cpp | 4 +- src/tasks/acceleration/Cartesian.cpp | 6 +- src/tasks/acceleration/CoM.cpp | 6 +- src/tasks/acceleration/Contact.cpp | 6 +- src/tasks/acceleration/DynamicFeasibility.cpp | 4 +- src/tasks/acceleration/MinJointVel.cpp | 6 +- src/tasks/acceleration/Postural.cpp | 6 +- src/tasks/floating_base/Contact.cpp | 4 +- src/tasks/floating_base/IMU.cpp | 4 +- src/tasks/force/Cartesian.cpp | 4 +- src/tasks/force/CoM.cpp | 90 ++----------------- src/tasks/force/FloatingBase.cpp | 4 +- src/tasks/force/Force.cpp | 12 +-- src/tasks/velocity/AngularMomentum.cpp | 4 +- src/tasks/velocity/Cartesian.cpp | 6 +- src/tasks/velocity/CartesianAdmittance.cpp | 4 +- src/tasks/velocity/CoM.cpp | 4 +- src/tasks/velocity/Contact.cpp | 4 +- src/tasks/velocity/Gaze.cpp | 6 +- src/tasks/velocity/JointAdmittance.cpp | 9 +- src/tasks/velocity/LinearMomentum.cpp | 4 +- src/tasks/velocity/Manipulability.cpp | 6 +- src/tasks/velocity/MinimumEffort.cpp | 4 +- src/tasks/velocity/Postural.cpp | 6 +- src/tasks/velocity/PureRolling.cpp | 20 ++--- src/utils/AffineUtils.cpp | 8 +- src/utils/AutoStack.cpp | 6 +- src/utils/ForceOptimization.cpp | 2 +- .../acceleration/TestJointLimits.cpp | 8 +- .../acceleration/TestJointLimitsECBF.cpp | 8 +- .../acceleration/TestJointLimitsViability.cpp | 8 +- tests/constraints/force/TestFrictionCones.cpp | 12 ++- .../velocity/TestCollisionAvoidance.cpp | 20 ++--- .../velocity/TestJointLimitsInvariance.cpp | 8 +- .../TestEigenSVD_StaticWalk_FloatingBase.cpp | 4 +- tests/solvers/TestGLPK.cpp | 6 +- tests/solvers/TestLP.cpp | 6 +- tests/solvers/TestOSQP.cpp | 24 ++--- tests/solvers/TestQPOases.cpp | 32 +++---- tests/solvers/TestQPOases_AutoStack.cpp | 6 +- tests/solvers/TestQPOases_ConvexHull.cpp | 16 ++-- tests/solvers/TestQPOases_FF.cpp | 30 +++---- tests/solvers/TestQPOases_Options.cpp | 24 ++--- tests/solvers/TestQPOases_SetActiveStack.cpp | 34 +++---- tests/solvers/TestQPOases_SubTask.cpp | 6 +- tests/solvers/TestSOTH.cpp | 2 +- tests/solvers/TesteiQuadProg.cpp | 24 ++--- tests/solvers/TestiHQP.cpp | 4 +- tests/solvers/Testl1HQP.cpp | 4 +- tests/solvers/TestqpSWIFT.cpp | 26 +++--- tests/tasks/TestAggregated.cpp | 32 +++---- tests/tasks/TestGenericTask.cpp | 12 +-- tests/tasks/TestSubTask.cpp | 36 ++++---- tests/tasks/TestTask.cpp | 8 +- tests/tasks/acceleration/TestCartesian.cpp | 4 +- tests/tasks/acceleration/TestCoM.cpp | 4 +- tests/tasks/acceleration/TestPostural.cpp | 8 +- tests/tasks/floating_base/TestContact.cpp | 8 +- tests/tasks/force/TestCoM.cpp | 76 +++++++++------- tests/tasks/velocity/TestCartesian.cpp | 30 +++---- tests/tasks/velocity/TestCoM.cpp | 6 +- tests/tasks/velocity/TestManipulability.cpp | 12 +-- tests/tasks/velocity/TestMinimumEffort.cpp | 6 +- tests/tasks/velocity/TestPostural.cpp | 8 +- tests/utils/TestAffineUtils.cpp | 2 +- tests/utils/TestAutoStack.cpp | 10 +-- 109 files changed, 450 insertions(+), 529 deletions(-) diff --git a/include/OpenSoT/SubTask.h b/include/OpenSoT/SubTask.h index fc09a67d..7edad868 100644 --- 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 index e3bd9587..6e7134e4 100644 --- a/include/OpenSoT/Task.h +++ b/include/OpenSoT/Task.h @@ -112,9 +112,8 @@ */ std::vector _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 index e60d6069..ddcbe083 100644 --- 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 _high_task; std::weak_ptr _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 index 3352f6f5..d6fae0bf 100644 --- 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 index b22d459c..57497b27 100644 --- a/include/OpenSoT/tasks/GenericLPTask.h +++ b/include/OpenSoT/tasks/GenericLPTask.h @@ -37,7 +37,7 @@ class GenericLPTask: public Task { ~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 index d2a858c4..a3687743 100644 --- a/include/OpenSoT/tasks/GenericTask.h +++ b/include/OpenSoT/tasks/GenericTask.h @@ -40,7 +40,7 @@ class GenericTask: public Task { ~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 index b8be2e23..f730bdaa 100644 --- a/include/OpenSoT/tasks/MinimizeVariable.h +++ b/include/OpenSoT/tasks/MinimizeVariable.h @@ -37,7 +37,7 @@ class MinimizeVariable : public Task { 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 index 94e4f4fa..c145833c 100644 --- 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 index 6d060c9f..9226b6c3 100644 --- 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 index 3c6d0823..f95863c6 100644 --- 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 index 622658c3..e9d68247 100644 --- 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 index 18a22908..f4989d87 100644 --- 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 index 3939673b..8ea6fd48 100644 --- 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 index 72f8bea0..2c3bddb4 100644 --- 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 index 2963e357..d4ea14b0 100755 --- 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 index b7ce0428..f3b97dad 100644 --- 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 index 70ad76cf..0b2e5142 100644 --- 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 index e31c0ada..16578d23 100644 --- 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& links_in_contact, - XBot::ModelInterface& robot); + CoM(std::vector 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 index 8e087a4c..3f730c76 100644 --- 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& enabled_contacts); private: - virtual void _update(const Eigen::VectorXd& x); + virtual void _update(); virtual void _log(XBot::MatLogger2::Ptr logger); std::vector _contact_links; diff --git a/include/OpenSoT/tasks/force/Force.h b/include/OpenSoT/tasks/force/Force.h index 35930daf..7d0cdda4 100644 --- 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 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 index beda2918..9a4a14e6 100644 --- 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 index 026479fc..dfef2614 100644 --- 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 index 89b41820..cf6e1c95 100644 --- 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 _tmp; - void _update(const Eigen::VectorXd& x); + void _update(); SecondOrderFilterArray _filter; diff --git a/include/OpenSoT/tasks/velocity/CoM.h b/include/OpenSoT/tasks/velocity/CoM.h index 3a9adecb..c9e5858e 100644 --- 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 index 9ec89284..3f4aef66 100644 --- a/include/OpenSoT/tasks/velocity/Contact.h +++ b/include/OpenSoT/tasks/velocity/Contact.h @@ -42,7 +42,7 @@ class Contact : public Task { ~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 index d3e4aa19..b8bf9e28 100644 --- a/include/OpenSoT/tasks/velocity/Gaze.h +++ b/include/OpenSoT/tasks/velocity/Gaze.h @@ -88,7 +88,7 @@ class Gaze: public OpenSoT::Task /** 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 index ef5a9ca7..ec7763bd 100644 --- 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 _filter; diff --git a/include/OpenSoT/tasks/velocity/LinearMomentum.h b/include/OpenSoT/tasks/velocity/LinearMomentum.h index d1590f3f..e7eb611d 100644 --- 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 index 70c45538..276a39ae 100644 --- 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 index 296b4b56..23af8514 100644 --- 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 index a5ba4383..efeba97e 100644 --- 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. diff --git a/include/OpenSoT/tasks/velocity/PureRolling.h b/include/OpenSoT/tasks/velocity/PureRolling.h index 3bbb2dd4..5c2b1dd4 100644 --- a/include/OpenSoT/tasks/velocity/PureRolling.h +++ b/include/OpenSoT/tasks/velocity/PureRolling.h @@ -38,7 +38,7 @@ namespace OpenSoT { namespace tasks { namespace velocity { double radius, const XBot::ModelInterface& model); - virtual void _update(const Eigen::VectorXd& x); + virtual void _update(); void setOutwardNormal(const Eigen::Vector3d& n); @@ -76,7 +76,7 @@ namespace OpenSoT { namespace tasks { namespace velocity { void setOutwardNormal(const Eigen::Vector3d& n); - virtual void _update(const Eigen::VectorXd& x); + virtual void _update(); virtual void _log(XBot::MatLogger2::Ptr logger); @@ -98,7 +98,7 @@ namespace OpenSoT { namespace tasks { namespace velocity { double radius, const XBot::ModelInterface& model); - virtual void _update(const Eigen::VectorXd& x); + virtual void _update(); private: diff --git a/include/OpenSoT/utils/AffineUtils.h b/include/OpenSoT/utils/AffineUtils.h index e7877e6a..7de360bb 100644 --- a/include/OpenSoT/utils/AffineUtils.h +++ b/include/OpenSoT/utils/AffineUtils.h @@ -21,7 +21,7 @@ class AffineTask: public Task AffineTask(const OpenSoT::tasks::Aggregated::TaskPtr& task, const AffineHelper& var); - virtual void _update(const Eigen::VectorXd &x); + virtual void _update(); ~AffineTask(); static AffineTask::Ptr toAffine(const OpenSoT::tasks::Aggregated::TaskPtr& task, diff --git a/include/OpenSoT/utils/AutoStack.h b/include/OpenSoT/utils/AutoStack.h index f7551509..28adf121 100644 --- a/include/OpenSoT/utils/AutoStack.h +++ b/include/OpenSoT/utils/AutoStack.h @@ -68,7 +68,7 @@ namespace OpenSoT { /*AutoStack(OpenSoT::solvers::iHQP::Stack stack, OpenSoT::constraints::Aggregated::ConstraintPtr bound);*/ - void update(const Eigen::VectorXd & state); + void update(); void log(XBot::MatLogger2::Ptr logger); diff --git a/src/constraints/TaskToConstraint.cpp b/src/constraints/TaskToConstraint.cpp index f809c747..61773b77 100644 --- a/src/constraints/TaskToConstraint.cpp +++ b/src/constraints/TaskToConstraint.cpp @@ -54,7 +54,7 @@ TaskToConstraint::TaskToConstraint(TaskToConstraint::TaskPtr task, void TaskToConstraint::update() { - _task->update(Eigen::VectorXd(0)); + _task->update(); this->generateAll(); } diff --git a/src/constraints/velocity/CartesianPositionConstraint.cpp b/src/constraints/velocity/CartesianPositionConstraint.cpp index 23a90e7f..c66fa505 100644 --- a/src/constraints/velocity/CartesianPositionConstraint.cpp +++ b/src/constraints/velocity/CartesianPositionConstraint.cpp @@ -81,7 +81,7 @@ void CartesianPositionConstraint::getCurrentPosition(Eigen::VectorXd& current_po void CartesianPositionConstraint::update() { if(_is_Cartesian){ - _cartesianTask->update(Eigen::VectorXd(0)); + _cartesianTask->update(); /************************ COMPUTING BOUNDS ****************************/ J = _cartesianTask->getA().block(0,0,3,_x_size); assert(J.rows() == 3 && "Jacobian doesn't have 3 rows. Something went wrong."); @@ -95,7 +95,7 @@ void CartesianPositionConstraint::update() { /**********************************************************************/ }else{ - _comTask->update(Eigen::VectorXd(0)); + _comTask->update(); J = _comTask->getA(); _Aineq = _A_Cartesian * J; diff --git a/src/solvers/l1HQP.cpp b/src/solvers/l1HQP.cpp index 88726fc1..b3dbb2d8 100644 --- a/src/solvers/l1HQP.cpp +++ b/src/solvers/l1HQP.cpp @@ -83,7 +83,7 @@ void l1HQP::creates_internal_problem() _internal_stack = std::make_shared(aggregated, constraint_list); - _internal_stack->update(Eigen::VectorXd(0)); + _internal_stack->update(); } void l1HQP::creates_constraints() @@ -169,7 +169,7 @@ void l1HQP::creates_problem_variables() bool l1HQP::solve(Eigen::VectorXd& solution) { - _internal_stack->update(Eigen::VectorXd(0)); + _internal_stack->update(); if(!_solver->updateProblem(_H, _internal_stack->getStack()[0]->getc().transpose(), _internal_stack->getBounds()->getAineq(), @@ -224,10 +224,10 @@ task_to_constraint_helper::task_to_constraint_helper(std::string id, OpenSoT::ta _bLowerBound.resize(3*task->getA().rows()); _bLowerBound << -inf, -inf, o; - update(Eigen::VectorXd(0)); + update(); } -void task_to_constraint_helper::update(const Eigen::VectorXd& x) +void task_to_constraint_helper::update() { _AA.pile(_task->getWA()); _AA.pile(-_task->getWA()); @@ -254,10 +254,10 @@ constraint_helper::constraint_helper(std::string id, OpenSoT::constraints::Aggre if(_constraints->getLowerBound().size() > 0) I.setIdentity(_constraints->getLowerBound().size(), _constraints->getLowerBound().size()); - update(Eigen::VectorXd(0)); + update(); } -void constraint_helper::update(const Eigen::VectorXd& x) +void constraint_helper::update() { @@ -307,7 +307,7 @@ priority_constraint::priority_constraint(const std::string& id, _Aineq = _ones.transpose(); } -void priority_constraint::update(const Eigen::VectorXd &x) +void priority_constraint::update() { } diff --git a/src/solvers/nHQP.cpp b/src/solvers/nHQP.cpp index 52db609c..20b66077 100644 --- a/src/solvers/nHQP.cpp +++ b/src/solvers/nHQP.cpp @@ -27,7 +27,7 @@ OpenSoT::solvers::nHQP::nHQP(OpenSoT::Solver:: auto t = stack_of_tasks[i]; // update task (NB: with x = zeros(nx)) - t->update(_solution); + t->update(); // the current layer does not have any dof to move // the optimization problem is ill-formed diff --git a/src/tasks/Aggregated.cpp b/src/tasks/Aggregated.cpp index 255919d7..a70077a6 100644 --- a/src/tasks/Aggregated.cpp +++ b/src/tasks/Aggregated.cpp @@ -89,11 +89,11 @@ Aggregated::~Aggregated() { } -void Aggregated::_update(const Eigen::VectorXd& x) { +void Aggregated::_update() { for(std::list< TaskPtr >::iterator i = _tasks.begin(); i != _tasks.end(); ++i) { TaskPtr t = *i; - t->update(x); + t->update(); } this->generateAll(); diff --git a/src/tasks/GenericLPTask.cpp b/src/tasks/GenericLPTask.cpp index d0ba0b57..5c5c9078 100644 --- a/src/tasks/GenericLPTask.cpp +++ b/src/tasks/GenericLPTask.cpp @@ -10,9 +10,9 @@ GenericLPTask::GenericLPTask(const std::string &task_id, const Eigen::VectorXd & _task.reset(new GenericTask(task_id+"_internal", O, o)); _hessianType = HST_ZERO; _task->setc(c); - _task->update(Eigen::VectorXd(1)); + _task->update(); - _update(Eigen::VectorXd(1)); + _update(); _W = _task->getWeight(); _W.setZero(_W.rows(), _W.cols()); @@ -30,9 +30,9 @@ GenericLPTask::GenericLPTask(const std::string &task_id, const Eigen::VectorXd& _task.reset(new GenericTask(task_id+"_internal", O, o, var)); _hessianType = HST_ZERO; _task->setc(c); - _task->update(Eigen::VectorXd(1)); + _task->update(); - _update(Eigen::VectorXd(1)); + _update(); _W = _task->getWeight(); _W.setZero(_W.rows(), _W.cols()); @@ -41,9 +41,9 @@ GenericLPTask::GenericLPTask(const std::string &task_id, const Eigen::VectorXd& throw std::runtime_error(s.c_str());} } -void GenericLPTask::_update(const Eigen::VectorXd &x) +void GenericLPTask::_update() { - _task->update(x); + _task->update(); _A = _task->getA(); _b = _task->getb(); diff --git a/src/tasks/GenericTask.cpp b/src/tasks/GenericTask.cpp index 83936c6c..6546c5a5 100644 --- a/src/tasks/GenericTask.cpp +++ b/src/tasks/GenericTask.cpp @@ -17,7 +17,7 @@ GenericTask::GenericTask(const std::string &task_id, const Eigen::MatrixXd &A, c _hessianType = HST_SEMIDEF; - _update(Eigen::VectorXd(1)); + _update(); _W.setIdentity(_A.rows(), _A.rows()); } @@ -36,7 +36,7 @@ GenericTask::GenericTask(const std::string &task_id, const Eigen::MatrixXd &A, c _hessianType = HST_SEMIDEF; - _update(Eigen::VectorXd(1)); + _update(); _W.setIdentity(_A.rows(), _A.rows()); } @@ -46,7 +46,7 @@ GenericTask::~GenericTask() } -void GenericTask::_update(const Eigen::VectorXd &x) +void GenericTask::_update() { _task = __A*_var - __b; @@ -66,7 +66,7 @@ bool GenericTask::setc(const Eigen::VectorXd& c) __c = c; - _update(Eigen::VectorXd(1)); + _update(); return true; } diff --git a/src/tasks/MinimizeVariable.cpp b/src/tasks/MinimizeVariable.cpp index e3ac5c50..41c409b7 100644 --- a/src/tasks/MinimizeVariable.cpp +++ b/src/tasks/MinimizeVariable.cpp @@ -1,7 +1,7 @@ #include -void OpenSoT::tasks::MinimizeVariable::_update(const Eigen::VectorXd& x) +void OpenSoT::tasks::MinimizeVariable::_update() { } diff --git a/src/tasks/SubTask.cpp b/src/tasks/SubTask.cpp index ef07005c..f0ea713b 100644 --- a/src/tasks/SubTask.cpp +++ b/src/tasks/SubTask.cpp @@ -103,9 +103,9 @@ const unsigned int OpenSoT::SubTask::getTaskSize() const return size; } -void OpenSoT::SubTask::_update(const Eigen::VectorXd &x) +void OpenSoT::SubTask::_update() { - _taskPtr->update(x); + _taskPtr->update(); this->generateA(); this->generateb(); this->generateHessianAtype(); diff --git a/src/tasks/acceleration/AngularMomentum.cpp b/src/tasks/acceleration/AngularMomentum.cpp index cefe0a3c..6dc2c6ef 100644 --- a/src/tasks/acceleration/AngularMomentum.cpp +++ b/src/tasks/acceleration/AngularMomentum.cpp @@ -20,7 +20,7 @@ AngularMomentum::AngularMomentum(XBot::ModelInterface &robot, const AffineHelper _K.setIdentity(3,3); _Ldot_d.setZero(); - _update(Eigen::VectorXd(1)); + _update(); } AngularMomentum::~AngularMomentum() @@ -28,7 +28,7 @@ AngularMomentum::~AngularMomentum() } -void AngularMomentum::_update(const Eigen::VectorXd &x) +void AngularMomentum::_update() { //1. get centroidal momentum matrix and momentum _robot.computeCentroidalMomentumMatrix(_Mom); diff --git a/src/tasks/acceleration/Cartesian.cpp b/src/tasks/acceleration/Cartesian.cpp index f99c784b..14e4e374 100644 --- a/src/tasks/acceleration/Cartesian.cpp +++ b/src/tasks/acceleration/Cartesian.cpp @@ -41,7 +41,7 @@ Cartesian::Cartesian(const std::string task_id, _Kp.setIdentity(); _Kd.setIdentity(); - update(Eigen::VectorXd(1)); + update(); setWeight(Eigen::MatrixXd::Identity(6,6)); @@ -81,7 +81,7 @@ Cartesian::Cartesian(const std::string task_id, _Kp.setIdentity(); _Kd.setIdentity(); - update(Eigen::VectorXd(1)); + update(); setWeight(Eigen::MatrixXd::Identity(6,6)); @@ -124,7 +124,7 @@ const double Cartesian::getOrientationErrorGain() const } -void Cartesian::_update(const Eigen::VectorXd& x) +void Cartesian::_update() { _vel_ref_cached = _vel_ref; _acc_ref_cached = _acc_ref; diff --git a/src/tasks/acceleration/CoM.cpp b/src/tasks/acceleration/CoM.cpp index c15bfb5e..4882d7c5 100644 --- a/src/tasks/acceleration/CoM.cpp +++ b/src/tasks/acceleration/CoM.cpp @@ -28,7 +28,7 @@ OpenSoT::tasks::acceleration::CoM::CoM(const XBot::ModelInterface& robot): _Kd.setIdentity(); - update(Eigen::VectorXd(1)); + update(); setWeight(Eigen::MatrixXd::Identity(3,3)); } @@ -56,7 +56,7 @@ OpenSoT::tasks::acceleration::CoM::CoM(const XBot::ModelInterface &robot, const _Kd.setIdentity(); - update(Eigen::VectorXd(1)); + update(); setWeight(Eigen::MatrixXd::Identity(3,3)); } @@ -71,7 +71,7 @@ const std::string& OpenSoT::tasks::acceleration::CoM::getDistalLink() const return _distal_link; } -void OpenSoT::tasks::acceleration::CoM::_update(const Eigen::VectorXd& x) +void OpenSoT::tasks::acceleration::CoM::_update() { _vel_ref_cached = _vel_ref; _acc_ref_cached = _acc_ref; diff --git a/src/tasks/acceleration/Contact.cpp b/src/tasks/acceleration/Contact.cpp index 214f352b..d5f78a82 100644 --- a/src/tasks/acceleration/Contact.cpp +++ b/src/tasks/acceleration/Contact.cpp @@ -1,7 +1,7 @@ #include #include -void OpenSoT::tasks::acceleration::Contact::_update(const Eigen::VectorXd& x) +void OpenSoT::tasks::acceleration::Contact::_update() { _robot.getJacobian(_contact_link, _J); @@ -44,7 +44,7 @@ OpenSoT::tasks::acceleration::Contact::Contact(const std::string& task_id, throw std::invalid_argument("Invalid contact matrix"); } - update(Eigen::VectorXd()); + update(); _hessianType = HST_SEMIDEF; @@ -76,7 +76,7 @@ OpenSoT::tasks::acceleration::Contact::Contact(const std::string& task_id, throw std::invalid_argument("Invalid contact matrix"); } - update(Eigen::VectorXd()); + update(); _hessianType = HST_SEMIDEF; diff --git a/src/tasks/acceleration/DynamicFeasibility.cpp b/src/tasks/acceleration/DynamicFeasibility.cpp index 560f33f3..abadb084 100644 --- a/src/tasks/acceleration/DynamicFeasibility.cpp +++ b/src/tasks/acceleration/DynamicFeasibility.cpp @@ -16,10 +16,10 @@ OpenSoT::tasks::acceleration::DynamicFeasibility::DynamicFeasibility(const std:: _enabled_contacts.assign(_contact_links.size(), true); _W.setIdentity(6,6); _hessianType = OpenSoT::HessianType::HST_SEMIDEF; - update(_h); + update(); } -void OpenSoT::tasks::acceleration::DynamicFeasibility::_update(const Eigen::VectorXd& x) +void OpenSoT::tasks::acceleration::DynamicFeasibility::_update() { _robot.computeInertiaMatrix(_B); _robot.computeNonlinearTerm(_h); diff --git a/src/tasks/acceleration/MinJointVel.cpp b/src/tasks/acceleration/MinJointVel.cpp index 2645472a..22e7e243 100644 --- a/src/tasks/acceleration/MinJointVel.cpp +++ b/src/tasks/acceleration/MinJointVel.cpp @@ -14,7 +14,7 @@ MinJointVel::MinJointVel(const XBot::ModelInterface &robot, double dT, AffineHel _hessianType = HST_SEMIDEF; - _update(Eigen::VectorXd(1)); + _update(); I = _postural->getWeight(); @@ -25,9 +25,9 @@ MinJointVel::MinJointVel(const XBot::ModelInterface &robot, double dT, AffineHel } -void MinJointVel::_update(const Eigen::VectorXd &x) +void MinJointVel::_update() { - _postural->update(x); + _postural->update(); _A = _postural->getA(); _b = _postural->getb(); diff --git a/src/tasks/acceleration/Postural.cpp b/src/tasks/acceleration/Postural.cpp index 3eb50b67..32ee4526 100644 --- a/src/tasks/acceleration/Postural.cpp +++ b/src/tasks/acceleration/Postural.cpp @@ -28,7 +28,7 @@ OpenSoT::tasks::acceleration::Postural::Postural( _Mi.setIdentity(robot.getNv(), robot.getNv()); - _update(_q); + _update(); } OpenSoT::tasks::acceleration::Postural::Postural(const XBot::ModelInterface& robot, @@ -57,7 +57,7 @@ OpenSoT::tasks::acceleration::Postural::Postural(const XBot::ModelInterface& rob _qddot_ref_cached = _qddot_ref; - _update(_q); + _update(); } void OpenSoT::tasks::acceleration::Postural::setGainType(GainType type) @@ -132,7 +132,7 @@ void OpenSoT::tasks::acceleration::Postural::setReference(const Eigen::VectorXd& } -void OpenSoT::tasks::acceleration::Postural::_update(const Eigen::VectorXd& x) +void OpenSoT::tasks::acceleration::Postural::_update() { _qdot_ref_cached = _qdot_ref; _qddot_ref_cached = _qddot_ref; diff --git a/src/tasks/floating_base/Contact.cpp b/src/tasks/floating_base/Contact.cpp index 86d4e22e..4898bdae 100755 --- a/src/tasks/floating_base/Contact.cpp +++ b/src/tasks/floating_base/Contact.cpp @@ -36,7 +36,7 @@ OpenSoT::tasks::floating_base::Contact::Contact(XBot::ModelInterface &robot, _lambda = 0.0; - _update(Eigen::VectorXd::Zero(robot.getJointNum())); + _update(); } OpenSoT::tasks::floating_base::Contact::~Contact() @@ -44,7 +44,7 @@ OpenSoT::tasks::floating_base::Contact::~Contact() } -void OpenSoT::tasks::floating_base::Contact::_update(const Eigen::VectorXd &x) +void OpenSoT::tasks::floating_base::Contact::_update() { _robot.getJacobian(_link_in_contact, _J); _Jrot.resize(_J.rows(), _J.cols()); diff --git a/src/tasks/floating_base/IMU.cpp b/src/tasks/floating_base/IMU.cpp index a99ff036..c127de69 100644 --- a/src/tasks/floating_base/IMU.cpp +++ b/src/tasks/floating_base/IMU.cpp @@ -22,7 +22,7 @@ OpenSoT::tasks::floating_base::IMU::IMU(XBot::ModelInterface &robot, XBot::ImuSe throw std::runtime_error("Imu is not attached to floating_base link!"); - _update(Eigen::VectorXd::Zero(robot.getJointNum())); + _update(); } OpenSoT::tasks::floating_base::IMU::~IMU() @@ -30,7 +30,7 @@ OpenSoT::tasks::floating_base::IMU::~IMU() } -void OpenSoT::tasks::floating_base::IMU::_update(const Eigen::VectorXd &x) +void OpenSoT::tasks::floating_base::IMU::_update() { _robot.getJacobian(_fb_link, _J); _imu->getAngularVelocity(_angular_velocity); diff --git a/src/tasks/force/Cartesian.cpp b/src/tasks/force/Cartesian.cpp index 13c29068..7f448a6d 100644 --- a/src/tasks/force/Cartesian.cpp +++ b/src/tasks/force/Cartesian.cpp @@ -26,12 +26,12 @@ Cartesian::Cartesian(const std::string task_id, I.setIdentity(); - update(Eigen::VectorXd(1)); + update(); setWeight(Eigen::MatrixXd::Identity(6,6)); } -void Cartesian::_update(const Eigen::VectorXd& x) +void Cartesian::_update() { if (_base_link == world_name) { diff --git a/src/tasks/force/CoM.cpp b/src/tasks/force/CoM.cpp index a216b545..e45a88eb 100644 --- a/src/tasks/force/CoM.cpp +++ b/src/tasks/force/CoM.cpp @@ -25,85 +25,12 @@ using namespace OpenSoT::tasks::force; #define LAMBDA_THS 1E-12 -CoM::CoM( const Eigen::VectorXd& x, - std::vector& links_in_contact, - XBot::ModelInterface &robot) : - Task("CoM", x.rows()), _robot(robot), - _desiredPosition(), _desiredVelocity(), _desiredAcceleration(), - _actualPosition(), _actualVelocity(), _actualAngularMomentum(), - _desiredVariationAngularMomentum(), _desiredAngularMomentum(), - positionError(), velocityError(), angularMomentumError(), - _links_in_contact(links_in_contact),_I(), _O(), _P(), _T(), - _g(), - _lambda2(1.0), _lambdaAngularMomentum(1.0), - A(3,6),B(3,6) -{ - A.setZero(3,6); - B.setZero(3,6); - G.setZero(6, 6*links_in_contact.size()); - - _desiredPosition.setZero(); - _desiredVelocity.setZero(); - _desiredAcceleration.setZero(); - - _desiredVariationAngularMomentum.setZero(); - _desiredAngularMomentum.setZero(); - - _actualPosition.setZero(); - _actualVelocity.setZero(); - _actualAngularMomentum.setZero(); - - positionError.setZero(); - velocityError.setZero(); - angularMomentumError.setZero(); - - _centroidalMomentum.setZero(); - - _P.setZero(); - _T.matrix().setZero(); - - _g.setZero(); - _g(2) = -9.81; - - _desiredPosition = _robot.getCOM(); - _desiredVelocity = _robot.getCOMVelocity(); - _centroidalMomentum.noalias() = _robot.computeCentroidalMomentumMatrix() * _robot.getJointVelocity(); - _desiredAngularMomentum = _centroidalMomentum.segment(3,3); - /* first update. Setting desired pose equal to the actual pose */ - - _W.resize(6,6); - _W.setIdentity(6,6); - _lambda = 1.0; - - - _I.setIdentity(); - _O.setZero(); - - - - OptvarHelper::VariableVector var; - - for(auto l : links_in_contact){ - var.emplace_back(l + "_wrench", 6); - } - - OptvarHelper opt(var); - - _wrenches.setZero(x.size(), 0); - - for( auto v : opt.getAllVariables() ){ - _wrenches = _wrenches / v; - } - - this->_update(x); - _hessianType = HST_SEMIDEF; -} CoM::CoM( std::vector wrenches, std::vector& links_in_contact, XBot::ModelInterface &robot) : - Task("CoM", wrenches[0].getInputSize()), _robot(robot), + Task("CoM", wrenches[0].getInputSize() * links_in_contact.size()), _robot(robot), _desiredPosition(), _desiredVelocity(), _desiredAcceleration(), _actualPosition(), _actualVelocity(), _actualAngularMomentum(), _desiredVariationAngularMomentum(), _desiredAngularMomentum(), @@ -154,15 +81,10 @@ CoM::CoM( std::vector wrenches, _I.setIdentity(); _O.setZero(); - - _wrenches.setZero(wrenches[0].getInputSize(), 0); - for( auto v : wrenches ){ - _wrenches = _wrenches / v; - } - + _hessianType = HST_SEMIDEF; - _update(Eigen::VectorXd()); + _update(); } @@ -181,7 +103,7 @@ void CoM::setLinksInContact(const std::vector& links_in_contact) _links_in_contact = links_in_contact; } -void CoM::_update(const Eigen::VectorXd &x) +void CoM::_update() { /************************* COMPUTING TASK *****************************/ @@ -203,8 +125,8 @@ void CoM::_update(const Eigen::VectorXd &x) **/ __A = computeA(_links_in_contact); - - _com_task = __A*_wrenches - __b; + + _com_task.set(__A, -__b); _A = _com_task.getM(); _b = -_com_task.getq(); diff --git a/src/tasks/force/FloatingBase.cpp b/src/tasks/force/FloatingBase.cpp index 1e58e940..87268c6e 100644 --- a/src/tasks/force/FloatingBase.cpp +++ b/src/tasks/force/FloatingBase.cpp @@ -33,7 +33,7 @@ FloatingBase::FloatingBase(XBot::ModelInterface &model, _floating_base_torque.setZero(); - update(Eigen::VectorXd()); + update(); _W.setIdentity(_A.rows(), _A.rows()); } @@ -55,7 +55,7 @@ void FloatingBase::_log(XBot::MatLogger2::Ptr logger) logger->add(_task_id + "_floating_base_torque", _floating_base_torque); } -void FloatingBase::_update(const Eigen::VectorXd &x) +void FloatingBase::_update() { _task.setZero(_wrenches[0].getInputSize(), 6); diff --git a/src/tasks/force/Force.cpp b/src/tasks/force/Force.cpp index 79d3df2e..310b9a48 100644 --- a/src/tasks/force/Force.cpp +++ b/src/tasks/force/Force.cpp @@ -14,12 +14,12 @@ Wrench::Wrench(const std::string& id, Eigen::VectorXd zeros; zeros.setZero(wrench.getOutputSize()); _min_var->setReference(zeros); _hessianType = OpenSoT::HessianType::HST_SEMIDEF; - _update(Eigen::VectorXd(0)); + _update(); } -void Wrench::_update(const Eigen::VectorXd &x) +void Wrench::_update() { - _min_var->update(x); + _min_var->update(); _A = _min_var->getA(); _b = _min_var->getb(); @@ -68,7 +68,7 @@ Wrenches::Wrenches(const std::string& id, _aggregated_task = std::make_shared (task_list, wrenches[0].getInputSize()); - _update(Eigen::VectorXd(0)); + _update(); } Wrench::Ptr Wrenches::getWrenchTask(const std::string& contact_name) @@ -79,9 +79,9 @@ Wrench::Ptr Wrenches::getWrenchTask(const std::string& contact_name) return NULL; } -void Wrenches::_update(const Eigen::VectorXd& x) +void Wrenches::_update() { - _aggregated_task->update(x); + _aggregated_task->update(); _A = _aggregated_task->getA(); _b = _aggregated_task->getb(); } diff --git a/src/tasks/velocity/AngularMomentum.cpp b/src/tasks/velocity/AngularMomentum.cpp index 0c76c93f..4b20e3e9 100644 --- a/src/tasks/velocity/AngularMomentum.cpp +++ b/src/tasks/velocity/AngularMomentum.cpp @@ -24,7 +24,7 @@ AngularMomentum::AngularMomentum(XBot::ModelInterface& robot): _base_link(BASE_LINK_COM), _distal_link(DISTAL_LINK_COM) { _desiredAngularMomentum.setZero(); - this->_update(Eigen::VectorXd(0)); + this->_update(); _W.resize(3,3); _W.setIdentity(3,3); @@ -41,7 +41,7 @@ AngularMomentum::~AngularMomentum() } -void AngularMomentum::_update(const Eigen::VectorXd& x) +void AngularMomentum::_update() { _robot.computeCentroidalMomentumMatrix(_Momentum); _A = _Momentum.block(3,0,3,_x_size); diff --git a/src/tasks/velocity/Cartesian.cpp b/src/tasks/velocity/Cartesian.cpp index 251db1f1..8685024e 100644 --- a/src/tasks/velocity/Cartesian.cpp +++ b/src/tasks/velocity/Cartesian.cpp @@ -51,7 +51,7 @@ Cartesian::Cartesian(std::string task_id, assert(this->_distal_link_index != _base_link_index); /* first update. Setting desired pose equal to the actual pose */ - this->_update(Eigen::VectorXd(0)); + this->_update(); _W.setIdentity(_A.rows(), _A.rows()); @@ -65,7 +65,7 @@ Cartesian::~Cartesian() { } -void Cartesian::_update(const Eigen::VectorXd &x) { +void Cartesian::_update() { /************************* COMPUTING TASK *****************************/ _desiredTwistRef = _desiredTwist; @@ -355,7 +355,7 @@ void Cartesian::_log(XBot::MatLogger2::Ptr logger) bool Cartesian::reset() { _is_initialized = false; - _update(Eigen::VectorXd(1)); + _update(); return true; } diff --git a/src/tasks/velocity/CartesianAdmittance.cpp b/src/tasks/velocity/CartesianAdmittance.cpp index c7113535..56a8ee11 100644 --- a/src/tasks/velocity/CartesianAdmittance.cpp +++ b/src/tasks/velocity/CartesianAdmittance.cpp @@ -47,7 +47,7 @@ CartesianAdmittance::CartesianAdmittance(std::string task_id, _tmp.assign(CHANNELS, 0.); } -void CartesianAdmittance::_update(const Eigen::VectorXd &x) +void CartesianAdmittance::_update() { _ft_sensor->getWrench(_wrench_measured); if(_base_link == "world") @@ -71,7 +71,7 @@ void CartesianAdmittance::_update(const Eigen::VectorXd &x) _desiredTwist = _C.asDiagonal()*_wrench_filt; - Cartesian::_update(Eigen::VectorXd(0)); + Cartesian::_update(); } bool CartesianAdmittance::reset() diff --git a/src/tasks/velocity/CoM.cpp b/src/tasks/velocity/CoM.cpp index ea7650ef..ee4e239e 100644 --- a/src/tasks/velocity/CoM.cpp +++ b/src/tasks/velocity/CoM.cpp @@ -38,7 +38,7 @@ CoM::CoM(XBot::ModelInterface &robot, _desiredVelocityRef = _desiredVelocity; /* first update. Setting desired pose equal to the actual pose */ - this->_update(Eigen::VectorXd(0)); + this->_update(); /* initializing to zero error */ @@ -56,7 +56,7 @@ CoM::~CoM() { } -void CoM::_update(const Eigen::VectorXd &x) +void CoM::_update() { /************************* COMPUTING TASK *****************************/ diff --git a/src/tasks/velocity/Contact.cpp b/src/tasks/velocity/Contact.cpp index d0c86811..ff023c91 100644 --- a/src/tasks/velocity/Contact.cpp +++ b/src/tasks/velocity/Contact.cpp @@ -11,12 +11,12 @@ OpenSoT::tasks::velocity::Contact::Contact(std::string task_id, _model(model), _K(contact_matrix) { - _update(Eigen::VectorXd()); + _update(); _W.setIdentity(getTaskSize(), getTaskSize()); } -void OpenSoT::tasks::velocity::Contact::_update(const Eigen::VectorXd& x) +void OpenSoT::tasks::velocity::Contact::_update() { /* Body jacobian */ _model.getJacobian(_distal_link, _Jtmp); diff --git a/src/tasks/velocity/Gaze.cpp b/src/tasks/velocity/Gaze.cpp index 9122f9f3..9305495b 100644 --- a/src/tasks/velocity/Gaze.cpp +++ b/src/tasks/velocity/Gaze.cpp @@ -15,7 +15,7 @@ Gaze::Gaze(std::string task_id, _robot(robot), _tmp_vector(3), _bl_T_gaze_kdl(), _gaze_goal() { - this->_update(Eigen::VectorXd(0)); + this->_update(); } Gaze::~Gaze() @@ -99,9 +99,9 @@ const unsigned int Gaze::getTaskSize() const return _subtask->getTaskSize(); } -void Gaze::_update(const Eigen::VectorXd &x) +void Gaze::_update() { - _subtask->update(Eigen::VectorXd(0)); + _subtask->update(); this->_A = _subtask->getA(); this->_b = _subtask->getb(); this->_hessianType = _subtask->getHessianAtype(); diff --git a/src/tasks/velocity/JointAdmittance.cpp b/src/tasks/velocity/JointAdmittance.cpp index d000a89c..dd49633b 100644 --- a/src/tasks/velocity/JointAdmittance.cpp +++ b/src/tasks/velocity/JointAdmittance.cpp @@ -22,13 +22,10 @@ JointAdmittance::JointAdmittance(XBot::ModelInterface &robot, XBot::ModelInterfa _h.setZero(this->getXSize()); _qdot_desired.setZero(this->getXSize()); - - _model.getJointPosition(_q); - - _update(Eigen::VectorXd(0)); + _update(); } -void JointAdmittance::_update(const Eigen::VectorXd &x) +void JointAdmittance::_update() { _robot.getJointEffort(_tau); _robot.computeNonlinearTerm(_h); @@ -43,7 +40,7 @@ void JointAdmittance::_update(const Eigen::VectorXd &x) if(_model.isFloatingBase()) _qdot_desired.head(6).setZero(); - Postural::_update(Eigen::VectorXd(0)); + Postural::_update(); } void JointAdmittance::setJointCompliance(const Eigen::MatrixXd &C) diff --git a/src/tasks/velocity/LinearMomentum.cpp b/src/tasks/velocity/LinearMomentum.cpp index 3473de6b..ab5354e9 100644 --- a/src/tasks/velocity/LinearMomentum.cpp +++ b/src/tasks/velocity/LinearMomentum.cpp @@ -23,7 +23,7 @@ LinearMomentum::LinearMomentum(XBot::ModelInterface& robot): Task("LinearMomentum", robot.getNv()), _robot(robot) { _desiredLinearMomentum.setZero(); - this->_update(Eigen::VectorXd(0)); + this->_update(); _W.resize(3,3); _W.setIdentity(3,3); @@ -40,7 +40,7 @@ LinearMomentum::~LinearMomentum() } -void LinearMomentum::_update(const Eigen::VectorXd& x) +void LinearMomentum::_update() { _robot.computeCentroidalMomentumMatrix(_Momentum); _A = _Momentum.block(0,0,3,_x_size); diff --git a/src/tasks/velocity/Manipulability.cpp b/src/tasks/velocity/Manipulability.cpp index 68de2c67..25623467 100644 --- a/src/tasks/velocity/Manipulability.cpp +++ b/src/tasks/velocity/Manipulability.cpp @@ -23,7 +23,7 @@ Manipulability::Manipulability(const XBot::ModelInterface& robot_model, _deltas.resize(_model.getNv()); /* first update. Setting desired pose equal to the actual pose */ - this->_update(Eigen::VectorXd(0)); + this->_update(); } @@ -47,7 +47,7 @@ Manipulability::Manipulability(const XBot::ModelInterface& robot_model, /* first update. Setting desired pose equal to the actual pose */ - this->_update(Eigen::VectorXd(0)); + this->_update(); } Manipulability::~Manipulability() @@ -55,7 +55,7 @@ Manipulability::~Manipulability() } -void Manipulability::_update(const Eigen::VectorXd &x) { +void Manipulability::_update() { _model.getJointPosition(_q); diff --git a/src/tasks/velocity/MinimumEffort.cpp b/src/tasks/velocity/MinimumEffort.cpp index 4f3ad275..6f23f42a 100644 --- a/src/tasks/velocity/MinimumEffort.cpp +++ b/src/tasks/velocity/MinimumEffort.cpp @@ -40,7 +40,7 @@ MinimumEffort::MinimumEffort(const XBot::ModelInterface& robot_model, const doub _deltas.resize(_model.getNv()); /* first update. Setting desired pose equal to the actual pose */ - this->_update(Eigen::VectorXd(0)); + this->_update(); } MinimumEffort::~MinimumEffort() @@ -48,7 +48,7 @@ MinimumEffort::~MinimumEffort() } -void MinimumEffort::_update(const Eigen::VectorXd &x) { +void MinimumEffort::_update() { _q = _model.getJointPosition(); /************************* COMPUTING TASK *****************************/ diff --git a/src/tasks/velocity/Postural.cpp b/src/tasks/velocity/Postural.cpp index d96bdfa2..740b6f34 100644 --- a/src/tasks/velocity/Postural.cpp +++ b/src/tasks/velocity/Postural.cpp @@ -40,14 +40,14 @@ Postural::Postural(const XBot::ModelInterface& robot, /* first update. Setting desired pose equal to the actual pose */ this->setReference(_q); - this->_update(Eigen::VectorXd(0)); + this->_update(); } Postural::~Postural() { } -void Postural::_update(const Eigen::VectorXd &q) { +void Postural::_update() { _v_desired_ref = _v_desired; _q = _robot.getJointPosition(); @@ -120,7 +120,7 @@ Eigen::VectorXd OpenSoT::tasks::velocity::Postural::getActualPositions() bool OpenSoT::tasks::velocity::Postural::reset() { _q_desired = _q; - _update(_q_desired); + _update(); return true; } diff --git a/src/tasks/velocity/PureRolling.cpp b/src/tasks/velocity/PureRolling.cpp index 3adaef0d..821cda00 100644 --- a/src/tasks/velocity/PureRolling.cpp +++ b/src/tasks/velocity/PureRolling.cpp @@ -12,7 +12,7 @@ OpenSoT::tasks::velocity::PureRolling::PureRolling(std::string wheel_link_name, { Eigen::VectorXd q; model.getJointPosition(q); - _update(q); + _update(); setOutwardNormal(_world_contact_plane_normal); } @@ -48,7 +48,7 @@ void OpenSoT::tasks::velocity::PureRolling::setOutwardNormal(const Eigen::Vector } -void OpenSoT::tasks::velocity::PureRolling::_update(const Eigen::VectorXd& x) +void OpenSoT::tasks::velocity::PureRolling::_update() { /* Compute tranforms */ @@ -105,9 +105,7 @@ OpenSoT::tasks::velocity::PureRollingPosition::PureRollingPosition(std::string w _W.setIdentity(_position_indices.size(), _position_indices.size()); - Eigen::VectorXd q; - model.getJointPosition(q); - _update(q); + _update(); } void OpenSoT::tasks::velocity::PureRollingPosition::setOutwardNormal(const Eigen::Vector3d& n) @@ -116,9 +114,9 @@ void OpenSoT::tasks::velocity::PureRollingPosition::setOutwardNormal(const Eigen } -void OpenSoT::tasks::velocity::PureRollingPosition::_update(const Eigen::VectorXd &x) +void OpenSoT::tasks::velocity::PureRollingPosition::_update() { - _subtask->update(x); + _subtask->update(); _A = _subtask->getA(); _b = _subtask->getb(); @@ -140,14 +138,12 @@ OpenSoT::tasks::velocity::PureRollingOrientation::PureRollingOrientation(std::st _orientation_indices.push_back(3); _subtask.reset(new OpenSoT::SubTask(_pure_rolling, _orientation_indices)); - Eigen::VectorXd q; - model.getJointPosition(q); - _update(q); + _update(); } -void OpenSoT::tasks::velocity::PureRollingOrientation::_update(const Eigen::VectorXd &x) +void OpenSoT::tasks::velocity::PureRollingOrientation::_update() { - _subtask->update(x); + _subtask->update(); _A = _subtask->getA(); _b = _subtask->getb(); diff --git a/src/utils/AffineUtils.cpp b/src/utils/AffineUtils.cpp index 22f3b7d4..14a81e5c 100644 --- a/src/utils/AffineUtils.cpp +++ b/src/utils/AffineUtils.cpp @@ -21,7 +21,7 @@ AffineTask::AffineTask(const OpenSoT::tasks::Aggregated::TaskPtr &task, const Af _lambda = 1.; _internal_generic_task->setWeight(task->getWeight()); - _update(Eigen::VectorXd(1)); + _update(); } AffineTask::~AffineTask() @@ -29,17 +29,17 @@ AffineTask::~AffineTask() } -void AffineTask::_update(const Eigen::VectorXd &x) +void AffineTask::_update() { //1. Update original task which will be used to set desired quantities - _internal_task->update(x); + _internal_task->update(); //2. Update internal generic task and get matrices with affines _internal_generic_task->setA(_internal_task->getA()); _internal_generic_task->setb(_internal_task->getb()); _internal_generic_task->setc(_internal_task->getc()); _internal_generic_task->setWeight(_internal_task->getWeight()); - _internal_generic_task->update(x); + _internal_generic_task->update(); //3. Update Affine Task _A = _internal_generic_task->getA(); diff --git a/src/utils/AutoStack.cpp b/src/utils/AutoStack.cpp index 9ca88f6f..fbaa7bff 100644 --- a/src/utils/AutoStack.cpp +++ b/src/utils/AutoStack.cpp @@ -382,14 +382,14 @@ OpenSoT::AutoStack::AutoStack(OpenSoT::solvers::iHQP::Stack stack, } -void OpenSoT::AutoStack::update(const Eigen::VectorXd &state) +void OpenSoT::AutoStack::update() { _boundsAggregated->update(); typedef std::vector::iterator it_t; for(it_t task = _stack.begin(); task != _stack.end(); ++task) - (*task)->update(state); + (*task)->update(); if(_regularisation_task) - _regularisation_task->update(state); + _regularisation_task->update(); } std::list& OpenSoT::AutoStack::getBoundsList() diff --git a/src/utils/ForceOptimization.cpp b/src/utils/ForceOptimization.cpp index c92dbd58..24ce753e 100644 --- a/src/utils/ForceOptimization.cpp +++ b/src/utils/ForceOptimization.cpp @@ -144,7 +144,7 @@ bool OpenSoT::utils::ForceOptimization::compute(const Eigen::VectorXd& fixed_bas Fc.resize(_contact_links.size()); _forza_giusta->setFloatingBaseTorque(fixed_base_torque.head<6>()); - _autostack->update(_x_value); + _autostack->update(); if(!_solver->solve(_x_value)) { diff --git a/tests/constraints/acceleration/TestJointLimits.cpp b/tests/constraints/acceleration/TestJointLimits.cpp index 0aa5bc19..ad493709 100644 --- a/tests/constraints/acceleration/TestJointLimits.cpp +++ b/tests/constraints/acceleration/TestJointLimits.cpp @@ -192,7 +192,7 @@ TEST_F(testJointLimits, testBoundsWithTrajectory) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); //this->autostack->log(this->logger); Eigen::VectorXd qddot; @@ -247,7 +247,7 @@ TEST_F(testJointLimits, testBoundsWithTrajectory) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); this->autostack->log(this->logger); Eigen::VectorXd qddot; @@ -295,7 +295,7 @@ TEST_F(testJointLimits, testBoundsWithRegulation) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); Eigen::VectorXd qddot; ASSERT_TRUE(this->solver->solve(qddot)); @@ -339,7 +339,7 @@ TEST_F(testJointLimits, testBoundsWithRegulation) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); this->autostack->log(this->logger); Eigen::VectorXd qddot; diff --git a/tests/constraints/acceleration/TestJointLimitsECBF.cpp b/tests/constraints/acceleration/TestJointLimitsECBF.cpp index 8115c9d3..8eb26dd3 100644 --- a/tests/constraints/acceleration/TestJointLimitsECBF.cpp +++ b/tests/constraints/acceleration/TestJointLimitsECBF.cpp @@ -179,7 +179,7 @@ TEST_F(testJointLimits, testBoundsWithTrajectory) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); //this->autostack->log(this->logger); Eigen::VectorXd qddot; @@ -229,7 +229,7 @@ TEST_F(testJointLimits, testBoundsWithTrajectory) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); this->autostack->log(this->logger); Eigen::VectorXd qddot; @@ -269,7 +269,7 @@ TEST_F(testJointLimits, testBoundsWithRegulation) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); Eigen::VectorXd qddot; ASSERT_TRUE(this->solver->solve(qddot)); @@ -313,7 +313,7 @@ TEST_F(testJointLimits, testBoundsWithRegulation) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); this->autostack->log(this->logger); Eigen::VectorXd qddot; diff --git a/tests/constraints/acceleration/TestJointLimitsViability.cpp b/tests/constraints/acceleration/TestJointLimitsViability.cpp index 5dea9f15..ea50bb06 100644 --- a/tests/constraints/acceleration/TestJointLimitsViability.cpp +++ b/tests/constraints/acceleration/TestJointLimitsViability.cpp @@ -191,7 +191,7 @@ TEST_F(testJointLimitsViability, testBoundsWithTrajectory) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); // this->autostack->log(this->logger); Eigen::VectorXd qddot; @@ -240,7 +240,7 @@ TEST_F(testJointLimitsViability, testBoundsWithTrajectory) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); this->autostack->log(this->logger); Eigen::VectorXd qddot; @@ -281,7 +281,7 @@ TEST_F(testJointLimitsViability, testBoundsWithRegulation) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); Eigen::VectorXd qddot; ASSERT_TRUE(this->solver->solve(qddot)); @@ -325,7 +325,7 @@ TEST_F(testJointLimitsViability, testBoundsWithRegulation) { this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); this->autostack->log(this->logger); Eigen::VectorXd qddot; diff --git a/tests/constraints/force/TestFrictionCones.cpp b/tests/constraints/force/TestFrictionCones.cpp index dfb72c9e..393356e1 100644 --- a/tests/constraints/force/TestFrictionCones.cpp +++ b/tests/constraints/force/TestFrictionCones.cpp @@ -118,13 +118,16 @@ TEST_F(testFrictionCones, testFrictionCones_) { _model_ptr->getPose("l_sole", w_T_l_sole); - Eigen::VectorXd QPcontact_wrenches_d(6*links_in_contact.size()); - QPcontact_wrenches_d.setZero(QPcontact_wrenches_d.rows()); + std::vector QPcontact_wrenches; + for(unsigned int i = 0; i < links_in_contact.size(); ++i) + QPcontact_wrenches.push_back(OpenSoT::AffineHelper::Identity(6)); + + Eigen::VectorXd SVDcontact_wrenches_d(6*links_in_contact.size()); SVDcontact_wrenches_d.setZero(SVDcontact_wrenches_d.rows()); - com.reset(new OpenSoT::tasks::force::CoM(QPcontact_wrenches_d, links_in_contact, *_model_ptr)); - com->update(QPcontact_wrenches_d); + com.reset(new OpenSoT::tasks::force::CoM(QPcontact_wrenches, links_in_contact, *_model_ptr)); + com->update(); Eigen::VectorXd wrench_lims; @@ -147,6 +150,7 @@ TEST_F(testFrictionCones, testFrictionCones_) { QPsolver.reset(new OpenSoT::solvers::iHQP(stack_of_tasks,wrench_limits,2E5)); std::cout<<"QP Solver started"<solve(QPcontact_wrenches_d); diff --git a/tests/constraints/velocity/TestCollisionAvoidance.cpp b/tests/constraints/velocity/TestCollisionAvoidance.cpp index a9977cd9..febc7b2b 100644 --- a/tests/constraints/velocity/TestCollisionAvoidance.cpp +++ b/tests/constraints/velocity/TestCollisionAvoidance.cpp @@ -399,8 +399,8 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testCartesianTaskWithoutSC){ this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - taskCartesianAggregated->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + taskCartesianAggregated->update(); + postural_task->update(); bounds->update(); if(!sot->solve(dq)){ @@ -535,8 +535,8 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testCartesianTaskWithSC){ auto time_for_update = std::chrono::duration_cast(toc-tic).count(); std::cout<<"SCA Update time: "<update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + taskCartesianAggregated->update(); + postural_task->update(); bounds->update(); if(!sot->solve(dq)){ @@ -765,8 +765,8 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testMultipleCapsulePairsSC){ this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - taskCartesianAggregated->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + taskCartesianAggregated->update(); + postural_task->update(); bounds->update(); if(!sot->solve(dq)){ @@ -986,8 +986,8 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testChangeWhitelistOnline){ this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - taskCartesianAggregated->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + taskCartesianAggregated->update(); + postural_task->update(); bounds->update(); if(!sot->solve(dq)){ @@ -1098,8 +1098,8 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testChangeWhitelistOnline){ this->_model_ptr->setJointPosition(this->q); this->_model_ptr->update(); - taskCartesianAggregated->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + taskCartesianAggregated->update(); + postural_task->update(); bounds->update(); if(!sot->solve(dq)){ diff --git a/tests/constraints/velocity/TestJointLimitsInvariance.cpp b/tests/constraints/velocity/TestJointLimitsInvariance.cpp index 6df044c5..2d363570 100644 --- a/tests/constraints/velocity/TestJointLimitsInvariance.cpp +++ b/tests/constraints/velocity/TestJointLimitsInvariance.cpp @@ -271,7 +271,7 @@ TEST_F(testJointLimits, test_bounds) this->_model_ptr->setJointVelocity(qdot_prev); this->_model_ptr->update(); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); EXPECT_TRUE(solver->solve(dq)); autostack->log(this->logger); @@ -323,7 +323,7 @@ TEST_F(testJointLimits, test_bounds) this->_model_ptr->setJointVelocity(qdot_prev); this->_model_ptr->update(); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); EXPECT_TRUE(solver->solve(dq)); autostack->log(this->logger); @@ -400,7 +400,7 @@ TEST_F(testJointLimitsNaive, test_bounds) this->_model_ptr->setJointVelocity(qdot_prev); this->_model_ptr->update(); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); EXPECT_TRUE(solver->solve(dq)); autostack->log(this->logger); @@ -437,7 +437,7 @@ TEST_F(testJointLimitsNaive, test_bounds) this->_model_ptr->setJointVelocity(qdot_prev); this->_model_ptr->update(); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); EXPECT_TRUE(solver->solve(dq)); autostack->log(this->logger); diff --git a/tests/solvers/TestEigenSVD_StaticWalk_FloatingBase.cpp b/tests/solvers/TestEigenSVD_StaticWalk_FloatingBase.cpp index 040476a0..e14ad3ba 100644 --- a/tests/solvers/TestEigenSVD_StaticWalk_FloatingBase.cpp +++ b/tests/solvers/TestEigenSVD_StaticWalk_FloatingBase.cpp @@ -302,7 +302,7 @@ class theWalkingStack (postural)<update(Eigen::VectorXd(0)); + auto_stack->update(); solver.reset(new OpenSoT::solvers::eHQP(auto_stack->getStack())); @@ -323,7 +323,7 @@ class theWalkingStack #if USE_INERTIA_MATRIX setInertiaPostureTask(); #endif - auto_stack->update(Eigen::VectorXd(0)); + auto_stack->update(); } diff --git a/tests/solvers/TestGLPK.cpp b/tests/solvers/TestGLPK.cpp index 5b8ffca9..a1bb173a 100644 --- a/tests/solvers/TestGLPK.cpp +++ b/tests/solvers/TestGLPK.cpp @@ -218,7 +218,7 @@ TEST_F(testGLPKProblem, testIKMILP) // this->_model_ptr->getInertiaMatrix(M); // postural->setWeight(M); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); auto tic = std::chrono::steady_clock::now(); EXPECT_TRUE(solver->solve(dq)); @@ -387,7 +387,7 @@ std::cout<<" SECOND RUN"<_model_ptr->setJointPosition(q); this->_model_ptr->update(); - autostack2->update(Eigen::VectorXd(0)); + autostack2->update(); auto tic = std::chrono::steady_clock::now(); bool a = solver2->solve(dx); @@ -462,7 +462,7 @@ TEST_F(testGLPKProblem, testMILPProblem) c<<3.,-2.,-1; task->setHessianType(OpenSoT::HST_ZERO); task->setc(c); - task->update(Eigen::VectorXd(1)); + task->update(); Eigen::VectorXd lb(3), ub(3); lb.setZero(3); diff --git a/tests/solvers/TestLP.cpp b/tests/solvers/TestLP.cpp index b365f6b6..0ff11c8f 100644 --- a/tests/solvers/TestLP.cpp +++ b/tests/solvers/TestLP.cpp @@ -45,7 +45,7 @@ TEST_F(testLProblem, testSingleLPProblem) c.setOnes(4); OpenSoT::tasks::GenericTask::Ptr lp_task(new OpenSoT::tasks::GenericTask("lp_task", A, b)); - lp_task->update(Eigen::VectorXd(4)); + lp_task->update(); lp_task->setHessianType(OpenSoT::HessianType::HST_ZERO); lp_task->setc(c); std::cout<<"lp_task->getc(): "<getc()<update(Eigen::VectorXd(1)); + task_qp->update(); Eigen::VectorXd c_CBC(3); c_CBC<<-3.,-2.,-1; OpenSoT::tasks::GenericLPTask::Ptr task_CBC(new OpenSoT::tasks::GenericLPTask("task_CBC",c_CBC)); - task_CBC->update(Eigen::VectorXd(1)); + task_CBC->update(); Eigen::VectorXd lb(3), ub(3); lb.setZero(3); diff --git a/tests/solvers/TestOSQP.cpp b/tests/solvers/TestOSQP.cpp index 8a09c7b6..a80c62f8 100644 --- a/tests/solvers/TestOSQP.cpp +++ b/tests/solvers/TestOSQP.cpp @@ -141,7 +141,7 @@ TEST_F(testOSQPProblem, testTask) OpenSoT::tasks::velocity::Postural postural_task(*_model_ptr); postural_task.setReference(q_ref); - postural_task.update(Eigen::VectorXd(0)); + postural_task.update(); Eigen::MatrixXd H(_model_ptr->getNv(),_model_ptr->getNv()); H.setIdentity(H.rows(), H.cols()); Eigen::VectorXd g(-1.0*postural_task.getb()); @@ -161,7 +161,7 @@ TEST_F(testOSQPProblem, testTask) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task.update(Eigen::VectorXd(0)); + postural_task.update(); qp_postural_problem->updateTask(H, -1.0*postural_task.getb()); @@ -221,7 +221,7 @@ TEST_F(testOSQPProblem, testProblemWithConstraint) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); qp_postural_problem->updateBounds(constraint->getLowerBound(), constraint->getUpperBound()); qp_postural_problem->updateTask(postural_task->getA(), -1.0*postural_task->getb()); @@ -301,7 +301,7 @@ TEST_F(testOSQPProblem, testCartesian) new OpenSoT::tasks::velocity::Cartesian("cartesian::left_wrist", *_model_ptr, "l_wrist", "Waist")); - cartesian_task->update(q); + cartesian_task->update(); Eigen::MatrixXd T_actual = cartesian_task->getActualPose(); std::cout<<"T_actual: \n"<setReference(T_ref); - cartesian_task->update(q); + cartesian_task->update(); //OpenSoT::solvers::OSQPBackEnd qp_cartesian_problem(cartesian_task->getXSize(), 0); OpenSoT::solvers::BackEnd::Ptr qp_cartesian_problem = OpenSoT::solvers::BackEndFactory( @@ -329,7 +329,7 @@ TEST_F(testOSQPProblem, testCartesian) _model_ptr->update(); - cartesian_task->update(q); + cartesian_task->update(); qp_cartesian_problem->updateTask(cartesian_task->getA().transpose()*cartesian_task->getA(), -1.0*cartesian_task->getA().transpose()*cartesian_task->getb()); ASSERT_TRUE(qp_cartesian_problem->solve()); Eigen::VectorXd dq = qp_cartesian_problem->getSolution(); @@ -365,7 +365,7 @@ TEST_F(testOSQPProblem, testEpsRegularisation) new OpenSoT::tasks::velocity::Cartesian("cartesian::left_wrist", *_model_ptr, "l_wrist", "Waist")); - cartesian_task->update(q); + cartesian_task->update(); Eigen::MatrixXd T_actual = cartesian_task->getActualPose(); std::cout<<"T_actual: \n"<setReference(T_ref); - cartesian_task->update(q); + cartesian_task->update(); //OpenSoT::solvers::OSQPBackEnd qp_cartesian_problem(cartesian_task->getXSize(), 0); OpenSoT::solvers::BackEnd::Ptr qp_cartesian_problem = OpenSoT::solvers::BackEndFactory( @@ -399,7 +399,7 @@ TEST_F(testOSQPProblem, testEpsRegularisation) _model_ptr->update(); - cartesian_task->update(q); + cartesian_task->update(); qp_cartesian_problem->updateTask(cartesian_task->getA().transpose()*cartesian_task->getA(), -1.0*cartesian_task->getA().transpose()*cartesian_task->getb()); ASSERT_TRUE(qp_cartesian_problem->solve()); Eigen::VectorXd dq = qp_cartesian_problem->getSolution(); @@ -493,8 +493,8 @@ TEST_F(testOSQPProblem, testContructor2Problems) _model_ptr->setJointPosition(q); _model_ptr->update(); - cartesian_task->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); + postural_task->update(); joint_constraints->update(); ASSERT_TRUE(sot.solve(dq)); @@ -583,7 +583,7 @@ TEST_F(testiHQP, testContructor1Problem) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); bounds->update(); EXPECT_TRUE(sot.solve(dq)); diff --git a/tests/solvers/TestQPOases.cpp b/tests/solvers/TestQPOases.cpp index 3f1d7055..e8367964 100644 --- a/tests/solvers/TestQPOases.cpp +++ b/tests/solvers/TestQPOases.cpp @@ -422,7 +422,7 @@ TEST_F(testQPOasesProblem, testTask) OpenSoT::tasks::velocity::Postural postural_task(*_model_ptr); postural_task.setReference(q_ref); - postural_task.update(Eigen::VectorXd(0)); + postural_task.update(); Eigen::MatrixXd H(_model_ptr->getNv(),_model_ptr->getNv()); H.setIdentity(); Eigen::VectorXd g(-1.0*postural_task.getb()); @@ -458,7 +458,7 @@ TEST_F(testQPOasesTask, testQPOasesTask) OpenSoT::tasks::velocity::Postural::Ptr postural_task( new OpenSoT::tasks::velocity::Postural(*_model_ptr)); postural_task->setReference(q_ref); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); std::cout<<"error: "<getb()<getXSize(), 0, @@ -527,7 +527,7 @@ TEST_F(testQPOasesTask, testProblemWithConstraint) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); qp_postural_problem->updateBounds(constraint->getLowerBound(), constraint->getUpperBound()); qp_postural_problem->updateTask(postural_task->getA(), -1.0*postural_task->getb()); @@ -639,7 +639,7 @@ TEST_F(testiHQP, testContructor1Problem) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); bounds->update(); EXPECT_TRUE(sot.solve(dq)); @@ -715,7 +715,7 @@ TEST_F(testQPOasesTask, testCoMTask) _model_ptr->setJointPosition(q); _model_ptr->update(); - com_task->update(Eigen::VectorXd(0)); + com_task->update(); qp_CoM_problem->updateProblem(com_task->getA().transpose()*com_task->getA(), -1.0*com_task->getA().transpose()*com_task->getb(), constraint->getAineq(), constraint->getbLowerBound(), constraint->getbUpperBound(), @@ -749,7 +749,7 @@ TEST_F(testQPOasesTask, testCartesian) new OpenSoT::tasks::velocity::Cartesian("cartesian::left_wrist", *_model_ptr, "l_wrist", "Waist")); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); Eigen::MatrixXd T_actual = cartesian_task->getActualPose(); std::cout<<"T_actual: \n"<setReference(T_ref); - cartesian_task->update(q); + cartesian_task->update(); // OpenSoT::solvers::QPOasesBackEnd qp_cartesian_problem(cartesian_task->getXSize(), 0, cartesian_task->getHessianAtype()); OpenSoT::solvers::BackEnd::Ptr qp_cartesian_problem = OpenSoT::solvers::BackEndFactory( @@ -776,7 +776,7 @@ TEST_F(testQPOasesTask, testCartesian) _model_ptr->update(); - cartesian_task->update(q); + cartesian_task->update(); qp_cartesian_problem->updateTask(cartesian_task->getA().transpose()*cartesian_task->getA(), -1.0*cartesian_task->getA().transpose()*cartesian_task->getb()); ASSERT_TRUE(qp_cartesian_problem->solve()); Eigen::VectorXd dq = qp_cartesian_problem->getSolution(); @@ -812,7 +812,7 @@ TEST_F(testQPOasesTask, testEpsRegularisation) new OpenSoT::tasks::velocity::Cartesian("cartesian::left_wrist", *_model_ptr, "l_wrist", "Waist")); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); Eigen::MatrixXd T_actual = cartesian_task->getActualPose(); std::cout<<"T_actual: \n"<setReference(T_ref); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); // OpenSoT::solvers::QPOasesBackEnd qp_cartesian_problem(cartesian_task->getXSize(), 0, cartesian_task->getHessianAtype()); OpenSoT::solvers::BackEnd::Ptr qp_cartesian_problem = OpenSoT::solvers::BackEndFactory( @@ -846,7 +846,7 @@ TEST_F(testQPOasesTask, testEpsRegularisation) _model_ptr->update(); - cartesian_task->update(q); + cartesian_task->update(); qp_cartesian_problem->updateTask(cartesian_task->getA().transpose()*cartesian_task->getA(), -1.0*cartesian_task->getA().transpose()*cartesian_task->getb()); ASSERT_TRUE(qp_cartesian_problem->solve()); Eigen::VectorXd dq = qp_cartesian_problem->getSolution(); @@ -937,8 +937,8 @@ TEST_F(testiHQP, testContructor2Problems) _model_ptr->setJointPosition(q); _model_ptr->update(); - cartesian_task->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); + postural_task->update(); joint_constraints->update(); ASSERT_TRUE(sot.solve(dq)); @@ -993,7 +993,7 @@ TEST_F(testiHQP, testSingleTask) OpenSoT::AutoStack::Ptr autostack(new OpenSoT::AutoStack(postural)); autostack = autostack<update(Eigen::VectorXd(0)); + autostack->update(); OpenSoT::solvers::iHQP::Ptr solver; solver = std::make_shared(autostack->getStack(), autostack->getBounds(), 1e8); @@ -1001,7 +1001,7 @@ TEST_F(testiHQP, testSingleTask) Eigen::VectorXd x(_model_ptr->getNv()); EXPECT_TRUE(solver->solve(x)); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); solver.reset(); solver = std::make_shared(autostack->getStack(), autostack->getBounds(), 1e8); @@ -1050,7 +1050,7 @@ TEST_F(testiHQP, testMinEffort) _model_ptr->setJointPosition(q); _model_ptr->update(); - joint_space_task->update(Eigen::VectorXd(0)); + joint_space_task->update(); bounds->update(); Eigen::VectorXd old_gradient = oldGravityGradient.computeMinEffort(q); diff --git a/tests/solvers/TestQPOases_AutoStack.cpp b/tests/solvers/TestQPOases_AutoStack.cpp index 21a4aef5..ca6418fe 100644 --- a/tests/solvers/TestQPOases_AutoStack.cpp +++ b/tests/solvers/TestQPOases_AutoStack.cpp @@ -96,7 +96,7 @@ TEST_F(testQPOases_AutoStack, testSolveUsingAutoStack) model->setJointPosition(q); model->update(); - subTaskTest->update(Eigen::VectorXd(0)); + subTaskTest->update(); if(dq.norm() > 1e-3) EXPECT_NE(oldBoundsNorm,sqrt(DHS->jointLimits->getbLowerBound().squaredNorm())); @@ -192,7 +192,7 @@ TEST_F(testQPOases_AutoStack, testComplexAutoStack) { model->setJointPosition(q); model->update(); - AutoStack->update(Eigen::VectorXd(0)); + AutoStack->update(); ASSERT_TRUE(solver->solve(dq)); q = model->sum(q, dq); @@ -290,7 +290,7 @@ TEST_F(testQPOases_AutoStack, testAutoStackConstructor) { model->setJointPosition(q); model->update(); - AutoStack->update(Eigen::VectorXd(0)); + AutoStack->update(); ASSERT_TRUE(solver->solve(dq)); q = model->sum(q, dq); diff --git a/tests/solvers/TestQPOases_ConvexHull.cpp b/tests/solvers/TestQPOases_ConvexHull.cpp index 2a666848..98c6049a 100644 --- a/tests/solvers/TestQPOases_ConvexHull.cpp +++ b/tests/solvers/TestQPOases_ConvexHull.cpp @@ -293,13 +293,13 @@ TEST_P(testQPOases_ConvexHull, tryFollowingBounds) { _model_ptr_com->setJointPosition(q); _model_ptr_com->update(); - right_foot_task->update(Eigen::VectorXd(0)); + right_foot_task->update(); std::cout<<"right_foot_task->getb().norm(): "<getb().norm()<<" @ iter: "<update(Eigen::VectorXd(0)); + left_foot_task->update(); std::cout<<"left_foot_task->getb().norm(): "<getb().norm()<<" @ iter: "<update(Eigen::VectorXd(0)); + com_task->update(); boundsConvexHull->log(logger); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); bounds->update(); _log << com_task->getActualPosition()[0] << "," @@ -386,11 +386,11 @@ TEST_P(testQPOases_ConvexHull, tryFollowingBounds) { _model_ptr_com->setJointPosition(q); _model_ptr_com->update(); - right_foot_task->update(Eigen::VectorXd(0)); - left_foot_task->update(Eigen::VectorXd(0)); - com_task->update(q); + right_foot_task->update(); + left_foot_task->update(); + com_task->update(); boundsConvexHull->log(logger); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); bounds->update(); _log << com_task->getActualPosition()[0] << "," diff --git a/tests/solvers/TestQPOases_FF.cpp b/tests/solvers/TestQPOases_FF.cpp index 2e494d35..70880c66 100644 --- a/tests/solvers/TestQPOases_FF.cpp +++ b/tests/solvers/TestQPOases_FF.cpp @@ -284,8 +284,8 @@ TEST_P(testQPOases_CartesianFF, testCartesianFF) q = getGoodInitialPosition(_model_ptr); _model_ptr->setJointPosition(q); _model_ptr->update(); - l_arm_task->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + l_arm_task->update(); + postural_task->update(); bounds->update(); current_pose_y = l_arm_task->getActualPose(); @@ -421,8 +421,8 @@ TEST_P(testQPOases_CartesianFF, testCartesianFF) _model_ptr->setJointPosition(q); _model_ptr->update(); - l_arm_task->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + l_arm_task->update(); + postural_task->update(); bounds->update(); EXPECT_TRUE(sot->solve(dq)); @@ -619,10 +619,10 @@ std::cout<<"floating_base_pose:\n"<setJointPosition(q); _model_ptr->update(); - com->update(Eigen::VectorXd(0)); - l_sole->update(Eigen::VectorXd(0)); - r_sole->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + com->update(); + l_sole->update(); + r_sole->update(); + postural_task->update(); bounds->update(); @@ -706,7 +706,7 @@ std::cout<<"floating_base_pose:\n"<update(Eigen::VectorXd(1)); + com->update(); previous_norm = sqrt(com->getb().squaredNorm()); } @@ -717,10 +717,10 @@ std::cout<<"floating_base_pose:\n"<setJointPosition(q); _model_ptr->update(); - com->update(Eigen::VectorXd(0)); - l_sole->update(Eigen::VectorXd(0)); - r_sole->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + com->update(); + l_sole->update(); + r_sole->update(); + postural_task->update(); bounds->update(); EXPECT_TRUE(sot->solve(dq)); @@ -861,7 +861,7 @@ TEST_P(testQPOases_CoMAndPosturalFF, testPosturalFF) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); bounds->update(); if(!hasInitialError) { @@ -944,7 +944,7 @@ TEST_P(testQPOases_CoMAndPosturalFF, testPosturalFF) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); bounds->update(); EXPECT_TRUE(sot->solve(dq)); diff --git a/tests/solvers/TestQPOases_Options.cpp b/tests/solvers/TestQPOases_Options.cpp index 2d4c2de9..0356a92c 100644 --- a/tests/solvers/TestQPOases_Options.cpp +++ b/tests/solvers/TestQPOases_Options.cpp @@ -66,7 +66,7 @@ class testIKProblem T_ref_0(0,3) = T_ref_0(0,3) + 0.1; _cartesian_task_0->setReference(T_ref_0); - _cartesian_task_0->update(q); + _cartesian_task_0->update(); _postural_task = std::make_shared(*_model_ptr); @@ -116,8 +116,8 @@ class testIKProblem void update() { - _cartesian_task_0->update(Eigen::VectorXd(0)); - _postural_task->update(Eigen::VectorXd(0)); + _cartesian_task_0->update(); + _postural_task->update(); _joint_constraints->update(); _log<<_cartesian_task_0->getb()<<" "; @@ -238,10 +238,10 @@ class testIKProblem2 T_ref_1(1,3) = T_ref_1(1,3) - 0.05; _cartesian_task_0->setReference(T_ref_0); - _cartesian_task_0->update(q); + _cartesian_task_0->update(); _cartesian_task_1->setReference(T_ref_1); - _cartesian_task_1->update(q); + _cartesian_task_1->update(); _postural_task = std::make_shared(*_model_ptr); @@ -292,9 +292,9 @@ class testIKProblem2 void update() { - _cartesian_task_0->update(Eigen::VectorXd(0)); - _cartesian_task_1->update(Eigen::VectorXd(0)); - _postural_task->update(Eigen::VectorXd(0)); + _cartesian_task_0->update(); + _cartesian_task_1->update(); + _postural_task->update(); _joint_constraints->update(); _log<<_cartesian_task_0->getb()<<" "<<_cartesian_task_1->getb()<<" "; @@ -431,10 +431,10 @@ class testIKProblem3 T_ref_1(1,3) = T_ref_1(1,3) - 0.05; _cartesian_task_0->setReference(T_ref_0); - _cartesian_task_0->update(q); + _cartesian_task_0->update(); _cartesian_task_1->setReference(T_ref_1); - _cartesian_task_1->update(q); + _cartesian_task_1->update(); _postural_task = std::make_shared(*_model_ptr); @@ -488,8 +488,8 @@ class testIKProblem3 void update() { - _cartesian_tasks->update(Eigen::VectorXd(0)); - _postural_task->update(Eigen::VectorXd(0)); + _cartesian_tasks->update(); + _postural_task->update(); _joint_constraints->update(); _log<<_cartesian_task_0->getb()<<" "<<_cartesian_task_1->getb()<<" "; diff --git a/tests/solvers/TestQPOases_SetActiveStack.cpp b/tests/solvers/TestQPOases_SetActiveStack.cpp index 54c877ff..03e69c35 100644 --- a/tests/solvers/TestQPOases_SetActiveStack.cpp +++ b/tests/solvers/TestQPOases_SetActiveStack.cpp @@ -67,47 +67,47 @@ TEST_F(testActivateStack, test_deactivate_task) //1) Test setActive with postural task OpenSoT::tasks::velocity::Postural::Ptr taskA(new OpenSoT::tasks::velocity::Postural(*_model_ptr)); - taskA->update(Eigen::VectorXd(0)); + taskA->update(); Eigen::MatrixXd JA = taskA->getA(); //here we deactivate the task taskA->setActive(false); - taskA->update(Eigen::VectorXd(0)); + taskA->update(); EXPECT_TRUE(taskA->getA() == Eigen::MatrixXd::Zero(JA.rows(), JA.cols())); //here we reactivate the task taskA->setActive(true); - taskA->update(Eigen::VectorXd(0)); + taskA->update(); EXPECT_TRUE(taskA->getA() == JA); //2) Test setActive with Cartesian task OpenSoT::tasks::velocity::Cartesian::Ptr taskB(new OpenSoT::tasks::velocity::Cartesian("foo", *_model_ptr, "l_sole", "Waist")); - taskB->update(Eigen::VectorXd(0)); + taskB->update(); Eigen::MatrixXd JB = taskB->getA(); //here we deactivate the task taskB->setActive(false); - taskB->update(Eigen::VectorXd(0)); + taskB->update(); EXPECT_TRUE(taskB->getA() == Eigen::MatrixXd::Zero(JB.rows(), JB.cols())); //here we reactivate the task taskB->setActive(true); - taskB->update(Eigen::VectorXd(0)); + taskB->update(); EXPECT_TRUE(taskB->getA() == JB); //3) Test setActive with postural + Cartesian tasks OpenSoT::tasks::Aggregated::Ptr taskAB = (taskA + taskB); - taskAB->update(Eigen::VectorXd(0)); + taskAB->update(); Eigen::MatrixXd JAB = taskAB->getA(); //here we deactivate the postural task taskA->setActive(false); - taskAB->update(Eigen::VectorXd(0)); + taskAB->update(); EXPECT_TRUE(taskAB->getA().block(0,0,JA.rows(),JA.cols()) == Eigen::MatrixXd::Zero(JA.rows(), JA.cols())); EXPECT_TRUE(taskAB->getA().block(JA.rows(),0,JB.rows(),JA.cols()) == JB); @@ -115,7 +115,7 @@ TEST_F(testActivateStack, test_deactivate_task) //here we reactivate the postural task and we deactivate the Cartesian task taskA->setActive(true); taskB->setActive(false); - taskAB->update(Eigen::VectorXd(0)); + taskAB->update(); EXPECT_TRUE(taskAB->getA().block(0,0,JA.rows(),JA.cols()) == JA); EXPECT_TRUE(taskAB->getA().block(JA.rows(),0,JB.rows(),JA.cols()) == Eigen::MatrixXd::Zero(JB.rows(), JB.cols())); @@ -123,14 +123,14 @@ TEST_F(testActivateStack, test_deactivate_task) //here both tasks are active taskA->setActive(true); taskB->setActive(true); - taskAB->update(Eigen::VectorXd(0)); + taskAB->update(); EXPECT_TRUE(taskAB->getA() == JAB); //here both tasks are not active taskA->setActive(false); taskB->setActive(false); - taskAB->update(Eigen::VectorXd(0)); + taskAB->update(); EXPECT_TRUE(taskAB->getA() == Eigen::MatrixXd::Zero(JA.rows() + JB.rows(), JB.cols())); @@ -138,24 +138,24 @@ TEST_F(testActivateStack, test_deactivate_task) taskA->setActive(true); taskB->setActive(true); taskAB->setActive(false); - taskAB->update(Eigen::VectorXd(0)); + taskAB->update(); EXPECT_TRUE(taskAB->getA() == Eigen::MatrixXd::Zero(JA.rows() + JB.rows(), JB.cols())); //4)Check Cartesian task with q update taskB->setActive(false); - taskB->update(Eigen::VectorXd(0)); + taskB->update(); q = _model_ptr->generateRandomQ(); _model_ptr->setJointPosition(q); _model_ptr->update(); - taskB->update(Eigen::VectorXd(0)); + taskB->update(); EXPECT_TRUE(taskB->getA() == Eigen::MatrixXd::Zero(JB.rows(), JB.cols())); //The Jacobian is changed wrt the previous q! taskB->setActive(true); - taskB->update(Eigen::VectorXd(0)); + taskB->update(); EXPECT_FALSE(taskB->getA() == JB); @@ -187,7 +187,7 @@ TEST_F(testActivateStack, test_deactivate_stack) autostack = ((left_arm)/ (postural)) << joint_limits; - autostack->update(Eigen::VectorXd(0)); + autostack->update(); OpenSoT::solvers::iHQP sot(autostack->getStack(), autostack->getBounds()); @@ -214,7 +214,7 @@ TEST_F(testActivateStack, test_deactivate_stack) _model_ptr->setJointPosition(q); _model_ptr->update(); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); ASSERT_TRUE(sot.solve(dq)); diff --git a/tests/solvers/TestQPOases_SubTask.cpp b/tests/solvers/TestQPOases_SubTask.cpp index 5d9fb265..13a496d5 100644 --- a/tests/solvers/TestQPOases_SubTask.cpp +++ b/tests/solvers/TestQPOases_SubTask.cpp @@ -112,7 +112,7 @@ TEST_F(testQPOases_SubTask, testSolveUsingSubTasks) q = _model_ptr->sum(q, dq); _model_ptr->setJointPosition(q); _model_ptr->update(); - subTaskTest->update(Eigen::VectorXd(0)); + subTaskTest->update(); ASSERT_TRUE(solver->solve(dq)); } @@ -142,7 +142,7 @@ TEST_F(testQPOases_SubTask, testSolveUsingSubTasksAndAggregated) q = _model_ptr->sum(q, dq); _model_ptr->setJointPosition(q); _model_ptr->update(); - subTaskTest->update(Eigen::VectorXd(0)); + subTaskTest->update(); ASSERT_TRUE(solver->solve(dq)); } @@ -171,7 +171,7 @@ TEST_F(testQPOases_SubTask, testSolveCartesianThroughSubTasksAndAggregated) { _model_ptr->setJointPosition(q); _model_ptr->update(); - subTaskTest->update(q); + subTaskTest->update(); Eigen::VectorXd dq(_model_ptr->getNv()); dq.setZero(); ASSERT_TRUE(solver->solve(dq)); q = _model_ptr->sum(q, dq); diff --git a/tests/solvers/TestSOTH.cpp b/tests/solvers/TestSOTH.cpp index c4a9c988..56a0f52f 100644 --- a/tests/solvers/TestSOTH.cpp +++ b/tests/solvers/TestSOTH.cpp @@ -174,7 +174,7 @@ TEST_F(testSOTH, constrainedVariableLinearSystemOpenSoT) ub[0] = 1.; ub[1] = 1.; ub[2] = 1.; constr->setBounds(ub, lb); - stack->update(Eigen::VectorXd(1)); + stack->update(); hcod.solve(solution); diff --git a/tests/solvers/TesteiQuadProg.cpp b/tests/solvers/TesteiQuadProg.cpp index a6005045..7211aacb 100644 --- a/tests/solvers/TesteiQuadProg.cpp +++ b/tests/solvers/TesteiQuadProg.cpp @@ -145,7 +145,7 @@ TEST_F(testeiQuadProgProblem, testTask) OpenSoT::tasks::velocity::Postural postural_task(*_model_ptr); postural_task.setReference(q_ref); - postural_task.update(Eigen::VectorXd(0)); + postural_task.update(); Eigen::MatrixXd H(_model_ptr->getNv(), _model_ptr->getNv()); H.setIdentity(H.rows(), H.cols()); Eigen::VectorXd g(-1.0*postural_task.getb()); @@ -166,7 +166,7 @@ TEST_F(testeiQuadProgProblem, testTask) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task.update(Eigen::VectorXd(0)); + postural_task.update(); qp_postural_problem->updateTask(H, -1.0*postural_task.getb()); @@ -230,7 +230,7 @@ TEST_F(testeiQuadProgProblem, testProblemWithConstraint) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); qp_postural_problem->updateBounds(constraint->getLowerBound(), constraint->getUpperBound()); qp_postural_problem->updateTask(postural_task->getA(), -1.0*postural_task->getb()); @@ -309,7 +309,7 @@ TEST_F(testeiQuadProgProblem, testCartesian) new OpenSoT::tasks::velocity::Cartesian("cartesian::left_wrist", *_model_ptr, "l_wrist", "Waist")); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); Eigen::MatrixXd T_actual = cartesian_task->getActualPose(); std::cout<<"T_actual: \n"<setReference(T_ref); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); //OpenSoT::solvers::OSQPBackEnd qp_cartesian_problem(cartesian_task->getXSize(), 0); OpenSoT::solvers::BackEnd::Ptr qp_cartesian_problem = OpenSoT::solvers::BackEndFactory( @@ -337,7 +337,7 @@ TEST_F(testeiQuadProgProblem, testCartesian) _model_ptr->update(); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); qp_cartesian_problem->updateTask(cartesian_task->getA().transpose()*cartesian_task->getA(), -1.0*cartesian_task->getA().transpose()*cartesian_task->getb()); ASSERT_TRUE(qp_cartesian_problem->solve()); Eigen::VectorXd dq = qp_cartesian_problem->getSolution(); @@ -372,7 +372,7 @@ TEST_F(testeiQuadProgProblem, testEpsRegularisation) new OpenSoT::tasks::velocity::Cartesian("cartesian::left_wrist", *_model_ptr, "l_wrist", "Waist")); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); Eigen::MatrixXd T_actual = cartesian_task->getActualPose(); std::cout<<"T_actual: \n"<setReference(T_ref); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); //OpenSoT::solvers::OSQPBackEnd qp_cartesian_problem(cartesian_task->getXSize(), 0); OpenSoT::solvers::BackEnd::Ptr qp_cartesian_problem = OpenSoT::solvers::BackEndFactory( @@ -406,7 +406,7 @@ TEST_F(testeiQuadProgProblem, testEpsRegularisation) _model_ptr->update(); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); qp_cartesian_problem->updateTask(cartesian_task->getA().transpose()*cartesian_task->getA(), -1.0*cartesian_task->getA().transpose()*cartesian_task->getb()); ASSERT_TRUE(qp_cartesian_problem->solve()); Eigen::VectorXd dq = qp_cartesian_problem->getSolution(); @@ -500,8 +500,8 @@ TEST_F(testeiQuadProgProblem, testContructor2Problems) _model_ptr->setJointPosition(q); _model_ptr->update(); - cartesian_task->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); + postural_task->update(); joint_constraints->update(); ASSERT_TRUE(sot.solve(dq)); @@ -598,7 +598,7 @@ TEST_F(testiHQP, testContructor1Problem) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); bounds->update(); EXPECT_TRUE(sot.solve(dq)); diff --git a/tests/solvers/TestiHQP.cpp b/tests/solvers/TestiHQP.cpp index 02f85402..afe5daeb 100644 --- a/tests/solvers/TestiHQP.cpp +++ b/tests/solvers/TestiHQP.cpp @@ -82,7 +82,7 @@ TEST_F(testClass, testUserRegularisation) _postural = std::make_shared( "_postural", I, (qd-q)); - _postural->update(Eigen::VectorXd(0)); + _postural->update(); OpenSoT::AutoStack::Ptr stack; stack /= _postural; @@ -115,7 +115,7 @@ TEST_F(testClass, testUserRegularisation) _minvel = std::make_shared( "minvel", I, -(1./dt)*qdot); - _minvel->update(Eigen::VectorXd(0)); + _minvel->update(); stack->setRegularisationTask(_minvel); diff --git a/tests/solvers/Testl1HQP.cpp b/tests/solvers/Testl1HQP.cpp index a6ac7e9a..8dae08fa 100644 --- a/tests/solvers/Testl1HQP.cpp +++ b/tests/solvers/Testl1HQP.cpp @@ -78,7 +78,7 @@ TEST_F(testl1HQP, testContructor) std::make_shared(*_model_ptr, qmax, qmin); OpenSoT::AutoStack::Ptr stack = ((l_sole + r_sole)/CoM)<update(Eigen::VectorXd(0)); + stack->update(); OpenSoT::solvers::l1HQP::Ptr l1_solver = std::make_shared(*stack); @@ -137,7 +137,7 @@ TEST_F(testl1HQP, testContructor) std::vector priority_constraints = l1_solver->getPriorityConstraints(); if(priority_constraints.size() > 0) { - priority_constraints[0]->update(Eigen::VectorXd(0)); + priority_constraints[0]->update(); EXPECT_TRUE(priority_constraints.size() == stack->getStack().size()-1); EXPECT_EQ(priority_constraints[0]->getAineq().rows(), 1); EXPECT_EQ(priority_constraints[0]->getAineq().cols(), linear_tasks["t0"]->getc().size()); diff --git a/tests/solvers/TestqpSWIFT.cpp b/tests/solvers/TestqpSWIFT.cpp index e51707d0..fec3dc39 100644 --- a/tests/solvers/TestqpSWIFT.cpp +++ b/tests/solvers/TestqpSWIFT.cpp @@ -197,7 +197,7 @@ TEST_F(testqpSWIFTProblem, testTask) OpenSoT::tasks::velocity::Postural postural_task(*_model_ptr); postural_task.setReference(q_ref); - postural_task.update(Eigen::VectorXd(0)); + postural_task.update(); Eigen::MatrixXd H(_model_ptr->getNv(), _model_ptr->getNv()); H.setIdentity(H.rows(), H.cols()); Eigen::VectorXd g(-1.0*postural_task.getb()); @@ -226,7 +226,7 @@ TEST_F(testqpSWIFTProblem, testTask) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task.update(Eigen::VectorXd(0)); + postural_task.update(); qp_postural_problem->updateTask(H, -1.0*postural_task.getb()); @@ -265,7 +265,7 @@ TEST_F(testqpSWIFTProblem, testProblemWithConstraint) new JointLimits(*_model_ptr, q_max, q_min)); postural_task->getConstraints().push_back(joint_limits); postural_task->setLambda(1.); - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); OpenSoT::solvers::BackEnd::Ptr qp_postural_problem = OpenSoT::solvers::BackEndFactory( OpenSoT::solvers::solver_back_ends::qpSWIFT, postural_task->getXSize(), 0, @@ -287,7 +287,7 @@ TEST_F(testqpSWIFTProblem, testProblemWithConstraint) Eigen::VectorXd l, u; for(unsigned int i = 0; i < 100; ++i) { - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); qp_postural_problem->updateBounds(constraint->getLowerBound(), constraint->getUpperBound()); qp_postural_problem->updateTask(postural_task->getA(), -1.0*postural_task->getb()); @@ -364,7 +364,7 @@ TEST_F(testqpSWIFTProblem, testCartesian) new OpenSoT::tasks::velocity::Cartesian("cartesian::left_wrist", *_model_ptr, "l_wrist", "Waist")); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); Eigen::MatrixXd T_actual = cartesian_task->getActualPose(); std::cout<<"T_actual: \n"<setReference(T_ref); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); OpenSoT::solvers::BackEnd::Ptr qp_cartesian_problem = OpenSoT::solvers::BackEndFactory( OpenSoT::solvers::solver_back_ends::qpSWIFT, cartesian_task->getXSize(), 0, @@ -391,7 +391,7 @@ TEST_F(testqpSWIFTProblem, testCartesian) _model_ptr->update(); - cartesian_task->update(q); + cartesian_task->update(); qp_cartesian_problem->updateTask(cartesian_task->getA().transpose()*cartesian_task->getA(), -1.0*cartesian_task->getA().transpose()*cartesian_task->getb()); ASSERT_TRUE(qp_cartesian_problem->solve()); Eigen::VectorXd dq = qp_cartesian_problem->getSolution(); @@ -426,7 +426,7 @@ TEST_F(testqpSWIFTProblem, testEpsRegularisation) new OpenSoT::tasks::velocity::Cartesian("cartesian::left_wrist", *_model_ptr, "l_wrist", "Waist")); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); Eigen::MatrixXd T_actual = cartesian_task->getActualPose(); std::cout<<"T_actual: \n"<setReference(T_ref); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); OpenSoT::solvers::BackEnd::Ptr qp_cartesian_problem = OpenSoT::solvers::BackEndFactory( OpenSoT::solvers::solver_back_ends::qpSWIFT, cartesian_task->getXSize(), 0, @@ -459,7 +459,7 @@ TEST_F(testqpSWIFTProblem, testEpsRegularisation) _model_ptr->update(); - cartesian_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); qp_cartesian_problem->updateTask(cartesian_task->getA().transpose()*cartesian_task->getA(), -1.0*cartesian_task->getA().transpose()*cartesian_task->getb()); ASSERT_TRUE(qp_cartesian_problem->solve()); Eigen::VectorXd dq = qp_cartesian_problem->getSolution(); @@ -553,8 +553,8 @@ TEST_F(testqpSWIFTProblem, testContructor2Problems) _model_ptr->setJointPosition(q); _model_ptr->update(); - cartesian_task->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); + postural_task->update(); joint_constraints->update(); ASSERT_TRUE(sot.solve(dq)); @@ -641,7 +641,7 @@ TEST_F(testiHQP, testContructor1Problem) double obj_; for(unsigned int i = 0; i < 100; ++i) { - postural_task->update(Eigen::VectorXd(0)); + postural_task->update(); bounds->update(); EXPECT_TRUE(sot.solve(dq)); diff --git a/tests/tasks/TestAggregated.cpp b/tests/tasks/TestAggregated.cpp index 84e61a00..1035afe6 100644 --- a/tests/tasks/TestAggregated.cpp +++ b/tests/tasks/TestAggregated.cpp @@ -130,8 +130,8 @@ TEST_F(testAggregatedTask, testAggregatedTask_) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task->update(Eigen::VectorXd(0)); - aggregated_task->update(Eigen::VectorXd(0)); + postural_task->update(); + aggregated_task->update(); EXPECT_TRUE(aggregated_task->getA() == postural_task->getA()); EXPECT_TRUE(aggregated_task->getb() == postural_task->getb()); @@ -160,8 +160,8 @@ TEST_F(testAggregatedTask, testAggregatedTask_) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural_task->update(Eigen::VectorXd(0)); - aggregated_task->update(Eigen::VectorXd(0)); + postural_task->update(); + aggregated_task->update(); EXPECT_TRUE(aggregated_task->getA() == postural_task->getA()); EXPECT_TRUE(aggregated_task->getb() == postural_task->getb()) << "aggregated_task b is " << aggregated_task->getb() << std::endl << " while postural_task b is " << postural_task->getb(); @@ -260,8 +260,8 @@ TEST_F(testAggregatedTask, testAggregatedCost) Eigen::VectorXd dq(_model_ptr->getNv()); dq.setRandom(); - Cartesian1->update(Eigen::VectorXd(0)); - Cartesian2->update(Eigen::VectorXd(0)); + Cartesian1->update(); + Cartesian2->update(); double Cartesian1_cost = Cartesian1->computeCost(dq); double Cartesian2_cost = Cartesian2->computeCost(dq); @@ -328,9 +328,9 @@ TEST_F(testAggregatedTask, testConstraintsUpdate) _task1->getConstraints().push_back(constraintCoMVelocity); EXPECT_EQ(_task1->getConstraints().size(), 2)<<"3.2"<update(Eigen::VectorXd(0)); + _task0->update(); EXPECT_EQ(_task0->getConstraints().size(), 2)<<"4"<update(Eigen::VectorXd(0)); + _task1->update(); EXPECT_EQ(_task1->getConstraints().size(), 2)<<"5"<getOwnConstraints().size(), 1); ASSERT_EQ(_task1->getAggregatedConstraints().size(), 1); @@ -393,9 +393,9 @@ TEST_F(testAggregatedTask, testConstraintsUpdate) _model_ptr->setJointPosition(q); _model_ptr->update(); - _task0->update(Eigen::VectorXd(0)); + _task0->update(); EXPECT_EQ(_task0->getConstraints().size(), 2)<<"6"<update(Eigen::VectorXd(0)); + _task1->update(); EXPECT_EQ(_task1->getConstraints().size(), 2)<<"7"<getA(); @@ -482,7 +482,7 @@ TEST_F(testAggregatedTask, testWeightsUpdate) W = Eigen::MatrixXd::Identity(W.rows(), W.cols()); waist->setWeight(10*W); lwrist->setWeight(20*W); - t1->update(q); + t1->update(); std::cout<<"t1->getWeight(): \n"<getWeight()<getWeight(): \n"<getWeight()<getWeight(): \n"<getWeight()<getWeight(), t1->getWeight().block(0,0,6,6))); Eigen::MatrixXd W2(12,12); W2.setOnes(); t1->setWeight(W2); - t1->update(q); + t1->update(); std::cout<<"t1->getWeight(): \n"<getWeight()<getWeight(): \n"<getWeight()<getWeight(): \n"<getWeight()<getWeight(), t1->getWeight().block(0,0,6,6))); waist->setWeight(10*W); lwrist->setWeight(20*W); - t1->update(q); + t1->update(); std::cout<<"t1->getWeight(): \n"<getWeight()<getWeight(): \n"<getWeight()<getWeight(): \n"<getWeight()< id = {1,2}; auto s1 = rwrist%id; - s1->update(Eigen::VectorXd(0)); + s1->update(); auto t2 = (t1+s1); - t2->update(Eigen::VectorXd(0)); + t2->update(); std::cout<<"t1->getWeight(): \n"<getWeight()<getWeight(): \n"<getWeight()<(waist, _model_ptr->getNv()); - aggr->update(Eigen::VectorXd(0)); + aggr->update(); EXPECT_TRUE(aggr->getA() == waist->getA()); EXPECT_TRUE(aggr->getb() == waist->getb()); diff --git a/tests/tasks/TestGenericTask.cpp b/tests/tasks/TestGenericTask.cpp index e3cea165..5309a433 100644 --- a/tests/tasks/TestGenericTask.cpp +++ b/tests/tasks/TestGenericTask.cpp @@ -107,7 +107,7 @@ TEST_F(testGenericLPTask, testMethods) std::cout<<"new_c: "<_generic_lp_task->setc(new_c); - this->_generic_lp_task->update(Eigen::VectorXd(1)); + this->_generic_lp_task->update(); EXPECT_TRUE(this->_generic_lp_task->getA() == Eigen::MatrixXd::Zero(0, new_c.size())); EXPECT_TRUE(this->_generic_lp_task->getb() == Eigen::VectorXd::Zero(0)); @@ -132,7 +132,7 @@ TEST_F(testGenericTask, testMethods) Eigen::MatrixXd newA(this->A.rows(), this->A.cols()); newA = 2.*this->A; EXPECT_TRUE(this->_generic_task->setA(newA)); - this->_generic_task->update(Eigen::VectorXd(1)); + this->_generic_task->update(); EXPECT_EQ(newA, this->_generic_task->getA()); std::cout<<"newA: "<_generic_task->getA(): "<_generic_task->getA()<b.size()); newb = 3.*this->b; EXPECT_TRUE(this->_generic_task->setb(newb)); - this->_generic_task->update(Eigen::VectorXd(1)); + this->_generic_task->update(); EXPECT_EQ(newA, this->_generic_task->getA()); std::cout<<"newA: "<_generic_task->getA(): "<_generic_task->getA()<A.cols()); EXPECT_TRUE(this->_generic_task->setAb(F,g)); - this->_generic_task->update(Eigen::VectorXd(1)); + this->_generic_task->update(); EXPECT_EQ(F, this->_generic_task->getA()); std::cout<<"F: "<_generic_task->getA(): "<_generic_task->getA()<update(Eigen::VectorXd(1)); - generic_min_var->update(Eigen::VectorXd(1)); + min_var->update(); + generic_min_var->update(); for(unsigned int i = 0; i < M.rows(); ++i) { diff --git a/tests/tasks/TestSubTask.cpp b/tests/tasks/TestSubTask.cpp index a54a0b57..d4209d90 100644 --- a/tests/tasks/TestSubTask.cpp +++ b/tests/tasks/TestSubTask.cpp @@ -61,7 +61,7 @@ class TestSubTask: public TestBase _joint_limits = std::make_shared(*_model_ptr, qmax, qmin); - _postural->update(Eigen::VectorXd(0)); + _postural->update(); _joint_limits->update(); @@ -378,7 +378,7 @@ TEST_F(TestSubTask, testgetb) b = subTask->getb(); subTask->setLambda(0.1); - subTask->update(_postural->getActualPositions()); + subTask->update(); bLambda = subTask->getb(); EXPECT_TRUE(vectorAreEqual(bLambda,b*0.1)); @@ -401,7 +401,7 @@ TEST_F(TestSubTask, testgetWeight) ASSERT_EQ(subTask->getWeight().rows(), 3); ASSERT_EQ(subTask->getWeight().cols(), 3); - subTask->update(Eigen::VectorXd(0)); + subTask->update(); std::cout<<"subTask->getWeight(): \n"<getWeight()<setWeight(W); std::cout<<"_postural->getWeight(): \n"<<_postural->getWeight()<update(Eigen::VectorXd(0)); + subTask->update(); std::cout<<"subTask->getWeight(): \n"<getWeight()<update(Eigen::VectorXd(0)); + tmp->update(); std::cout<<"tmp->getWeight(): \n"<getWeight()<getWeight(),_postural->getWeight().block(0,0,3,3))); EXPECT_TRUE(matrixAreEqual(subTask->getWeight(),tmp->getWeight())); tmp->setWeight(2*W.block(0,0,3,3)); - subTask->update(Eigen::VectorXd(0)); - tmp->update(Eigen::VectorXd(0)); + subTask->update(); + tmp->update(); std::cout<<"_postural->getWeight(): \n"<<_postural->getWeight()<getWeight(): \n"<getWeight()<getWeight(): \n"<getWeight()<update(); - subTask->update(Eigen::VectorXd(0)); + subTask->update(); EXPECT_FALSE( b == subTask->getb())<<"b: "<getb(): "<getb()<getConstraints().front()->getLowerBound()); @@ -541,7 +541,7 @@ TEST_F(TestSubTask, testGetConstraints) _model_ptr->setJointPosition(q); _model_ptr->update(); - subTask->update(Eigen::VectorXd(0)); + subTask->update(); EXPECT_FALSE( b == subTask->getb())<<"b: "<getb(): "<getb()<getConstraints().front()->getLowerBound()); @@ -617,8 +617,8 @@ TEST_F(TestSubTask, testWithCartesian) indices.push_back(2); OpenSoT::SubTask::Ptr pose = std::make_shared(left_leg, indices); - left_leg->update(Eigen::VectorXd(0)); - pose->update(Eigen::VectorXd(0)); + left_leg->update(); + pose->update(); check_matrix_are_equal(pose->getA(), left_leg->getA().block(0,0,3,_model_ptr->getNv())); @@ -635,8 +635,8 @@ TEST_F(TestSubTask, testWithCartesian) indices.push_back(5); OpenSoT::SubTask::Ptr pose2 = std::make_shared(left_leg, indices); - left_leg->update(Eigen::VectorXd(0)); - pose2->update(Eigen::VectorXd(0)); + left_leg->update(); + pose2->update(); check_matrix_are_equal(pose2->getA(), left_leg->getA().block(3,0,3,_model_ptr->getNv())); @@ -653,7 +653,7 @@ TEST_F(TestSubTask, testWithCartesian) indices.push_back(4); indices.push_back(5); OpenSoT::SubTask::Ptr subtask = std::make_shared(left_leg, indices); - subtask->update(Eigen::VectorXd(0)); + subtask->update(); EXPECT_EQ(subtask->getA().rows(), indices.size()); EXPECT_EQ(subtask->getA().cols(), _model_ptr->getNv()); @@ -677,7 +677,7 @@ TEST_F(TestSubTask, testWithCartesian) indices.push_back(0); indices.push_back(4); OpenSoT::SubTask::Ptr subtask2 = std::make_shared(left_leg, indices); - subtask2->update(Eigen::VectorXd(0)); + subtask2->update(); EXPECT_EQ(subtask2->getA().rows(), indices.size()); EXPECT_EQ(subtask2->getA().cols(), _model_ptr->getNv()); @@ -734,7 +734,7 @@ TEST_F(TestSubTask, testSubTaskCost) orientation_indices.push_back(5); OpenSoT::SubTask::Ptr orientation = std::make_shared(left_leg, orientation_indices); - left_leg->update(Eigen::VectorXd(0)); + left_leg->update(); Eigen::VectorXd dq(_model_ptr->getNv()); dq.setRandom(); @@ -760,7 +760,7 @@ TEST_F(TestSubTask, testWithPostural) velocity::Postural::Ptr postural = std::make_shared(*_model_ptr); postural->setReference(q_ref); - postural->update(Eigen::VectorXd()); + postural->update(); std::list indices; // first chunk of size 3 @@ -782,7 +782,7 @@ TEST_F(TestSubTask, testWithPostural) indices.push_back(11); OpenSoT::SubTask::Ptr subtask = std::make_shared(postural, indices); - subtask->update(Eigen::VectorXd(0)); + subtask->update(); for(unsigned int j = 0; j < _model_ptr->getNv(); ++j) diff --git a/tests/tasks/TestTask.cpp b/tests/tasks/TestTask.cpp index 1f18ae2b..fa31a508 100644 --- a/tests/tasks/TestTask.cpp +++ b/tests/tasks/TestTask.cpp @@ -28,7 +28,7 @@ class fooTask: public OpenSoT::Task } ~fooTask(){} - void _update(const Eigen::VectorXd &x){} + void _update(){} }; class testTask: public TestBase @@ -62,7 +62,7 @@ TEST_F(testTask, testCheckConsistency) _model_ptr->setJointPosition(q); _model_ptr->update(); postural.reset(new OpenSoT::tasks::velocity::Postural(*_model_ptr)); - postural->update(Eigen::VectorXd(0)); + postural->update(); EXPECT_TRUE(postural->checkConsistency()); @@ -71,7 +71,7 @@ TEST_F(testTask, testCheckConsistency) fooTask::Ptr footask; footask.reset(new fooTask(fooA, foob)); - footask->update(Eigen::VectorXd::Ones(10)); + footask->update(); EXPECT_FALSE(footask->checkConsistency()); @@ -125,7 +125,7 @@ TEST_F(testTask, testComputeCost) postural->setWeight(10.); postural->setLambda(1.); - postural->update(Eigen::VectorXd(0)); + postural->update(); Eigen::VectorXd x(_model_ptr->getNv()); x.setRandom(); diff --git a/tests/tasks/acceleration/TestCartesian.cpp b/tests/tasks/acceleration/TestCartesian.cpp index dbe98303..30eee514 100644 --- a/tests/tasks/acceleration/TestCartesian.cpp +++ b/tests/tasks/acceleration/TestCartesian.cpp @@ -46,7 +46,7 @@ class testCartesianTask: public TestBase OpenSoT::tasks::acceleration::Postural(*_model)); autostack = (l_arm + r_arm)/(postural); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); eHQP.reset(new OpenSoT::solvers::eHQP(autostack->getStack())); @@ -114,7 +114,7 @@ TEST_F(testCartesianTask, testCartesianTask_) _model->setJointVelocity(_dq); _model->update(); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); if(!(eHQP->solve(ddq))) std::cout<<"CAN NOT SOLVE"<update(Eigen::VectorXd(0)); + autostack->update(); iHQP.reset(new OpenSoT::solvers::iHQP(autostack->getStack(), autostack->getBounds())); @@ -137,7 +137,7 @@ TEST_F(testCoMTask, testCoMTask_) this->_model_ptr->setJointVelocity(_dq); this->_model_ptr->update(); - this->autostack->update(Eigen::VectorXd(0)); + this->autostack->update(); if(!(iHQP->solve(ddq))) diff --git a/tests/tasks/acceleration/TestPostural.cpp b/tests/tasks/acceleration/TestPostural.cpp index 5b871625..2cfd9816 100644 --- a/tests/tasks/acceleration/TestPostural.cpp +++ b/tests/tasks/acceleration/TestPostural.cpp @@ -40,14 +40,14 @@ TEST_F(testPosturalTask, testPosturalTask_subtask) OpenSoT::tasks::acceleration::Postural::Ptr postural( new OpenSoT::tasks::acceleration::Postural(*_model_ptr)); - postural->update(Eigen::VectorXd(0)); + postural->update(); std::cout<<"A size is: ["<getA().rows()<<" x "<getA().cols()<<"]"<getJointNum()<getNeutralQ(); postural->setReference(q); - postural->update(Eigen::VectorXd(0)); + postural->update(); //std::list idx = {_model_ptr->getDofIndex("WaistLat")}; @@ -56,7 +56,7 @@ TEST_F(testPosturalTask, testPosturalTask_subtask) std::cout<<"idx: "<<*(idx.begin())<update(Eigen::VectorXd(0)); + sub_postural->update(); std::cout<<"A task: "<getA()<getb()<setJointVelocity(dq); _model_ptr->update(); - postural.update(Eigen::VectorXd(0)); + postural.update(); Eigen::VectorXd ddq(q.size()); ddq = postural.getb(); dq += ddq*dT; diff --git a/tests/tasks/floating_base/TestContact.cpp b/tests/tasks/floating_base/TestContact.cpp index 20afa179..08b67dbe 100644 --- a/tests/tasks/floating_base/TestContact.cpp +++ b/tests/tasks/floating_base/TestContact.cpp @@ -51,7 +51,7 @@ class testPosturalTask: public TestBase vel_lims.reset(new VelocityLimits(*_model_ptr, M_PI, dT)); autostack = ((l_sole + r_sole)/com/postural)<update(Eigen::VectorXd(0)); + autostack->update(); ik_solver.reset( new OpenSoT::solvers::iHQP(autostack->getStack(), autostack->getBounds(), 1e6)); @@ -143,7 +143,7 @@ TEST_F(testPosturalTask, floating_base_open_loop) OpenSoT::AutoStack::Ptr autostack_fb( new OpenSoT::AutoStack((l_sole_fb + r_sole_fb))); autostack_fb<update(Eigen::VectorXd(0)); + autostack_fb->update(); OpenSoT::solvers::iHQP::Ptr solver_fb( new OpenSoT::solvers::iHQP(autostack_fb->getStack(), autostack_fb->getBounds(), 1e6)); @@ -163,7 +163,7 @@ TEST_F(testPosturalTask, floating_base_open_loop) robot->setJointVelocity(dqm/dT); robot->update(); - autostack_fb->update(Eigen::VectorXd(0)); + autostack_fb->update(); if(!solver_fb->solve(dQ)){ std::cout<<"solver ik can not solve!"<setJointVelocity(dq/dT); _model_ptr->update(); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); if(!ik_solver->solve(dq)){ std::cout<<"solver ik can not solve!"<getNeutralQ(); - _q[_model_ptr->getDofIndex("RHipSag")+1 ] = -25.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("RKneeSag")+1 ] = 50.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("RAnkSag")+1 ] = -25.0*M_PI/180.0; + _q[_model_ptr->getQIndex("RHipSag") ] = -25.0*M_PI/180.0; + _q[_model_ptr->getQIndex("RKneeSag") ] = 50.0*M_PI/180.0; + _q[_model_ptr->getQIndex("RAnkSag") ] = -25.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("LHipSag")+1 ] = -25.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("LKneeSag")+1 ] = 50.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("LAnkSag")+1 ] = -25.0*M_PI/180.0; + _q[_model_ptr->getQIndex("LHipSag") ] = -25.0*M_PI/180.0; + _q[_model_ptr->getQIndex("LKneeSag") ] = 50.0*M_PI/180.0; + _q[_model_ptr->getQIndex("LAnkSag") ] = -25.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("LShSag")+1 ] = -90.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("LForearmPlate")+1 ] = -90.0*M_PI/180.0; + _q[_model_ptr->getQIndex("LShSag") ] = -90.0*M_PI/180.0; + _q[_model_ptr->getQIndex("LForearmPlate") ] = -90.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("RShSag")+1 ] = -90.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("RForearmPlate")+1 ] = -90.0*M_PI/180.0; + _q[_model_ptr->getQIndex("RShSag") ] = -90.0*M_PI/180.0; + _q[_model_ptr->getQIndex("RForearmPlate") ] = -90.0*M_PI/180.0; return _q; } @@ -104,12 +104,13 @@ TEST_F(testForceCoM, testForceCoM_StaticCase) { it != links_in_contact.end(); it++) std::cout<<"link in contact "<<":"<<*it< contact_wrenches; + for(unsigned int i = 0; i < links_in_contact.size(); ++i) + contact_wrenches.push_back(OpenSoT::AffineHelper::Identity(6)); - Eigen::VectorXd contact_wrenches_d(6*links_in_contact.size()); - contact_wrenches_d.setZero(contact_wrenches_d.size()); OpenSoT::tasks::force::CoM::Ptr force_com_task( - new OpenSoT::tasks::force::CoM(contact_wrenches_d, links_in_contact, *(_model_ptr.get()))); - force_com_task->update(contact_wrenches_d); + new OpenSoT::tasks::force::CoM(contact_wrenches, links_in_contact, *(_model_ptr.get()))); + force_com_task->update(); Eigen::MatrixXd A = force_com_task->getA(); EXPECT_EQ(A.rows(), 6); @@ -119,6 +120,8 @@ TEST_F(testForceCoM, testForceCoM_StaticCase) { Eigen::VectorXd b = force_com_task->getb(); EXPECT_EQ(b.rows(), 6); std::cout<<"b = [ "<getA().rows()<<" x "<getA().cols()<<" ]"<getb().size()<<" ]"<getAineq()<<" ]"<getbUpperBound()<<" "<getbLowerBound()<<" "<getAineq().rows()<<" x "<getAineq().cols()<<" ]"<getbUpperBound().size()<<"]"<getbLowerBound().size()<<"]"<getAineq().rows()<<" x "<getAineq().cols()<<" ]"<getbUpperBound().size()<<"]"<getbLowerBound().size()<<"]"<getConstraints().push_back(fc); force_com_task->getConstraints().push_back(wrench_limits); - force_com_task->update(contact_wrenches_d); + force_com_task->update(); - Eigen::VectorXd wrench_reference(contact_wrenches_d.size()); - wrench_reference.setZero(contact_wrenches_d.size()); + Eigen::VectorXd wrench_reference(contact_wrenches.size()*6); + wrench_reference.setZero(); wrench_reference(2) = 100.; wrench_reference(8) = 100.; wrench_reference(14) = 100.; @@ -187,9 +199,9 @@ TEST_F(testForceCoM, testForceCoM_StaticCase) { _model_ptr->setJointPosition(q); _model_ptr->update(); - force_com_task->update(Eigen::VectorXd(0)); - + force_com_task->update(); + Eigen::VectorXd contact_wrenches_d; EXPECT_TRUE(sot->solve(contact_wrenches_d)); std::cout<<"contact_wrenches_d = [ "<getq"<("l_sole_wrench", "l_sole","world", wrench); - _wrench_task->update(Eigen::VectorXd(0)); + _wrench_task->update(); Eigen::VectorXd tmp; _wrench_task->getReference(tmp); @@ -241,7 +253,7 @@ TEST_F(testWrench, testWrench_) { EXPECT_DOUBLE_EQ(tmp[i], 0.0); _wrench_task->setReference(wrench_desired); - _wrench_task->update(Eigen::VectorXd(0)); + _wrench_task->update(); _wrench_task->getReference(tmp); EXPECT_TRUE(tmp.size() == 6)<<"tmp.size(): "<setReference(wrench_desired); - _wrench_task->update(Eigen::VectorXd(0)); + _wrench_task->update(); EXPECT_TRUE(sot->solve(x)); std::cout<<"wrench desired: ["<setReference(wrench_desired); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); OpenSoT::solvers::iHQP::Ptr sot2(new OpenSoT::solvers::iHQP(autostack->getStack(), autostack->getBounds(),0)); @@ -305,7 +317,7 @@ TEST_F(testWrench, testWrench_) { wrench_lims->releaseContact(true); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); EXPECT_TRUE(sot2->solve(x)); @@ -317,7 +329,7 @@ TEST_F(testWrench, testWrench_) { wrench_lims->releaseContact(false); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); EXPECT_TRUE(sot2->solve(x)); @@ -395,7 +407,7 @@ TEST_F(testWrench, testWrenches) { OpenSoT::tasks::force::Wrenches::Ptr wrenches_task = std::make_shared ("wrenches", contacts, base_links, wrenches); - wrenches_task->update(Eigen::VectorXd(0)); + wrenches_task->update(); std::cout<<"wrenches_task->getA(): \n"<getA()<getb(): \n"<getb().transpose()<getWrenchTask("wrench2")->setReference(wrench_desired); - wrenches_task->update(Eigen::VectorXd(0)); + wrenches_task->update(); OpenSoT::solvers::iHQP::Stack stack_of_tasks; @@ -447,7 +459,7 @@ TEST_F(testWrench, testWrenches) { wrenches_task->getWrenchTask("wrench1")->setReference(wrench_desired_1); wrenches_task->getWrenchTask("wrench2")->setReference(wrench_desired_2); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); OpenSoT::solvers::iHQP::Ptr sot2(new OpenSoT::solvers::iHQP(autostack->getStack(), @@ -461,7 +473,7 @@ TEST_F(testWrench, testWrenches) { EXPECT_NEAR(x[i+6], lowerLims[i], 1e-9);} wrenches_lims->getWrenchLimits("wrench2")->releaseContact(true); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); std::cout<<"wrenches_lims->getbLowerBound()"<getbLowerBound().transpose()<getbUpperBound()"<getbUpperBound().transpose()<getWrenchLimits("wrench2")->releaseContact(false); - autostack->update(Eigen::VectorXd(0)); + autostack->update(); EXPECT_TRUE(sot2->solve(x)); std::cout<<"wrench desired: ["<update(Eigen::VectorXd(0)); - l_arm2->update(Eigen::VectorXd(0)); - l_arm3->update(Eigen::VectorXd(0)); + l_arm1->update(); + l_arm2->update(); + l_arm3->update(); EXPECT_TRUE(l_arm1->setBaseLink(base_link2)); - l_arm1->update(Eigen::VectorXd(0)); + l_arm1->update(); EXPECT_NEAR((l_arm1->getA() - l_arm2->getA()).norm(), 0.0, 1e-12); EXPECT_NEAR((l_arm1->getb() - l_arm2->getb()).norm(), 0.0, 1e-12); @@ -72,7 +72,7 @@ TEST_F(testCartesianTask, testSetBaseLink) EXPECT_NEAR((l_arm1->getActualPose() - l_arm2->getActualPose()).norm(), 0.0, 1e-12); EXPECT_TRUE(l_arm1->setBaseLink(base_link3)); - l_arm1->update(Eigen::VectorXd(0)); + l_arm1->update(); EXPECT_NEAR((l_arm1->getA() - l_arm3->getA()).norm(), 0.0, 1e-12); EXPECT_NEAR((l_arm1->getb() - l_arm3->getb()).norm(), 0.0, 1e-12); @@ -128,7 +128,7 @@ TEST_F(testCartesianTask, testCartesianTaskWorldGlobal_) EXPECT_DOUBLE_EQ(cartesian.getLambda(), K); cartesian.setReference(x_ref.matrix()); - cartesian.update(q_whole); + cartesian.update(); Eigen::Vector3d positionError, orientationError; cartesian_utils::computeCartesianError(x.matrix(), x_ref.matrix(), positionError, orientationError); @@ -149,7 +149,7 @@ TEST_F(testCartesianTask, testCartesianTaskWorldGlobal_) _model_ptr->setJointPosition(q_whole); _model_ptr->update(); - cartesian._update(Eigen::VectorXd(0)); + cartesian._update(); q_whole = _model_ptr->sum(q_whole, cartesian.getA().transpose()*cartesian.getb()); _model_ptr->setJointPosition(q_whole); @@ -219,7 +219,7 @@ TEST_F(testCartesianTask, testCartesianTaskWorldLocal_) EXPECT_DOUBLE_EQ(cartesian.getLambda(), K); cartesian.setReference(x_ref.matrix()); - cartesian.update(q_whole); + cartesian.update(); Eigen::Vector3d positionError, orientationError; cartesian_utils::computeCartesianError(x.matrix(), x_ref.matrix(), positionError, orientationError); @@ -241,7 +241,7 @@ TEST_F(testCartesianTask, testCartesianTaskWorldLocal_) _model_ptr->setJointPosition(q_whole); _model_ptr->update(); - cartesian._update(Eigen::VectorXd(0)); + cartesian._update(); q_whole = _model_ptr->sum(q_whole, cartesian.getA().transpose()*cartesian.getb()); _model_ptr->setJointPosition(q_whole); _model_ptr->update(); @@ -320,7 +320,7 @@ TEST_F(testCartesianTask, testCartesianTaskRelativeUpdateWorld_) EXPECT_DOUBLE_EQ(cartesian.getLambda(), K); cartesian.setReference(x_ref.matrix()); - cartesian.update(q_whole); + cartesian.update(); Eigen::Vector3d positionError, orientationError; cartesian_utils::computeCartesianError(x.matrix(), x_ref.matrix(), positionError, orientationError); @@ -341,7 +341,7 @@ TEST_F(testCartesianTask, testCartesianTaskRelativeUpdateWorld_) { _model_ptr->setJointPosition(q_whole); _model_ptr->update(); - cartesian._update(Eigen::VectorXd(0)); + cartesian._update(); q_whole = _model_ptr->sum(q_whole, cartesian.getA().transpose()*cartesian.getb()); _model_ptr->setJointPosition(q_whole); _model_ptr->update(); @@ -414,7 +414,7 @@ TEST_F(testCartesianTask, testCartesianTaskRelativeWaistUpdateWorld_) EXPECT_DOUBLE_EQ(cartesian.getLambda(), K); cartesian.setReference(x_ref.matrix()); - cartesian.update(q_whole); + cartesian.update(); Eigen::Vector3d positionError, orientationError; cartesian_utils::computeCartesianError(x.matrix(), x_ref.matrix(), positionError, orientationError); @@ -435,7 +435,7 @@ TEST_F(testCartesianTask, testCartesianTaskRelativeWaistUpdateWorld_) { _model_ptr->setJointPosition(q_whole); _model_ptr->update(); - cartesian._update(Eigen::VectorXd(0)); + cartesian._update(); q_whole = _model_ptr->sum(q_whole, cartesian.getA().transpose()*cartesian.getb()); _model_ptr->setJointPosition(q_whole); _model_ptr->update(); @@ -480,7 +480,7 @@ TEST_F(testCartesianTask, testActiveJointsMask) "torso", "l_sole"); - cartesian.update(Eigen::VectorXd(0)); + cartesian.update(); std::cout<<"J:\n "< active_joint_mask = cartesian.getActiveJointsMask(); @@ -589,7 +589,7 @@ TEST_F(testCartesianTask, testReset) _model_ptr->setJointPosition(q_whole); _model_ptr->update(); - cartesian.update(Eigen::VectorXd(0)); + cartesian.update(); actual_pose = cartesian.getActualPose(); std::cout<<"actual_pose: \n"<update(); - cartesian_task->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); - manipulability_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); + postural_task->update(); + manipulability_task->update(); joint_constraints->update(); sot.solve(dq); @@ -171,9 +171,9 @@ TEST_F(testManipolability, testManipolabilityTask) _model_ptr->update(); - cartesian_task->update(Eigen::VectorXd(0)); - postural_task->update(Eigen::VectorXd(0)); - manipulability_task->update(Eigen::VectorXd(0)); + cartesian_task->update(); + postural_task->update(); + manipulability_task->update(); joint_constraints->update(); sot_manip.solve(dq); diff --git a/tests/tasks/velocity/TestMinimumEffort.cpp b/tests/tasks/velocity/TestMinimumEffort.cpp index 5b628411..8bcfa158 100644 --- a/tests/tasks/velocity/TestMinimumEffort.cpp +++ b/tests/tasks/velocity/TestMinimumEffort.cpp @@ -79,7 +79,7 @@ TEST_F(testMinimumEffortTask, testMinimumEffortTask_) OpenSoT::tasks::velocity::MinimumEffort::Ptr minimumEffort; minimumEffort.reset(new OpenSoT::tasks::velocity::MinimumEffort(*_model_ptr)); - minimumEffort->update(Eigen::VectorXd(0)); + minimumEffort->update(); OpenSoT::Solver::Stack stack; @@ -125,7 +125,7 @@ TEST_F(testMinimumEffortTask, testMinimumEffortTask_) _model_ptr->setJointPosition(q_whole); _model_ptr->update(); - minimumEffort->update(Eigen::VectorXd(0)); + minimumEffort->update(); double old_effort = minimumEffort->computeEffort(); @@ -151,7 +151,7 @@ TEST_F(testMinimumEffortTask, testMinimumEffortTask_) - minimumEffort->update(Eigen::VectorXd(0)); + minimumEffort->update(); EXPECT_LE(minimumEffort->computeEffort(), old_effort); std::cout << "Effort at step" << i << ": " << minimumEffort->computeEffort() << std::endl; diff --git a/tests/tasks/velocity/TestPostural.cpp b/tests/tasks/velocity/TestPostural.cpp index f74bb32e..0cff733b 100644 --- a/tests/tasks/velocity/TestPostural.cpp +++ b/tests/tasks/velocity/TestPostural.cpp @@ -74,14 +74,14 @@ TEST_F(testPosturalTask, testPosturalTask_) _model_ptr->update(); postural.setReference(q_ref); - postural.update(Eigen::VectorXd(0)); + postural.update(); EXPECT_TRUE(postural.getb() == postural.getLambda()*_model_ptr->difference(q_ref, q)); for(unsigned int i = 0; i < 100; ++i) { _model_ptr->setJointPosition(q); _model_ptr->update(); - postural.update(q); + postural.update(); Eigen::VectorXd qq = postural.getb(); q = _model_ptr->sum(q, qq); } @@ -127,7 +127,7 @@ TEST_F(testPosturalTask, testPosturalTaskWithJointLimits_) Eigen::VectorXd old_UpperBound = bound->getUpperBound(); _model_ptr->setJointPosition(q_next); _model_ptr->update(); - postural->update(q_next); + postural->update(); Eigen::VectorXd new_b = postural->getb(); Eigen::VectorXd new_LowerBound = bound->getLowerBound(); Eigen::VectorXd new_UpperBound = bound->getUpperBound(); @@ -170,7 +170,7 @@ TEST_F(testPosturalTask, testReset) _model_ptr->setJointPosition(q); _model_ptr->update(); - postural.update(Eigen::VectorXd(0)); + postural.update(); actual_q = postural.getActualPositions(); std::cout<<"actual_q: \n"<setReference(T); - affine_task->update(Eigen::VectorXd(0)); + affine_task->update(); Eigen::Affine3d T_ref; task->getReference(T_ref); EXPECT_EQ(T.matrix(), T_ref.matrix()); diff --git a/tests/utils/TestAutoStack.cpp b/tests/utils/TestAutoStack.cpp index 34b6a71e..2333df7f 100644 --- a/tests/utils/TestAutoStack.cpp +++ b/tests/utils/TestAutoStack.cpp @@ -50,7 +50,7 @@ TEST_F(testAutoStack, test_complexAutostack) Eigen::VectorXd q = _model_ptr->generateRandomQ(); - auto_stack->update(Eigen::VectorXd(0)); + auto_stack->update(); std::cout<<"# of constraints: "<getBounds()->getAineq().rows()<getBounds()->getAineq()<leftLeg->setActive(false); - auto_stack->update(Eigen::VectorXd(0)); + auto_stack->update(); std::cout<<"Aineq: \n"<getBounds()->getAineq()<getBounds()->getAineq().topRows(3) == 0.*DHS->leftLeg->getA().topRows(3)); @@ -70,7 +70,7 @@ TEST_F(testAutoStack, test_complexAutostack) DHS->leftLeg->setActive(true); DHS->rightLeg->setActive(false); - auto_stack->update(Eigen::VectorXd(0)); + auto_stack->update(); EXPECT_TRUE(auto_stack->getBounds()->getAineq().topRows(3) == DHS->leftLeg->getA().topRows(3)); EXPECT_TRUE(auto_stack->getBounds()->getAineq().bottomRows(3) == 0*DHS->rightLeg->getA().topRows(3)); @@ -86,7 +86,7 @@ TEST_F(testAutoStack, test_getTask_with_task_id) AutoStack::Ptr auto_stack = (DHS->right2LeftLeg)/ (DHS->com + DHS->leftArm)/ DHS->postural; - auto_stack->update(Eigen::VectorXd(0)); + auto_stack->update(); OpenSoT::solvers::iHQP::TaskPtr com_task = auto_stack->getTask(task_id); EXPECT_TRUE(com_task != NULL); @@ -292,7 +292,7 @@ TEST_F(testAutoStack, testOperatorModulo) KDL::Frame ref; ref.Identity(); DHS->leftArm->setReference(ref); - sub_task->update(Eigen::VectorXd::Zero(DHS->leftArm->getA().cols())); + sub_task->update(); EXPECT_TRUE(sub_task->getA().row(0) == DHS->leftArm->getA().row(0)); EXPECT_TRUE(sub_task->getA().row(1) == DHS->leftArm->getA().row(2));