<commit_before>/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 * Copyright Projet JRL-Japan, 2008
 *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * File:      PatternGenerator.cpp
 * Project:   SOT
 * Author:    <NAME>
 *
 * Version control
 * ===============
 *
 *  $Id$
 *
 * Description
 * ============
 *
 *
 * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

/* Local Variables:  */
/* mode: c++         */
/* comment-column: 0            */
/* visual-fill-column-width: 80 */
/* indent-tabs-mode: nil */
/* End:                         */

//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <pinocchio/fwd.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

#include <sot/core/debug.hh>
#ifdef VP_DEBUG
class sotPG__INIT {
 public:
  sotPG__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); }
};
sotPG__INIT sotPG_initiator;
#endif  //#ifdef VP_DEBUG

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

#include "pinocchio/parsers/srdf.hpp"
#include "pinocchio/parsers/urdf.hpp"

#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
#include <sot/core/matrix-geometry.hh>

#include <sot/pattern-generator/pg.h>

using namespace std;
namespace dynamicgraph {
namespace sot {

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PatternGenerator, "PatternGenerator");

PatternGenerator::PatternGenerator(const std::string &name)
    : Entity(name),
      m_PGI(0),
      m_PreviewControlParametersFile(),
      m_urdfFile(""),
      m_srdfFile(""),
      m_xmlRankFile(""),
      m_soleWidth(0),
      m_init(false),
      m_InitPositionByRealState(true),
      firstSINTERN(
          boost::bind(&PatternGenerator::InitOneStepOfControl, this, _1, _2),
          sotNOSIGNAL, "PatternGenerator(" + name + ")::intern(dummy)::init")

      ,
      OneStepOfControlS(
          boost::bind(&PatternGenerator::OneStepOfControl, this, _1, _2),
          firstSINTERN << jointPositionSIN,
          "PatternGenerator(" + name + ")::onestepofcontrol")

      ,
      m_dataInProcess(0),
      m_rightFootContact(true)  // It is assumed that the robot is standing.
      ,
      m_leftFootContact(true),
      jointPositionSIN(
          NULL, "PatternGenerator(" + name + ")::input(vector)::position")

      ,
      motorControlJointPositionSIN(
          NULL, "PatternGenerator(" + name + ")::input(vector)::motorcontrol")

      ,
      ZMPPreviousControllerSIN(NULL,
                               "PatternGenerator(" + name +
                                   ")::input(vector)::zmppreviouscontroller")

      ,
      ZMPRefSOUT(boost::bind(&PatternGenerator::getZMPRef, this, _1, _2),
                 OneStepOfControlS,
                 "PatternGenerator(" + name + ")::output(vector)::zmpref")

      ,
      CoMRefSOUT(boost::bind(&PatternGenerator::getCoMRef, this, _1, _2),
                 OneStepOfControlS,
                 "PatternGenerator(" + name + ")::output(vector)::comref")

      ,
      dCoMRefSOUT(boost::bind(&PatternGenerator::getdCoMRef, this, _1, _2),
                  OneStepOfControlS,
                  "PatternGenerator(" + name + ")::output(vector)::dcomref")

      ,
      ddCoMRefSOUT(boost::bind(&PatternGenerator::getddCoMRef, this, _1, _2),
                   OneStepOfControlS,
                   "PatternGenerator(" + name + ")::output(vector)::ddcomref")

      ,
      comSIN(NULL, "PatternGenerator(" + name + ")::input(vector)::com")

      ,
      comStateSIN(NULL,
                  "PatternGenerator(" + name + ")::input(vector)::comStateSIN")

      ,
      zmpSIN(NULL, "PatternGenerator(" + name + ")::input(vector)::zmpSIN")

      ,
      forceSIN(NULL, "PatternGenerator(" + name + ")::input(vector)::forceSIN")

      ,
      forceSOUT(boost::bind(&PatternGenerator::getExternalForces, this, _1, _2),
                OneStepOfControlS,
                "PatternGenerator(" + name + ")::output(vector)::forceSOUT")

      ,
      velocitydesSIN(
          NULL, "PatternGenerator(" + name + ")::input(vector)::velocitydes")

      ,
      triggerSIN(NULL, "PatternGenerator(" + name + ")::input(bool)::trigger")

      ,
      LeftFootCurrentPosSIN(
          NULL, "PatternGenerator(" + name +
                    ")::input(homogeneousmatrix)::leftfootcurrentpos")

      ,
      RightFootCurrentPosSIN(
          NULL, "PatternGenerator(" + name +
                    ")::input(homogeneousmatrix)::rightfootcurrentpos")

