AsibotConfiguration.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __ASIBOT_CONFIGURATION_HPP__
#define __ASIBOT_CONFIGURATION_HPP__
 
#include <vector>
#include <string>
 
namespace roboticslab
{
 
class AsibotConfiguration
{
public:
    using JointsIn = const std::vector<double> &;
 
    using JointsOut = std::vector<double> &;
 
    AsibotConfiguration(JointsIn qMin, JointsIn qMax)
        : _qMin(qMin), _qMax(qMax)
    {}
 
    virtual ~AsibotConfiguration() = default;
 
    virtual bool configure(double q1, double q2u, double q2d, double q3, double q4u, double q4d, double q5);
 
    virtual bool findOptimalConfiguration(JointsIn qGuess) = 0;
 
    virtual void retrieveAngles(JointsOut q) const
    {
        optimalPose.retrieveAngles(q);
    }
 
protected:
    struct Pose
    {
        enum orientation { FORWARD, REVERSED };
 
        enum elbow { UP, DOWN };
 
        Pose()
            : _q1(0.0), _q2(0.0), _q3(0.0), _q4(0.0), _q5(0.0), valid(true), _orient(FORWARD), _elb(UP)
        {}
 
        void storeAngles(double q1, double q2, double q3, double q4, double q5, orientation orient, elbow elb);
 
        bool checkJointsInLimits(JointsIn qMin, JointsIn qMax) const;
 
        void retrieveAngles(JointsOut q) const;
 
        std::string toString() const;
 
        double _q1, _q2, _q3, _q4, _q5;
        bool valid;
 
        orientation _orient;
        elbow _elb;
    };
 
    JointsIn _qMin, _qMax;
 
    Pose optimalPose;
 
    Pose forwardElbowUp;
    Pose forwardElbowDown;
    Pose reversedElbowUp;
    Pose reversedElbowDown;
};
 
class AsibotConfigurationLeastOverallAngularDisplacement : public AsibotConfiguration
{
public:
    AsibotConfigurationLeastOverallAngularDisplacement(JointsIn qMin, JointsIn qMax)
        : AsibotConfiguration(qMin, qMax)
    {}
 
    bool findOptimalConfiguration(JointsIn qGuess) override;
 
private:
    std::vector<double> getDiffs(JointsIn qGuess, const Pose & pose);
};
 
class AsibotConfigurationFactory
{
public:
    virtual AsibotConfiguration * create() const = 0;
    virtual ~AsibotConfigurationFactory() = default;
 
protected:
    AsibotConfigurationFactory(AsibotConfiguration::JointsIn qMin, AsibotConfiguration::JointsIn qMax)
        : _qMin(qMin), _qMax(qMax)
    {}
 
    AsibotConfiguration::JointsIn _qMin, _qMax;
};
 
class AsibotConfigurationLeastOverallAngularDisplacementFactory : public AsibotConfigurationFactory
{
public:
    AsibotConfigurationLeastOverallAngularDisplacementFactory(AsibotConfiguration::JointsIn qMin, AsibotConfiguration::JointsIn qMax)
        : AsibotConfigurationFactory(qMin, qMax)
    {}
 
    AsibotConfiguration * create() const override
    {
        return new AsibotConfigurationLeastOverallAngularDisplacement(_qMin, _qMax);
    }
};
 
} // namespace roboticslab
 
#endif // __ASIBOT_CONFIGURATION_HPP__