MotionModule3D.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 __MOTION_MODULE3D_H_
#define __MOTION_MODULE3D_H_

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

#include <stdlib.h>
#include <string>
#include <sstream>
#include <vector>
#include <assert.h>

#include <baselib.h>
#include <Resource.h>



namespace MipResources
{

 /* @{ */



class MotionModule3DPar
{
 private:
 protected:
  string _name;     

  void setName(string n)
  {
   _name=n;
  }
 public:
  MotionModule3DPar ()
  {
   _name  = string("");
  }  
  
  MotionModule3DPar(const MotionModule3DPar &p)
  {
   _name  = p._name;
  }
  
  MotionModule3DPar& operator=(const MotionModule3DPar& p)
  {
   if (this != &p)
   {
    _name  = p._name;
   }
   return *this;
  }
  
  string name()
  {
   return _name;
  }
  
  virtual string print()
  {
   stringstream s;
   s << "name" << name() << endl;
   return s.str();
  }
};


class MotionModule3DVar
{ 
 private:
 protected:
  Pose3D _odometry;
 public:
  MotionModule3DVar ()
  {
   _odometry = Pose3D();
  }
  
  MotionModule3DVar(const MotionModule3DVar &v)
  {
   _odometry = v._odometry;
  }
  
  MotionModule3DVar& operator=(const MotionModule3DVar& v)
  {
   if (this != &v)
   {
    _odometry = v._odometry;
   }
   return *this;
  }
  
  Pose3D odometry(void)
  {
          cout << "MotionModule3DVar::odometry()" << endl;
   cout << _odometry.print() << "\n";
   return _odometry;
  }
  
  void setOdometry(Pose3D value)
  {
          cout << "MotionModule3DVar::setOdometry()" << value.print()<< endl;
   _odometry = value;
   return;
  }
  
  virtual string print()
  {
   stringstream s;
   s << "odometry      " << _odometry.print() << endl;
   return s.str();
  }
};



class MotionModule3D : public Resource
{
 protected:
  MotionModule3DPar *_par;
  MotionModule3DVar *_var;
  bool on; 
 public:
  MotionModule3D()
  {
   on = false; 
  };
  
  virtual ResourcePlate getPlate() const =0;
  
  virtual Pose3D odometry()
  {
   cout << "MotionModule3D::odometry()" << endl;
   /*TODO check if correct*/
   assert(on);
   return _var->odometry();
  }
  
  virtual void setOdometry(Pose3D p)
  {
     cout << "MotionModule3D::setOdometry()" << endl;
   assert(on);
   _var->setOdometry(p);
   p.print();
  }
  
  virtual string name()
  {
   assert(on);
   return _par->name();
  }
  
/*  virtual void setName(string n){
   assert(on);
   _par->setName(n);
  }*/
  
  virtual MotionModule3DPar* par(void)
  {
   assert(on);
   return _par;
  }
};


class Unicycle3DPar : public MotionModule3DPar{
  Decimal _maxDrive;    
  Decimal _maxTurnRate; 
 protected:
  void setMaxDrive(Decimal m){
   assert(m>0.0);
   _maxDrive=m;
  }
  void setMaxTurnRate(Decimal m){
   assert(m>0.0);
   _maxTurnRate=m;
  }
 public:
  Unicycle3DPar():MotionModule3DPar(){
   _maxDrive = 0.0;
   _maxTurnRate = 0.0;
  }  
  Unicycle3DPar(const Unicycle3DPar &p):MotionModule3DPar(p){
   _maxDrive = p._maxDrive;
   _maxTurnRate = p._maxTurnRate;
  }
  Unicycle3DPar& operator=(const Unicycle3DPar& p){
   if (this != &p){
    _name = p._name;
    _maxDrive = p._maxDrive;
    _maxTurnRate = p._maxTurnRate;
   }
   return *this;
  }
  
  Decimal maxDrive(void){
   return _maxDrive;
  }
  Decimal maxTurnRate(void){
   return _maxTurnRate;
  }
  virtual string print(){
   stringstream s;
   s << "name     " << name() << endl
    << "max drive   " << _maxDrive << endl
    << "max turn-rate " << _maxTurnRate << endl;
   return s.str();
  }
};


class Unicycle3DVar : public MotionModule3DVar{
 private:
  Decimal _linVel;  
  Decimal _angVel;  
  Pose3D _oldOdometry; 

 protected:

