sot-hrp2-device.cpp 6.12 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
  mlforces (6),
42
43
  pose (),
  accelerometer_ (3),
44
  gyrometer_ (3),
45
  torques_(),
46
  baseff_ ()
47
{
48
  sotDEBUGIN(25) ;
49
  for( int i=0;i<4;++i ) { withForceSignals[i] = true; }
50
51
52
53
  signalRegistration (robotState_ << accelerometerSOUT_ << gyrometerSOUT_);
  ml::Vector data (3); data.setZero ();
  accelerometerSOUT_.setConstant (data);
  gyrometerSOUT_.setConstant (data);
54
  baseff_.resize(12);
55

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

SoTHRP2Device::~SoTHRP2Device()
{ }

75
void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
76
{
77
  sotDEBUGIN(25) ;
78
  map<string,dgsot::SensorValues>::iterator it;
79
80
  int t = stateSOUT.getTime () + 1;

81
82
  it = SensorsIn.find("forces");
  if (it!=SensorsIn.end())
83
    {
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119

      // 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);
        }
    }

  it = SensorsIn.find("attitude");
  if (it!=SensorsIn.end())
    {
      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);
    }

  it = SensorsIn.find("joints");
  if (it!=SensorsIn.end())
    {
      const vector<double>& anglesIn = it->second.getValues();
      mlRobotState.resize (anglesIn.size () + 6);
      for (unsigned i = 0; i < 6; ++i)
        mlRobotState (i) = 0.;
      updateRobotState(anglesIn);
    }

  it = SensorsIn.find("accelerometer_0");
  if (it!=SensorsIn.end())
    {
andreadelprete's avatar
andreadelprete committed
120
121
122
      const vector<double>& accelerometer =
          SensorsIn ["accelerometer_0"].getValues ();
      for (std::size_t i=0; i<3; ++i)
123
124
        accelerometer_ (i) = accelerometer [i];
      accelerometerSOUT_.setConstant (accelerometer_);
125
    }
126

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

  it = SensorsIn.find("torques");
  if (it!=SensorsIn.end())
    {
      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_);
    }
145
  
146
  sotDEBUGOUT(25);
147
148
}

149
150
151
152
153
void SoTHRP2Device::setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
  setSensors (SensorsIn);
}

154
155
156

void SoTHRP2Device::nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
157
  setSensors (SensorsIn);
158
159
160
161
}

void SoTHRP2Device::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn)
{
162
  setSensors (SensorsIn);
163
164
165
166
}

void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
{
167
  sotDEBUGIN(25) ;
168
169
170
171
  vector<double> anglesOut;
  anglesOut.resize(state_.size());

  // Integrate control
172
  increment(timestep_);
173
174

  sotDEBUG (25) << "state = " << state_ << std::endl;
175
  sotDEBUG (25) << "diff  = " << ((previousState_.size() == state_.size())?
andreadelprete's avatar
andreadelprete committed
176
                                    (state_ - previousState_) : state_ ) << std::endl;
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
  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);
  
  // 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);
  
  // Update position of freeflyer in global frame
  for (int i = 0;i < 3; ++i)
207
    baseff_[i*4+3] = freeFlyerPose () (i, 3);
208
209
  for(unsigned i = 0;i < 3; ++i)
    for(unsigned j = 0; j < 3; ++j)
210
      baseff_[i * 4 + j] = freeFlyerPose () (i, j);
211

212
  controlOut["baseff"].setValues(baseff_);
213
  sotDEBUGOUT(25) ;
214
215
}

216
void SoTHRP2Device::updateRobotState(const vector<double> &anglesIn)
217
{
218
  sotDEBUGIN(25) ;
219
  for (unsigned i = 0; i < anglesIn.size(); ++i)
220
221
    mlRobotState (i + 6) = anglesIn[i];
  robotState_.setConstant(mlRobotState);
222
  sotDEBUGOUT(25) ;
223
224
}