newPGstepStudy.h

Go to the documentation of this file.

/*
 *  Copyright AIST-CNRS Joint Robotics Laboratory
 *  Author: Nicolas Perrin
 *
 *  Convention: m_ for public members, mp_ for private members.
 */

#ifndef newPGstepStudy_H
#define newPGstepStudy_H

#include <sys/time.h>
#include <stdlib.h>
#include <time.h>
#include <cmath>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <fstream>
#include <vector>
#include <set>

#define PI 3.14159265359

using namespace std;

struct StepFeatures
{
  explicit StepFeatures()
  {}

  ~StepFeatures()
  {}

  StepFeatures& operator= (const StepFeatures& sf)
  {
    if (&sf == this)
      return *this;

    comTrajX = sf.comTrajX;
    zmpTrajX = sf.zmpTrajX;
    comTrajY = sf.comTrajY;
    zmpTrajY = sf.zmpTrajY;
    leftfootXtraj = sf.leftfootXtraj;
    leftfootYtraj = sf.leftfootYtraj;
    leftfootHeight = sf.leftfootHeight;
    leftfootOrient = sf.leftfootOrient;
    rightfootXtraj = sf.rightfootXtraj;
    rightfootYtraj = sf.rightfootYtraj;
    rightfootHeight = sf.rightfootHeight;
    rightfootOrient = sf.rightfootOrient;
    waistOrient = sf.waistOrient;
    incrTime = sf.incrTime;
    zc = sf.zc;
    size = sf.size;

    return *this;
  }

  std::vector<double> comTrajX;
  std::vector<double> zmpTrajX;
  std::vector<double> comTrajY;
  std::vector<double> zmpTrajY;
  std::vector<double> leftfootXtraj;
  std::vector<double> leftfootYtraj;
  std::vector<double> leftfootHeight;
  std::vector<double> leftfootOrient;
  std::vector<double> rightfootXtraj;
  std::vector<double> rightfootYtraj;
  std::vector<double> rightfootHeight;
  std::vector<double> rightfootOrient;
  std::vector<double> waistOrient;
  double incrTime;
  double zc;
  unsigned int size;
};

class CnewPGstepStudy
{

        public:

                CnewPGstepStudy();

                ~CnewPGstepStudy();

                void drawSeqStepFeatures(ofstream & fb, double incrTime, double zc, double g, double t1, double t2, double t3, double t4, double t5, vector<double> vect_input, char leftOrRightFootStable, double coefFeet);

                void drawSeqHalfStepFeatures(ofstream & fb, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable, double coefFeet);

                void drawSeqSlidedHalfStepFeatures(ofstream & fb, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable, double coefFeet);

                void plotOneDimensionCOMZMPSeqStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, double t4, double t5, vector<double> vect_input, char leftOrRightFootStable);

                void plotOneDimensionCOMZMPSeqHalfStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable);

                void plotOneDimensionCOMZMPSeqSlidedHalfStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable);

                void plotFootHeightSeqStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, double t4, double t5, vector<double> vect_input, char leftOrRightFootStable);

                void plotFootHeightSeqHalfStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable);

                void plotFootHeightSeqSlidedHalfStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable);




                void produceOneStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, double t4, double t5, vector<double> vectStep_input, char leftOrRightFootStable);

                void produceOneUPHalfStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vectStep_input, char leftOrRightFootStable);

                void produceOneDOWNHalfStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vectStep_input, char leftOrRightFootStable);

                void produceSeqStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, double t4, double t5, vector<double> vectSteps_input, char leftOrRightFootStable);

                void produceSeqHalfStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vectSteps_input, char leftOrRightFootStable);

                void produceSeqSlidedHalfStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vectSteps_input, char leftOrRightFootStable);

                void addStepFeaturesWithSlide(StepFeatures & stepF1, StepFeatures & stepF2, double negativeSlideTime);


        private:

                void genCOMZMPtrajectory(vector<double>& outputCOM, vector<double>& outputZMP, double incrTime, double zc, double g, double delta0, double deltaX, double deltaX2, double t1, double t2, double t3, double t4, double t5);

                void genFOOTposition(vector<double>& outputX, vector<double>& outputY, double incrTime, double xinit, double yinit, double xend, double yend, double delay, double t1, double t2, double t3, double t4, double t5, char dh);

                void genFOOTheight(vector<double>& output, double incrTime, double heightMax, double delay, double t1, double t2, double t3, double t4, double t5);

                void genFOOTupDOWNheight(vector<double> & output, double incrTime, double heightMax, double delay, double t1, double t2, double t3);

                void genFOOTdownUPheight(vector<double> & output, double incrTime, double heightMax, double delay, double t1, double t2, double t3);

                void genFOOTorientation(vector<double>& output, double incrTime, double initOrient, double endOrient, double delay, double t1, double t2, double t3, double t4, double t5, char dh);

                void genWAISTorientation(vector<double>& output, double incrTime, double initOrient, double endOrient, double delay, double t1, double t2, double t3, double t4, double t5, char dh);


};
#endif