ConfigurationSelector.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __CONFIGURATION_SELECTOR_HPP__
#define __CONFIGURATION_SELECTOR_HPP__
 
#include <vector>
 
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/utilities/utility.h>
 
namespace roboticslab
{
 
class ConfigurationSelector
{
public:
    ConfigurationSelector(const KDL::JntArray & qMin, const KDL::JntArray & qMax)
        : _qMin(qMin),
          _qMax(qMax)
    {}
 
    virtual ~ConfigurationSelector() = default;
 
    virtual bool configure(const std::vector<KDL::JntArray> & solutions, const std::vector<bool> & reachability);
 
    virtual bool findOptimalConfiguration(const KDL::JntArray & qGuess) = 0;
 
    virtual void retrievePose(KDL::JntArray & q) const
    {
        q = *configs[lastValid].retrievePose();
    }
 
    int getValidSolutionIndex() const
    { return lastValid; }
 
    static constexpr int INVALID_CONFIG = -1;
 
protected:
    class Configuration
    {
    public:
        void store(const KDL::JntArray * q)
        { this->q = q; }
 
        const KDL::JntArray * retrievePose() const
        { return q; }
 
        bool isValid() const
        { return valid; }
 
        void invalidate()
        { valid = false; }
 
    private:
        const KDL::JntArray * q {nullptr};
        bool valid {true};
    };
 
    virtual bool validate(Configuration & config);
 
    static bool checkJointInLimits(double q, double qMin, double qMax)
    {
        return q >= (qMin - KDL::epsilon) && q <= (qMax + KDL::epsilon);
    }
 
    KDL::JntArray _qMin, _qMax;
 
    std::vector<Configuration> configs;
    int lastValid {INVALID_CONFIG};
};
 
class ConfigurationSelectorLeastOverallAngularDisplacement : public ConfigurationSelector
{
public:
    ConfigurationSelectorLeastOverallAngularDisplacement(const KDL::JntArray & qMin, const KDL::JntArray & qMax)
        : ConfigurationSelector(qMin, qMax)
    {}
 
    bool findOptimalConfiguration(const KDL::JntArray & qGuess) override;
 
protected:
    std::vector<double> getDiffs(const KDL::JntArray & qGuess, const Configuration & config);
};
 
class ConfigurationSelectorHumanoidGait : public ConfigurationSelectorLeastOverallAngularDisplacement
{
public:
    ConfigurationSelectorHumanoidGait(const KDL::JntArray & qMin, const KDL::JntArray & qMax)
        : ConfigurationSelectorLeastOverallAngularDisplacement(qMin, qMax)
    {}
 
    bool findOptimalConfiguration(const KDL::JntArray & qGuess) override;
 
private:
    bool applyConstraints(const Configuration & config);
};
 
class ConfigurationSelectorFactory
{
public:
    virtual ConfigurationSelector * create() const = 0;
    virtual ~ConfigurationSelectorFactory() = default;
 
protected:
    ConfigurationSelectorFactory(const KDL::JntArray & qMin, const KDL::JntArray & qMax)
        : _qMin(qMin), _qMax(qMax)
    {}
 
    KDL::JntArray _qMin, _qMax;
};
 
class ConfigurationSelectorLeastOverallAngularDisplacementFactory : public ConfigurationSelectorFactory
{
public:
    ConfigurationSelectorLeastOverallAngularDisplacementFactory(const KDL::JntArray & qMin, const KDL::JntArray & qMax)
        : ConfigurationSelectorFactory(qMin, qMax)
    {}
 
    ConfigurationSelector * create() const override
    {
        return new ConfigurationSelectorLeastOverallAngularDisplacement(_qMin, _qMax);
    }
};
 
class ConfigurationSelectorHumanoidGaitFactory : public ConfigurationSelectorFactory
{
public:
    ConfigurationSelectorHumanoidGaitFactory(const KDL::JntArray & qMin, const KDL::JntArray & qMax)
        : ConfigurationSelectorFactory(qMin, qMax)
    {}
 
    ConfigurationSelector * create() const override
    {
        if (_qMin.rows() == 6 && _qMax.rows() == 6)
        {
            return new ConfigurationSelectorHumanoidGait(_qMin, _qMax);
        }
        else
        {
            return nullptr;
        }
    }
};
 
} // namespace roboticslab
 
#endif // __CONFIGURATION_SELECTOR_HPP__