 public:
  Unicycle3DVar ():MotionModule3DVar(){
   _linVel  = 0.0;
   _angVel  = 0.0;
  }
  Unicycle3DVar(const Unicycle3DVar &v):MotionModule3DVar(v){
   _linVel  = v._linVel;
   _angVel  = v._angVel;
  }
  Unicycle3DVar& operator=(const Unicycle3DVar& v){
   if (this != &v){
    _odometry = v._odometry;
    _linVel  = v._linVel;
    _angVel  = v._angVel;
   }
   return *this;
  }
  
  Decimal linVel(void){
   return _linVel;
  }
  // TODO da verificarne la correttezza
  Position3D vel(void){
   return Position3D(cos(odometry().ori().pitch().dCast2Pi())*cos(odometry().ori().yaw().dCast2Pi()), -sin(odometry().ori().yaw().dCast2Pi()), sin(odometry().ori().pitch().dCast2Pi())*cos(odometry().ori().yaw().dCast2Pi()))*_linVel;
  }
  Decimal angVel(void){
   return _angVel;
  }
  
  void setLinVel(Decimal value){
   _linVel = value;
  }
  void setAngVel(Decimal value){
   _angVel = value;
   return;
  }
  
  void setOldOdometry(Pose3D value){
   _oldOdometry = value;
   cout << "Unicycle3DVar::setOldOdometry" << value.print() << endl;
   return;
  }
  
  Pose3D odometryDiff(MIPMatrix &RobStat)
  {
          Pose3D tmp;
   Decimal d_x, d_y;
   
   cout << "Unicycle3DVar::odometryDiff(_odometry)" << _odometry.print() << endl;
   cout << "Unicycle3DVar::odometryDiff(_oldOdometry)" << _oldOdometry.print() << endl;
   tmp.operator=(_odometry.operator-(_oldOdometry));
          cout << "Unicycle3DVar::odometryDiff(_odometry - _oldOdometry)" << tmp.print() << endl;
   d_x = tmp.pos().x();
   d_y = tmp.pos().y();
   
   RobStat.SetMIPMatrixVal(4,1,sqrt(d_x*d_x + d_y*d_y));//Robot linear displacement during the last time slice
   RobStat.SetMIPMatrixVal(5,1,tmp.ori().yaw().dCast2Pi());//Robot angular displacement during the last time slice
   
   return _odometry - _oldOdometry;
  }

  virtual string print(){
   stringstream s;
   s << "odometry      " << odometry().print() << endl
    << "linear speed  " << _linVel << " m/sec"<< endl
    << "angular speed " << _angVel << " rad/sec" << endl;
   return s.str();
  }
};
class Unicycle3D : public MotionModule3D
{
  
 protected:
  
  Unicycle3DPar* _uniPar;
  Unicycle3DVar* _uniVar;
 
 public:
  
  virtual void odometryDiff(MIPMatrix &RobStat, Decimal *ENCTime)
  {
          cout << "Unicycle3D::odometryDiff()" << endl;
   assert(on);
   updateOdometry(RobStat, ENCTime);//Update robot status components X,Y,Theta
   _uniVar->odometryDiff(RobStat);  //Update robot status: Components (d_x,d_th)=(linear displacement, angular displacement) 
                                    //occurred during the last time slice.
   return;
  }
  
  virtual void updateOdometry(MIPMatrix &RobStat, Decimal *ENCTime)=0;
  
  virtual ResourcePlate getPlate() const =0;
  
  virtual void velCmd(Decimal drive, Decimal turnrate)=0;
  
