AccelerationSceneTSID.hpp

Go to the documentation of this file.

#ifndef WBCACCELERATIONSCENETSID_HPP
#define WBCACCELERATIONSCENETSID_HPP
 
#include "../../core/Scene.hpp"
#include "../../types/Wrench.hpp"
 
namespace wbc{

class AccelerationSceneTSID : public Scene{
protected:
    static SceneRegistry<AccelerationSceneTSID> reg;
 
    // Helper variables
    std::vector<types::Contact> contacts;
    Eigen::VectorXd robot_acc, solver_output_acc;
    std::vector<types::Wrench> contact_wrenches;
    double hessian_regularizer;
    std::vector< TaskPtr > tasks;
    std::vector< ConstraintPtr > constraints;
    HierarchicalQP hqp;
    bool configured;
    types::JointCommand solver_output_joints;
    Eigen::VectorXd solver_output;
 
    bool contactsHaveChanged(const std::vector<types::Contact>& old_contacts, const std::vector<types::Contact>& new_contacts){
        if(old_contacts.size() != new_contacts.size())
            return true;
        for(uint i = 0; i < old_contacts.size(); i++){
            if(old_contacts[i].active != new_contacts[i].active)
                return true;
        }
        return false;
    }
 
public:
    AccelerationSceneTSID(RobotModelPtr robot_model, QPSolverPtr solver, const double dt);
    virtual ~AccelerationSceneTSID(){
    }

    virtual bool configure(const std::vector<TaskPtr> &tasks);

    virtual const HierarchicalQP& update();

    virtual const types::JointCommand& solve(const HierarchicalQP& hqp);

    const std::vector<types::Wrench>& getContactWrenches(){return contact_wrenches;}

    void setHessianRegularizer(const double reg){hessian_regularizer=reg;}

    double getHessianRegularizer(){return hessian_regularizer;}

    const Eigen::VectorXd& getSolverOutputRaw() const { return solver_output; }
};
 
} // namespace wbc
 
#endif