Head

GCC Code Coverage Report

Directory:	src/		Exec	Total	Coverage
File:	src/sot-talos-device.cpp	Lines:	0	182	0.0 %
Date:	2024-05-05 16:03:44	Branches:	0	448	0.0 %


/*
 * Copyright 2016,
 *
 * Olivier Stasse
 * Rohan Budhiraja
 *
 * LAAS, CNRS
 *
 * This file is part of TalosController.
 * TALOSController is a free software,
 *
 */

#include <fstream>
#include <map>

#if DEBUG
#define ODEBUG(x) std::cout << x << std::endl
#else
#define ODEBUG(x)
#endif
#define ODEBUG3(x) std::cout << x << std::endl

#define DBGFILE "/tmp/sot-talos-device.txt"

#if 0
#define RESETDEBUG5()                            \
  {                                              \
    std::ofstream DebugFile;                     \
    DebugFile.open(DBGFILE, std::ofstream::out); \
    DebugFile.close();                           \
  }
#define ODEBUG5FULL(x)                                               \
  {                                                                  \
    std::ofstream DebugFile;                                         \
    DebugFile.open(DBGFILE, std::ofstream::app);                     \
    DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
              << "):" << x << std::endl;                             \
    DebugFile.close();                                               \
  }
#define ODEBUG5(x)                               \
  {                                              \
    std::ofstream DebugFile;                     \
    DebugFile.open(DBGFILE, std::ofstream::app); \
    DebugFile << x << std::endl;                 \
    DebugFile.close();                           \
  }

#else
// Void the macro
#define RESETDEBUG5()
#define ODEBUG5FULL(x)
#define ODEBUG5(x)
#endif

#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>

#include <sot/core/debug.hh>

#include "sot-talos-device.hh"

using namespace std;

const double SoTTalosDevice::TIMESTEP_DEFAULT = 0.001;

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTTalosDevice, "DeviceTalos");

SoTTalosDevice::SoTTalosDevice(std::string RobotName)
    : dgsot::Device(RobotName),
      previousState_(),
      baseff_(),
      accelerometerSOUT_("Device(" + RobotName +
                         ")::output(vector)::accelerometer"),
      gyrometerSOUT_("Device(" + RobotName + ")::output(vector)::gyrometer"),
      currentsSOUT_("Device(" + RobotName + ")::output(vector)::currents"),
      joint_anglesSOUT_("Device(" + RobotName +
                        ")::output(vector)::joint_angles"),
      motor_anglesSOUT_("Device(" + RobotName +
                        ")::output(vector)::motor_angles"),
      p_gainsSOUT_("Device(" + RobotName + ")::output(vector)::p_gains"),
      d_gainsSOUT_("Device(" + RobotName + ")::output(vector)::d_gains"),
      dgforces_(6),
      accelerometer_(3),
      gyrometer_(3) {
  RESETDEBUG5();
  timestep_ = TIMESTEP_DEFAULT;
  sotDEBUGIN(25);

  for (int i = 0; i < 4; ++i) {
    withForceSignals[i] = true;
  }
  signalRegistration(accelerometerSOUT_
                     << gyrometerSOUT_ << currentsSOUT_ << joint_anglesSOUT_
                     << motor_anglesSOUT_ << p_gainsSOUT_ << d_gainsSOUT_);
  dg::Vector data(3);
  data.setZero();
  accelerometerSOUT_.setConstant(data);
  gyrometerSOUT_.setConstant(data);
  baseff_.resize(7);
  dg::Vector dataForces(6);
  dataForces.setZero();
  for (int i = 0; i < 4; i++) forcesSOUT[i]->setConstant(dataForces);

  using namespace dynamicgraph::command;
  std::string docstring;
  /* Command increment. */
  docstring =
      "\n"
      "    Integrate dynamics for time step provided as input\n"
      "\n"
      "      take one floating point number as input\n"
      "\n";
  addCommand("increment",
             makeCommandVoid1((Device&)*this, &Device::increment, docstring));

  sotDEBUGOUT(25);
}

SoTTalosDevice::~SoTTalosDevice() {}

