sot-hrp2-device.cpp 7.32 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
  dg::Vector data (3); data.setZero ();
56 57
  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
  // Read zmp reference from input signal if plugged
  int time = controlSIN.getTime ();
  zmpSIN.recompute (time + 1);
  // Express ZMP in free flyer reference frame
232
  dg::Vector zmpGlobal (4);
233 234 235 236
  for (unsigned int i = 0; i < 3; ++i)
    zmpGlobal(i) = zmpSIN(time + 1)(i);
  zmpGlobal(3) = 1.;
  dgsot::MatrixHomogeneous inversePose;
237 238
  inversePose = freeFlyerPose().inverse(Eigen::Affine);
  dg::Vector localZmp(4); localZmp = inversePose.matrix() * zmpGlobal;
239 240 241 242 243 244
  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
}