pg.h

Go to the documentation of this file.

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 * Copyright Projet JRL-Japan, 2008
 *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * File:      sotDynamic.h
 * Project:   SOT
 * Author:    Olivier Stasse
 *
 * Version control
 * ===============
 *
 *  $Id$
 *
 * Description
 * ============
 *
 *
 * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
 
#ifndef __SOT_PATTERN_GENERATOR_H__
#define __SOT_PATTERN_GENERATOR_H__
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
/* STD */
#include <pinocchio/fwd.hpp>
#include <string>
 
/* SOT */
 
#include <dynamic-graph/entity.h>
#include <dynamic-graph/pool.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/pattern-generator/exception-pg.h>
 
#include <pinocchio/fwd.hpp>
#include <sot/core/flags.hh>
#include <sot/core/matrix-geometry.hh>
 
/* Pattern Generator */
#include <jrl/walkgen/patterngeneratorinterface.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
namespace pg = PatternGeneratorJRL;
 
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#if defined(WIN32)
#if defined(pg_EXPORTS)
#define PatternGenerator_EXPORT __declspec(dllexport)
#else
#define PatternGenerator_EXPORT __declspec(dllimport)
#endif
#else
#define PatternGenerator_EXPORT
#endif
 
namespace dynamicgraph {
namespace sot {
 
enum SupportPhase {
  DOUBLE_SUPPORT_PHASE = 0,
  LEFT_SUPPORT_PHASE = 1,
  RIGHT_SUPPORT_PHASE = -1,
};
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
class PatternGenerator_EXPORT PatternGenerator : public Entity {
 public:
  // overload the new[] eigen operator
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  static const int WORLD_FRAME = 0;
 
  static const int EGOCENTERED_FRAME = 1;
 
  static const int LEFT_FOOT_CENTERED_FRAME = 2;
 
  static const int WAIST_CENTERED_FRAME = 3;
 
 protected:
 public:
  DYNAMIC_GRAPH_ENTITY_DECL();
 
 protected:
  pinocchio::Model m_robotModel;
  pg::PinocchioRobot *m_PR;
  pinocchio::Data *m_robotData;
  pg::PatternGeneratorInterface *m_PGI;
 
  std::string m_PreviewControlParametersFile;
 
  std::string m_urdfFile;
 
  std::string m_srdfFile;
 
  std::string m_xmlRankFile;
 
  std::vector<unsigned> m_wrml2urdfIndex;
 
  double m_soleLength;
 
  double m_soleWidth;
 
  /* \brief Special joints map for the parser */
  std::map<std::string, std::string> specialJoints_;
 
  bool m_init;
 
  bool m_InitPositionByRealState;
 
  int m_SupportFoot;
 
  int m_ReferenceFrame;
 
  double m_AnkleSoilDistance;
 
  double m_TimeStep;
 
  SupportPhase m_ContactPhase;
  int m_DSStartingTime;
 
  int m_LocalTime;
 
  unsigned int m_count;
 
  /* \name Body names of the end effectors
     @{ */
  /* \brief Left ankle */
  std::string m_left_ankle_body_name;
 
  /* \brief Right ankle */
  std::string m_right_ankle_body_name;
 
  /* \brief Left wrist */
  std::string m_left_wrist_body_name;
 
  /* \brief Right wrist */
  std::string m_right_wrist_body_name;
 
  /* @} */
 public: /* --- CONSTRUCTION --- */
  PatternGenerator(const std::string &name = "PatternGenerator");
  virtual ~PatternGenerator(void);
 
 public: /* --- MODEL CREATION --- */
  bool buildPGI(void);
 
  bool addComplementaryFrames();
 
  bool buildReducedModel(void);
 
  void readFootParameters(std::string &rootFootPath, pg::PRFoot &aFoot);
 
  bool InitState(void);
 
  void setPreviewControlParametersFile(const std::string &filename);
 
  void setURDFFile(const std::string &filename);
 
  void setSRDFFile(const std::string &filename);
 