void SoTTalosDevice::setSensorsForce(
    map<string, dgsot::SensorValues>& SensorsIn, int t) {
  int map_sot_2_urdf[4] = {2, 0, 3, 1};
  sotDEBUGIN(15);
  map<string, dgsot::SensorValues>::iterator it;
  it = SensorsIn.find("forces");
  if (it != SensorsIn.end()) {
    // Implements force recollection.
    const vector<double>& forcesIn = it->second.getValues();
    for (int i = 0; i < 4; ++i) {
      sotDEBUG(15) << "Force sensor " << i << std::endl;
      int idx_sensor = map_sot_2_urdf[i];
      for (int j = 0; j < 6; ++j) {
        dgforces_(j) = forcesIn[idx_sensor * 6 + j];
        sotDEBUG(15) << "Force value " << j << ":" << dgforces_(j) << std::endl;
      }
      forcesSOUT[i]->setConstant(dgforces_);
      forcesSOUT[i]->setTime(t);
    }
  }
  sotDEBUGIN(15);
}

void SoTTalosDevice::setSensorsIMU(map<string, dgsot::SensorValues>& SensorsIn,
                                   int t) {
  map<string, dgsot::SensorValues>::iterator it;
  // TODO: Confirm if this can be made quaternion
  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("accelerometer_0");
  if (it != SensorsIn.end()) {
    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);
  }

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

void SoTTalosDevice::setSensorsEncoders(
    map<string, dgsot::SensorValues>& SensorsIn, int t) {
  map<string, dgsot::SensorValues>::iterator it;

  it = SensorsIn.find("motor-angles");
  if (it != SensorsIn.end()) {
    const vector<double>& anglesIn = it->second.getValues();
    dgRobotState_.resize(anglesIn.size() + 6);
    motor_angles_.resize(anglesIn.size());
    for (unsigned i = 0; i < 6; ++i) dgRobotState_(i) = 0.;
    for (unsigned i = 0; i < anglesIn.size(); ++i) {
      dgRobotState_(i + 6) = anglesIn[i];
      motor_angles_(i) = anglesIn[i];
    }
    robotState_.setConstant(dgRobotState_);
    robotState_.setTime(t);
    motor_anglesSOUT_.setConstant(motor_angles_);
    motor_anglesSOUT_.setTime(t);
  }

  it = SensorsIn.find("joint-angles");
  if (it != SensorsIn.end()) {
    const vector<double>& joint_anglesIn = it->second.getValues();
    joint_angles_.resize(joint_anglesIn.size());
    for (unsigned i = 0; i < joint_anglesIn.size(); ++i)
      joint_angles_(i) = joint_anglesIn[i];
    joint_anglesSOUT_.setConstant(joint_angles_);
    joint_anglesSOUT_.setTime(t);
  }
}

void SoTTalosDevice::setSensorsVelocities(
    map<string, dgsot::SensorValues>& SensorsIn, int t) {
  map<string, dgsot::SensorValues>::iterator it;

  it = SensorsIn.find("velocities");
  if (it != SensorsIn.end()) {
    const vector<double>& velocitiesIn = it->second.getValues();
    dgRobotVelocity_.resize(velocitiesIn.size() + 6);
    for (unsigned i = 0; i < 6; ++i) dgRobotVelocity_(i) = 0.;
    for (unsigned i = 0; i < velocitiesIn.size(); ++i) {
      dgRobotVelocity_(i + 6) = velocitiesIn[i];
    }
    robotVelocity_.setConstant(dgRobotVelocity_);
    robotVelocity_.setTime(t);
  }
}

void SoTTalosDevice::setSensorsTorquesCurrents(
    map<string, dgsot::SensorValues>& SensorsIn, int t) {
  map<string, dgsot::SensorValues>::iterator it;
  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_);
    pseudoTorqueSOUT.setTime(t);
  }

  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];
    currentsSOUT_.setConstant(currents_);
    currentsSOUT_.setTime(t);
  }
}

void SoTTalosDevice::setSensorsGains(
    map<string, dgsot::SensorValues>& SensorsIn, int t) {
  map<string, dgsot::SensorValues>::iterator it;
  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);
  }
}

