Head

GCC Code Coverage Report

Directory:	src/		Exec	Total	Coverage
File:	src/sot-tiago-device.cpp	Lines:	0	171	0.0 %
Date:	2022-09-12 09:50:59	Branches:	0	502	0.0 %


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

#include <stdlib.h>

#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-tiago-device.txt"

#if 1
#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-tiago-device.hh"

using namespace std;

Eigen::Matrix3d rpy(const double roll, double pitch, double yaw) {
  Eigen::Matrix3d R;
  // Build rotation matrix as a vector of colums
  R(0, 0) = cos(pitch) * cos(yaw);
  R(1, 0) = cos(pitch) * sin(yaw);
  R(2, 0) = -sin(pitch);

  R(0, 1) = sin(roll) * sin(pitch) * cos(yaw) - cos(roll) * sin(yaw);
  R(1, 1) = sin(roll) * sin(pitch) * sin(yaw) + cos(roll) * cos(yaw);
  R(2, 1) = sin(roll) * cos(pitch);

  R(0, 2) = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * sin(yaw);
  R(1, 2) = cos(roll) * sin(pitch) * sin(yaw) - sin(roll) * cos(yaw);
  R(2, 2) = cos(roll) * cos(pitch);

  return R;
}

const double SoTTiagoDevice::TIMESTEP_DEFAULT = 0.001;

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTTiagoDevice, "DeviceTiago");

SoTTiagoDevice::SoTTiagoDevice(std::string RobotName)
    : dgsot::Device(RobotName),
      closedLoop_(false),
      previousState_(),
      baseff_(),
      accelerometerSOUT_("StackOfTasks(" + RobotName +
                         ")::output(vector)::accelerometer"),
      gyrometerSOUT_("StackOfTasks(" + RobotName +
                     ")::output(vector)::gyrometer"),
      currentSOUT_("StackOfTasks(" + RobotName + ")::output(vector)::currents"),
      p_gainsSOUT_("StackOfTasks(" + RobotName + ")::output(vector)::p_gains"),
      d_gainsSOUT_("StackOfTasks(" + RobotName + ")::output(vector)::d_gains"),
      dgforces_(6),
      pose(),
      accelerometer_(3),
      gyrometer_(3),
      torques_(),
      leftWheelIdx_(-1),
      rightWheelIdx_(-1) {
  RESETDEBUG5();
  timestep_ = TIMESTEP_DEFAULT;
  sotDEBUGIN(25);
  for (int i = 0; i < 4; ++i) {
    withForceSignals[i] = true;
  }
  signalRegistration(accelerometerSOUT_ << gyrometerSOUT_ << currentSOUT_
                                        << p_gainsSOUT_ << d_gainsSOUT_);
  dg::Vector data(3);
  data.setZero();
  accelerometerSOUT_.setConstant(data);
  gyrometerSOUT_.setConstant(data);
  baseff_.resize(7);
  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));

  docstring =
      "    Set the integration in closed loop (express mobile base velocity in "
      "odometry frame)\n"
      "\n"
      "      - Input: boolean\n"
      "\n";
  addCommand(
      "setClosedLoop",
      makeCommandVoid1(*this, &SoTTiagoDevice::setClosedLoop, docstring));
  sotDEBUGOUT(25);
}

void SoTTiagoDevice::setLeftWheelIndex(int idx) { leftWheelIdx_ = idx; }

void SoTTiagoDevice::setRightWheelIndex(int idx) { rightWheelIdx_ = idx; }

