CoMAndFootOnlyStrategy.cpp 7.48 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
/*
2
 * Copyright 2007, 2008, 2009, 2010,
Thomas Moulard's avatar
Thomas Moulard committed
3
4
5
 *
 * Fumio    Kanehiro
 * Francois Keith
6
 * Florent  Lamiraux
Thomas Moulard's avatar
Thomas Moulard committed
7
8
9
10
11
 * Anthony  Mallet
 * Olivier  Stasse
 *
 * JRL, CNRS/AIST
 *
olivier-stasse's avatar
olivier-stasse committed
12
13
 * This file is part of jrl-walkgen.
 * jrl-walkgen is free software: you can redistribute it and/or modify
Thomas Moulard's avatar
Thomas Moulard committed
14
15
16
17
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
olivier-stasse's avatar
olivier-stasse committed
18
 * jrl-walkgen is distributed in the hope that it will be useful,
Thomas Moulard's avatar
Thomas Moulard committed
19
20
21
22
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Lesser Public License for more details.
 * You should have received a copy of the GNU Lesser General Public License
olivier-stasse's avatar
olivier-stasse committed
23
 * along with jrl-walkgen.  If not, see <http://www.gnu.org/licenses/>.
Thomas Moulard's avatar
Thomas Moulard committed
24
 *
25
 *  Research carried out within the scope of the
Thomas Moulard's avatar
Thomas Moulard committed
26
27
28
29
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */

/*! \file CoMAndFootOnlyStrategy.h
30
  \brief This object defines a global strategy object to generate
Thomas Moulard's avatar
Thomas Moulard committed
31
32
  only foot, ZMP reference and CoM trajectories position every 5 ms. */

33
34
#include <Debug.hh>
#include <GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh>
Thomas Moulard's avatar
Thomas Moulard committed
35
36
37

using namespace PatternGeneratorJRL;

38
39
CoMAndFootOnlyStrategy::CoMAndFootOnlyStrategy(
    SimplePluginManager *aSimplePluginManager)
Guilhem Saurel's avatar
Guilhem Saurel committed
40
41
    : GlobalStrategyManager(aSimplePluginManager), m_NbOfHitBottom(0),
      m_BufferSizeLimit(0) {}
Thomas Moulard's avatar
Thomas Moulard committed
42

43
CoMAndFootOnlyStrategy::~CoMAndFootOnlyStrategy() {}
Thomas Moulard's avatar
Thomas Moulard committed
44

45
46
47
int CoMAndFootOnlyStrategy::InitInterObjects(
    PinocchioRobot * /* aPR */, std::vector<ComAndFootRealization *> aCFR,
    StepStackHandler * /* aSSH */) {
Thomas Moulard's avatar
Thomas Moulard committed
48
49
50
51
  m_ComAndFootRealization = aCFR;
  return 0;
}

52
53
54
int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(
    FootAbsolutePosition &LeftFootPosition,
    FootAbsolutePosition &RightFootPosition, Eigen::VectorXd &ZMPRefPos,
Guilhem Saurel's avatar
Guilhem Saurel committed
55
56
    COMState &finalCOMPosition, Eigen::VectorXd &CurrentConfiguration,
    Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration) {
57
  ODEBUG("Begin OneGlobalStepOfControl "
58
59
         << m_LeftFootPositions->size() << " " << m_RightFootPositions->size()
         << " " << m_COMBuffer->size() << " " << m_ZMPPositions->size());
60

61
  /* The strategy of this class is simply to pull
62
     off values from the buffers. */
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
  if (m_LeftFootPositions->size() > 0) {
    LeftFootPosition = (*m_LeftFootPositions)[0];
    m_LeftFootPositions->pop_front();
  } else {
    std::cerr << "Problem on the left foot position queue: empty" << std::endl;
    return -2;
  }

  if (m_RightFootPositions->size() > 0) {
    RightFootPosition = (*m_RightFootPositions)[0];
    m_RightFootPositions->pop_front();
  } else {
    std::cerr << "Problem on the right foot position queue: empty" << std::endl;
    return -3;
  }

  if (m_COMBuffer->size() > 0) {
    finalCOMPosition = (*m_COMBuffer)[0];
    m_COMBuffer->pop_front();
  } else {
    std::cerr << "Problem on the COM queue: empty" << std::endl;
    return -4;
  }

87
88
89
90
91
92
93
94
95
96
97
98
  CurrentConfiguration[3] = finalCOMPosition.roll[0];
  CurrentConfiguration[4] = finalCOMPosition.pitch[0];
  CurrentConfiguration[5] = finalCOMPosition.yaw[0];

  CurrentVelocity[3] = finalCOMPosition.roll[1];
  CurrentVelocity[4] = finalCOMPosition.pitch[1];
  CurrentVelocity[5] = finalCOMPosition.yaw[1];

  CurrentAcceleration[3] = finalCOMPosition.roll[2];
  CurrentAcceleration[4] = finalCOMPosition.pitch[2];
  CurrentAcceleration[5] = finalCOMPosition.yaw[2];

99
100
101
102
103
104
105
106
107
108
  if (m_ZMPPositions->size() > 0) {
    ZMPPosition aZMPPosition = (*m_ZMPPositions)[0];
    ZMPRefPos(0) = aZMPPosition.px;
    ZMPRefPos(1) = aZMPPosition.py;
    ZMPRefPos(2) = aZMPPosition.pz;
    m_ZMPPositions->pop_front();
  } else {
    ODEBUG("Problem on the ZMP size: empty");
    return -5;
  }
Thomas Moulard's avatar
Thomas Moulard committed
109
110

  ODEBUG("End of OneGlobalStepOfControl"
111
112
         << m_LeftFootPositions->size() << " " << m_RightFootPositions->size()
         << " " << m_COMBuffer->size() << " " << m_ZMPPositions->size());
Thomas Moulard's avatar
Thomas Moulard committed
113
114
115
  return 0;
}