  void setXmlRankFile(const std::string &filename);
 
  void setParamPreviewFile(const std::string &filename);
 
  void setSoleParameters(const double &inSoleLength, const double &inSoleWidth);
 
  void addJointMapping(const std::string &link, const std::string &repName);
 
  pg::PatternGeneratorInterface *GetPatternGeneratorInterface() {
    return m_PGI;
  };
 
 public: /* --- SIGNALS --- */
  typedef int Dummy;
 
  SignalTimeDependent<Dummy, int> firstSINTERN;
 
  SignalTimeDependent<Dummy, int> OneStepOfControlS;
 
 protected:
  dynamicgraph::Vector &getZMPRef(dynamicgraph::Vector &res, int time);
 
  dynamicgraph::Vector &getCoMRef(dynamicgraph::Vector &res, int time);
 
  dynamicgraph::Vector &getdCoMRef(dynamicgraph::Vector &res, int time);
 
  dynamicgraph::Vector &getddCoMRef(dynamicgraph::Vector &res, int time);
 
  dynamicgraph::Vector &getExternalForces(dynamicgraph::Vector &forces,
                                          int time);
 
  MatrixHomogeneous &getLeftFootRef(MatrixHomogeneous &res, int time);
 
  MatrixHomogeneous &getRightFootRef(MatrixHomogeneous &res, int time);
 
  MatrixHomogeneous &getdotLeftFootRef(MatrixHomogeneous &res, int time);
 
  MatrixHomogeneous &getdotRightFootRef(MatrixHomogeneous &res, int time);
 
  MatrixHomogeneous &getFlyingFootRef(MatrixHomogeneous &res, int time);
 
  dynamicgraph::Vector &getjointWalkingErrorPosition(dynamicgraph::Vector &res,
                                                     int time);
 
  dynamicgraph::Vector &getdComAttitude(dynamicgraph::Vector &res, int time);
 
  dynamicgraph::Vector &getddComAttitude(dynamicgraph::Vector &res, int time);
 
  dynamicgraph::Vector &getComAttitude(dynamicgraph::Vector &res, int time);
 
  VectorRollPitchYaw &getWaistAttitude(VectorRollPitchYaw &res, int time);
 
  VectorRollPitchYaw &getWaistAttitudeAbsolute(VectorRollPitchYaw &res,
                                               int time);
 
  MatrixHomogeneous &getWaistAttitudeMatrixAbsolute(MatrixHomogeneous &res,
                                                    int time);
 
  unsigned &getDataInProcess(unsigned &res, int time);
 
  dynamicgraph::Vector &getWaistPosition(dynamicgraph::Vector &res, int time);
 
  dynamicgraph::Vector &getWaistPositionAbsolute(dynamicgraph::Vector &res,
                                                 int time);
 
  int &getSupportFoot(int &res, int time);
 
  int &InitOneStepOfControl(int &dummy, int time);
  int &OneStepOfControl(int &dummy, int time);
 
  MatrixHomogeneous m_k_Waist_kp1;
 
  MatrixHomogeneous m_LeftFootPosition, m_RightFootPosition;
  PatternGeneratorJRL::FootAbsolutePosition m_PrevSamplingRightFootAbsPos,
      m_PrevSamplingLeftFootAbsPos;
  PatternGeneratorJRL::FootAbsolutePosition m_NextSamplingRightFootAbsPos,
      m_NextSamplingLeftFootAbsPos;
  PatternGeneratorJRL::FootAbsolutePosition m_InitRightFootAbsPos,
      m_InitLeftFootAbsPos;
 
  MatrixHomogeneous m_dotLeftFootPosition, m_dotRightFootPosition;
 
  MatrixHomogeneous m_InitLeftFootPosition, m_InitRightFootPosition;
 
  MatrixHomogeneous m_MotionSinceInstanciationToThisSequence;
 
  MatrixHomogeneous m_FlyingFootPosition;
 
  dynamicgraph::Vector m_ZMPRefPos;
 