SoTTiagoDevice::~SoTTiagoDevice() {}

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

  it = SensorsIn.find("forces");
  if (it != SensorsIn.end()) {
    // Implements force recollection.
    const vector<double>& forcesIn = it->second.getValues();
    assert(std::div(forcesIn.size(), 6).rem == 0);
    int K = (int)forcesIn.size() / 6;
    for (int i = 0; i < K; ++i) {
      for (int j = 0; j < 6; ++j) dgforces_(j) = forcesIn[i * 6 + j];
      forcesSOUT[i]->setConstant(dgforces_);
      forcesSOUT[i]->setTime(t);
    }
  }

  // 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("motor-angles");
  if (it != SensorsIn.end()) {
    const vector<double>& anglesIn = it->second.getValues();
    dgRobotState_.resize(anglesIn.size() + 6);
    for (unsigned i = 0; i < 6; ++i) dgRobotState_(i) = 0.;
    for (unsigned i = 0; i < anglesIn.size(); ++i)
      dgRobotState_(i + 6) = anglesIn[i];
    setRobotState = true;
  }

  it = SensorsIn.find("odometry");
  if (it != SensorsIn.end()) {
    const vector<double>& odomIn = it->second.getValues();
    dgRobotState_(0) = odomIn[0];
    dgRobotState_(1) = odomIn[1];
    dgRobotState_(5) = odomIn[2];
    setRobotState = true;
  }

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

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

  if (setRobotState) {
    robotState_.setConstant(dgRobotState_);
    robotState_.setTime(t);
  }

  sotDEBUGOUT(25);
}

void SoTTiagoDevice::setupSetSensors(
    map<string, dgsot::SensorValues>& SensorsIn) {
  setSensors(SensorsIn);

  setState(robotState_);
}

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

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

