sot-hrp2-device.cpp 7.29 KB
Newer Older
1
2
3
4
5
6
7
8
/*
 * Copyright 2011,
 *
 * Olivier Stasse
 *
 * LAAS, CNRS
 *
 * This file is part of HRP2Controller.
andreadelprete's avatar
andreadelprete committed
9
 * HRP2Controller is not a free software,
10
 * it contains information related to HRP-2 which involves
11
 * that you either purchased the proper license to have access to
12
13
14
15
16
 * those informations, or that you signed the appropriate
 * Non-Disclosure agreement.
 *
 *
 */
17
18

#include <fstream>
19
20
21
22
23
#include <map>

#include <sot/core/debug.hh>

#include "sot-hrp2-device.hh"
24
25
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
26
27
28
29
30

using namespace std;

const double SoTHRP2Device::TIMESTEP_DEFAULT = 0.005;

31
32
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTHRP2Device,"Device");

33
34
35
36
SoTHRP2Device::SoTHRP2Device(std::string RobotName):
  dgsot::Device(RobotName),
  timestep_(TIMESTEP_DEFAULT),
  previousState_ (),
37
  robotState_ ("StackOfTasks(" + RobotName + ")::output(vector)::robotState"),
38
39
40
  accelerometerSOUT_
  ("StackOfTasks(" + RobotName + ")::output(vector)::accelerometer"),
  gyrometerSOUT_ ("StackOfTasks(" + RobotName + ")::output(vector)::gyrometer"),
41
42
43
  currentSOUT_ ("StackOfTasks(" + RobotName + ")::output(vector)::currents"),
  p_gainsSOUT_ ("StackOfTasks(" + RobotName + ")::output(vector)::p_gains"),
  d_gainsSOUT_ ("StackOfTasks(" + RobotName + ")::output(vector)::d_gains"),
44
  mlforces (6),
45
46
  pose (),
  accelerometer_ (3),
47
  gyrometer_ (3),
48
  torques_(),
49
  baseff_ ()
50
{
51
  sotDEBUGIN(25) ;
52
  for( int i=0;i<4;++i ) { withForceSignals[i] = true; }
53
54
  signalRegistration (robotState_ << accelerometerSOUT_ << gyrometerSOUT_
                      << currentSOUT_ << p_gainsSOUT_ << d_gainsSOUT_);
55
56
57
  ml::Vector data (3); data.setZero ();
  accelerometerSOUT_.setConstant (data);
  gyrometerSOUT_.setConstant (data);
58
  baseff_.resize(12);
59

60
61
62
63
  using namespace dynamicgraph::command;
  std::string docstring;
  /* Command increment. */
  docstring =
andreadelprete's avatar
andreadelprete committed
64
65
66
67
68
      "\n"
      "    Integrate dynamics for time step provided as input\n"
      "\n"
      "      take one floating point number as input\n"
      "\n";
69
  addCommand("increment",
andreadelprete's avatar
andreadelprete committed
70
71
             makeCommandVoid1((Device&)*this,
                              &Device::increment, docstring));
72
73
  
  sotDEBUGOUT(25);
74
75
76
77
78
}

SoTHRP2Device::~SoTHRP2Device()
{ }