  dynamicgraph::Vector m_ComAttitude;
 
  dynamicgraph::Vector m_dComAttitude;
 
  dynamicgraph::Vector m_ddComAttitude;
 
  dynamicgraph::Vector m_COMRefPos;
  dynamicgraph::Vector m_PrevSamplingCOMRefPos;
  dynamicgraph::Vector m_NextSamplingCOMRefPos;
 
  dynamicgraph::Vector m_dCOMRefPos;
  dynamicgraph::Vector m_PrevSamplingdCOMRefPos;
  dynamicgraph::Vector m_NextSamplingdCOMRefPos;
 
  dynamicgraph::Vector m_ddCOMRefPos;
  dynamicgraph::Vector m_PrevSamplingddCOMRefPos;
  dynamicgraph::Vector m_NextSamplingddCOMRefPos;
 
  dynamicgraph::Vector m_InitZMPRefPos;
 
  dynamicgraph::Vector m_InitWaistRefPos, m_InitWaistRefAtt;
 
  dynamicgraph::Vector m_InitCOMRefPos;
 
  dynamicgraph::Vector m_WaistPosition;
 
  dynamicgraph::Vector m_WaistPositionAbsolute;
 
  dynamicgraph::Vector m_WaistAttitude;
 
  dynamicgraph::Vector m_WaistAttitudeAbsolute;
  dynamicgraph::Vector m_PrevSamplingWaistAttAbs;
  dynamicgraph::Vector m_NextSamplingWaistAttAbs;
 
  MatrixHomogeneous m_WaistAttitudeMatrixAbsolute;
 
  dynamicgraph::Vector m_JointErrorValuesForWalking;
 
  dynamicgraph::Vector m_VelocityReference;
 
  bool m_trigger;
 
  unsigned int m_dataInProcess;
 
  bool m_feedBackControl;
 
  bool m_forceFeedBack;
 
  bool m_rightFootContact, m_leftFootContact;
 
  void ParseCmdFile(std::istringstream &cmdArg, std::ostream &os);
 
  void FromAbsoluteFootPosToDotHomogeneous(
      pg::FootAbsolutePosition aFootPosition, MatrixHomogeneous &aFootMH,
      MatrixHomogeneous &adotFootMH);
 
  void FromAbsoluteFootPosToHomogeneous(pg::FootAbsolutePosition aFootPosition,
                                        MatrixHomogeneous &aFootMH);
 
  void getAbsoluteWaistPosAttHomogeneousMatrix(MatrixHomogeneous &aWaistMH);
 
  void SubsamplingFootPos(pg::FootAbsolutePosition &PrevFootPosition,
                          pg::FootAbsolutePosition &NextFootPosition,
                          MatrixHomogeneous &FootPositionOut,
                          MatrixHomogeneous &dotFootPositionOut,
                          unsigned int &count);
 
  void SubsamplingVector(dynamicgraph::Vector &PrevPosition,
                         dynamicgraph::Vector &NextPosition,
                         dynamicgraph::Vector &PositionOut,
                         unsigned int &count);
 
  void CopyFootPosition(pg::FootAbsolutePosition &FootPositionIn,
                        pg::FootAbsolutePosition &FootPositionOut);
 
  dynamicgraph::Vector &getInitZMPRef(dynamicgraph::Vector &res, int time);
 
  dynamicgraph::Vector &getInitCoMRef(dynamicgraph::Vector &res, int time);
 
  dynamicgraph::Vector &getInitWaistPosRef(dynamicgraph::Vector &res, int time);
 
  VectorRollPitchYaw &getInitWaistAttRef(VectorRollPitchYaw &res, int time);
 
  MatrixHomogeneous &getInitLeftFootRef(MatrixHomogeneous &res, int time);
 
  MatrixHomogeneous &getInitRightFootRef(MatrixHomogeneous &res, int time);
 
  bool &getLeftFootContact(bool &res, int time);
  bool &getRightFootContact(bool &res, int time);
  int &getContactPhase(int &res, int time);
 
