hierarchical_optimization_controller.h

Go to the documentation of this file.

/*******************************************************************/
/*    AUTHOR: Paal Arthur S. Thorseth                              */
/*    ORGN:   Dept of Eng Cybernetics, NTNU Trondheim              */
/*    FILE:   hierarchical_optimization_controller.h               */
/*    DATE:   May 15, 2021                                         */
/*                                                                 */
/* Copyright (C) 2021 Paal Arthur S. Thorseth,                     */
/*                    Adrian B. Ghansah                            */
/*                                                                 */
/* This program is free software: you can redistribute it          */
/* and/or modify it under the terms of the GNU General             */
/* Public License as published by the Free Software Foundation,    */
/* either version 3 of the License, or (at your option) any        */
/* later version.                                                  */
/*                                                                 */
/* This program is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty     */
/* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.         */
/* See the GNU General Public License for more details.            */
/*                                                                 */
/* You should have received a copy of the GNU General Public       */
/* License along with this program. If not, see                    */
/* <https://www.gnu.org/licenses/>.                                */
/*                                                                 */
/*******************************************************************/
 
#pragma once
 
// C++ Standard Library
#include <cmath>
#include <thread>
 
// ROS
#include "ros/ros.h" 
#include "ros/callback_queue.h"
 
// ROS messages
#include "std_msgs/Int8MultiArray.h"
#include "std_msgs/Float64MultiArray.h"
#include "sensor_msgs/JointState.h"
 
// ROS Package Libraries
#include <kinematics/kinematics.h>
#include <debug_utils/debug_utils.h>
#include <math_utils/Core>
 
// Eigen
#include <Eigen/Core>
 
// Kindr
#include <kindr/Core>
 
// Drake C++ Toolbox
#include <drake/common/symbolic.h>
#include <drake/solvers/mathematical_program.h>
#include <drake/solvers/solve.h>
#include <drake/solvers/osqp_solver.h>
#include <drake/solvers/equality_constrained_qp_solver.h>
#include <drake/solvers/clp_solver.h>
#include <drake/solvers/scs_solver.h>
#include <drake/solvers/snopt_solver.h>
 
class HierarchicalOptimizationControl
{
    public: struct Task {
        bool has_eq_constraint = false;     
        bool has_ineq_constraint = false;   
        Eigen::MatrixXd A_eq;               
        Eigen::VectorXd b_eq;               
        Eigen::MatrixXd A_ineq;             
        Eigen::VectorXd b_ineq;             
    };
 
    public: enum SolverType { OSQP = 1, ECQP = 2, CLP = 3, SCS = 4, SNOPT = 5, UNSPECIFIED};
 
    public: enum LegType { frontLeft = 1, frontRight = 2, rearLeft = 3, rearRight = 4, NONE };
 
    public: enum BodyType { base = 1, hip = 2, thigh = 3, leg = 4, foot = 5 };
 
    public: HierarchicalOptimizationControl();
 
    public: virtual ~HierarchicalOptimizationControl();
 
    // TODO Remove
    public: void StaticTorqueTest();
 
    // TODO Remove
    public: void HeightRollYawTest();
 
    public: Eigen::Matrix<double, 12, 1> HierarchicalOptimization(const Eigen::Matrix<double, 3, 1> &_desired_base_pos,
                                                                  const Eigen::Matrix<double, 3, 1> &_desired_base_vel,
                                                                  const Eigen::Matrix<double, 3, 1> &_desired_base_acc,
                                                                  const Eigen::Matrix<double, 3, 1> &_desired_base_ori,
                                                                  const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_desired_f_pos,
                                                                  const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_desired_f_vel,
                                                                  const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_desired_f_acc,
                                                                  const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_f_pos,
                                                                  const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_f_vel,
                                                                  const Eigen::Matrix<double, 18, 1> &_q,
                                                                  const Eigen::Matrix<double, 18, 1> &_u,
                                                                  const int &_v = 0); 
 
    public: Eigen::Matrix<double, Eigen::Dynamic, 1> HierarchicalQPOptimization(const int &_state_dim,
                                                                                const std::vector<Task> &_tasks,
                                                                                const SolverType &_solver = SolverType::OSQP,
                                                                                const int &_v = 0);
 
    public: Eigen::Matrix<double, Eigen::Dynamic, 1> HierarchicalLeastSquareOptimization(const Eigen::Matrix<Eigen::MatrixXd, Eigen::Dynamic, 1> &_A,
                                                                                         const Eigen::Matrix<Eigen::Matrix<double, Eigen::Dynamic, 1>, Eigen::Dynamic, 1> &_b,
                                                                                         const int &_v = 0);
 
    public: bool SolveQP(const Eigen::MatrixXd &_Q,
                         const Eigen::VectorXd &_c,
                         const Eigen::MatrixXd &_E_ineq,
                         const Eigen::VectorXd &_f_ineq,
                         Eigen::VectorXd &_sol,
                         const SolverType &_solver = SolverType::UNSPECIFIED,
                         const int &_v = 0);
 
    public: bool SolveQP(const Eigen::MatrixXd &_Q,
                         const Eigen::VectorXd &_c,
                         const Eigen::MatrixXd &_A,
                         const Eigen::VectorXd &_lb,
                         const Eigen::VectorXd &_ub,
                         Eigen::VectorXd &_sol,
                         const SolverType &_solver = SolverType::UNSPECIFIED,
                         const int &_v = 0);
 
    public: bool SolveQP(const Eigen::MatrixXd &_Q,
                         const Eigen::VectorXd &_c,
                         const Eigen::MatrixXd &_E_eq,
                         const Eigen::VectorXd &_f_eq,
                         const Eigen::MatrixXd &_E_ineq,
                         const Eigen::VectorXd &_f_ineq,
                         Eigen::VectorXd &_sol,
                         const SolverType &_solver = SolverType::UNSPECIFIED,
                         const int &_v = 0);
 
    public: void PublishTorqueMsg(const Eigen::Matrix<double, 12, 1> &_desired_tau);
 
    public: void OnGenCoordMsg(const std_msgs::Float64MultiArrayConstPtr &_msg);
 
    public: void OnGenVelMsg(const std_msgs::Float64MultiArrayConstPtr &_msg);
 
    public: void OnContactStateMsg(const std_msgs::Int8MultiArrayConstPtr &_msg);
 
    public: void ProcessQueueThread();
 
    public: void PublishQueueThread();
 
    protected: void InitRos();
 
    protected: void InitRosQueueThreads();
 
    // TODO Make private
    public: Kinematics kinematics;
 
    private: Eigen::Matrix<double, 18, 1> genCoord;
 
    private: Eigen::Matrix<double, 18, 1> genVel;
 
    private: Eigen::Matrix<Eigen::Vector3d, 4, 1> fPos;
 
    private: Eigen::Matrix<Eigen::Vector3d, 4, 1> fVel;
 
    private: int contactState[4];
 
    private: std::unique_ptr<ros::NodeHandle> rosNode;
 
    private: ros::Subscriber genCoordSub;
 
    private: ros::Subscriber genVelSub;
 
    private: ros::Subscriber contactStateSub;
 
    private: ros::Publisher jointStatePub;
 
    // TODO remove as it is only needed to log to plots
    private: ros::Publisher basePoseCmdPub;
 
    // TODO remove as it is only needed to log to plots
    private: ros::Publisher baseTwistCmdPub;
 
    private: ros::CallbackQueue rosProcessQueue;
 
    private: ros::CallbackQueue rosPublishQueue;
 
    private: std::thread rosProcessQueueThread;
 
    private: std::thread rosPublishQueueThread;
};