UndercarriageCtrlGeom.h

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#ifndef UndercarriageCtrlGeom_INCLUDEDEF_H
#define UndercarriageCtrlGeom_INCLUDEDEF_H

#include <vector>
#include <string>
#include <stdexcept>
#include <boost/make_shared.hpp>

struct PlatformState{
    double dVelLongMMS;
    double dVelLatMMS;
    double dRotRobRadS;

    const double getVelX() { return dVelLongMMS / 1000.0; }
    const double getVelY() { return dVelLatMMS / 1000.0; }

    void setVelX(const double val) { dVelLongMMS  = val * 1000.0; }
    void setVelY(const double val) { dVelLatMMS = val * 1000.0; }

    PlatformState() : dVelLongMMS(0), dVelLatMMS(0),dRotRobRadS(0) {}
};
struct WheelState {
    double dVelGearDriveRadS;
    double dVelGearSteerRadS;
    double dAngGearSteerRad;
    WheelState() : dVelGearDriveRadS(0), dVelGearSteerRadS(0), dAngGearSteerRad(0) {}
};
struct WheelCommand : public WheelState {
    double dAngGearSteerRadDelta;
    WheelCommand() : dAngGearSteerRadDelta(0) {}
};
struct WheelGeom{
    std::string steer_name, drive_name;

    double dWheelXPosMM;
    double dWheelYPosMM;

    double dSteerDriveCoupling;

    double dRadiusWheelMM;
    double dDistSteerAxisToDriveWheelMM;

};
struct WheelData {
    WheelGeom geom_;

    double dFactorVel;

    WheelState state_;

    double m_dExWheelXPosMM;
    double m_dExWheelYPosMM;
    double m_dExWheelDistMM;
    double m_dExWheelAngRad;

    double m_dVelWheelMMS;

    void updateState(const WheelState &state);
    double getVelX() const;
    double getVelY() const;

    static double mergeRotRobRadS(const WheelData &wheel1, const WheelData &wheel2);

    WheelData(const WheelGeom &geom)
    : geom_(geom),
        dFactorVel(-geom_.dSteerDriveCoupling + geom_.dDistSteerAxisToDriveWheelMM / geom_.dRadiusWheelMM) {
        updateState(WheelState());
    }
};

class UndercarriageGeomBase
{
public:
    // Get result of direct kinematics
    virtual void calcDirect(PlatformState &state) const = 0;

    // Set actual values of wheels (steer/drive velocity/position) (Istwerte)
    virtual void updateWheelStates(const std::vector<WheelState> &states) = 0;

    virtual ~UndercarriageGeomBase() {}

protected:
    template<typename V> static void updateWheelStates(V& wheels, const std::vector<WheelState> &states)
    {
        if(wheels.size() != states.size()) throw std::length_error("number of states does not match number of wheels");

        for(size_t i = 0; i < wheels.size(); ++i){
            wheels[i]->updateState(states[i]);
        }
    }

    template<typename V> static void calcDirect(PlatformState &state, const V& wheels)
    {
        double dtempRotRobRADPS = 0;    // Robot-Rotation-Rate in rad/s (in Robot-Coordinateframe)
        double dtempVelXRobMMS = 0;     // Robot-Velocity in x-Direction (longitudinal) in mm/s (in Robot-Coordinateframe)
        double dtempVelYRobMMS = 0;     // Robot-Velocity in y-Direction (lateral) in mm/s (in Robot-Coordinateframe)

        // calculate rotational rate of robot and current "virtual" axis between all wheels
        for(int i = 0; i < wheels.size() ; i++)
        {
            const WheelData &wheel = *wheels[i];
            const WheelData &other_wheel = *wheels[(i+1) % wheels.size()];

            dtempRotRobRADPS += WheelData::mergeRotRobRadS(wheel, other_wheel);
            dtempVelXRobMMS += wheel.getVelX();
            dtempVelYRobMMS += wheel.getVelY();
        }

        // assign rotational velocities for output
        state.dRotRobRadS = dtempRotRobRADPS/wheels.size();

        // assign linear velocity of robot for output
        state.dVelLongMMS = dtempVelXRobMMS/wheels.size();
        state.dVelLatMMS = dtempVelYRobMMS/wheels.size();

    }

};

class UndercarriageGeom : public UndercarriageGeomBase {
public:
    struct WheelParams {
        WheelGeom geom;
    };

    // Constructor
    UndercarriageGeom(const std::vector<WheelParams> &params);

    // Get result of direct kinematics
    virtual void calcDirect(PlatformState &state) const;