void SoTTiagoDevice::getControl(map<string, dgsot::ControlValues>& controlOut) {
  ODEBUG5FULL("start");
  sotDEBUGIN(25);
  vector<double> anglesOut;

  Eigen::Matrix3d R;
  if (closedLoop_)
    R = rpy(dgRobotState_[3], dgRobotState_[4], dgRobotState_[5]);
  else
    R = rpy(state_[3], state_[4], state_[5]);

  // 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.
  anglesOut.resize(state_.size() - 6);

  for (unsigned int i = 6; i < state_.size(); ++i) anglesOut[i - 6] = state_(i);
  bool hasWheels = (leftWheelIdx_ >= 0 && rightWheelIdx_ >= 0);
  if (hasWheels) {
    // Control wheels in velocity.
    // 6 and 7 correspond to left and right wheel joints.
    anglesOut[0] = vel_control_(leftWheelIdx_);
    anglesOut[1] = vel_control_(rightWheelIdx_);
  }
  controlOut["control"].setValues(anglesOut);
  // Read zmp reference from input signal if plugged
  if (zmpSIN.isPlugged()) {
    int time = controlSIN.getTime();
    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);
}


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 TiagoController.
10			* TIAGOController is a free software,
11			*
12			*/
13
14			#include <stdlib.h>
15
16			#include <fstream>
17			#include <map>
18
19			#if DEBUG
20			#define ODEBUG(x) std::cout << x << std::endl
21			#else
22			#define ODEBUG(x)
23			#endif
24			#define ODEBUG3(x) std::cout << x << std::endl
25
26			#define DBGFILE "/tmp/sot-tiago-device.txt"
27
28			#if 1
29			#define RESETDEBUG5() \
30			{ \
31			std::ofstream DebugFile; \
32			DebugFile.open(DBGFILE, std::ofstream::out); \
33			DebugFile.close(); \
34			}
35			#define ODEBUG5FULL(x) \
36			{ \
37			std::ofstream DebugFile; \
38			DebugFile.open(DBGFILE, std::ofstream::app); \
39			DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
40			<< "):" << x << std::endl; \
41			DebugFile.close(); \
42			}
43			#define ODEBUG5(x) \
44			{ \
45			std::ofstream DebugFile; \
46			DebugFile.open(DBGFILE, std::ofstream::app); \
47			DebugFile << x << std::endl; \
48			DebugFile.close(); \
49			}
50
51			#else
52			// Void the macro
53			#define RESETDEBUG5()
54			#define ODEBUG5FULL(x)
55			#define ODEBUG5(x)
56			#endif
57
58			#include <dynamic-graph/all-commands.h>
59			#include <dynamic-graph/factory.h>
60
61			#include <sot/core/debug.hh>
62
63			#include "sot-tiago-device.hh"
64
65			using namespace std;
66
67			Eigen::Matrix3d rpy(const double roll, double pitch, double yaw) {
68			Eigen::Matrix3d R;
69			// Build rotation matrix as a vector of colums
70			R(0, 0) = cos(pitch) * cos(yaw);
71			R(1, 0) = cos(pitch) * sin(yaw);
72			R(2, 0) = -sin(pitch);
73
74			R(0, 1) = sin(roll) * sin(pitch) * cos(yaw) - cos(roll) * sin(yaw);
75			R(1, 1) = sin(roll) * sin(pitch) * sin(yaw) + cos(roll) * cos(yaw);
76			R(2, 1) = sin(roll) * cos(pitch);
77
78			R(0, 2) = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * sin(yaw);
79			R(1, 2) = cos(roll) * sin(pitch) * sin(yaw) - sin(roll) * cos(yaw);
80			R(2, 2) = cos(roll) * cos(pitch);
81
82			return R;
83			}
84
85			const double SoTTiagoDevice::TIMESTEP_DEFAULT = 0.001;
86
87			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTTiagoDevice, "DeviceTiago");
88
89			SoTTiagoDevice::SoTTiagoDevice(std::string RobotName)
90			: dgsot::Device(RobotName),
91			closedLoop_(false),
92			previousState_(),
93			baseff_(),
94			accelerometerSOUT_("StackOfTasks(" + RobotName +
95			")::output(vector)::accelerometer"),
96			gyrometerSOUT_("StackOfTasks(" + RobotName +
97			")::output(vector)::gyrometer"),
98			currentSOUT_("StackOfTasks(" + RobotName + ")::output(vector)::currents"),
99			p_gainsSOUT_("StackOfTasks(" + RobotName + ")::output(vector)::p_gains"),
100			d_gainsSOUT_("StackOfTasks(" + RobotName + ")::output(vector)::d_gains"),
101			dgforces_(6),
102			pose(),
103			accelerometer_(3),
104			gyrometer_(3),
105			torques_(),
106			leftWheelIdx_(-1),
107			rightWheelIdx_(-1) {
108			RESETDEBUG5();
109			timestep_ = TIMESTEP_DEFAULT;
110			sotDEBUGIN(25);
111			for (int i = 0; i < 4; ++i) {
112			withForceSignals[i] = true;
113			}
114			signalRegistration(accelerometerSOUT_ << gyrometerSOUT_ << currentSOUT_
115			<< p_gainsSOUT_ << d_gainsSOUT_);
116			dg::Vector data(3);
117			data.setZero();
118			accelerometerSOUT_.setConstant(data);
119			gyrometerSOUT_.setConstant(data);
120			baseff_.resize(7);
121			using namespace dynamicgraph::command;
122			std::string docstring;
123			/* Command increment. */
124			docstring =
125			"\n"
126			" Integrate dynamics for time step provided as input\n"
127			"\n"
128			" take one floating point number as input\n"
129			"\n";
130			addCommand("increment",
131			makeCommandVoid1((Device&)*this, &Device::increment, docstring));
132
133			docstring =
134			" Set the integration in closed loop (express mobile base velocity in "
135			"odometry frame)\n"
136			"\n"
137			" - Input: boolean\n"
138			"\n";
139			addCommand(
140			"setClosedLoop",
141			makeCommandVoid1(*this, &SoTTiagoDevice::setClosedLoop, docstring));
142			sotDEBUGOUT(25);
143			}
144
145			void SoTTiagoDevice::setLeftWheelIndex(int idx) { leftWheelIdx_ = idx; }
146
147			void SoTTiagoDevice::setRightWheelIndex(int idx) { rightWheelIdx_ = idx; }
148
149			SoTTiagoDevice::~SoTTiagoDevice() {}
150
151			void SoTTiagoDevice::setSensors(map<string, dgsot::SensorValues>& SensorsIn) {
152			sotDEBUGIN(25);
153			map<string, dgsot::SensorValues>::iterator it;
154			int t = stateSOUT.getTime() + 1;
155			bool setRobotState = false;
156
157			it = SensorsIn.find("forces");
158			if (it != SensorsIn.end()) {
159			// Implements force recollection.
160			const vector<double>& forcesIn = it->second.getValues();
161			assert(std::div(forcesIn.size(), 6).rem == 0);
162			int K = (int)forcesIn.size() / 6;
163			for (int i = 0; i < K; ++i) {
164			for (int j = 0; j < 6; ++j) dgforces_(j) = forcesIn[i * 6 + j];
165			forcesSOUT[i]->setConstant(dgforces_);
166			forcesSOUT[i]->setTime(t);
167			}
168			}
169
170			// TODO: Confirm if this can be made quaternion
171			it = SensorsIn.find("attitude");
172			if (it != SensorsIn.end()) {
173			const vector<double>& attitude = it->second.getValues();
174			for (unsigned int i = 0; i < 3; ++i)
175			for (unsigned int j = 0; j < 3; ++j) pose(i, j) = attitude[i * 3 + j];
176			attitudeSOUT.setConstant(pose);
177			attitudeSOUT.setTime(t);
178			}
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			for (unsigned i = 0; i < 6; ++i) dgRobotState_(i) = 0.;
185			for (unsigned i = 0; i < anglesIn.size(); ++i)
186			dgRobotState_(i + 6) = anglesIn[i];
187			setRobotState = true;
188			}
189
190			it = SensorsIn.find("odometry");
191			if (it != SensorsIn.end()) {
192			const vector<double>& odomIn = it->second.getValues();
193			dgRobotState_(0) = odomIn[0];
194			dgRobotState_(1) = odomIn[1];
195			dgRobotState_(5) = odomIn[2];
196			setRobotState = true;
197			}
198
199			it = SensorsIn.find("accelerometer_0");
200			if (it != SensorsIn.end()) {
201			const vector<double>& accelerometer =
202			SensorsIn["accelerometer_0"].getValues();
203			for (std::size_t i = 0; i < 3; ++i) accelerometer_(i) = accelerometer[i];
204			accelerometerSOUT_.setConstant(accelerometer_);
205			accelerometerSOUT_.setTime(t);
206			}
207
208			it = SensorsIn.find("gyrometer_0");
209			if (it != SensorsIn.end()) {
210			const vector<double>& gyrometer = SensorsIn["gyrometer_0"].getValues();
211			for (std::size_t i = 0; i < 3; ++i) gyrometer_(i) = gyrometer[i];
212			gyrometerSOUT_.setConstant(gyrometer_);
213			gyrometerSOUT_.setTime(t);
214			}
215
216			it = SensorsIn.find("torques");
217			if (it != SensorsIn.end()) {
218			const std::vector<double>& torques = SensorsIn["torques"].getValues();
219			torques_.resize(torques.size());
220			for (std::size_t i = 0; i < torques.size(); ++i) torques_(i) = torques[i];
221			pseudoTorqueSOUT.setConstant(torques_);
222			pseudoTorqueSOUT.setTime(t);
223			}
224
225			it = SensorsIn.find("currents");
226			if (it != SensorsIn.end()) {
227			const std::vector<double>& currents = SensorsIn["currents"].getValues();
228			currents_.resize(currents.size());
229			for (std::size_t i = 0; i < currents.size(); ++i)
230			currents_(i) = currents[i];
231			currentSOUT_.setConstant(currents_);
232			currentSOUT_.setTime(t);
233			}
234
235			it = SensorsIn.find("p_gains");
236			if (it != SensorsIn.end()) {
237			const std::vector<double>& p_gains = SensorsIn["p_gains"].getValues();
238			p_gains_.resize(p_gains.size());
239			for (std::size_t i = 0; i < p_gains.size(); ++i) p_gains_(i) = p_gains[i];
240			p_gainsSOUT_.setConstant(p_gains_);
241			p_gainsSOUT_.setTime(t);
242			}
243
244			it = SensorsIn.find("d_gains");
245			if (it != SensorsIn.end()) {
246			const std::vector<double>& d_gains = SensorsIn["d_gains"].getValues();
247			d_gains_.resize(d_gains.size());
248			for (std::size_t i = 0; i < d_gains.size(); ++i) d_gains_(i) = d_gains[i];
249			d_gainsSOUT_.setConstant(d_gains_);
250			d_gainsSOUT_.setTime(t);
251			}
252
253			if (setRobotState) {
254			robotState_.setConstant(dgRobotState_);
255			robotState_.setTime(t);
256			}
257
258			sotDEBUGOUT(25);
259			}
260
261			void SoTTiagoDevice::setupSetSensors(
262			map<string, dgsot::SensorValues>& SensorsIn) {
263			setSensors(SensorsIn);
264
265			setState(robotState_);
266			}
267
268			void SoTTiagoDevice::nominalSetSensors(
269			map<string, dgsot::SensorValues>& SensorsIn) {
270			setSensors(SensorsIn);
271			}
272
273			void SoTTiagoDevice::cleanupSetSensors(
274			map<string, dgsot::SensorValues>& SensorsIn) {
275			setSensors(SensorsIn);
276			}
277
278			void SoTTiagoDevice::getControl(map<string, dgsot::ControlValues>& controlOut) {
279			ODEBUG5FULL("start");
280			sotDEBUGIN(25);
281			vector<double> anglesOut;
282
283			Eigen::Matrix3d R;
284			if (closedLoop_)
285			R = rpy(dgRobotState_[3], dgRobotState_[4], dgRobotState_[5]);
286			else
287			R = rpy(state_[3], state_[4], state_[5]);
288
289			// Integrate control
290			increment(timestep_);
291			sotDEBUG(25) << "state = " << state_ << std::endl;
292			sotDEBUG(25) << "diff = "
293			<< ((previousState_.size() == state_.size())
294			? (state_ - previousState_)
295			: state_)
296			<< std::endl;
297			ODEBUG5FULL("state = " << state_);
298			ODEBUG5FULL("diff = " << ((previousState_.size() == state_.size())
299			? (state_ - previousState_)
300			: state_));
301			previousState_ = state_;
302
303			// Specify the joint values for the controller.
304			anglesOut.resize(state_.size() - 6);
305
306			for (unsigned int i = 6; i < state_.size(); ++i) anglesOut[i - 6] = state_(i);
307			bool hasWheels = (leftWheelIdx_ >= 0 && rightWheelIdx_ >= 0);
308			if (hasWheels) {
309			// Control wheels in velocity.
310			// 6 and 7 correspond to left and right wheel joints.
311			anglesOut[0] = vel_control_(leftWheelIdx_);
312			anglesOut[1] = vel_control_(rightWheelIdx_);
313			}
314			controlOut["control"].setValues(anglesOut);
315			// Read zmp reference from input signal if plugged
316			if (zmpSIN.isPlugged()) {
317			int time = controlSIN.getTime();
318			zmpSIN.recompute(time + 1);
319			// Express ZMP in free flyer reference frame
320			dg::Vector zmpGlobal(4);
321			for (unsigned int i = 0; i < 3; ++i) zmpGlobal(i) = zmpSIN(time + 1)(i);
322			zmpGlobal(3) = 1.;
323			dgsot::MatrixHomogeneous inversePose;
324
325			inversePose = freeFlyerPose().inverse(Eigen::Affine);
326			dg::Vector localZmp(4);
327			localZmp = inversePose.matrix() * zmpGlobal;
328			vector<double> ZMPRef(3);
329			for (unsigned int i = 0; i < 3; ++i) ZMPRef[i] = localZmp(i);
330
331			controlOut["zmp"].setName("zmp");
332			controlOut["zmp"].setValues(ZMPRef);
333			}
334
335			// Update position of freeflyer in global frame
336			Eigen::Vector3d transq_(freeFlyerPose().translation());
337			dg::sot::VectorQuaternion qt_(freeFlyerPose().linear());
338
339			// translation
340			for (int i = 0; i < 3; i++) baseff_[i] = transq_(i);
341
342			// rotation: quaternion
343			baseff_[3] = qt_.w();
344			baseff_[4] = qt_.x();
345			baseff_[5] = qt_.y();
346			baseff_[6] = qt_.z();
347
348			controlOut["baseff"].setValues(baseff_);
349			ODEBUG5FULL("end");
350			sotDEBUGOUT(25);
351			}