 public:
  SignalPtr<dynamicgraph::Vector, int> jointPositionSIN;
 
  SignalPtr<dynamicgraph::Vector, int> motorControlJointPositionSIN;
 
  SignalPtr<dynamicgraph::Vector, int> ZMPPreviousControllerSIN;
 
  SignalTimeDependent<dynamicgraph::Vector, int> ZMPRefSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> CoMRefSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> dCoMRefSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> ddCoMRefSOUT;
  SignalPtr<dynamicgraph::Vector, int> comSIN;
 
  SignalPtr<dynamicgraph::Vector, int> comStateSIN;
 
  SignalPtr<dynamicgraph::Vector, int> zmpSIN;
 
  SignalPtr<dynamicgraph::Vector, int> forceSIN;
  SignalTimeDependent<dynamicgraph::Vector, int> forceSOUT;
  dynamicgraph::Vector m_initForce;
  dynamicgraph::Vector m_currentForces;
  std::deque<dynamicgraph::Vector> m_bufferForce;
  std::vector<double> m_filterWindow;
 
  SignalPtr<dynamicgraph::Vector, int> velocitydesSIN;
 
  SignalPtr<bool, int> triggerSIN;
 
  SignalPtr<MatrixHomogeneous, int> LeftFootCurrentPosSIN;
 
  SignalPtr<MatrixHomogeneous, int> RightFootCurrentPosSIN;
 
  SignalTimeDependent<MatrixHomogeneous, int> LeftFootRefSOUT;
 
  SignalTimeDependent<MatrixHomogeneous, int> RightFootRefSOUT;
 
  SignalTimeDependent<MatrixHomogeneous, int> dotLeftFootRefSOUT;
 
  SignalTimeDependent<MatrixHomogeneous, int> dotRightFootRefSOUT;
 
  SignalTimeDependent<MatrixHomogeneous, int> FlyingFootRefSOUT;
 
  SignalTimeDependent<int, int> SupportFootSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> jointWalkingErrorPositionSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> comattitudeSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> dcomattitudeSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> ddcomattitudeSOUT;
 
  SignalTimeDependent<VectorRollPitchYaw, int> waistattitudeSOUT;
 
  SignalTimeDependent<VectorRollPitchYaw, int> waistattitudeabsoluteSOUT;
 
  SignalTimeDependent<MatrixHomogeneous, int> waistattitudematrixabsoluteSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> waistpositionSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> waistpositionabsoluteSOUT;
 
  SignalTimeDependent<unsigned int, int> dataInProcessSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> InitZMPRefSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> InitCoMRefSOUT;
 
  SignalTimeDependent<dynamicgraph::Vector, int> InitWaistPosRefSOUT;
 
  SignalTimeDependent<VectorRollPitchYaw, int> InitWaistAttRefSOUT;
 
  SignalTimeDependent<MatrixHomogeneous, int> InitLeftFootRefSOUT;
 
  SignalTimeDependent<MatrixHomogeneous, int> InitRightFootRefSOUT;
 
  SignalTimeDependent<bool, int> leftFootContactSOUT;
  SignalTimeDependent<bool, int> rightFootContactSOUT;
  SignalTimeDependent<int, int> contactPhaseSOUT;
 
 protected:
  Eigen::VectorXd m_ZMPPrevious;
 
 public: /* --- PARAMS --- */
  void initCommands(void);
  int stringToReferenceEnum(const std::string &FrameReference);
  void setReferenceFromString(const std::string &str);
  void addOnLineStep(const double &x, const double &y, const double &th);
  void addStep(const double &x, const double &y, const double &th);
  void pgCommandLine(const std::string &cmdline);
  void useFeedBackSignals(const bool &feedBack);
  void useDynamicFilter(const bool &dynamicFilter);
 
  void debug(void);
};
 
}  // namespace sot
}  // namespace dynamicgraph
 
#endif  // #ifndef __SOT_PATTERN_GENERATOR_H__