void SoTTalosDevice::setSensors(map<string, dgsot::SensorValues>& SensorsIn) {
  sotDEBUGIN(25);
  map<string, dgsot::SensorValues>::iterator it;
  int t = stateSOUT.getTime() + 1;

  setSensorsForce(SensorsIn, t);
  setSensorsIMU(SensorsIn, t);
  setSensorsEncoders(SensorsIn, t);
  setSensorsVelocities(SensorsIn, t);
  setSensorsTorquesCurrents(SensorsIn, t);
  setSensorsGains(SensorsIn, t);

  sotDEBUGOUT(25);
}

void SoTTalosDevice::setupSetSensors(
    map<string, dgsot::SensorValues>& SensorsIn) {
  // The first time we read the sensors, we need to copy the state of the
  // robot into the signal device.state.
  setSensors(SensorsIn);
  setState(robotState_(0));
}

void SoTTalosDevice::nominalSetSensors(
    map<string, dgsot::SensorValues>& SensorsIn) {
  setSensors(SensorsIn);
}

void SoTTalosDevice::cleanupSetSensors(
    map<string, dgsot::SensorValues>& SensorsIn) {
  setSensors(SensorsIn);
}

void SoTTalosDevice::getControl(map<string, dgsot::ControlValues>& controlOut) {

  ODEBUG5FULL("start");
  sotDEBUGIN(25);
  vector<double> anglesOut, velocityOut;
  anglesOut.resize(state_.size());
  velocityOut.resize(state_.size());

  // Integrate control
  increment(timestep_);
  sotDEBUG(25) << "state = " << state_ << std::endl;
  sotDEBUG(25) << "diff  = "
               << ((previousState_.size() == state_.size())
                       ? (state_ - previousState_)
                       : state_)
               << std::endl;
  ODEBUG5FULL("state = " << state_);
  ODEBUG5FULL("diff  = " << ((previousState_.size() == state_.size())
                                 ? (state_ - previousState_)
                                 : state_));
  previousState_ = state_;

  // Specify the joint values for the controller.
  // warning: we make here the asumption that the control signal contains the
  // velocity of the freeflyer joint. This may change in the future.
  if ((int)anglesOut.size() != state_.size() - 6){
    anglesOut.resize(state_.size() - 6);
    velocityOut.resize(state_.size() - 6);
  }

  int time = controlSIN.getTime();
  for (unsigned int i = 6; i < state_.size(); ++i){
    anglesOut[i - 6] = state_(i);
    velocityOut[i-6] = controlSIN(time)(i);
  }
  // Store in "control" the joint values
  controlOut["control"].setValues(anglesOut);
  // Store in "velocity" the joint velocity values
  controlOut["velocity"].setValues(velocityOut);
  // Read zmp reference from input signal if plugged
  zmpSIN.recompute(time + 1);
  // Express ZMP in free flyer reference frame
  dg::Vector zmpGlobal(4);
  for (unsigned int i = 0; i < 3; ++i) zmpGlobal(i) = zmpSIN(time + 1)(i);
  zmpGlobal(3) = 1.;
  dgsot::MatrixHomogeneous inversePose;

  inversePose = freeFlyerPose().inverse(Eigen::Affine);
  dg::Vector localZmp(4);
  localZmp = inversePose.matrix() * 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
  Eigen::Vector3d transq_(freeFlyerPose().translation());
  dg::sot::VectorQuaternion qt_(freeFlyerPose().linear());

  // translation
  for (int i = 0; i < 3; i++) baseff_[i] = transq_(i);

  // rotation: quaternion
  baseff_[3] = qt_.w();
  baseff_[4] = qt_.x();
  baseff_[5] = qt_.y();
  baseff_[6] = qt_.z();

  controlOut["baseff"].setValues(baseff_);
  ODEBUG5FULL("end");
  sotDEBUGOUT(25);
}

using namespace dynamicgraph::sot;

