AccelerationSceneReducedTSID.hpp

Go to the documentation of this file.

#ifndef WBCACCELERATIONSCENEREDUCEDTSID_HPP
#define WBCACCELERATIONSCENEREDUCEDTSID_HPP
 
#include "../../core/Scene.hpp"
#include "../../types/Wrench.hpp"
 
namespace wbc{
 
class ContactsFrictionSurfaceConstraint;

class AccelerationSceneReducedTSID : public Scene{
protected:
    static SceneRegistry<AccelerationSceneReducedTSID> reg;
 
    Eigen::VectorXd robot_acc, solver_output_acc;
    std::vector<types::Contact> contacts;
    std::vector< TaskPtr > tasks;
    std::vector< ConstraintPtr > constraints;
    HierarchicalQP hqp;
    bool configured;
    types::JointCommand solver_output_joints;
    uint dim_contact;
    bool use_spatial_acc_bias;
    double acceleration_penalty;
    double contact_wrench_penalty;
    double acceleration_delta_penalty;
    double wrench_delta_penalty;
    double friction_cone_slack_penalty;
    std::shared_ptr<ContactsFrictionSurfaceConstraint> friction_surface_constraint;
 
    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:
    AccelerationSceneReducedTSID(RobotModelPtr robot_model, QPSolverPtr solver, const double dt, uint dim_contact = 3, bool use_spatial_acc_bias = true);
    virtual ~AccelerationSceneReducedTSID(){}

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

    virtual const HierarchicalQP& update();

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

    void setAccelerationPenalty(const double reg){acceleration_penalty=reg;}

    void setContactWrenchPenalty(const double reg){contact_wrench_penalty=reg;}

    void setAccelerationDeltaPenalty(const double w){acceleration_delta_penalty=w;}

    void setContactWrenchDeltaPenalty(const double w){wrench_delta_penalty=w;}

    void setFrictionConeSlackPenalty(const double w);
};
 
} // namespace wbc
 
#endif