    // Set actual values of wheels (steer/drive velocity/position) (Istwerte)
    virtual void updateWheelStates(const std::vector<WheelState> &states);

private:
    std::vector<boost::shared_ptr<WheelData> > wheels_;
};

double limitValue(double value, double limit);

template<typename T> class UndercarriageCtrlBase : public UndercarriageGeomBase {
public:
    // Constructor
    template<typename T2> UndercarriageCtrlBase(const std::vector<T2> &params){
        for(typename std::vector<T2>::const_iterator it = params.begin(); it != params.end(); ++it){
            wheels_.push_back(boost::make_shared<T>(*it));
        }
    }

    // Get result of direct kinematics
    virtual void calcDirect(PlatformState &state) const {
        UndercarriageGeomBase::calcDirect(state, wheels_);
    }

    // Set actual values of wheels (steer/drive velocity/position) (Istwerte)
    virtual void updateWheelStates(const std::vector<WheelState> &states) {
        UndercarriageGeomBase::updateWheelStates(wheels_, states);
    }
    // Set desired value for Plattform Velocity to UndercarriageCtrl (Sollwertvorgabe)
    void setTarget(const PlatformState &state)  {
        for(size_t i = 0; i < wheels_.size(); ++i){
            wheels_[i]->setTarget(state);
        }
    }

    // Get set point values for the Wheels (including controller) from UndercarriangeCtrl
    void calcControlStep(std::vector<WheelCommand> &commands, double dCmdRateS, bool reset) {
        commands.resize(wheels_.size());

        for(size_t i = 0; i < wheels_.size(); ++i){
            wheels_[i]->calcControlStep(commands[i], dCmdRateS, reset);
        }
    }

    void reset() {
        for(size_t i = 0; i < wheels_.size(); ++i){
            wheels_[i]->reset();
        }
    }

protected:
    std::vector<boost::shared_ptr<T> > wheels_;
};

struct CtrlParams{
    double dWheelNeutralPos;

    double dMaxDriveRateRadpS;
    double dMaxSteerRateRadpS;
};

struct WheelCtrlParams {
    WheelGeom geom;
    CtrlParams ctrl;
};

struct CtrlData : public WheelData {
    CtrlParams params_;

    double m_dAngGearSteerTargetRad; // choosen alternativ for steering angle
    double m_dVelGearDriveTargetRadS;

    // calculate inverse kinematics
    void setTarget(const PlatformState &state);

    virtual void calcControlStep(WheelCommand &command, double dCmdRateS, bool reset);

    virtual void reset();

    template<typename P> CtrlData(const P &params)
    : WheelData(params.geom), params_(params.ctrl) {
        state_.dAngGearSteerRad = params_.dWheelNeutralPos;
        updateState(WheelState());
        setTarget(PlatformState());
        m_dAngGearSteerTargetRad = params_.dWheelNeutralPos;
    }
};

class UndercarriageDirectCtrl : public UndercarriageCtrlBase < CtrlData > {
public:
    typedef WheelCtrlParams WheelParams;
    UndercarriageDirectCtrl(const std::vector<WheelParams> &params) : UndercarriageCtrlBase(params) {}
};

struct PosCtrlParams {
    double dSpring, dDamp, dVirtM, dDPhiMax, dDDPhiMax;
};

struct WheelCtrlPosParams {
    WheelGeom geom;
    CtrlParams ctrl;
    PosCtrlParams pos_ctrl;
};

struct PosCtrlData : public CtrlData {
    PosCtrlParams pos_params_;

    // previous Commanded deltaPhi e(k-1)
    // double m_dCtrlDeltaPhi; not used
    // previous Commanded Velocity u(k-1)
    double m_dCtrlVelCmdInt;

    virtual void calcControlStep(WheelCommand &command, double dCmdRateS, bool reset);

    virtual void reset();

    PosCtrlData(const WheelCtrlPosParams &params)
    : CtrlData(params), pos_params_(params.pos_ctrl),m_dCtrlVelCmdInt(0) {
        state_.dAngGearSteerRad = params_.dWheelNeutralPos;
        updateState(WheelState());
        setTarget(PlatformState());
        m_dAngGearSteerTargetRad = params_.dWheelNeutralPos;
    }
};

class UndercarriageCtrl : public UndercarriageCtrlBase < PosCtrlData > {
public:
    typedef WheelCtrlPosParams WheelParams;
    void configure(const std::vector<PosCtrlParams> &pos_ctrl);
    UndercarriageCtrl(const std::vector<WheelParams> &params) : UndercarriageCtrlBase(params) {}
};

#endif