SMURFTrajectoryTracker.hpp

Go to the documentation of this file.

/*
 * SMURFTrajectoryTracker.hpp
 *
 *  Created on: Jun 8, 2012
 *      Author: rspica
 */

#ifndef SMURFTRAJECTORYTRACKER_HPP_
#define SMURFTRAJECTORYTRACKER_HPP_

#include <telekyb_defines/telekyb_defines.hpp>


// Interface Definition
#include <tk_trajctrl/TrajectoryTracker.hpp>

// Dynamic Mass Estimator
// Class Loading
#include <pluginlib/class_loader.h>

// Dynamic parameter estimators
#include <tk_param_estimator/MassEstimator.hpp>
#include <tk_param_estimator/InertiaMatrixEstimator.hpp>

// Ros
#include <ros/ros.h>

#include <telekyb_base/Options.hpp>
#include <telekyb_base/Time.hpp>

// Boost
#include <boost/thread/mutex.hpp>


TELEKYB_ENUM_VALUES(SaturationType, const char*,
        (none)("No command saturation applied")
        (uniform)("Uniform command saturation")
        (qp)("Quadratic programming command saturation")
)


namespace TELEKYB_NAMESPACE {

class SMURFTrajectoryTrackerOptions : public OptionContainer {
public:
        Option<std::string>* tCommandTopic;

        Option<SaturationTypeBaseEnum<const char*>::Type>* tSaturationType;

        Option<std::string>* tMassPluginLookupName;
        Option<std::string>* tInertiaPluginLookupName;

        Option<Eigen::Vector3d>* tPositionGain;
        Option<Eigen::Vector3d>* tVelocityGain;
        Option<Eigen::Vector3d>* tOrientationGain;
        Option<Eigen::Vector3d>* tAngVelGain;
        Option<Eigen::Vector3d>* tRotIntGain;
        Option<Eigen::Vector3d>* tSatRotInt;
        Option<Eigen::Vector3d>* tPosIntGain;
        Option<Eigen::Vector3d>* tSatPosInt;

        Option<double>* tMinThrust;
        Option<double>* tMinForce;
        Option<double>* tMaxForce;

        Option<double>* tArmLength;
        Option<double>* tCParam;

        SMURFTrajectoryTrackerOptions();
};

class SMURFTrajectoryTracker : public TrajectoryTracker {
private:
        SMURFTrajectoryTrackerOptions options;


        inline
        Eigen::Matrix3d hat(const Eigen::Vector3d& w)
        {
                Eigen::Matrix3d Sw;
                Sw <<   0.0, -w(2),  w(1),
                           w(2),   0.0, -w(0),
                      -w(1),  w(0),   0.0;
                return Sw;
        }


        inline
        Eigen::Vector3d vee(const Eigen::Matrix3d& S)
        {
                return Eigen::Vector3d(S(2,1), S(0,2), S(1,0));
        }
        //double polyVal(Eigen::VectorXd coefficients, double x);

        Eigen::Matrix4d invA;

        void saturateUniform(Eigen::Vector4d& force, double mass);
        void saturateQP(Eigen::Vector4d& force, double mass){};

protected:

        // Dynamic parameter Estimation Option
        Option<bool>* tDoMassEstimation;
        Option<bool>* tDoInertiaMatrixEstimation;

        // ClassLoader
        pluginlib::ClassLoader<MassEstimator> meLoader;
        pluginlib::ClassLoader<InertiaMatrixEstimator> imeLoader;

        // Loaded Dynamic parameter Estimator
        MassEstimator* massEstimator;
        InertiaMatrixEstimator* inertiaEstimator;

        double currentMass;
        Eigen::Matrix3d currentInertiaMatrix;

        boost::mutex refTrajectoryMutex;
        TKTrajectory refTrajectory;

        ros::NodeHandle nodeHandle;
        ros::NodeHandle commandNodeHandle;
        ros::Publisher tTcCommandsPub;

        Eigen::Vector3d rotIntState;
        Eigen::Vector3d posIntState;
        ros::Publisher tIntPub;
        Timer integralTimer;
        bool firstExecution;

public:
        // gravity is provided by
        SMURFTrajectoryTracker();
        virtual ~SMURFTrajectoryTracker();

        // Standard Interface functions
        void initialize();
        void destroy();
        std::string getName() const;

        // Callback Functions
        void trajectoryCB(const TKTrajectory& trajectory);
        void stateCB(const TKState& state);

        void run(const TKTrajectory& input, const TKState& currentState, const double mass, const Eigen::Matrix3d& inertia , Eigen::Vector4d& inputs);
        //void setMass(double mass_);
};

}

#endif /* SMURFTRAJECTORYTRACKER_HPP_ */