ObsAvoiAlgoDyn.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------




/* @{ */


#ifndef __OBS_AVOI_ALGO_DYN_H
#define __OBS_AVOI_ALGO_DYN_H

#ifdef MIP_HOST_APPLE
 #include <applePatch.h>
#endif

#include <baselib.h>

#include <algorithm>

using namespace MipBaselib;

enum ObsAvoiAlgoDynTypes {
 BS_AVOI_DYN_TYPE_SPR_DAM_BOX,
 BS_AVOI_DYN_TYPE_BANG_BANG,
 OBS_AVOI_DYN_TYPES_NUM
};


static const char* ObsAvoiAlgoDynTypesNames[OBS_AVOI_DYN_TYPES_NUM] = {
 "Spring-Damper Box Obs. Avoi",
};


class ObsAvoiAlgoDynOptions : public Options {
 public:
  
  ObsAvoiAlgoDynOptions();

  string getObjectName() const {
   return "ObsAvoiAlgoDynOptions";
  }
};


class ObsAvoiAlgoDynIn{
 public:
  Position3D curPos; 
  Velocity3D curVel; 
  Acceleration3D desAcc; 
  
  ObsAvoiAlgoDynIn(Position3D curPos_,Velocity3D curVel_,Acceleration3D desAcc_ = Acceleration3D(0.0,0.0,0.0));
};


class ObsAvoiAlgoDynOut{
 public:
  Acceleration3D cmdAcc; 
  bool active;
  
//  /// \brief  Constructor.
//  /// \param[in]&d Desired acceleration.
   ObsAvoiAlgoDynOut(){
   active = false;
  }
};


class ObsAvoiAlgoDynBoxPar : public MIPObject{
  ObsAvoiAlgoDynOptions _options;
  
  stringstream _ssdebug;
  
  Position3D _freeMotionBoxMax;
  Position3D _freeMotionBoxMin;
  
  Position3D _propGains;
  Position3D _derivGains;
  
 public:
  
  ObsAvoiAlgoDynBoxPar(Position3D freeMotionMaxPos = Position3D(0.0,0.0,0.0) ,Position3D freeMotionMinPos = Position3D(0.0,0.0,0.0),
             Position3D propGains = Position3D(1.0,1.0,1.0),Position3D derivGains = Position3D(1.0,1.0,1.0));
    
  ObsAvoiAlgoDynBoxPar(const ObsAvoiAlgoDynBoxPar& p);

  ObsAvoiAlgoDynBoxPar& operator=(const ObsAvoiAlgoDynBoxPar& p);
  
  ObsAvoiAlgoDynBoxPar(int argc, const char* argv[]);
  
  Position3D getFreeMotBoxMax();
  
  Position3D getFreeMotBoxMin();
  
  Position3D getPropGains();
  
  Position3D getDerivGains();
  
  string getObjectName() const {
   return "ObsAvoiAlgoDyn";
  }
};


class ObsAvoiAlgoBangBangPar : public MIPObject{
  
 stringstream _ssdebug;
  
 Position3D _posMin;
 Position3D _posMax;
 Velocity3D _velMin;
 Velocity3D _velMax;
 Acceleration3D _accMin;
 Acceleration3D _accMax;
 Acceleration3D _accMult;
  
 public:
  
  ObsAvoiAlgoBangBangPar(Position3D posMin,Position3D posMax,Velocity3D velMin,Velocity3D velMax,Acceleration3D accMin,Acceleration3D accMax,Acceleration3D accMult);
    
  ObsAvoiAlgoBangBangPar(const ObsAvoiAlgoBangBangPar& p);

  ObsAvoiAlgoBangBangPar& operator=(const ObsAvoiAlgoBangBangPar& p);
  
  Position3D getPosMin();
  
  Position3D getPosMax();
  
  Velocity3D getVelMin();
  
  Velocity3D getVelMax();
  
  Acceleration3D getAccMin();
  
  Acceleration3D getAccMax();
 
  Acceleration3D getAccMult();
  
  string getObjectName() const {
   return "ObsAvoi BangBang Par";
  }
};


class ObsAvoiAlgoDynVar {
  
 public:
  int activeX,activeY,activeZ;
  
  ObsAvoiAlgoDynVar();
  
  bool active(){
   return activeX || activeY || activeZ;
  }
};


class ObsAvoiAlgoDyn : public MIPObject {
 private:
  ObsAvoiAlgoDynVar _var;
  ObsAvoiAlgoDynBoxPar   *_sdPar;
  ObsAvoiAlgoBangBangPar *_bbPar;
  
  ObsAvoiAlgoDynTypes _algoType;

  stringstream _ssdebug;
  
  void _runSprDamBox(ObsAvoiAlgoDynIn in, ObsAvoiAlgoDynOut &out);
  void _runBangBang(ObsAvoiAlgoDynIn in, ObsAvoiAlgoDynOut &out);
  
  void _oneDimBangBang(Decimal pos,Decimal vel,Decimal desAcc,
             Decimal posMin,Decimal posMax,Decimal velMin,Decimal velMax,Decimal accMin,Decimal accMax,Decimal accMult,
       Decimal& cmdAcc,int& active);
  
 public:
  ObsAvoiAlgoDyn(ObsAvoiAlgoDynBoxPar& inPar);
  
  ObsAvoiAlgoDyn(ObsAvoiAlgoBangBangPar& inPar);
 
  void run(ObsAvoiAlgoDynIn in, ObsAvoiAlgoDynOut &out);
  
  string getObjectName() const {
   return "ObsAvoiAlgoDyn";
  }
};



#endif


/* @} */