      ,
      LeftFootRefSOUT(
          boost::bind(&PatternGenerator::getLeftFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::leftfootref")

      ,
      RightFootRefSOUT(
          boost::bind(&PatternGenerator::getRightFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::rightfootref")

      ,
      dotLeftFootRefSOUT(
          boost::bind(&PatternGenerator::getdotLeftFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::dotleftfootref")

      ,
      dotRightFootRefSOUT(
          boost::bind(&PatternGenerator::getdotRightFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::dotrightfootref")

      ,
      FlyingFootRefSOUT(
          boost::bind(&PatternGenerator::getFlyingFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::flyingfootref")

      ,
      SupportFootSOUT(
          boost::bind(&PatternGenerator::getSupportFoot, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(uint)::SupportFoot"),
      jointWalkingErrorPositionSOUT(
          boost::bind(&PatternGenerator::getjointWalkingErrorPosition, this, _1,
                      _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(vector)::walkingerrorposition")

      ,
      comattitudeSOUT(
          boost::bind(&PatternGenerator::getComAttitude, this, _1, _2),
          OneStepOfControlS,
          "sotPatternGenerator(" + name + ")::output(vectorRPY)::comattitude")

      ,
      dcomattitudeSOUT(
          boost::bind(&PatternGenerator::getdComAttitude, this, _1, _2),
          OneStepOfControlS,
          "sotPatternGenerator(" + name + ")::output(vectorRPY)::dcomattitude")

      ,
      ddcomattitudeSOUT(
          boost::bind(&PatternGenerator::getddComAttitude, this, _1, _2),
          OneStepOfControlS,
          "sotPatternGenerator(" + name + ")::output(vectorRPY)::ddcomattitude")

      ,
      waistattitudeSOUT(
          boost::bind(&PatternGenerator::getWaistAttitude, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(vectorRPY)::waistattitude")

      ,
      waistattitudeabsoluteSOUT(
          boost::bind(&PatternGenerator::getWaistAttitudeAbsolute, this, _1,
                      _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(vectorRPY)::waistattitudeabsolute")

      ,
      waistattitudematrixabsoluteSOUT(
          boost::bind(&PatternGenerator::getWaistAttitudeMatrixAbsolute, this,
                      _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::waistattitudematrixabsolute")

      ,
      waistpositionSOUT(
          boost::bind(&PatternGenerator::getWaistPosition, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(vector)::waistposition")

      ,
      waistpositionabsoluteSOUT(
          boost::bind(&PatternGenerator::getWaistPositionAbsolute, this, _1,
                      _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(vector)::waistpositionabsolute")

      ,
      dataInProcessSOUT(
          boost::bind(&PatternGenerator::getDataInProcess, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(bool)::inprocess")

      ,
      InitZMPRefSOUT(
          boost::bind(&PatternGenerator::getInitZMPRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(vector)::initzmpref")

      ,
      InitCoMRefSOUT(
          boost::bind(&PatternGenerator::getInitCoMRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(matrix)::initcomref")

      ,
      InitWaistPosRefSOUT(
          boost::bind(&PatternGenerator::getInitWaistPosRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(vector)::initwaistposref")

      ,
      InitWaistAttRefSOUT(
          boost::bind(&PatternGenerator::getInitWaistAttRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(vectorRPY)::initwaistattref")

      ,
      InitLeftFootRefSOUT(
          boost::bind(&PatternGenerator::getInitLeftFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::initleftfootref")

      ,
      InitRightFootRefSOUT(
          boost::bind(&PatternGenerator::getInitRightFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::initrightfootref")

      ,
      leftFootContactSOUT(
          boost::bind(&PatternGenerator::getLeftFootContact, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(bool)::leftfootcontact")

      ,
      rightFootContactSOUT(
          boost::bind(&PatternGenerator::getRightFootContact, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(bool)::rightfootcontact")

{
  m_MotionSinceInstanciationToThisSequence.setIdentity();

  m_LocalTime = 0;
  m_count = 0;
  m_TimeStep = 0.005;
  m_DoubleSupportPhaseState = false;
  m_forceFeedBack = false;
  m_feedBackControl = false;

  m_ZMPRefPos.resize(4);
  m_ZMPRefPos.fill(0.0);
  m_ZMPRefPos(3) = 1.0;
  m_COMRefPos.resize(3);
  m_COMRefPos.fill(0.0);
  m_PrevSamplingCOMRefPos.resize(3);
  m_PrevSamplingCOMRefPos.fill(0.0);
  m_NextSamplingCOMRefPos.resize(3);
  m_NextSamplingCOMRefPos.fill(0.0);
  m_ZMPPrevious.resize(4);
  m_ZMPPrevious(3) = 1.0;
  m_dCOMRefPos.resize(3);
  m_dCOMRefPos.fill(0.0);
  m_PrevSamplingdCOMRefPos.resize(3);
  m_PrevSamplingdCOMRefPos.fill(0.0);
  m_NextSamplingdCOMRefPos.resize(3);
  m_NextSamplingdCOMRefPos.fill(0.0);
  m_ddCOMRefPos.resize(3);
  m_ddCOMRefPos.fill(0.0);
  m_PrevSamplingddCOMRefPos.resize(3);
  m_PrevSamplingddCOMRefPos.fill(0.0);
  m_NextSamplingddCOMRefPos.resize(3);
  m_NextSamplingddCOMRefPos.fill(0.0);
  m_InitZMPRefPos.resize(3);
  m_InitZMPRefPos.fill(0);
  m_InitCOMRefPos.resize(3);
  m_InitCOMRefPos.fill(0);
  m_InitWaistRefPos.resize(3);
  m_InitWaistRefPos.fill(0);
  m_InitWaistRefAtt.resize(3);
  m_InitWaistRefAtt.fill(0);
  m_dComAttitude.resize(3);
  m_dComAttitude.fill(0);
  m_ddComAttitude.resize(3);
  m_ddComAttitude.fill(0);
  m_VelocityReference.resize(3);
  m_VelocityReference.fill(0.0);
  m_trigger = false;
  m_WaistAttitude.resize(3);
  m_WaistAttitude.fill(0);
  m_ComAttitude.resize(3);
  m_ComAttitude.fill(0);
  m_WaistPosition.resize(3);
  m_WaistPosition.fill(0);
  m_WaistAttitudeAbsolute.resize(3);
  m_WaistAttitudeAbsolute.fill(0);
  m_PrevSamplingWaistAttAbs.resize(3);
  m_PrevSamplingWaistAttAbs.fill(0);
  m_NextSamplingWaistAttAbs.resize(3);
  m_NextSamplingWaistAttAbs.fill(0);
  m_WaistPositionAbsolute.resize(3);
  m_WaistPositionAbsolute.fill(0);

  m_WaistAttitudeMatrixAbsolute.setIdentity();
  m_LeftFootPosition.setIdentity();
  m_RightFootPosition.setIdentity();

  m_dotLeftFootPosition.setIdentity();
  m_dotRightFootPosition.setIdentity();

  m_InitLeftFootPosition.setIdentity();
  m_InitRightFootPosition.setIdentity();
  m_FlyingFootPosition.setIdentity();

  m_k_Waist_kp1.setIdentity();

  m_SupportFoot = 1;  // Means that we do not know which support foot it is.
  m_ReferenceFrame = WORLD_FRAME;

  sotDEBUGIN(5);

  firstSINTERN.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
  // TODO: here, the 'setConstant' destroy the pointer toward
  // function initOneStepOfControl. By calling firstSINTERN(t), whatever t,
  // nothing will happen (well, it will just return 0).
  // To initialize firstSINTERN (without destroying the pointer), use
  // firstSINTERN.setReady() instead.
  // TODO: Remove the next line: // firstSINTERN.setConstant(0);
  firstSINTERN.setReady(true);

  // OneStepOfControlS.setDependencyType(TimeDependency<int>::ALWAYS_READY);
  //  OneStepOfControlS.setConstant(0);

  OneStepOfControlS.addDependency(LeftFootCurrentPosSIN);
  OneStepOfControlS.addDependency(RightFootCurrentPosSIN);
  OneStepOfControlS.addDependency(velocitydesSIN);
  OneStepOfControlS.addDependency(triggerSIN);
  OneStepOfControlS.addDependency(firstSINTERN);
  OneStepOfControlS.addDependency(motorControlJointPositionSIN);
  OneStepOfControlS.addDependency(comSIN);
  OneStepOfControlS.addDependency(comStateSIN);
  OneStepOfControlS.addDependency(zmpSIN);
  OneStepOfControlS.addDependency(forceSIN);

  // For debug, register OSOC (not relevant for normal use).
  signalRegistration(OneStepOfControlS);

#if 0

      signalRegistration( jointPositionSIN <<
                          motorControlJointPositionSIN <<
                          ZMPPreviousControllerSIN <<
                          ZMPRefSOUT <<
                          CoMRefSOUT <<
                          dCoMRefSOUT);
      signalRegistration( dataInProcessSOUT <<
                          LeftFootCurrentPosSIN <<
                          RightFootCurrentPosSIN <<
                          LeftFootRefSOUT <<
                          RightFootRefSOUT);

      signalRegistration( SupportFootSOUT <<
                          jointWalkingErrorPositionSOUT <<
                          waistattitudeSOUT <<
                          waistpositionSOUT <<
                          waistattitudeabsoluteSOUT <<
                          waistpositionabsoluteSOUT);

      signalRegistration( comattitudeSOUT <<
                          dcomattitudeSOUT );

      signalRegistration( dotLeftFootRefSOUT <<
                          dotRightFootRefSOUT);

      signalRegistration( InitZMPRefSOUT <<
                          InitCoMRefSOUT <<
                          InitWaistPosRefSOUT <<
                          InitWaistAttRefSOUT <<
                          InitLeftFootRefSOUT <<
                          InitRightFootRefSOUT <<
                          comSIN <<
                          velocitydesSIN);
#else
  signalRegistration(dataInProcessSOUT);

  signalRegistration(jointPositionSIN << motorControlJointPositionSIN
                                      << ZMPPreviousControllerSIN << ZMPRefSOUT
                                      << CoMRefSOUT << dCoMRefSOUT
                                      << ddCoMRefSOUT);

  signalRegistration(comStateSIN << zmpSIN << forceSIN << forceSOUT);

  signalRegistration(comSIN << velocitydesSIN << triggerSIN
                            << LeftFootCurrentPosSIN << RightFootCurrentPosSIN
                            << LeftFootRefSOUT << RightFootRefSOUT);

  signalRegistration(SupportFootSOUT << jointWalkingErrorPositionSOUT
                                     << comattitudeSOUT << dcomattitudeSOUT
                                     << ddcomattitudeSOUT << waistattitudeSOUT
                                     << waistattitudematrixabsoluteSOUT);

  signalRegistration(waistpositionSOUT << waistattitudeabsoluteSOUT
                                       << waistpositionabsoluteSOUT);

  signalRegistration(dotLeftFootRefSOUT << dotRightFootRefSOUT);

  signalRegistration(InitZMPRefSOUT << InitCoMRefSOUT << InitWaistPosRefSOUT
                                    << InitWaistAttRefSOUT
                                    << InitLeftFootRefSOUT
                                    << InitRightFootRefSOUT);

  signalRegistration(leftFootContactSOUT << rightFootContactSOUT);

#endif
  initCommands();

  // init filter for force signals "to be removed"
  m_bufferForce.clear();
  int n = 10;
  double sum = 0, tmp = 0;
  m_filterWindow.resize(n + 1);
  for (int i = 0; i < n + 1; i++) {
    tmp = sin((M_PI * i) / n);
    m_filterWindow[i] = tmp * tmp;
  }

  for (int i = 0; i < n + 1; i++) sum += m_filterWindow[i];

  for (int i = 0; i < n + 1; i++) m_filterWindow[i] /= sum;

  m_initForce.resize(6);
  m_currentForces.resize(6);
  // dataInProcessSOUT.setReference( &m_dataInProcess );
  // m_wrml2urdfIndex.clear();

  sotDEBUGOUT(5);
}

bool PatternGenerator::InitState(void) {
  sotDEBUGIN(5);
  // TODO
  // Instead of (0) ie .access(0), it could be rather used:
  // .accessCopy()
  // Instead of copy value (ml::Vector pos) it could be rather
  // used reference (const ml::Vector & post)
  Vector res;
  Eigen::Matrix<double, 6, 1> lWaistPosition;
  if (m_InitPositionByRealState) {
    const Vector &pos = jointPositionSIN(m_LocalTime);

    lWaistPosition.resize(6);
    for (unsigned int i = 0; i < 6; ++i) {
      lWaistPosition(i) = pos(i);
    }
    //  m_ZMPPrevious[2] =m_AnkleSoilDistance;
    // Changed the reference frame.

    res.resize(pos.size() - 6);

    for (unsigned i = 0; i < res.size(); i++) res(i) = pos(i + 6);

    // lWaistPosition(0) = 0.0;
    // lWaistPosition(1) = 0.0;
    // lWaistPosition(2) = 1.019272;
    // lWaistPosition(3) = 0.0;
    // lWaistPosition(4) = 0.0;
    // lWaistPosition(5) = 0.0;

    // res(18) = 0.0;
    // res(19) = -0.000285;
    // res(20) = -0.4113544;
    // res(21) = 0.8593958;
    // res(22) = -0.4480414;
    // res(23) = 0.000285;

    // res(24) = 0.0;
    // res(25) = -0.000285;
    // res(26) = -0.4113544;
    // res(27) = 0.8593958;
    // res(28) = -0.4480414;
    // res(29) = 0.000285;

    // res(30) = 0.0;
    // res(31) = 0.006761;

    // res(0) = 0.0;
    // res(1) = 0.0;
    // res(2) = 0.0;
    // res(3) = 0.0;
    // res(4) = 0.0;
    // res(5) = 0.0;
    // res(6) = 0.1;

    // res(14) = 0.05;

    // res(7) = 0.0;
    // res(8) = 0.0;
    // res(9) = 0.0;
    // res(10) = 0.0;
    // res(11) = 0.0;
    // res(12) = 0.0;
    // res(13) = 0.1;

    // res(15) = 0.05;

    // res(16) = 0.0;
    // res(17) = 0.0;

    Vector lZMPPrevious = ZMPPreviousControllerSIN(m_LocalTime);
    for (unsigned int i = 0; i < 3; i++) m_ZMPPrevious[i] = lZMPPrevious(i);
  } else {
    res = motorControlJointPositionSIN(m_LocalTime);
    // for(unsigned i=0;i<res.size();i++)
    // res(m_wrml2urdfIndex[i]) = res(i);
  }

  Vector com = comSIN(m_LocalTime);

  m_JointErrorValuesForWalking.resize(res.size());

  sotDEBUG(5) << "m_LocalTime:" << m_LocalTime << endl;
  sotDEBUG(5) << "Joint Values:" << res << endl;

  try {
    Eigen::VectorXd bres;
    bres.resize(res.size());
    for (int i = 0; i < res.size(); i++) bres(i) = res(i);
    m_PGI->SetCurrentJointValues(bres);

    // Evaluate current position of the COM, ZMP and feet
    // according to the state of the robot.
    PatternGeneratorJRL::COMState lStartingCOMState;
    Eigen::Vector3d lStartingZMPPosition;

    m_PGI->EvaluateStartingState(lStartingCOMState, lStartingZMPPosition,
                                 lWaistPosition, m_InitLeftFootAbsPos,
                                 m_InitRightFootAbsPos);

    // Put inside sotHomogeneous representation
    m_InitCOMRefPos(0) = lStartingCOMState.x[0];
    m_InitCOMRefPos(1) = lStartingCOMState.y[0];
    m_InitCOMRefPos(2) = lStartingCOMState.z[0];

    m_InitZMPRefPos(0) = lStartingCOMState.x[0];
    m_InitZMPRefPos(1) = lStartingCOMState.y[0];
    m_InitZMPRefPos(2) = 0;

    if (m_InitPositionByRealState) {
      m_ZMPPrevious[0] = lStartingCOMState.x[0];
      m_ZMPPrevious[1] = lStartingCOMState.y[0];
      m_ZMPPrevious[2] = 0;
    }
    sotDEBUG(5) << "InitZMPRefPos :" << m_InitZMPRefPos << endl;

    m_InitWaistRefPos(0) = m_WaistPositionAbsolute(0) = lWaistPosition(0);
    m_InitWaistRefPos(1) = m_WaistPositionAbsolute(1) = lWaistPosition(1);
    m_InitWaistRefPos(2) = m_WaistPositionAbsolute(2) = lWaistPosition(2);

    m_InitWaistRefAtt(0) = m_WaistAttitudeAbsolute(0) = lWaistPosition(3);
    m_InitWaistRefAtt(1) = m_WaistAttitudeAbsolute(1) = lWaistPosition(4);
    m_InitWaistRefAtt(2) = m_WaistAttitudeAbsolute(2) = lWaistPosition(5);

    FromAbsoluteFootPosToDotHomogeneous(
        m_InitRightFootAbsPos, m_InitRightFootPosition, m_dotRightFootPosition);
    FromAbsoluteFootPosToDotHomogeneous(
        m_InitLeftFootAbsPos, m_InitLeftFootPosition, m_dotLeftFootPosition);

    Eigen::Matrix<double, 4, 1> newtmp, oldtmp;
    oldtmp(0) = m_InitCOMRefPos(0);
    oldtmp(1) = m_InitCOMRefPos(1);
    oldtmp(2) = m_InitCOMRefPos(2);
    oldtmp(3) = 1.0;
    newtmp = m_MotionSinceInstanciationToThisSequence * oldtmp;
    m_InitCOMRefPos(0) = newtmp(0);
    m_InitCOMRefPos(1) = newtmp(1);
    m_InitCOMRefPos(2) = newtmp(2);

    oldtmp(0) = m_InitZMPRefPos(0);
    oldtmp(1) = m_InitZMPRefPos(1);
    oldtmp(2) = m_InitZMPRefPos(2);
    newtmp = m_MotionSinceInstanciationToThisSequence * oldtmp;
    m_InitZMPRefPos(0) = newtmp(0);
    m_InitZMPRefPos(1) = newtmp(1);
    m_InitZMPRefPos(2) = newtmp(2);

    if (!m_InitPositionByRealState) {
      MatrixHomogeneous invInitLeftFootRef;
      invInitLeftFootRef = m_InitLeftFootPosition.inverse();
      m_k_Waist_kp1 = m_k_Waist_kp1 * invInitLeftFootRef;
      m_MotionSinceInstanciationToThisSequence =
          m_MotionSinceInstanciationToThisSequence * m_k_Waist_kp1;
    }

    m_k_Waist_kp1 = m_InitLeftFootPosition;

    m_InitLeftFootPosition =
        m_MotionSinceInstanciationToThisSequence * m_InitLeftFootPosition;
    m_InitRightFootPosition =
        m_MotionSinceInstanciationToThisSequence * m_InitRightFootPosition;

    m_LeftFootPosition = m_InitLeftFootPosition;
    m_RightFootPosition = m_InitRightFootPosition;

    sotDEBUG(5) << "m_InitCOMRefPos: " << m_InitCOMRefPos;

    sotDEBUG(5) << "m_InitZMPRefPos: " << m_InitZMPRefPos;
    sotDEBUG(5) << "m_LeftFootPosition: " << m_LeftFootPosition;
    sotDEBUG(5) << "m_RightFootPosition: " << m_RightFootPosition;
    sotDEBUG(5) << "m_MotionSinceInstanciationToThisSequence"
                << m_MotionSinceInstanciationToThisSequence << std::endl;

    sotDEBUG(5) << " Init Waist Ref. Position " << m_InitWaistRefPos << endl;
    sotDEBUG(5) << " Init Waist Ref. Attitude " << m_InitWaistRefAtt << endl;

    sotDEBUG(5) << "ILF :" << m_InitLeftFootPosition << " "
                << "LRF :" << m_InitRightFootPosition << endl;

  } catch (...) {
    SOT_THROW ExceptionPatternGenerator(
        ExceptionPatternGenerator::PATTERN_GENERATOR_JRL,
        "Error while setting the current joint values of the WPG.");
    return false;
  }

  m_InitPositionByRealState = false;
  sotDEBUGOUT(5);
  return true;
}

bool PatternGenerator::buildModel(void) {
  bool ok = true;
  // Parsing the file.
  pinocchio::urdf::buildModel(m_urdfFile, pinocchio::JointModelFreeFlyer(),
                              m_robotModel);

  m_robotData = new pinocchio::Data(m_robotModel);
  // Creating the humanoid robot.
  m_PR = new pg::PinocchioRobot();
  m_PR->initializeRobotModelAndData(&m_robotModel, m_robotData);
  // Read xml/srdf stream
  std::ifstream srdf_stream(m_srdfFile.c_str());
  using boost::property_tree::ptree;
  ptree pt;
  try {
    read_xml(srdf_stream, pt);
    // Initialize the Right Foot
    pg::PRFoot aFoot;
    string path = "robot.specificities.feet.right.size";
    BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
      aFoot.soleHeight = v.second.get<double>("height");
      aFoot.soleWidth = v.second.get<double>("width");
      aFoot.soleDepth = v.second.get<double>("depth");
    }  // BOOST_FOREACH
    path = "robot.specificities.feet.right.anklePosition";
    BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
      aFoot.anklePosition(0) = v.second.get<double>("x");
      aFoot.anklePosition(1) = v.second.get<double>("y");
      aFoot.anklePosition(2) = v.second.get<double>("z");
    }  // BOOST_FOREACH
    pinocchio::FrameIndex ra = m_robotModel.getFrameId("r_ankle");
    aFoot.associatedAnkle = m_robotModel.frames.at(ra).parent;
    m_PR->initializeRightFoot(aFoot);
    // Initialize the Left Foot<commit_msg>[pg.cpp] Read the model from parameter-serverq<commit_after>/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 * Copyright Projet JRL-Japan, 2008
 *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * File:      PatternGenerator.cpp
 * Project:   SOT
 * Author:    <NAME>
 *
 * Version control
 * ===============
 *
 *  $Id$
 *
 * Description
 * ============
 *
 *
 * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

/* Local Variables:  */
/* mode: c++         */
/* comment-column: 0            */
/* visual-fill-column-width: 80 */
/* indent-tabs-mode: nil */
/* End:                         */

//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <pinocchio/fwd.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

#include <sot/core/debug.hh>
#ifdef VP_DEBUG
class sotPG__INIT {
 public:
  sotPG__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); }
};
sotPG__INIT sotPG_initiator;
#endif  //#ifdef VP_DEBUG

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

#include "pinocchio/parsers/srdf.hpp"
#include "pinocchio/parsers/urdf.hpp"

#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
#include <sot/core/matrix-geometry.hh>

#include <sot/pattern-generator/pg.h>

using namespace std;
namespace dynamicgraph {
namespace sot {

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PatternGenerator, "PatternGenerator");

PatternGenerator::PatternGenerator(const std::string &name)
    : Entity(name),
      m_PGI(0),
      m_PreviewControlParametersFile(),
      m_urdfFile(""),
      m_srdfFile(""),
      m_xmlRankFile(""),
      m_soleWidth(0),
      m_init(false),
      m_InitPositionByRealState(true),
      firstSINTERN(
          boost::bind(&PatternGenerator::InitOneStepOfControl, this, _1, _2),
          sotNOSIGNAL, "PatternGenerator(" + name + ")::intern(dummy)::init")

      ,
      OneStepOfControlS(
          boost::bind(&PatternGenerator::OneStepOfControl, this, _1, _2),
          firstSINTERN << jointPositionSIN,
          "PatternGenerator(" + name + ")::onestepofcontrol")

      ,
      m_dataInProcess(0),
      m_rightFootContact(true)  // It is assumed that the robot is standing.
      ,
      m_leftFootContact(true),
      jointPositionSIN(
          NULL, "PatternGenerator(" + name + ")::input(vector)::position")

      ,
      motorControlJointPositionSIN(
          NULL, "PatternGenerator(" + name + ")::input(vector)::motorcontrol")

      ,
      ZMPPreviousControllerSIN(NULL,
                               "PatternGenerator(" + name +
                                   ")::input(vector)::zmppreviouscontroller")

      ,
      ZMPRefSOUT(boost::bind(&PatternGenerator::getZMPRef, this, _1, _2),
                 OneStepOfControlS,
                 "PatternGenerator(" + name + ")::output(vector)::zmpref")

      ,
      CoMRefSOUT(boost::bind(&PatternGenerator::getCoMRef, this, _1, _2),
                 OneStepOfControlS,
                 "PatternGenerator(" + name + ")::output(vector)::comref")

      ,
      dCoMRefSOUT(boost::bind(&PatternGenerator::getdCoMRef, this, _1, _2),
                  OneStepOfControlS,
                  "PatternGenerator(" + name + ")::output(vector)::dcomref")

      ,
      ddCoMRefSOUT(boost::bind(&PatternGenerator::getddCoMRef, this, _1, _2),
                   OneStepOfControlS,
                   "PatternGenerator(" + name + ")::output(vector)::ddcomref")

      ,
      comSIN(NULL, "PatternGenerator(" + name + ")::input(vector)::com")

      ,
      comStateSIN(NULL,
                  "PatternGenerator(" + name + ")::input(vector)::comStateSIN")

      ,
      zmpSIN(NULL, "PatternGenerator(" + name + ")::input(vector)::zmpSIN")

      ,
      forceSIN(NULL, "PatternGenerator(" + name + ")::input(vector)::forceSIN")

      ,
      forceSOUT(boost::bind(&PatternGenerator::getExternalForces, this, _1, _2),
                OneStepOfControlS,
                "PatternGenerator(" + name + ")::output(vector)::forceSOUT")

      ,
      velocitydesSIN(
          NULL, "PatternGenerator(" + name + ")::input(vector)::velocitydes")

      ,
      triggerSIN(NULL, "PatternGenerator(" + name + ")::input(bool)::trigger")

      ,
      LeftFootCurrentPosSIN(
          NULL, "PatternGenerator(" + name +
                    ")::input(homogeneousmatrix)::leftfootcurrentpos")

      ,
      RightFootCurrentPosSIN(
          NULL, "PatternGenerator(" + name +
                    ")::input(homogeneousmatrix)::rightfootcurrentpos")

      ,
      LeftFootRefSOUT(
          boost::bind(&PatternGenerator::getLeftFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::leftfootref")

      ,
      RightFootRefSOUT(
          boost::bind(&PatternGenerator::getRightFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::rightfootref")

      ,
      dotLeftFootRefSOUT(
          boost::bind(&PatternGenerator::getdotLeftFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::dotleftfootref")

      ,
      dotRightFootRefSOUT(
          boost::bind(&PatternGenerator::getdotRightFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::dotrightfootref")

      ,
      FlyingFootRefSOUT(
          boost::bind(&PatternGenerator::getFlyingFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::flyingfootref")

      ,
      SupportFootSOUT(
          boost::bind(&PatternGenerator::getSupportFoot, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(uint)::SupportFoot"),
      jointWalkingErrorPositionSOUT(
          boost::bind(&PatternGenerator::getjointWalkingErrorPosition, this, _1,
                      _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(vector)::walkingerrorposition")

      ,
      comattitudeSOUT(
          boost::bind(&PatternGenerator::getComAttitude, this, _1, _2),
          OneStepOfControlS,
          "sotPatternGenerator(" + name + ")::output(vectorRPY)::comattitude")

      ,
      dcomattitudeSOUT(
          boost::bind(&PatternGenerator::getdComAttitude, this, _1, _2),
          OneStepOfControlS,
          "sotPatternGenerator(" + name + ")::output(vectorRPY)::dcomattitude")

      ,
      ddcomattitudeSOUT(
          boost::bind(&PatternGenerator::getddComAttitude, this, _1, _2),
          OneStepOfControlS,
          "sotPatternGenerator(" + name + ")::output(vectorRPY)::ddcomattitude")

      ,
      waistattitudeSOUT(
          boost::bind(&PatternGenerator::getWaistAttitude, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(vectorRPY)::waistattitude")

      ,
      waistattitudeabsoluteSOUT(
          boost::bind(&PatternGenerator::getWaistAttitudeAbsolute, this, _1,
                      _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(vectorRPY)::waistattitudeabsolute")

      ,
      waistattitudematrixabsoluteSOUT(
          boost::bind(&PatternGenerator::getWaistAttitudeMatrixAbsolute, this,
                      _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::waistattitudematrixabsolute")

      ,
      waistpositionSOUT(
          boost::bind(&PatternGenerator::getWaistPosition, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(vector)::waistposition")

      ,
      waistpositionabsoluteSOUT(
          boost::bind(&PatternGenerator::getWaistPositionAbsolute, this, _1,
                      _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(vector)::waistpositionabsolute")

      ,
      dataInProcessSOUT(
          boost::bind(&PatternGenerator::getDataInProcess, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(bool)::inprocess")

      ,
      InitZMPRefSOUT(
          boost::bind(&PatternGenerator::getInitZMPRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(vector)::initzmpref")

      ,
      InitCoMRefSOUT(
          boost::bind(&PatternGenerator::getInitCoMRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(matrix)::initcomref")

      ,
      InitWaistPosRefSOUT(
          boost::bind(&PatternGenerator::getInitWaistPosRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(vector)::initwaistposref")

      ,
      InitWaistAttRefSOUT(
          boost::bind(&PatternGenerator::getInitWaistAttRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(vectorRPY)::initwaistattref")

      ,
      InitLeftFootRefSOUT(
          boost::bind(&PatternGenerator::getInitLeftFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::initleftfootref")

      ,
      InitRightFootRefSOUT(
          boost::bind(&PatternGenerator::getInitRightFootRef, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name +
              ")::output(homogeneousmatrix)::initrightfootref")

      ,
      leftFootContactSOUT(
          boost::bind(&PatternGenerator::getLeftFootContact, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(bool)::leftfootcontact")

      ,
      rightFootContactSOUT(
          boost::bind(&PatternGenerator::getRightFootContact, this, _1, _2),
          OneStepOfControlS,
          "PatternGenerator(" + name + ")::output(bool)::rightfootcontact")

{
  m_MotionSinceInstanciationToThisSequence.setIdentity();

  m_LocalTime = 0;
  m_count = 0;
  m_TimeStep = 0.005;
  m_DoubleSupportPhaseState = false;
  m_forceFeedBack = false;
  m_feedBackControl = false;

  m_ZMPRefPos.resize(4);
  m_ZMPRefPos.fill(0.0);
  m_ZMPRefPos(3) = 1.0;
  m_COMRefPos.resize(3);
  m_COMRefPos.fill(0.0);
  m_PrevSamplingCOMRefPos.resize(3);
  m_PrevSamplingCOMRefPos.fill(0.0);
  m_NextSamplingCOMRefPos.resize(3);
  m_NextSamplingCOMRefPos.fill(0.0);
  m_ZMPPrevious.resize(4);
  m_ZMPPrevious(3) = 1.0;
  m_dCOMRefPos.resize(3);
  m_dCOMRefPos.fill(0.0);
  m_PrevSamplingdCOMRefPos.resize(3);
  m_PrevSamplingdCOMRefPos.fill(0.0);
  m_NextSamplingdCOMRefPos.resize(3);
  m_NextSamplingdCOMRefPos.fill(0.0);
  m_ddCOMRefPos.resize(3);
  m_ddCOMRefPos.fill(0.0);
  m_PrevSamplingddCOMRefPos.resize(3);
  m_PrevSamplingddCOMRefPos.fill(0.0);
  m_NextSamplingddCOMRefPos.resize(3);
  m_NextSamplingddCOMRefPos.fill(0.0);
  m_InitZMPRefPos.resize(3);
  m_InitZMPRefPos.fill(0);
  m_InitCOMRefPos.resize(3);
  m_InitCOMRefPos.fill(0);
  m_InitWaistRefPos.resize(3);
  m_InitWaistRefPos.fill(0);
  m_InitWaistRefAtt.resize(3);
  m_InitWaistRefAtt.fill(0);
  m_dComAttitude.resize(3);
  m_dComAttitude.fill(0);
  m_ddComAttitude.resize(3);
  m_ddComAttitude.fill(0);
  m_VelocityReference.resize(3);
  m_VelocityReference.fill(0.0);
  m_trigger = false;
  m_WaistAttitude.resize(3);
  m_WaistAttitude.fill(0);
  m_ComAttitude.resize(3);
  m_ComAttitude.fill(0);
  m_WaistPosition.resize(3);
  m_WaistPosition.fill(0);
  m_WaistAttitudeAbsolute.resize(3);
  m_WaistAttitudeAbsolute.fill(0);
  m_PrevSamplingWaistAttAbs.resize(3);
  m_PrevSamplingWaistAttAbs.fill(0);
  m_NextSamplingWaistAttAbs.resize(3);
  m_NextSamplingWaistAttAbs.fill(0);
  m_WaistPositionAbsolute.resize(3);
  m_WaistPositionAbsolute.fill(0);

  m_WaistAttitudeMatrixAbsolute.setIdentity();
  m_LeftFootPosition.setIdentity();
  m_RightFootPosition.setIdentity();

  m_dotLeftFootPosition.setIdentity();
  m_dotRightFootPosition.setIdentity();

  m_InitLeftFootPosition.setIdentity();
  m_InitRightFootPosition.setIdentity();
  m_FlyingFootPosition.setIdentity();

  m_k_Waist_kp1.setIdentity();

  m_SupportFoot = 1;  // Means that we do not know which support foot it is.
  m_ReferenceFrame = WORLD_FRAME;

  sotDEBUGIN(5);

  firstSINTERN.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
  // TODO: here, the 'setConstant' destroy the pointer toward
  // function initOneStepOfControl. By calling firstSINTERN(t), whatever t,
  // nothing will happen (well, it will just return 0).
  // To initialize firstSINTERN (without destroying the pointer), use
  // firstSINTERN.setReady() instead.
  // TODO: Remove the next line: // firstSINTERN.setConstant(0);
  firstSINTERN.setReady(true);

  // OneStepOfControlS.setDependencyType(TimeDependency<int>::ALWAYS_READY);
  //  OneStepOfControlS.setConstant(0);

  OneStepOfControlS.addDependency(LeftFootCurrentPosSIN);
  OneStepOfControlS.addDependency(RightFootCurrentPosSIN);
  OneStepOfControlS.addDependency(velocitydesSIN);
  OneStepOfControlS.addDependency(triggerSIN);
  OneStepOfControlS.addDependency(firstSINTERN);
  OneStepOfControlS.addDependency(motorControlJointPositionSIN);
  OneStepOfControlS.addDependency(comSIN);
  OneStepOfControlS.addDependency(comStateSIN);
  OneStepOfControlS.addDependency(zmpSIN);
  OneStepOfControlS.addDependency(forceSIN);

  // For debug, register OSOC (not relevant for normal use).
  signalRegistration(OneStepOfControlS);

#if 0

      signalRegistration( jointPositionSIN <<
                          motorControlJointPositionSIN <<
                          ZMPPreviousControllerSIN <<
                          ZMPRefSOUT <<
                          CoMRefSOUT <<
                          dCoMRefSOUT);
      signalRegistration( dataInProcessSOUT <<
                          LeftFootCurrentPosSIN <<
                          RightFootCurrentPosSIN <<
                          LeftFootRefSOUT <<
                          RightFootRefSOUT);

      signalRegistration( SupportFootSOUT <<
                          jointWalkingErrorPositionSOUT <<
                          waistattitudeSOUT <<
                          waistpositionSOUT <<
                          waistattitudeabsoluteSOUT <<
                          waistpositionabsoluteSOUT);

      signalRegistration( comattitudeSOUT <<
                          dcomattitudeSOUT );

      signalRegistration( dotLeftFootRefSOUT <<
                          dotRightFootRefSOUT);

      signalRegistration( InitZMPRefSOUT <<
                          InitCoMRefSOUT <<
                          InitWaistPosRefSOUT <<
                          InitWaistAttRefSOUT <<
                          InitLeftFootRefSOUT <<
                          InitRightFootRefSOUT <<
                          comSIN <<
                          velocitydesSIN);
#else
  signalRegistration(dataInProcessSOUT);

  signalRegistration(jointPositionSIN << motorControlJointPositionSIN
                                      << ZMPPreviousControllerSIN << ZMPRefSOUT
                                      << CoMRefSOUT << dCoMRefSOUT
                                      << ddCoMRefSOUT);

  signalRegistration(comStateSIN << zmpSIN << forceSIN << forceSOUT);

  signalRegistration(comSIN << velocitydesSIN << triggerSIN
                            << LeftFootCurrentPosSIN << RightFootCurrentPosSIN
                            << LeftFootRefSOUT << RightFootRefSOUT);

  signalRegistration(SupportFootSOUT << jointWalkingErrorPositionSOUT
                                     << comattitudeSOUT << dcomattitudeSOUT
                                     << ddcomattitudeSOUT << waistattitudeSOUT
                                     << waistattitudematrixabsoluteSOUT);

  signalRegistration(waistpositionSOUT << waistattitudeabsoluteSOUT
                                       << waistpositionabsoluteSOUT);

  signalRegistration(dotLeftFootRefSOUT << dotRightFootRefSOUT);

  signalRegistration(InitZMPRefSOUT << InitCoMRefSOUT << InitWaistPosRefSOUT
                                    << InitWaistAttRefSOUT
                                    << InitLeftFootRefSOUT
                                    << InitRightFootRefSOUT);

  signalRegistration(leftFootContactSOUT << rightFootContactSOUT);

#endif
  initCommands();

  // init filter for force signals "to be removed"
  m_bufferForce.clear();
  int n = 10;
  double sum = 0, tmp = 0;
  m_filterWindow.resize(n + 1);
  for (int i = 0; i < n + 1; i++) {
    tmp = sin((M_PI * i) / n);
    m_filterWindow[i] = tmp * tmp;
  }

  for (int i = 0; i < n + 1; i++) sum += m_filterWindow[i];

  for (int i = 0; i < n + 1; i++) m_filterWindow[i] /= sum;

  m_initForce.resize(6);
  m_currentForces.resize(6);
  // dataInProcessSOUT.setReference( &m_dataInProcess );
  // m_wrml2urdfIndex.clear();

  sotDEBUGOUT(5);
}

bool PatternGenerator::InitState(void) {
  sotDEBUGIN(5);
  // TODO
  // Instead of (0) ie .access(0), it could be rather used:
  // .accessCopy()
  // Instead of copy value (ml::Vector pos) it could be rather
  // used reference (const ml::Vector & post)
  Vector res;
  Eigen::Matrix<double, 6, 1> lWaistPosition;
  if (m_InitPositionByRealState) {
    const Vector &pos = jointPositionSIN(m_LocalTime);

    lWaistPosition.resize(6);
    for (unsigned int i = 0; i < 6; ++i) {
      lWaistPosition(i) = pos(i);
    }
    //  m_ZMPPrevious[2] =m_AnkleSoilDistance;
    // Changed the reference frame.

    res.resize(pos.size() - 6);

    for (unsigned i = 0; i < res.size(); i++) res(i) = pos(i + 6);

    Vector lZMPPrevious = ZMPPreviousControllerSIN(m_LocalTime);
    for (unsigned int i = 0; i < 3; i++) m_ZMPPrevious[i] = lZMPPrevious(i);
  } else {
    res = motorControlJointPositionSIN(m_LocalTime);
    // for(unsigned i=0;i<res.size();i++)
    // res(m_wrml2urdfIndex[i]) = res(i);
  }

  Vector com = comSIN(m_LocalTime);

  m_JointErrorValuesForWalking.resize(res.size());

  sotDEBUG(5) << "m_LocalTime:" << m_LocalTime << endl;
  sotDEBUG(5) << "Joint Values:" << res << endl;

  try {
    Eigen::VectorXd bres;
    bres.resize(res.size());
    for (int i = 0; i < res.size(); i++) bres(i) = res(i);
    m_PGI->SetCurrentJointValues(bres);

    // Evaluate current position of the COM, ZMP and feet
    // according to the state of the robot.
    PatternGeneratorJRL::COMState lStartingCOMState;
    Eigen::Vector3d lStartingZMPPosition;

    m_PGI->EvaluateStartingState(lStartingCOMState, lStartingZMPPosition,
                                 lWaistPosition, m_InitLeftFootAbsPos,
                                 m_InitRightFootAbsPos);

    // Put inside sotHomogeneous representation
    m_InitCOMRefPos(0) = lStartingCOMState.x[0];
    m_InitCOMRefPos(1) = lStartingCOMState.y[0];
    m_InitCOMRefPos(2) = lStartingCOMState.z[0];

    m_InitZMPRefPos(0) = lStartingCOMState.x[0];
    m_InitZMPRefPos(1) = lStartingCOMState.y[0];
    m_InitZMPRefPos(2) = 0;

    if (m_InitPositionByRealState) {
      m_ZMPPrevious[0] = lStartingCOMState.x[0];
      m_ZMPPrevious[1] = lStartingCOMState.y[0];
      m_ZMPPrevious[2] = 0;
    }
    sotDEBUG(5) << "InitZMPRefPos :" << m_InitZMPRefPos << endl;

    m_InitWaistRefPos(0) = m_WaistPositionAbsolute(0) = lWaistPosition(0);
    m_InitWaistRefPos(1) = m_WaistPositionAbsolute(1) = lWaistPosition(1);
    m_InitWaistRefPos(2) = m_WaistPositionAbsolute(2) = lWaistPosition(2);

    m_InitWaistRefAtt(0) = m_WaistAttitudeAbsolute(0) = lWaistPosition(3);
    m_InitWaistRefAtt(1) = m_WaistAttitudeAbsolute(1) = lWaistPosition(4);
    m_InitWaistRefAtt(2) = m_WaistAttitudeAbsolute(2) = lWaistPosition(5);

    FromAbsoluteFootPosToDotHomogeneous(
        m_InitRightFootAbsPos, m_InitRightFootPosition, m_dotRightFootPosition);
    FromAbsoluteFootPosToDotHomogeneous(
        m_InitLeftFootAbsPos, m_InitLeftFootPosition, m_dotLeftFootPosition);

    Eigen::Matrix<double, 4, 1> newtmp, oldtmp;
    oldtmp(0) = m_InitCOMRefPos(0);
    oldtmp(1) = m_InitCOMRefPos(1);
    oldtmp(2) = m_InitCOMRefPos(2);
    oldtmp(3) = 1.0;
    newtmp = m_MotionSinceInstanciationToThisSequence * oldtmp;
    m_InitCOMRefPos(0) = newtmp(0);
    m_InitCOMRefPos(1) = newtmp(1);
    m_InitCOMRefPos(2) = newtmp(2);

    oldtmp(0) = m_InitZMPRefPos(0);
    oldtmp(1) = m_InitZMPRefPos(1);
    oldtmp(2) = m_InitZMPRefPos(2);
    newtmp = m_MotionSinceInstanciationToThisSequence * oldtmp;
    m_InitZMPRefPos(0) = newtmp(0);
    m_InitZMPRefPos(1) = newtmp(1);
    m_InitZMPRefPos(2) = newtmp(2);

    if (!m_InitPositionByRealState) {
      MatrixHomogeneous invInitLeftFootRef;
      invInitLeftFootRef = m_InitLeftFootPosition.inverse();
      m_k_Waist_kp1 = m_k_Waist_kp1 * invInitLeftFootRef;
      m_MotionSinceInstanciationToThisSequence =
          m_MotionSinceInstanciationToThisSequence * m_k_Waist_kp1;
    }

    m_k_Waist_kp1 = m_InitLeftFootPosition;

    m_InitLeftFootPosition =
        m_MotionSinceInstanciationToThisSequence * m_InitLeftFootPosition;
    m_InitRightFootPosition =
        m_MotionSinceInstanciationToThisSequence * m_InitRightFootPosition;

    m_LeftFootPosition = m_InitLeftFootPosition;
    m_RightFootPosition = m_InitRightFootPosition;

    sotDEBUG(5) << "m_InitCOMRefPos: " << m_InitCOMRefPos;

    sotDEBUG(5) << "m_InitZMPRefPos: " << m_InitZMPRefPos;
    sotDEBUG(5) << "m_LeftFootPosition: " << m_LeftFootPosition;
    sotDEBUG(5) << "m_RightFootPosition: " << m_RightFootPosition;
    sotDEBUG(5) << "m_MotionSinceInstanciationToThisSequence"
                << m_MotionSinceInstanciationToThisSequence << std::endl;

    sotDEBUG(5) << " Init Waist Ref. Position " << m_InitWaistRefPos << endl;
    sotDEBUG(5) << " Init Waist Ref. Attitude " << m_InitWaistRefAtt << endl;

    sotDEBUG(5) << "ILF :" << m_InitLeftFootPosition << " "
                << "LRF :" << m_InitRightFootPosition << endl;

  } catch (...) {
    SOT_THROW ExceptionPatternGenerator(
        ExceptionPatternGenerator::PATTERN_GENERATOR_JRL,
        "Error while setting the current joint values of the WPG.");
    return false;
  }

  m_InitPositionByRealState = false;
  sotDEBUGOUT(5);
  return true;
}

bool PatternGenerator::buildModel(void) {
  bool ok = true;
  // Name of the parameter
  std::string lparameter_name("robot_description");
  // Model of the robot inside a string.
  std::string lrobot_description;

  // Reading the parameter.
  std::string model_name("robot");

  // Search for the robot util related to robot_name.
  sot::RobotUtilShrPtr aRobotUtil = sot::getRobotUtil(model_name);

  // If does not exist then it is created.
  if (aRobotUtil == sot::RefVoidRobotUtil())
  {
    ostringstream oss;
    oss << __FILE__
        << " PatternGenerator::buildModel "
        << "The robot with name " << model_name
        << " was not found !";
    throw std::invalid_arguments(oss.str());
    ok=false;
    return false;
  }

  // Then set the robot model.
  aRobotUtil->get_parameter(parameter_name,lrobot_description);

  pinocchio::urdf::buildModelFromXML(lrobot_description,
                                     pinocchio::JointModelFreeFlyer(),
                                     m_robotModel);

  m_robotData = new pinocchio::Data(m_robotModel);
  // Creating the humanoid robot.
  m_PR = new pg::PinocchioRobot();
  m_PR->initializeRobotModelAndData(&m_robotModel, m_robotData);
  // Read xml/srdf stream
  std::ifstream srdf_stream(m_srdfFile.c_str());
  using boost::property_tree::ptree;
  ptree pt;
  try {
    read_xml(srdf_stream, pt);
    // Initialize the Right Foot
    pg::PRFoot aFoot;
    string path = "robot.specificities.feet.right.size";
    BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
      aFoot.soleHeight = v.second.get<double>("height");
      aFoot.soleWidth = v.second.get<double>("width");
      aFoot.soleDepth = v.second.get<double>("depth");
    }  // BOOST_FOREACH
    path = "robot.specificities.feet.right.anklePosition";
    BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
      aFoot.anklePosition(0) = v.second.get<double>("x");
      aFoot.anklePosition(1) = v.second.get<double>("y");
      aFoot.anklePosition(2) = v.second.get<double>("z");
    }  // BOOST_FOREACH
    pinocchio::FrameIndex ra = m_robotModel.getFrameId("r_ankle");
    aFoot.associatedAnkle = m_robotModel.frames.at(ra).parent;
    m_PR->initializeRightFoot(aFoot);
    // Initialize the Left Foot
{'fraction_non_alphanumeric': 0.13117512314771645, 'fraction_numerical': 0.01508179440919503, 'mean_word_length': 2.914616727503571, 'pattern_counts': {'":': 0, '<': 415, '<?xml version=': 0, '>': 54, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 10}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 36, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7001320', 'n_tokens_mistral': 16697, 'n_tokens_neox': 16015, 'n_words': 2371}