RobotModelConfig.hpp

Go to the documentation of this file.

#ifndef ROBOTMODELCONFIG_HPP
#define ROBOTMODELCONFIG_HPP
 
#include <base/samples/RigidBodyStateSE3.hpp>
#include <base/NamedVector.hpp>
 
namespace wbc{
 
class ActiveContact{
public:
    ActiveContact(){
 
    }
    ActiveContact(int active, double mu) : active(active), mu(mu){
 
    }
    ActiveContact(int active, double mu, double wx, double wy) : active(active), mu(mu), wx(wx), wy(wy){
 
    }
    int active; 
    double mu;  
    double wx;  
    double wy;  
};
 
class ActiveContacts : public base::NamedVector<ActiveContact>{
public:
    uint getNumberOfActiveContacts() const{
        uint nac = 0;
        for(auto e : elements){
            if(e.active) nac++;
        }
        return nac;
    }
};
 
struct RobotModelConfig{
public:
    RobotModelConfig(){
        floating_base = false;
        type = "pinocchio";
    }
    RobotModelConfig(const std::string& file_or_string,
                     const bool floating_base = false,
                     const ActiveContacts &contact_points = ActiveContacts(),
                     const std::string& submechanism_file = "") :
        file_or_string(file_or_string),
        submechanism_file(submechanism_file),
        type("pinocchio"),
        floating_base(floating_base),
        contact_points(contact_points){
 
    }
    void validate(){
        if(file_or_string.empty())
            throw std::runtime_error("Invalid Robot model config. File path or string must not be empty!");
        if(type == "hyrodyn" && submechanism_file.empty())
            throw std::runtime_error("Invalid Robot model config. If you choose 'hyrodyn' as type, submechanism_file must not be empty!");
        if(floating_base && contact_points.empty())
            throw std::runtime_error("Invalid Robot model config. If floating_base is set to true, contact_points must not be empty!");
    }
 
    std::string file_or_string;
    std::string submechanism_file;
    std::string type;
    bool floating_base;
    ActiveContacts contact_points;
 
    std::vector<std::string> joint_blacklist;
};
 
}
#endif // ROBOTMODELCONFIG_HPP