79
void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
80
{
81
  sotDEBUGIN(25) ;
82
  map<string,dgsot::SensorValues>::iterator it;
83
84
  int t = stateSOUT.getTime () + 1;

85
86
  it = SensorsIn.find("forces");
  if (it!=SensorsIn.end())
andreadelprete's avatar
andreadelprete committed
87
  {
88

andreadelprete's avatar
andreadelprete committed
89
90
91
92
93
94
95
96
    // Implements force recollection.
    const vector<double>& forcesIn = it->second.getValues();
    for(int i=0;i<4;++i)
    {
      for(int j=0;j<6;++j)
        mlforces(j) = forcesIn[i*6+j];
      forcesSOUT[i]->setConstant(mlforces);
      forcesSOUT[i]->setTime (t);
97
    }
andreadelprete's avatar
andreadelprete committed
98
  }
99
100
101

  it = SensorsIn.find("attitude");
  if (it!=SensorsIn.end())
andreadelprete's avatar
andreadelprete committed
102
103
104
105
106
107
108
109
  {
    const vector<double>& attitude = it->second.getValues ();
    for (unsigned int i = 0; i < 3; ++i)
      for (unsigned int j = 0; j < 3; ++j)
        pose (i, j) = attitude [i * 3 + j];
    attitudeSOUT.setConstant (pose);
    attitudeSOUT.setTime (t);
  }
110
111
112

  it = SensorsIn.find("joints");
  if (it!=SensorsIn.end())
andreadelprete's avatar
andreadelprete committed
113
114
115
116
117
118
119
120
121
122
  {
    const vector<double>& anglesIn = it->second.getValues();
    mlRobotState.resize (anglesIn.size () + 6);
    for (unsigned i = 0; i < 6; ++i)
      mlRobotState (i) = 0.;
    for (unsigned i = 0; i < anglesIn.size(); ++i)
      mlRobotState (i + 6) = anglesIn[i];
    robotState_.setConstant(mlRobotState);
    robotState_.setTime(t);
  }
123
124
125

  it = SensorsIn.find("accelerometer_0");
  if (it!=SensorsIn.end())
andreadelprete's avatar
andreadelprete committed
126
127
128
129
130
131
132
133
  {
    const vector<double>& accelerometer =
        SensorsIn ["accelerometer_0"].getValues ();
    for (std::size_t i=0; i<3; ++i)
      accelerometer_ (i) = accelerometer [i];
    accelerometerSOUT_.setConstant (accelerometer_);
    accelerometerSOUT_.setTime (t);
  }
134

135
136
  it = SensorsIn.find("gyrometer_0");
  if (it!=SensorsIn.end())
andreadelprete's avatar
andreadelprete committed
137
138
139
140
141
142
143
  {
    const vector<double>& gyrometer = SensorsIn ["gyrometer_0"].getValues ();
    for (std::size_t i=0; i<3; ++i)
      gyrometer_ (i) = gyrometer [i];
    gyrometerSOUT_.setConstant (gyrometer_);
    gyrometerSOUT_.setTime (t);
  }
144
145
146

  it = SensorsIn.find("torques");
  if (it!=SensorsIn.end())
andreadelprete's avatar
andreadelprete committed
147
148
149
150
151
152
153
154
  {
    const std::vector<double>& torques = SensorsIn["torques"].getValues();
    torques_.resize(torques.size());
    for(std::size_t i = 0; i < torques.size(); ++i)
      torques_ (i) = torques [i];
    pseudoTorqueSOUT.setConstant(torques_);
    pseudoTorqueSOUT.setTime(t);
  }
155
  
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
  it = SensorsIn.find("currents");
  if (it!=SensorsIn.end())
  {
    const std::vector<double>& currents = SensorsIn["currents"].getValues();
    currents_.resize(currents.size());
    for(std::size_t i = 0; i < currents.size(); ++i)
      currents_ (i) = currents[i];
    currentSOUT_.setConstant(currents_);
    currentSOUT_.setTime(t);
  }

  it = SensorsIn.find("p_gains");
  if (it!=SensorsIn.end())
  {
    const std::vector<double>& p_gains = SensorsIn["p_gains"].getValues();
    p_gains_.resize(p_gains.size());
    for(std::size_t i = 0; i < p_gains.size(); ++i)
      p_gains_ (i) = p_gains[i];
    p_gainsSOUT_.setConstant(p_gains_);
    p_gainsSOUT_.setTime(t);
  }

  it = SensorsIn.find("d_gains");
  if (it!=SensorsIn.end())
  {
    const std::vector<double>& d_gains = SensorsIn["d_gains"].getValues();
    d_gains_.resize(d_gains.size());
    for(std::size_t i = 0; i < d_gains.size(); ++i)
      d_gains_ (i) = d_gains[i];
    d_gainsSOUT_.setConstant(d_gains_);
    d_gainsSOUT_.setTime(t);
  }

189
  sotDEBUGOUT(25);
190
191
}

192
193
194
195
196
void SoTHRP2Device::setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
  setSensors (SensorsIn);
}

197
198
void SoTHRP2Device::nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
199
  setSensors (SensorsIn);
200
201
202
203
}

void SoTHRP2Device::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn)
{
204
  setSensors (SensorsIn);
205
206
207
208
}

void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
{
209
  sotDEBUGIN(25) ;
210
211
  vector<double> anglesOut;
  anglesOut.resize(state_.size());
212
  
213
  // Integrate control
214
  increment(timestep_);
215
  sotDEBUG (25) << "state = " << state_ << std::endl;
216
  sotDEBUG (25) << "diff  = " << ((previousState_.size() == state_.size())?
andreadelprete's avatar
andreadelprete committed
217
                                    (state_ - previousState_) : state_ ) << std::endl;
218
219
220
221
222
223
224
225
226
  previousState_ = state_;

  // Specify the joint values for the controller.
  if (anglesOut.size()!=state_.size()-6)
    anglesOut.resize(state_.size()-6);

  for(unsigned int i=6; i < state_.size();++i)
    anglesOut[i-6] = state_(i);
  controlOut["joints"].setValues(anglesOut);
227

228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
  // Read zmp reference from input signal if plugged
  int time = controlSIN.getTime ();
  zmpSIN.recompute (time + 1);
  // Express ZMP in free flyer reference frame
  ml::Vector zmpGlobal (4);
  for (unsigned int i = 0; i < 3; ++i)
    zmpGlobal(i) = zmpSIN(time + 1)(i);
  zmpGlobal(3) = 1.;
  dgsot::MatrixHomogeneous inversePose;
  freeFlyerPose().inverse(inversePose);
  ml::Vector localZmp = inversePose * zmpGlobal;
  vector<double> ZMPRef(3);
  for(unsigned int i=0;i<3;++i)
    ZMPRef[i] = localZmp(i);

  controlOut["zmp"].setName("zmp");
  controlOut["zmp"].setValues(ZMPRef);
245

246
247
  // Update position of freeflyer in global frame
  for (int i = 0;i < 3; ++i)
248
    baseff_[i*4+3] = freeFlyerPose () (i, 3);
249
250
  for(unsigned i = 0;i < 3; ++i)
    for(unsigned j = 0; j < 3; ++j)
251
      baseff_[i * 4 + j] = freeFlyerPose () (i, j);
252

253
  controlOut["baseff"].setValues(baseff_);
254
  sotDEBUGOUT(25) ;
255
}