  virtual void getVel(Decimal& drive, Decimal& turnrate)=0;
};


class FlightModulePar : public MotionModule3DPar{
  Decimal _maxXVel;      
  Decimal _maxYVel;      
  Decimal _maxZVel;      
  Decimal _maxRollVel;   
  Decimal _maxPitchVel;  
  Decimal _maxYawVel;    
 protected:
  void setMaxXVel(Decimal m){
   assert(m>0.0);
   _maxXVel=m;
  }
  void setMaxYVel(Decimal m){
   assert(m>0.0);
   _maxYVel=m;
  }
  void setMaxZVel(Decimal m){
   assert(m>0.0);
   _maxZVel=m;
  }
  void setMaxRollVel(Decimal m){
   assert(m>0.0);
   _maxRollVel=m;
  }
  void setMaxPitchVel(Decimal m){
   assert(m>0.0);
   _maxPitchVel=m;
  }
  void setMaxYawVel(Decimal m){
   assert(m>0.0);
   _maxYawVel=m;
  }
 public:
  FlightModulePar():MotionModule3DPar(){
   _maxXVel = 0.0;
   _maxYVel = 0.0;
   _maxZVel = 0.0;
   _maxRollVel = 0.0;
   _maxPitchVel = 0.0;
   _maxYawVel = 0.0;
  }  
  FlightModulePar(const FlightModulePar &p):MotionModule3DPar(p){
   _maxXVel   = p._maxXVel;
   _maxYVel   = p._maxYVel;
   _maxZVel   = p._maxZVel;
   _maxRollVel  = p._maxRollVel;
   _maxPitchVel = p._maxPitchVel;
   _maxYawVel  = p._maxYawVel;
  }
  FlightModulePar& operator=(const FlightModulePar& p){
   if (this != &p){
    _name = p._name;
   _maxXVel   = p._maxXVel;
   _maxYVel   = p._maxYVel;
   _maxZVel   = p._maxZVel;
   _maxRollVel  = p._maxRollVel;
   _maxPitchVel = p._maxPitchVel;
   _maxYawVel  = p._maxYawVel;
   }
   return *this;
  }
  
  Decimal maxXVel(void){
   return _maxXVel;
  }
  Decimal maxYVel(void){
   return _maxYVel;
  }
  Decimal maxZVel(void){
   return _maxZVel;
  }
  Decimal maxRollRate(void){
   return _maxRollVel;
  }
  Decimal maxPitchRate(void){
   return _maxPitchVel;
  }
  Decimal maxYawRate(void){
   return _maxYawVel;
  }
  virtual string print(){
   stringstream s;
   s << "name     " << name() << endl
    << "max x vel   " << _maxXVel << endl
    << "max y vel   " << _maxYVel << endl
    << "max z vel   " << _maxZVel << endl
    << "max roll vel  " << _maxRollVel << endl
    << "max pitch vel " << _maxPitchVel << endl
    << "max yaw vel  " << _maxYawVel << endl;
   return s.str();
  }
};

class FlightModuleVar : public MotionModule3DVar{
 private:
  Decimal _linVel;  
  Decimal _angVel;  
 protected:

 public:
  FlightModuleVar ():MotionModule3DVar(){
   _linVel  = 0.0;
   _angVel  = 0.0;
  }
  FlightModuleVar(const FlightModuleVar &v):MotionModule3DVar(v){
   _linVel  = v._linVel;
   _angVel  = v._angVel;
  }
  FlightModuleVar& operator=(const FlightModuleVar& v){
   if (this != &v){
    _odometry = v._odometry;
    _linVel  = v._linVel;
    _angVel  = v._angVel;
   }
   return *this;
  }
  
  Decimal linVel(void){
   return _linVel;
  }
  // TODO da verificarne la correttezza
  Position3D vel(void){
   return Position3D(cos(odometry().ori().pitch().dCast2Pi())*cos(odometry().ori().yaw().dCast2Pi()), -sin(odometry().ori().yaw().dCast2Pi()), sin(odometry().ori().pitch().dCast2Pi())*cos(odometry().ori().yaw().dCast2Pi()))*_linVel;
  }
  Decimal angVel(void){
   return _angVel;
  }
  
  void setLinVel(Decimal value){
   _linVel = value;
  }
  void setAngVel(Decimal value){
   _angVel = value;
  }
  
  virtual string print(){
   stringstream s;
   s << "odometry      " << odometry().print() << endl
    << "linear speed  " << _linVel << " m/sec"<< endl
    << "angular speed " << _angVel << " rad/sec" << endl;
   return s.str();
  }
};

class FlightModule : public MotionModule3D{
 protected:
 public:
  FlightModule(){
   on = false; 
  };
  virtual ResourcePlate getPlate() const =0;
  
  virtual void velLinCmd(Position3D cmd)=0;
  
  virtual void velAngCmd(Position3D cmd)=0;
  
  virtual void velCmd(Position3D lcmd, Position3D acmd)=0;
  
  virtual void getVel(Position3D& linVel, Position3D& angVel)=0;
  
  
  virtual Pose3D odometry(){
   assert(on);
   return _var->odometry();
  }
  
  virtual void setOdometry(Pose3D p){
   assert(on);
   _var->setOdometry(p);
  }
  
//  /** \brief get parameters  */
/*  virtual FlightModulePar* par(void){
   assert(on);
   return _par;
  }*/
};

 /* @} */
 
};// end namespace MipResources{

#endif