PositionControl.cpp

Go to the documentation of this file.

/*
 * PositionControl.cpp
 *
 *  Created on: Oct 27, 2011
 *      Author: mriedel
 */

#include <tk_ctrlalgo/PositionControl.hpp>
#include <algorithm>

#include <telekyb_defines/physic_defines.hpp>

#define MAX_INT_TIME_STEP 0.04

namespace TELEKYB_NAMESPACE {

// Options
PositionControlOptions::PositionControlOptions()
        : OptionContainer("PositionControl")
{
        tPropGain = addOption<double>( "tPropGain", "Proportional gain on xy plane", 9.0, false, false);
        tDerivGain = addOption<double>( "tDerivGain", "Derivative  gain on xy plane", 7.5, false, false);
        tIntegGain = addOption<double>( "tIntegGain", "Integral  gain on xy plane", 4.5, false, false);

        tDerGainVelMode = addOption<double>( "tDerGainVelMode", "Derivative  gain on xy plane in velocity mode", 7.5, false, false);

        tZPropGain = addOption<double>( "tZPropGain", "Z axis Proportional gain", 9.0, false, false);
        tZDerivGain = addOption<double>( "tZDerivGain", "Z axis Derivative  gain", 7.5, false, false);

        tSatIntTerm = addOption<double>( "tSatIntTerm",
                        "Saturation value on the integral term  (sine of the maximum inclination angle) (on xy plane)", 0.475, false, false);

        tMinThrust = addOption<double>( "tMinThrust", "Mimimum thrust", 5.0, false, true);

        tMaxRollSin = addOption<double>( "tMaxRollSin", "Sinus of the max roll", sin(30.0*M_PI/180.0), false, true);
        tMaxPitchSin = addOption<double>( "tMaxPitchSin", "Sinus of the max pitch", sin(30.0*M_PI/180.0), false, true);

        //tMaxVelInVelMode = addOption<double>( "tMaxVelInVelMode", "Maximum velocity in velocity mode", 100.0, false, true);

        tGravity = addOption<double>( "tGravity", "Gravity Value for the Position Controller", GRAVITY, false, false);
}

PositionControl::PositionControl()
{

}

PositionControl::~PositionControl()
{

}

//void PositionControl::setMass(double mass_)
//{
//      mass = mass_;
//}

void PositionControl::run(const TKTrajectory& input, const TKState& currentState, const double mass, PosCtrlOutput& output)
{
        // Current State
        Position3D curPosition = currentState.position;
        Velocity3D curLinVelocity = currentState.linVelocity;
        Vector3D curOrientation = currentState.getEulerRPY();
        //Velocity3D curAngVelocity = currentState.angVelocity;

        // Desired State
        Position3D desPosition = input.position;
        Velocity3D desLinVelocity = input.velocity;
        Acceleration3D desLinAcceleration = input.acceleration;

        /*
        ROS_INFO_STREAM("Current State: Pos " << curPosition <<
                        " Vel " << curLinVelocity << " orientation(rpy) " << curOrientation << " angVel " << curAngVelocity);

        ROS_INFO_STREAM("Desired Traj: Pos " << desPosition <<
                        " Vel " << desLinVelocity << " Acc " << desLinAcceleration);
         */

        /*velocity vector saturation (only in  full velocity mode, 3 axes in vel mode)*/
//      if( input.xAxisCtrl == PosControlType::Velocity && input.yAxisCtrl == PosControlType::Velocity  && input.zAxisCtrl == PosControlType::Velocity ){
//              if(desLinVelocity.norm() > options.tMaxVelInVelMode->getValue()) {
//                      desLinVelocity.normalize();
//                      desLinVelocity *= fabs(options.tMaxVelInVelMode->getValue());
//                      desLinAcceleration = Acceleration3D(0.0,0.0,0.0);
//              }
//      }


        /*saturation on the sigle components (needed when the velocity modes are different among the axes)*/
//      if( input.xAxisCtrl == PosControlType::Velocity && fabs(desLinVelocity(0)) > options.tMaxVelInVelMode->getValue()){
//              desLinVelocity(0) = copysign(fabs(options.tMaxVelInVelMode->getValue()), desLinVelocity(0));
//              desLinAcceleration(0) = 0.0;
//      }
//      if( input.yAxisCtrl == PosControlType::Velocity && fabs(desLinVelocity(1)) > options.tMaxVelInVelMode->getValue()){
//              desLinVelocity(1) = copysign(fabs(options.tMaxVelInVelMode->getValue()), desLinVelocity(1));
//              desLinAcceleration(1) = 0.0;
//      }
//      if( input.zAxisCtrl == PosControlType::Velocity && fabs(desLinVelocity(2)) > options.tMaxVelInVelMode->getValue()){
//              desLinVelocity(2) = copysign(fabs(options.tMaxVelInVelMode->getValue()), desLinVelocity(2));
//              desLinAcceleration(2) = 0.0;
//      }


        double xPropGain = 0.0, yPropGain = 0.0, zPropGain = 0.0;
        double xIntegGain = 0.0, yIntegGain = 0.0;
        double xDerivGain = 0.0, yDerivGain = 0.0, zDerivGain = 0.0;

        if (input.xAxisCtrl == PosControlType::Position) {
                xPropGain   = options.tPropGain->getValue();
                xDerivGain  = options.tDerivGain->getValue();
                xIntegGain  = options.tIntegGain->getValue();
        } else if (input.xAxisCtrl == PosControlType::Velocity) {
//              xPropGain   = 0.0;
                xDerivGain  = options.tDerGainVelMode->getValue();
//              xIntegGain  = 0.0;
                xIntErr = 0.0;
        } else if (input.xAxisCtrl == PosControlType::Acceleration) {
//              xPropGain   = 0.0;
//              xDerivGain  = 0.0;
//              xIntegGain  = 0.0;
                xIntErr = 0.0;
        }

        if (input.yAxisCtrl == PosControlType::Position) {
                yPropGain   = options.tPropGain->getValue();
                yDerivGain  = options.tDerivGain->getValue();
                yIntegGain  = options.tIntegGain->getValue();
        } else if (input.yAxisCtrl == PosControlType::Velocity) {
//              yPropGain   = 0.0;
                yDerivGain  = options.tDerGainVelMode->getValue();
//              yIntegGain  = 0.0;
                yIntErr = 0.0;
        } else if (input.yAxisCtrl == PosControlType::Acceleration) {
//              yPropGain   = 0.0;
//              yDerivGain  = 0.0;
//              yIntegGain  = 0.0;
                yIntErr = 0.0;
        }

        if (input.zAxisCtrl == PosControlType::Position) {
                zPropGain  = options.tZPropGain->getValue();
                zDerivGain = options.tZDerivGain->getValue();
        } else if (input.zAxisCtrl == PosControlType::Velocity) {
//              zPropGain  = 0.0;
                zDerivGain = options.tZDerivGain->getValue();
        } else if (input.zAxisCtrl == PosControlType::Acceleration) {
//              zPropGain  = 0.0;
//              zDerivGain = 0.0;
        }


        //Decimal mass  = _mass;

        //Decimal gravity = getGravity();
        double maxIntTerm = options.tSatIntTerm->getValue() / options.tIntegGain->getValue();
        //double maxIntTerm = /*0.475*/getSatIntTerm() / getIntGain();

        double comX = desLinAcceleration(0) +
                                                xDerivGain* (desLinVelocity(0) - curLinVelocity(0)) +
                                                xPropGain* (desPosition(0) - curPosition(0)) +
                                                xIntegGain*(xIntErr);

        double comY = desLinAcceleration(1) +
                                                yDerivGain* (desLinVelocity(1) - curLinVelocity(1)) +
                                                yPropGain* (desPosition(1) - curPosition(1)) +
                                                yIntegGain*(yIntErr);

        double comZ = desLinAcceleration(2) +
                                                zDerivGain*(desLinVelocity(2) - curLinVelocity(2)) +
                                                zPropGain*(desPosition(2) - curPosition(2));

//      if(_logFile != 0){
//              fprintf(_logFile,"%lf %lf %lf \n", desVelX - velX, desVelY - velY, desVelZ - velZ);
//      }

        double timeStep = integTimer.getElapsed().toDSec();
        integTimer.reset();
        if (timeStep < MAX_INT_TIME_STEP) {
                xIntErr += (desPosition(0) - curPosition(0))*timeStep;
                yIntErr += (desPosition(1) - curPosition(1))*timeStep;
        }

        if (fabs(xIntErr) > maxIntTerm) {
                xIntErr = copysign(maxIntTerm, xIntErr);
                //xIntErr = DecimalUtilities::sign(xIntErr)*maxIntTerm;
        }

        if (fabs(yIntErr) > maxIntTerm) {
                yIntErr = copysign(maxIntTerm, yIntErr);
                //yIntErr=DecimalUtilities::sign(yIntErr)*maxIntTerm;
        }

//      double rollCos = cos(curOrientation(0));
//      double pitchCos = cos(curOrientation(1));

//      if(fabs(rollCos) <= MIN_COS){
//              rollCos = copysign(MIN_COS, rollCos);
//              //rollCos = DecimalUtilities::sign(rollCos) * MIN_COS;
//      }
//
//      if(fabs(pitchCos) <= MIN_COS){
//              pitchCos = copysign(MIN_COS, pitchCos);
//              //pitchCos = DecimalUtilities::sign(pitchCos) * MIN_COS;
//      }

        double comThrust =  ( mass/(cos( curOrientation(0) )*cos( curOrientation(1) )) ) * ( -1.0 * options.tGravity->getValue() + comZ );

        /*saturation min/max thrust*/
//      if (comThrust > -getMinThrust()){
//              comThrust = -getMinThrust();
//      }
//
//      if (comThrust < -2.0*mass * gravity + getMinThrust()){
//              comThrust = -2.0*mass * gravity + getMinThrust();
//      }

        comThrust = std::min( comThrust, -1.0 * options.tMinThrust->getValue() );
        comThrust = std::max( comThrust, -2.0 * mass * options.tGravity->getValue() + options.tMinThrust->getValue() );



//      double comRoll = ( mass / comThrust) * ( -sin(yaw)*comX + cos(yaw)*comY);
//      double comPitch = ( mass / (comThrust*cos(roll))) * ( cos(yaw)*comX + sin(yaw)*comY);
        double comRoll = ( mass / comThrust) * ( -sin(curOrientation(2))*comX + cos(curOrientation(2))*comY);
        double comPitch = ( mass / (comThrust*cos(curOrientation(0)))) * ( cos(curOrientation(2))*comX + sin(curOrientation(2))*comY);



        if (fabs(comRoll) > options.tMaxRollSin->getValue()){
//              comRoll = DecimalUtilities::sign(comRoll) * getMaxRollSin();
                comRoll = copysign(options.tMaxRollSin->getValue(), comRoll);
        }

        if (fabs(comPitch) > options.tMaxPitchSin->getValue()){
//              comPitch = DecimalUtilities::sign(comPitch) * getMaxPitchSin();
                comPitch = copysign(options.tMaxPitchSin->getValue(), comPitch);
        }

        output.comRoll = -asin( comRoll );
        output.comPitch = asin( comPitch );
        output.comThrust = comThrust;


//      out->commands.clear();
//      out->commands.reserve(4*sizeof(double));
//      out->commands.push_back(comRoll);
//      out->commands.push_back(comPitch);
//      out->commands.push_back(comThrust);
}


}