namespace dynamicgraph {
namespace sot {
#ifdef WIN32
const char* DebugTrace::DEBUG_FILENAME_DEFAULT = "c:/tmp/sot-core-traces.txt";
#else   // WIN32
const char* DebugTrace::DEBUG_FILENAME_DEFAULT = "/tmp/sot-core-traces.txt";
#endif  // WIN32

#ifdef VP_DEBUG
#ifdef WIN32
std::ofstream debugfile("C:/tmp/sot-core-traces.txt",
                        std::ios::trunc& std::ios::out);
#else   // WIN32
std::ofstream debugfile("/tmp/sot-core-traces.txt",
                        std::ios::trunc& std::ios::out);
#endif  // WIN32
#else   // VP_DEBUG

std::ofstream debugfile;

#endif  // VP_DEBUG

} /* namespace sot */
} /* namespace dynamicgraph */


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2016,
3			*
4			* Olivier Stasse
5			* Rohan Budhiraja
6			*
7			* LAAS, CNRS
8			*
9			* This file is part of TalosController.
10			* TALOSController is a free software,
11			*
12			*/
13
14			#include <fstream>
15			#include <map>
16
17			#if DEBUG
18			#define ODEBUG(x) std::cout << x << std::endl
19			#else
20			#define ODEBUG(x)
21			#endif
22			#define ODEBUG3(x) std::cout << x << std::endl
23
24			#define DBGFILE "/tmp/sot-talos-device.txt"
25
26			#if 0
27			#define RESETDEBUG5() \
28			{ \
29			std::ofstream DebugFile; \
30			DebugFile.open(DBGFILE, std::ofstream::out); \
31			DebugFile.close(); \
32			}
33			#define ODEBUG5FULL(x) \
34			{ \
35			std::ofstream DebugFile; \
36			DebugFile.open(DBGFILE, std::ofstream::app); \
37			DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
38			<< "):" << x << std::endl; \
39			DebugFile.close(); \
40			}
41			#define ODEBUG5(x) \
42			{ \
43			std::ofstream DebugFile; \
44			DebugFile.open(DBGFILE, std::ofstream::app); \
45			DebugFile << x << std::endl; \
46			DebugFile.close(); \
47			}
48
49			#else
50			// Void the macro
51			#define RESETDEBUG5()
52			#define ODEBUG5FULL(x)
53			#define ODEBUG5(x)
54			#endif
55
56			#include <dynamic-graph/all-commands.h>
57			#include <dynamic-graph/factory.h>
58
59			#include <sot/core/debug.hh>
60
61			#include "sot-talos-device.hh"
62
63			using namespace std;
64
65			const double SoTTalosDevice::TIMESTEP_DEFAULT = 0.001;
66
67			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTTalosDevice, "DeviceTalos");
68
69			SoTTalosDevice::SoTTalosDevice(std::string RobotName)
70			: dgsot::Device(RobotName),
71			previousState_(),
72			baseff_(),
73			accelerometerSOUT_("Device(" + RobotName +
74			")::output(vector)::accelerometer"),
75			gyrometerSOUT_("Device(" + RobotName + ")::output(vector)::gyrometer"),
76			currentsSOUT_("Device(" + RobotName + ")::output(vector)::currents"),
77			joint_anglesSOUT_("Device(" + RobotName +
78			")::output(vector)::joint_angles"),
79			motor_anglesSOUT_("Device(" + RobotName +
80			")::output(vector)::motor_angles"),
81			p_gainsSOUT_("Device(" + RobotName + ")::output(vector)::p_gains"),
82			d_gainsSOUT_("Device(" + RobotName + ")::output(vector)::d_gains"),
83			dgforces_(6),
84			accelerometer_(3),
85			gyrometer_(3) {
86			RESETDEBUG5();
87			timestep_ = TIMESTEP_DEFAULT;
88			sotDEBUGIN(25);
89
90			for (int i = 0; i < 4; ++i) {
91			withForceSignals[i] = true;
92			}
93			signalRegistration(accelerometerSOUT_
94			<< gyrometerSOUT_ << currentsSOUT_ << joint_anglesSOUT_
95			<< motor_anglesSOUT_ << p_gainsSOUT_ << d_gainsSOUT_);
96			dg::Vector data(3);
97			data.setZero();
98			accelerometerSOUT_.setConstant(data);
99			gyrometerSOUT_.setConstant(data);
100			baseff_.resize(7);
101			dg::Vector dataForces(6);
102			dataForces.setZero();
103			for (int i = 0; i < 4; i++) forcesSOUT[i]->setConstant(dataForces);
104
105			using namespace dynamicgraph::command;
106			std::string docstring;
107			/* Command increment. */
108			docstring =
109			"\n"
110			" Integrate dynamics for time step provided as input\n"
111			"\n"
112			" take one floating point number as input\n"
113			"\n";
114			addCommand("increment",
115			makeCommandVoid1((Device&)*this, &Device::increment, docstring));
116
117			sotDEBUGOUT(25);
118			}
119
120			SoTTalosDevice::~SoTTalosDevice() {}
121
122			void SoTTalosDevice::setSensorsForce(
123			map<string, dgsot::SensorValues>& SensorsIn, int t) {
124			int map_sot_2_urdf[4] = {2, 0, 3, 1};
125			sotDEBUGIN(15);
126			map<string, dgsot::SensorValues>::iterator it;
127			it = SensorsIn.find("forces");
128			if (it != SensorsIn.end()) {
129			// Implements force recollection.
130			const vector<double>& forcesIn = it->second.getValues();
131			for (int i = 0; i < 4; ++i) {
132			sotDEBUG(15) << "Force sensor " << i << std::endl;
133			int idx_sensor = map_sot_2_urdf[i];
134			for (int j = 0; j < 6; ++j) {
135			dgforces_(j) = forcesIn[idx_sensor * 6 + j];
136			sotDEBUG(15) << "Force value " << j << ":" << dgforces_(j) << std::endl;
137			}
138			forcesSOUT[i]->setConstant(dgforces_);
139			forcesSOUT[i]->setTime(t);
140			}
141			}
142			sotDEBUGIN(15);
143			}
144
145			void SoTTalosDevice::setSensorsIMU(map<string, dgsot::SensorValues>& SensorsIn,
146			int t) {
147			map<string, dgsot::SensorValues>::iterator it;
148			// TODO: Confirm if this can be made quaternion
149			it = SensorsIn.find("attitude");
150			if (it != SensorsIn.end()) {
151			const vector<double>& attitude = it->second.getValues();
152			for (unsigned int i = 0; i < 3; ++i)
153			for (unsigned int j = 0; j < 3; ++j) pose(i, j) = attitude[i * 3 + j];
154			attitudeSOUT.setConstant(pose);
155			attitudeSOUT.setTime(t);
156			}
157
158			it = SensorsIn.find("accelerometer_0");
159			if (it != SensorsIn.end()) {
160			const vector<double>& accelerometer =
161			SensorsIn["accelerometer_0"].getValues();
162			for (std::size_t i = 0; i < 3; ++i) accelerometer_(i) = accelerometer[i];
163			accelerometerSOUT_.setConstant(accelerometer_);
164			accelerometerSOUT_.setTime(t);
165			}
166
167			it = SensorsIn.find("gyrometer_0");
168			if (it != SensorsIn.end()) {
169			const vector<double>& gyrometer = SensorsIn["gyrometer_0"].getValues();
170			for (std::size_t i = 0; i < 3; ++i) gyrometer_(i) = gyrometer[i];
171			gyrometerSOUT_.setConstant(gyrometer_);
172			gyrometerSOUT_.setTime(t);
173			}
174			}
175
176			void SoTTalosDevice::setSensorsEncoders(
177			map<string, dgsot::SensorValues>& SensorsIn, int t) {
178			map<string, dgsot::SensorValues>::iterator it;
179
180			it = SensorsIn.find("motor-angles");
181			if (it != SensorsIn.end()) {
182			const vector<double>& anglesIn = it->second.getValues();
183			dgRobotState_.resize(anglesIn.size() + 6);
184			motor_angles_.resize(anglesIn.size());
185			for (unsigned i = 0; i < 6; ++i) dgRobotState_(i) = 0.;
186			for (unsigned i = 0; i < anglesIn.size(); ++i) {
187			dgRobotState_(i + 6) = anglesIn[i];
188			motor_angles_(i) = anglesIn[i];
189			}
190			robotState_.setConstant(dgRobotState_);
191			robotState_.setTime(t);
192			motor_anglesSOUT_.setConstant(motor_angles_);
193			motor_anglesSOUT_.setTime(t);
194			}
195
196			it = SensorsIn.find("joint-angles");
197			if (it != SensorsIn.end()) {
198			const vector<double>& joint_anglesIn = it->second.getValues();
199			joint_angles_.resize(joint_anglesIn.size());
200			for (unsigned i = 0; i < joint_anglesIn.size(); ++i)
201			joint_angles_(i) = joint_anglesIn[i];
202			joint_anglesSOUT_.setConstant(joint_angles_);
203			joint_anglesSOUT_.setTime(t);
204			}
205			}
206
207			void SoTTalosDevice::setSensorsVelocities(
208			map<string, dgsot::SensorValues>& SensorsIn, int t) {
209			map<string, dgsot::SensorValues>::iterator it;
210
211			it = SensorsIn.find("velocities");
212			if (it != SensorsIn.end()) {
213			const vector<double>& velocitiesIn = it->second.getValues();
214			dgRobotVelocity_.resize(velocitiesIn.size() + 6);
215			for (unsigned i = 0; i < 6; ++i) dgRobotVelocity_(i) = 0.;
216			for (unsigned i = 0; i < velocitiesIn.size(); ++i) {
217			dgRobotVelocity_(i + 6) = velocitiesIn[i];
218			}
219			robotVelocity_.setConstant(dgRobotVelocity_);
220			robotVelocity_.setTime(t);
221			}
222			}
223
224			void SoTTalosDevice::setSensorsTorquesCurrents(
225			map<string, dgsot::SensorValues>& SensorsIn, int t) {
226			map<string, dgsot::SensorValues>::iterator it;
227			it = SensorsIn.find("torques");
228			if (it != SensorsIn.end()) {
229			const std::vector<double>& torques = SensorsIn["torques"].getValues();
230			torques_.resize(torques.size());
231			for (std::size_t i = 0; i < torques.size(); ++i) torques_(i) = torques[i];
232			pseudoTorqueSOUT.setConstant(torques_);
233			pseudoTorqueSOUT.setTime(t);
234			}
235
236			it = SensorsIn.find("currents");
237			if (it != SensorsIn.end()) {
238			const std::vector<double>& currents = SensorsIn["currents"].getValues();
239			currents_.resize(currents.size());
240			for (std::size_t i = 0; i < currents.size(); ++i)
241			currents_(i) = currents[i];
242			currentsSOUT_.setConstant(currents_);
243			currentsSOUT_.setTime(t);
244			}
245			}
246
247			void SoTTalosDevice::setSensorsGains(
248			map<string, dgsot::SensorValues>& SensorsIn, int t) {
249			map<string, dgsot::SensorValues>::iterator it;
250			it = SensorsIn.find("p_gains");
251			if (it != SensorsIn.end()) {
252			const std::vector<double>& p_gains = SensorsIn["p_gains"].getValues();
253			p_gains_.resize(p_gains.size());
254			for (std::size_t i = 0; i < p_gains.size(); ++i) p_gains_(i) = p_gains[i];
255			p_gainsSOUT_.setConstant(p_gains_);
256			p_gainsSOUT_.setTime(t);
257			}
258
259			it = SensorsIn.find("d_gains");
260			if (it != SensorsIn.end()) {
261			const std::vector<double>& d_gains = SensorsIn["d_gains"].getValues();
262			d_gains_.resize(d_gains.size());
263			for (std::size_t i = 0; i < d_gains.size(); ++i) d_gains_(i) = d_gains[i];
264			d_gainsSOUT_.setConstant(d_gains_);
265			d_gainsSOUT_.setTime(t);
266			}
267			}
268
269			void SoTTalosDevice::setSensors(map<string, dgsot::SensorValues>& SensorsIn) {
270			sotDEBUGIN(25);
271			map<string, dgsot::SensorValues>::iterator it;
272			int t = stateSOUT.getTime() + 1;
273
274			setSensorsForce(SensorsIn, t);
275			setSensorsIMU(SensorsIn, t);
276			setSensorsEncoders(SensorsIn, t);
277			setSensorsVelocities(SensorsIn, t);
278			setSensorsTorquesCurrents(SensorsIn, t);
279			setSensorsGains(SensorsIn, t);
280
281			sotDEBUGOUT(25);
282			}
283
284			void SoTTalosDevice::setupSetSensors(
285			map<string, dgsot::SensorValues>& SensorsIn) {
286			// The first time we read the sensors, we need to copy the state of the
287			// robot into the signal device.state.
288			setSensors(SensorsIn);
289			setState(robotState_(0));
290			}
291
292			void SoTTalosDevice::nominalSetSensors(
293			map<string, dgsot::SensorValues>& SensorsIn) {
294			setSensors(SensorsIn);
295			}
296
297			void SoTTalosDevice::cleanupSetSensors(
298			map<string, dgsot::SensorValues>& SensorsIn) {
299			setSensors(SensorsIn);
300			}
301
302			void SoTTalosDevice::getControl(map<string, dgsot::ControlValues>& controlOut) {
303
304			ODEBUG5FULL("start");
305			sotDEBUGIN(25);
306			vector<double> anglesOut, velocityOut;
307			anglesOut.resize(state_.size());
308			velocityOut.resize(state_.size());
309
310			// Integrate control
311			increment(timestep_);
312			sotDEBUG(25) << "state = " << state_ << std::endl;
313			sotDEBUG(25) << "diff = "
314			<< ((previousState_.size() == state_.size())
315			? (state_ - previousState_)
316			: state_)
317			<< std::endl;
318			ODEBUG5FULL("state = " << state_);
319			ODEBUG5FULL("diff = " << ((previousState_.size() == state_.size())
320			? (state_ - previousState_)
321			: state_));
322			previousState_ = state_;
323
324			// Specify the joint values for the controller.
325			// warning: we make here the asumption that the control signal contains the
326			// velocity of the freeflyer joint. This may change in the future.
327			if ((int)anglesOut.size() != state_.size() - 6){
328			anglesOut.resize(state_.size() - 6);
329			velocityOut.resize(state_.size() - 6);
330			}
331
332			int time = controlSIN.getTime();
333			for (unsigned int i = 6; i < state_.size(); ++i){
334			anglesOut[i - 6] = state_(i);
335			velocityOut[i-6] = controlSIN(time)(i);
336			}
337			// Store in "control" the joint values
338			controlOut["control"].setValues(anglesOut);
339			// Store in "velocity" the joint velocity values
340			controlOut["velocity"].setValues(velocityOut);
341			// Read zmp reference from input signal if plugged
342			zmpSIN.recompute(time + 1);
343			// Express ZMP in free flyer reference frame
344			dg::Vector zmpGlobal(4);
345			for (unsigned int i = 0; i < 3; ++i) zmpGlobal(i) = zmpSIN(time + 1)(i);
346			zmpGlobal(3) = 1.;
347			dgsot::MatrixHomogeneous inversePose;
348
349			inversePose = freeFlyerPose().inverse(Eigen::Affine);
350			dg::Vector localZmp(4);
351			localZmp = inversePose.matrix() * zmpGlobal;
352			vector<double> ZMPRef(3);
353			for (unsigned int i = 0; i < 3; ++i) ZMPRef[i] = localZmp(i);
354
355			controlOut["zmp"].setName("zmp");
356			controlOut["zmp"].setValues(ZMPRef);
357
358			// Update position of freeflyer in global frame
359			Eigen::Vector3d transq_(freeFlyerPose().translation());
360			dg::sot::VectorQuaternion qt_(freeFlyerPose().linear());
361
362			// translation
363			for (int i = 0; i < 3; i++) baseff_[i] = transq_(i);
364
365			// rotation: quaternion
366			baseff_[3] = qt_.w();
367			baseff_[4] = qt_.x();
368			baseff_[5] = qt_.y();
369			baseff_[6] = qt_.z();
370
371			controlOut["baseff"].setValues(baseff_);
372			ODEBUG5FULL("end");
373			sotDEBUGOUT(25);
374			}
375
376			using namespace dynamicgraph::sot;
377
378			namespace dynamicgraph {
379			namespace sot {
380			#ifdef WIN32
381			const char* DebugTrace::DEBUG_FILENAME_DEFAULT = "c:/tmp/sot-core-traces.txt";
382			#else // WIN32
383			const char* DebugTrace::DEBUG_FILENAME_DEFAULT = "/tmp/sot-core-traces.txt";
384			#endif // WIN32
385
386			#ifdef VP_DEBUG
387			#ifdef WIN32
388			std::ofstream debugfile("C:/tmp/sot-core-traces.txt",
389			std::ios::trunc& std::ios::out);
390			#else // WIN32
391			std::ofstream debugfile("/tmp/sot-core-traces.txt",
392			std::ios::trunc& std::ios::out);
393			#endif // WIN32
394			#else // VP_DEBUG
395
396			std::ofstream debugfile;
397
398			#endif // VP_DEBUG
399
400			} /* namespace sot */
401			} /* namespace dynamicgraph */