116
117
118
119
120
121
int CoMAndFootOnlyStrategy::EvaluateStartingState(
    Eigen::VectorXd &BodyAngles, COMState &aStartingCOMState,
    Eigen::Vector3d &aStartingZMPPosition,
    Eigen::Matrix<double, 6, 1> &lStartingWaistPose,
    FootAbsolutePosition &InitLeftFootPosition,
    FootAbsolutePosition &InitRightFootPosition) {
122
  Eigen::Vector3d lStartingCOMState;
Thomas Moulard's avatar
Thomas Moulard committed
123
124
125
126
127

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

128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
  std::vector<ComAndFootRealization *>::iterator itCFR;
  for (itCFR = m_ComAndFootRealization.begin();
       itCFR != m_ComAndFootRealization.end(); ++itCFR) {
    // here we use the analytical forward kinematics
    // to initialise the position of the CoM of mass according to
    // the articular position of the robot.
    (*itCFR)->InitializationCoM(BodyAngles, lStartingCOMState,
                                lStartingWaistPose, InitLeftFootPosition,
                                InitRightFootPosition);

    ODEBUG("EvaluateStartingCOM: m_StartingCOMState: " << lStartingCOMState);
    aStartingCOMState.x[0] = lStartingCOMState(0);
    aStartingCOMState.y[0] = lStartingCOMState(1);
    aStartingCOMState.z[0] = lStartingCOMState(2);
    aStartingCOMState.yaw[0] = lStartingWaistPose(5);
    aStartingCOMState.pitch[0] = lStartingWaistPose(4);
    aStartingCOMState.roll[0] = lStartingWaistPose(3);
    aStartingZMPPosition = (*itCFR)->GetCOGInitialAnkles();
  }
147

148
149
  // We assume that the robot is not moving
  // at the beginning so the zmp is the projection of the com on the ground.
150
151
  aStartingZMPPosition(0) = aStartingCOMState.x[0];
  aStartingZMPPosition(1) = aStartingCOMState.y[0];
152
  // The  altitude of the zmp depend on the altitude of the support foot.
153
154
  aStartingZMPPosition(2) =
      0.5 * (InitLeftFootPosition.z + InitRightFootPosition.z);
155

156
  ///  cerr << "YOU SHOULD INITIALIZE PROPERLY aStartingZMPosition in
157
  ///  CoMAndFootOnlyStrategy::EvaluateStartingState" <<endl;
158

159
160
161
162
  ODEBUG("com = " << aStartingCOMState);
  ODEBUG("zmp = " << aStartingZMPPosition);
  ODEBUG("lf = " << InitLeftFootPosition);
  ODEBUG("rf = " << InitRightFootPosition);
Thomas Moulard's avatar
Thomas Moulard committed
163
164
165
  return 0;
}

166
int CoMAndFootOnlyStrategy::EndOfMotion() {
Thomas Moulard's avatar
Thomas Moulard committed
167
168
169
  // Just testing one buffer.
  // They suppose to have the same behavior.

170
171
172
173
174
  if (m_LeftFootPositions->size() > m_BufferSizeLimit) {
    if (m_LeftFootPositions->size() == m_BufferSizeLimit + 1) {
      ODEBUG("LeftFootPositions position ( "
             << (*m_LeftFootPositions)[0].x << " , "
             << (*m_LeftFootPositions)[0].y << " ) ");
Thomas Moulard's avatar
Thomas Moulard committed
175
    }
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190

    m_NbOfHitBottom = 0;
    return 1;
  } else if ((m_LeftFootPositions->size() == m_BufferSizeLimit) &&
             (m_NbOfHitBottom == 0)) {
    ODEBUG("LeftFootPositions size : " << m_LeftFootPositions->size()
                                       << "Buffer size limit: "
                                       << m_BufferSizeLimit);

    m_NbOfHitBottom++;
    return 0;
  } else if ((m_LeftFootPositions->size() == m_BufferSizeLimit) &&
             (m_NbOfHitBottom > 0)) {
    return -1;
  }
Thomas Moulard's avatar
Thomas Moulard committed
191
192
193
  return 0;
}

194
195
196
197
198
199
void CoMAndFootOnlyStrategy::Setup(
    deque<ZMPPosition> &,          // aZMPPositions,
    deque<COMState> &,             // aCOMBuffer,
    deque<FootAbsolutePosition> &, // aLeftFootAbsolutePositions,
    deque<FootAbsolutePosition> &) // aRightFootAbsolutePositions)
{}
Thomas Moulard's avatar
Thomas Moulard committed
200

201
202
203
void CoMAndFootOnlyStrategy::CallMethod(std::string &,        // Method,
                                        std::istringstream &) // astrm)
{}
Thomas Moulard's avatar
Thomas Moulard committed
204

205
206
void CoMAndFootOnlyStrategy::SetTheLimitOfTheBuffer(
    unsigned int lBufferSizeLimit) {
Thomas Moulard's avatar
Thomas Moulard committed
207
208
  m_BufferSizeLimit = lBufferSizeLimit;
}