HierarchicalLSSolver.hpp

Go to the documentation of this file.

#ifndef WBC_SOLVERS_HIERARCHICAL_LS_SOLVER_HPP
#define WBC_SOLVERS_HIERARCHICAL_LS_SOLVER_HPP
 
#include <base/Eigen.hpp>
#include <vector>
#include "../../core/QPSolver.hpp"
 
namespace wbc{
 
class HierarchicalQP;
 
class HierarchicalLSSolver : public QPSolver{
private:
    static QPSolverRegistry<HierarchicalLSSolver> reg;
 
public:
 
    class PriorityData{
    public:
        PriorityData(){}
        PriorityData(const unsigned int _n_constraint_variables, const unsigned int n_joints){
            n_constraint_variables = _n_constraint_variables;
            solution_prio.setZero(n_joints);
            A_proj.setZero(_n_constraint_variables, n_joints);
            A_proj_w.setZero(_n_constraint_variables,n_joints);
            U.setZero(_n_constraint_variables, n_joints);
            A_proj_inv_wls.setZero(_n_constraint_variables, n_joints);
            A_proj_inv_wdls.setZero(_n_constraint_variables, n_joints);
            y_comp.setZero(_n_constraint_variables);
            constraint_weight_mat.resize(_n_constraint_variables, _n_constraint_variables);
            constraint_weight_mat.setIdentity();
            joint_weight_mat.resize(n_joints, n_joints);
            joint_weight_mat.setIdentity();
            u_t_weight_mat.setZero(n_joints, _n_constraint_variables);
            sing_vals.resize(n_joints);
        }
        base::VectorXd solution_prio;         
        base::MatrixXd A_proj;                
        base::MatrixXd A_proj_w;              
        base::MatrixXd U;                     
        base::MatrixXd A_proj_inv_wls;        
        base::MatrixXd A_proj_inv_wdls;       
        base::VectorXd y_comp;                
        base::MatrixXd constraint_weight_mat; 
        base::MatrixXd joint_weight_mat;      
        base::MatrixXd u_t_weight_mat;        
        base::VectorXd sing_vals;             
        double damping;                        
        unsigned int n_constraint_variables;   
    };
 
    HierarchicalLSSolver();
    virtual ~HierarchicalLSSolver();
 
    bool configure(const std::vector<int>& n_constraints_per_prio, const unsigned int n_joints);
 
    virtual void solve(const wbc::HierarchicalQP &hierarchical_qp, base::VectorXd &solver_output);
 
    void setJointWeights(const base::VectorXd& weights);
 
    void setJointWeights(const base::VectorXd& weights, const uint prio);
 
    void setTaskWeights(const base::VectorXd& weights, const uint prio);
 
    void setMinEigenvalue(double min_eigenvalue);
 
    double getMinEigenvalue(){return min_eigenvalue;}
 
    void setMaxSolverOutputNorm(double norm_max);
 
    double getMaxSolverOutputNorm(){return max_solver_output_norm;}
 
    bool isConfigured(){return configured;}
 
protected:
    std::vector<PriorityData> priorities;     
    base::MatrixXd proj_mat;                 
    base::VectorXd s_vals;                   
    base::MatrixXd s_vals_inv;               
    base::MatrixXd sing_vect_r;              
    base::MatrixXd damped_s_vals_inv;        
    base::MatrixXd Wq_V;                     
    base::MatrixXd Wq_V_s_vals_inv;          
    base::MatrixXd Wq_V_damped_s_vals_inv;   
    unsigned int no_of_joints;             
    //Properties
    double min_eigenvalue;    
    double max_solver_output_norm;   
    //Helpers
    base::VectorXd tmp;
};
}
#endif