Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/device-torque-ctrl.cpp	Lines:	0	228	0.0 %
Date:	2023-06-05 17:45:50	Branches:	0	850	0.0 %


/*
 * Copyright 2014, Andrea Del Prete, LAAS-CNRS
 *
 */

#include "sot/torque_control/device-torque-ctrl.hh"

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

#include <fstream>
#include <map>
#include <pinocchio/algorithm/joint-configuration.hpp>  // integrate
#include <sot/core/debug.hh>
#include <tsid/math/constraint-base.hpp>
#include <tsid/math/utils.hpp>

using namespace std;
using namespace dynamicgraph;
using namespace sot::torque_control;
using namespace tsid;
using namespace tsid::tasks;
using namespace dynamicgraph::sot;

typedef tsid::math::ConstraintBase ConstraintBase;

const double DeviceTorqueCtrl::TIMESTEP_DEFAULT = 0.001;
const double DeviceTorqueCtrl::FORCE_SENSOR_NOISE_STD_DEV = 1e-10;

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DeviceTorqueCtrl, "DeviceTorqueCtrl");

DeviceTorqueCtrl::DeviceTorqueCtrl(std::string RobotName)
    : dgsot::Device(RobotName),
      timestep_(TIMESTEP_DEFAULT),
      m_initSucceeded(false),
      accelerometerSOUT_("DeviceTorqueCtrl(" + RobotName +
                         ")::output(vector)::accelerometer"),
      gyrometerSOUT_("DeviceTorqueCtrl(" + RobotName +
                     ")::output(vector)::gyrometer"),
      robotStateSOUT_("DeviceTorqueCtrl(" + RobotName +
                      ")::output(vector)::robotState"),
      jointsVelocitiesSOUT_("DeviceTorqueCtrl(" + RobotName +
                            ")::output(vector)::jointsVelocities"),
      jointsAccelerationsSOUT_("DeviceTorqueCtrl(" + RobotName +
                               ")::output(vector)::jointsAccelerations"),
      currentSOUT_("DeviceTorqueCtrl(" + RobotName +
                   ")::output(vector)::currents"),
      p_gainsSOUT_("DeviceTorqueCtrl(" + RobotName +
                   ")::output(vector)::p_gains"),
      d_gainsSOUT_("DeviceTorqueCtrl(" + RobotName +
                   ")::output(vector)::d_gains"),
      CONSTRUCT_SIGNAL_IN(kp_constraints, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(kd_constraints, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(gear_ratios, dynamicgraph::Vector),
      accelerometer_(3),
      gyrometer_(3),
      m_isTorqueControlled(true),
      m_numericalDamping(1e-8),
      normalDistribution_(0.0, FORCE_SENSOR_NOISE_STD_DEV),
      normalRandomNumberGenerator_(randomNumberGenerator_,
                                   normalDistribution_) {
  forcesSIN_[0] = new SignalPtr<dynamicgraph::Vector, int>(
      NULL, "DeviceTorqueCtrl::input(vector6)::inputForceRLEG");
  forcesSIN_[1] = new SignalPtr<dynamicgraph::Vector, int>(
      NULL, "DeviceTorqueCtrl::input(vector6)::inputForceLLEG");
  forcesSIN_[2] = new SignalPtr<dynamicgraph::Vector, int>(
      NULL, "DeviceTorqueCtrl::input(vector6)::inputForceRARM");
  forcesSIN_[3] = new SignalPtr<dynamicgraph::Vector, int>(
      NULL, "DeviceTorqueCtrl::input(vector6)::inputForceLARM");
  signalRegistration(accelerometerSOUT_
                     << gyrometerSOUT_ << robotStateSOUT_
                     << jointsVelocitiesSOUT_ << jointsAccelerationsSOUT_
                     << *(forcesSIN_[0]) << *(forcesSIN_[1]) << *(forcesSIN_[2])
                     << *(forcesSIN_[3]) << currentSOUT_ << p_gainsSOUT_
                     << d_gainsSOUT_ << m_kp_constraintsSIN
                     << m_kd_constraintsSIN << m_rotor_inertiasSIN
                     << m_gear_ratiosSIN);

  // set controlInputType to acceleration so that at the end of the increment
  // method the velocity is copied in the velocity output signal (and not the
  // control)
  controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;

  // initialize gyrometer and accelerometer
  dynamicgraph::Vector data(3);
  data.setZero();
  gyrometerSOUT_.setConstant(data);
  data(2) = 9.81;
  accelerometerSOUT_.setConstant(data);

  // initialize force/torque sensors
  for (int i = 0; i < 4; ++i) {
    withForceSignals[i] = true;
    wrenches_[i].resize(6);
    wrenches_[i].setZero();
    if (i == FORCE_SIGNAL_RLEG || i == FORCE_SIGNAL_LLEG)
      wrenches_[i](2) = 350.0;
    forcesSOUT[i]->setConstant(wrenches_[i]);
  }

  temp6_.resize(6);
  m_nk = 12;

  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));
  addCommand("init",
             makeCommandVoid2(*this, &DeviceTorqueCtrl::init,
                              docCommandVoid2("Initialize the entity.",
                                              "Time period in seconds (double)",
                                              "Robot reference (string)")));
}

DeviceTorqueCtrl::~DeviceTorqueCtrl() {}

void DeviceTorqueCtrl::init(const double& dt, const std::string& robotRef) {
  if (dt <= 0.0)
    return SEND_MSG("Init failed: Timestep must be positive", MSG_TYPE_ERROR);

  /* Retrieve m_robot_util  informations */
  std::string localName(robotRef);
  if (isNameInRobotUtil(localName))
    m_robot_util = getRobotUtil(localName);
  else {
    SEND_MSG("You should first initialize the entity control-manager",
             MSG_TYPE_ERROR);
    return;
  }

  m_nj = static_cast<int>(m_robot_util->m_nbJoints);

  const dynamicgraph::sot::Vector6d& kp_contact = m_kp_constraintsSIN(0);
  const dynamicgraph::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0);
  const Vector rotor_inertias = m_rotor_inertiasSIN(0);
  const Vector gear_ratios = m_gear_ratiosSIN(0);
  const std::string& urdfFile = m_robot_util->m_urdf_filename;

  try {
    vector<string> package_dirs;
    m_robot = new robots::RobotWrapper(urdfFile, package_dirs,
                                       pinocchio::JointModelFreeFlyer());
    m_data = new pinocchio::Data(m_robot->model());
    m_robot->rotor_inertias(rotor_inertias);
    m_robot->gear_ratios(gear_ratios);

    assert(m_robot->nq() == m_nj + 7);
    assert(m_robot->nv() == m_nj + 6);

    m_q.setZero(m_robot->nq());
    m_v.setZero(m_robot->nv());
    m_q_sot.setZero(m_nj + 6);
    m_v_sot.setZero(m_nj + 6);
    m_dv_sot.setZero(m_nj + 6);
    m_dvBar.setZero(m_nj + 6);
    m_tau_np6.setZero(m_nj + 6);
    m_K.setZero(m_robot->nv() + m_nk, m_robot->nv() + m_nk);
    m_k.setZero(m_robot->nv() + m_nk);
    m_f.setZero(m_nk);
    m_Jc.setZero(m_nk, m_robot->nv());
    m_dJcv.setZero(m_nk);

    m_contactRF =
        new TaskSE3Equality("contact_rfoot", *m_robot,
                            m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
    m_contactRF->Kp(kp_contact);
    m_contactRF->Kd(kd_contact);

    m_contactLF =
        new TaskSE3Equality("contact_lfoot", *m_robot,
                            m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
    m_contactLF->Kp(kp_contact);
    m_contactLF->Kd(kd_contact);
  } catch (const std::exception& e) {
    std::cout << e.what();
    return SEND_MSG("Init failed: Could load URDF :" + urdfFile,
                    MSG_TYPE_ERROR);
  }
  timestep_ = dt;
  setStateSize(m_nj + 6);
  m_initSucceeded = true;
}

void DeviceTorqueCtrl::setStateSize(const unsigned int& size) {
  assert(size == static_cast<unsigned int>(m_nj + 6));
  Device::setStateSize(size);

  base6d_encoders_.resize(size);
  base6d_encoders_.fill(0.0);
  jointsVelocities_.resize(size - 6);
  jointsVelocities_.fill(0.0);
  jointsAccelerations_ = jointsVelocities_;

  robotStateSOUT_.setConstant(base6d_encoders_);
  jointsVelocitiesSOUT_.setConstant(jointsVelocities_);
  jointsAccelerationsSOUT_.setConstant(jointsAccelerations_);
}

void DeviceTorqueCtrl::setState(const dynamicgraph::Vector& q) {
  assert(q.size() == m_nj + 6);
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot setState before initialization!");
    return;
  }
  Device::setState(q);
  m_q_sot = q;
  m_robot_util->config_sot_to_urdf(m_q_sot, m_q);

  m_robot->computeAllTerms(*m_data, m_q, m_v);
  pinocchio::SE3 H_lf = m_robot->position(
      *m_data, m_robot->model().getJointId(
                   m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
  tsid::trajectories::TrajectorySample s(12, 6);
  tsid::math::SE3ToVector(H_lf, s.pos);
  m_contactLF->setReference(s);
  SEND_MSG("Setting left foot reference to " + toString(H_lf), MSG_TYPE_DEBUG);

  pinocchio::SE3 H_rf = m_robot->position(
      *m_data, m_robot->model().getJointId(
                   m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
  tsid::math::SE3ToVector(H_rf, s.pos);
  m_contactRF->setReference(s);
  SEND_MSG("Setting right foot reference to " + toString(H_rf), MSG_TYPE_DEBUG);
  setVelocity(m_v_sot);
}

void DeviceTorqueCtrl::setVelocity(const dynamicgraph::Vector& v) {
  assert(v.size() == m_nj + 6);
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot setVelocity before initialization!");
    return;
  }
  Device::setVelocity(v);
  m_v_sot = v;
  m_robot_util->velocity_sot_to_urdf(m_q, m_v_sot, m_v);
}

void DeviceTorqueCtrl::setControlInputType(const std::string& cit) {
  if (cit == "torque") {
    m_isTorqueControlled = true;
    return;
  }
  m_isTorqueControlled = false;
  return Device::setControlInputType(cit);
}

void DeviceTorqueCtrl::computeForwardDynamics() {
  const Vector& tauDes = controlSIN.accessCopy();
  assert(tauDes.size() == m_nj);

  // compute mass matrix
  m_robot->computeAllTerms(*m_data, m_q, m_v);

  const ConstraintBase& constrRF = m_contactRF->compute(0.0, m_q, m_v, *m_data);
  const ConstraintBase& constrLF = m_contactLF->compute(0.0, m_q, m_v, *m_data);
  assert(constrRF.matrix().rows() == 6 && constrRF.matrix().cols() == m_nj + 6);
  assert(constrLF.matrix().rows() == 6 && constrLF.matrix().cols() == m_nj + 6);
  assert(constrRF.vector().size() == 6 && constrLF.vector().size() == 6);
  assert(m_Jc.rows() == 12 && m_Jc.cols() == m_nj + 6);
  assert(m_K.rows() == m_nj + 6 + 12 && m_K.cols() == m_nj + 6 + 12);
  m_Jc.topRows<6>() = constrRF.matrix();
  m_Jc.bottomRows<6>() = constrLF.matrix();
  Matrix JcT = m_Jc.transpose();
  m_dJcv.head<6>() = constrRF.vector();
  m_dJcv.tail<6>() = constrLF.vector();

  // compute constraint solution: ddqBar = - Jc^+ * dJc * dq
  m_Jc_svd.compute(m_Jc, Eigen::ComputeThinU | Eigen::ComputeFullV);
  tsid::math::solveWithDampingFromSvd(m_Jc_svd, m_dJcv, m_dvBar,
                                      m_numericalDamping);

  // compute base of null space of constraint Jacobian
  Eigen::VectorXd::Index r = (m_Jc_svd.singularValues().array() > 1e-8).count();
  m_Z = m_Jc_svd.matrixV().rightCols(m_nj + 6 - r);

  // compute constrained accelerations ddq_c = (Z^T*M*Z)^{-1}*Z^T*(S^T*tau - h -
  // M*ddqBar)
  const Matrix& M = m_robot->mass(*m_data);
  const Vector& h = m_robot->nonLinearEffects(*m_data);
  m_ZMZ = m_Z.transpose() * M * m_Z;
  m_robot_util->joints_sot_to_urdf(tauDes, m_tau_np6.tail(m_nj));
  m_dv_c = m_Z.transpose() * (m_tau_np6 - h - M * m_dvBar);
  Vector rhs = m_dv_c;
  //  m_ZMZ_chol.compute(m_ZMZ);
  //  m_ZMZ_chol.solveInPlace(m_dv_c);
  tsid::math::svdSolveWithDamping(m_ZMZ, rhs, m_dv_c, m_numericalDamping);

  if ((m_ZMZ * m_dv_c - rhs).norm() > 1e-10)
    SEND_MSG("ZMZ*dv - ZT*(tau-h-Mdv_bar) = " +
                 toString((m_ZMZ * m_dv_c - rhs).norm()),
             MSG_TYPE_DEBUG);

  // compute joint accelerations
  m_dv = m_dvBar + m_Z * m_dv_c;

  // compute contact forces
  Vector b = M * m_dv + h - m_tau_np6;
  //  const double d2 = m_numericalDamping*m_numericalDamping;
  //  const unsigned int nzsv = m_Jc_svd.nonzeroSingularValues();
  //  Eigen::VectorXd tmp(m_Jc_svd.rows());
  //  tmp.noalias() = m_Jc_svd.matrixV().leftCols(nzsv).adjoint() * b;
  //  double sv;
  //  for(int i=0; i<nzsv; i++)
  //  {
  //    sv = m_Jc_svd.singularValues()(i);
  //    tmp(i) *= sv/(sv*sv + d2);
  //  }
  //  m_f = m_Jc_svd.matrixU().leftCols(nzsv) * tmp;
  tsid::math::svdSolveWithDamping(JcT, b, m_f, m_numericalDamping);

  //  SEND_MSG("dv = "+toString(m_dv.norm()), MSG_TYPE_DEBUG);
  //  SEND_MSG("f = "+toString(m_f), MSG_TYPE_DEBUG);
  if ((JcT * m_f - b).norm() > 1e-10)
    SEND_MSG("M*dv +h - JT*f - tau = " + toString((JcT * m_f - b).norm()),
             MSG_TYPE_DEBUG);
  if ((m_Jc * m_dv - m_dJcv).norm() > 1e-10)
    SEND_MSG("Jc*dv - dJc*v = " + toString((m_Jc * m_dv - m_dJcv).norm()),
             MSG_TYPE_DEBUG);
}

void DeviceTorqueCtrl::integrate(const double& dt) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot integrate before initialization!");
    return;
  }

  if (m_isTorqueControlled) {
    computeForwardDynamics();
    // integrate
    m_q = pinocchio::integrate(m_robot->model(), m_q, dt * m_v);
    m_v += dt * m_dv;

    m_robot_util->config_urdf_to_sot(m_q, m_q_sot);
    m_robot_util->velocity_urdf_to_sot(m_q, m_v, m_v_sot);
    m_robot_util->velocity_urdf_to_sot(m_q, m_dv, m_dv_sot);

    state_ = m_q_sot;
    velocity_ = m_v_sot;
    jointsAccelerations_ = m_dv_sot.tail(m_nj);
  } else {
    Device::integrate(dt);
    if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION)
      jointsAccelerations_ = controlSIN.accessCopy().tail(m_nj);
    else
      jointsAccelerations_.setZero(m_nj);
  }

  base6d_encoders_ = state_;
  jointsVelocities_ = velocity_.tail(m_nj);

  // set the value for the encoders, joints velocities and accelerations output
  // signal
  robotStateSOUT_.setConstant(base6d_encoders_);
  jointsVelocitiesSOUT_.setConstant(jointsVelocities_);
  jointsAccelerationsSOUT_.setConstant(jointsAccelerations_);

  int time = robotStateSOUT_.getTime() + 1;
  for (int i = 0; i < 4; i++) {
    // read input force (if any)
    if (forcesSIN_[i]->isPlugged() &&
        forcesSIN_[i]->operator()(time).size() == 6)
      wrenches_[i] = forcesSIN_[i]->accessCopy();
    // add random noise
    for (int j = 0; j < 6; j++)
      temp6_(j) = wrenches_[i](j) + normalRandomNumberGenerator_();
    // set output force
    forcesSOUT[i]->setConstant(temp6_);
  }

  robotStateSOUT_.setTime(time);
  jointsVelocitiesSOUT_.setTime(time);
  jointsAccelerationsSOUT_.setTime(time);
  accelerometerSOUT_.setTime(time);
  gyrometerSOUT_.setTime(time);
  for (int i = 0; i < 4; ++i) forcesSOUT[i]->setTime(time);
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2014, Andrea Del Prete, LAAS-CNRS
3			*
4			*/
5
6			#include "sot/torque_control/device-torque-ctrl.hh"
7
8			#include <dynamic-graph/all-commands.h>
9			#include <dynamic-graph/factory.h>
10
11			#include <fstream>
12			#include <map>
13			#include <pinocchio/algorithm/joint-configuration.hpp> // integrate
14			#include <sot/core/debug.hh>
15			#include <tsid/math/constraint-base.hpp>
16			#include <tsid/math/utils.hpp>
17
18			using namespace std;
19			using namespace dynamicgraph;
20			using namespace sot::torque_control;
21			using namespace tsid;
22			using namespace tsid::tasks;
23			using namespace dynamicgraph::sot;
24
25			typedef tsid::math::ConstraintBase ConstraintBase;
26
27			const double DeviceTorqueCtrl::TIMESTEP_DEFAULT = 0.001;
28			const double DeviceTorqueCtrl::FORCE_SENSOR_NOISE_STD_DEV = 1e-10;
29
30			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DeviceTorqueCtrl, "DeviceTorqueCtrl");
31
32			DeviceTorqueCtrl::DeviceTorqueCtrl(std::string RobotName)
33			: dgsot::Device(RobotName),
34			timestep_(TIMESTEP_DEFAULT),
35			m_initSucceeded(false),
36			accelerometerSOUT_("DeviceTorqueCtrl(" + RobotName +
37			")::output(vector)::accelerometer"),
38			gyrometerSOUT_("DeviceTorqueCtrl(" + RobotName +
39			")::output(vector)::gyrometer"),
40			robotStateSOUT_("DeviceTorqueCtrl(" + RobotName +
41			")::output(vector)::robotState"),
42			jointsVelocitiesSOUT_("DeviceTorqueCtrl(" + RobotName +
43			")::output(vector)::jointsVelocities"),
44			jointsAccelerationsSOUT_("DeviceTorqueCtrl(" + RobotName +
45			")::output(vector)::jointsAccelerations"),
46			currentSOUT_("DeviceTorqueCtrl(" + RobotName +
47			")::output(vector)::currents"),
48			p_gainsSOUT_("DeviceTorqueCtrl(" + RobotName +
49			")::output(vector)::p_gains"),
50			d_gainsSOUT_("DeviceTorqueCtrl(" + RobotName +
51			")::output(vector)::d_gains"),
52			CONSTRUCT_SIGNAL_IN(kp_constraints, dynamicgraph::Vector),
53			CONSTRUCT_SIGNAL_IN(kd_constraints, dynamicgraph::Vector),
54			CONSTRUCT_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector),
55			CONSTRUCT_SIGNAL_IN(gear_ratios, dynamicgraph::Vector),
56			accelerometer_(3),
57			gyrometer_(3),
58			m_isTorqueControlled(true),
59			m_numericalDamping(1e-8),
60			normalDistribution_(0.0, FORCE_SENSOR_NOISE_STD_DEV),
61			normalRandomNumberGenerator_(randomNumberGenerator_,
62			normalDistribution_) {
63			forcesSIN_[0] = new SignalPtr<dynamicgraph::Vector, int>(
64			NULL, "DeviceTorqueCtrl::input(vector6)::inputForceRLEG");
65			forcesSIN_[1] = new SignalPtr<dynamicgraph::Vector, int>(
66			NULL, "DeviceTorqueCtrl::input(vector6)::inputForceLLEG");
67			forcesSIN_[2] = new SignalPtr<dynamicgraph::Vector, int>(
68			NULL, "DeviceTorqueCtrl::input(vector6)::inputForceRARM");
69			forcesSIN_[3] = new SignalPtr<dynamicgraph::Vector, int>(
70			NULL, "DeviceTorqueCtrl::input(vector6)::inputForceLARM");
71			signalRegistration(accelerometerSOUT_
72			<< gyrometerSOUT_ << robotStateSOUT_
73			<< jointsVelocitiesSOUT_ << jointsAccelerationsSOUT_
74			<< (forcesSIN_[0]) << (forcesSIN_[1]) << *(forcesSIN_[2])
75			<< *(forcesSIN_[3]) << currentSOUT_ << p_gainsSOUT_
76			<< d_gainsSOUT_ << m_kp_constraintsSIN
77			<< m_kd_constraintsSIN << m_rotor_inertiasSIN
78			<< m_gear_ratiosSIN);
79
80			// set controlInputType to acceleration so that at the end of the increment
81			// method the velocity is copied in the velocity output signal (and not the
82			// control)
83			controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;
84
85			// initialize gyrometer and accelerometer
86			dynamicgraph::Vector data(3);
87			data.setZero();
88			gyrometerSOUT_.setConstant(data);
89			data(2) = 9.81;
90			accelerometerSOUT_.setConstant(data);
91
92			// initialize force/torque sensors
93			for (int i = 0; i < 4; ++i) {
94			withForceSignals[i] = true;
95			wrenches_[i].resize(6);
96			wrenches_[i].setZero();
97			if (i == FORCE_SIGNAL_RLEG \|\| i == FORCE_SIGNAL_LLEG)
98			wrenches_[i](2) = 350.0;
99			forcesSOUT[i]->setConstant(wrenches_[i]);
100			}
101
102			temp6_.resize(6);
103			m_nk = 12;
104
105			using namespace dynamicgraph::command;
106			std::string docstring;
107			/* Command increment. */
108			docstring =
109			"\n Integrate dynamics for time step provided as input\n"
110			"\n take one floating point number as input\n\n";
111			addCommand("increment",
112			makeCommandVoid1((Device&)*this, &Device::increment, docstring));
113			addCommand("init",
114			makeCommandVoid2(*this, &DeviceTorqueCtrl::init,
115			docCommandVoid2("Initialize the entity.",
116			"Time period in seconds (double)",
117			"Robot reference (string)")));
118			}
119
120			DeviceTorqueCtrl::~DeviceTorqueCtrl() {}
121
122			void DeviceTorqueCtrl::init(const double& dt, const std::string& robotRef) {
123			if (dt <= 0.0)
124			return SEND_MSG("Init failed: Timestep must be positive", MSG_TYPE_ERROR);
125
126			/* Retrieve m_robot_util informations */
127			std::string localName(robotRef);
128			if (isNameInRobotUtil(localName))
129			m_robot_util = getRobotUtil(localName);
130			else {
131			SEND_MSG("You should first initialize the entity control-manager",
132			MSG_TYPE_ERROR);
133			return;
134			}
135
136			m_nj = static_cast<int>(m_robot_util->m_nbJoints);
137
138			const dynamicgraph::sot::Vector6d& kp_contact = m_kp_constraintsSIN(0);
139			const dynamicgraph::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0);
140			const Vector rotor_inertias = m_rotor_inertiasSIN(0);
141			const Vector gear_ratios = m_gear_ratiosSIN(0);
142			const std::string& urdfFile = m_robot_util->m_urdf_filename;
143
144			try {
145			vector<string> package_dirs;
146			m_robot = new robots::RobotWrapper(urdfFile, package_dirs,
147			pinocchio::JointModelFreeFlyer());
148			m_data = new pinocchio::Data(m_robot->model());
149			m_robot->rotor_inertias(rotor_inertias);
150			m_robot->gear_ratios(gear_ratios);
151
152			assert(m_robot->nq() == m_nj + 7);
153			assert(m_robot->nv() == m_nj + 6);
154
155			m_q.setZero(m_robot->nq());
156			m_v.setZero(m_robot->nv());
157			m_q_sot.setZero(m_nj + 6);
158			m_v_sot.setZero(m_nj + 6);
159			m_dv_sot.setZero(m_nj + 6);
160			m_dvBar.setZero(m_nj + 6);
161			m_tau_np6.setZero(m_nj + 6);
162			m_K.setZero(m_robot->nv() + m_nk, m_robot->nv() + m_nk);
163			m_k.setZero(m_robot->nv() + m_nk);
164			m_f.setZero(m_nk);
165			m_Jc.setZero(m_nk, m_robot->nv());
166			m_dJcv.setZero(m_nk);
167
168			m_contactRF =
169			new TaskSE3Equality("contact_rfoot", *m_robot,
170			m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
171			m_contactRF->Kp(kp_contact);
172			m_contactRF->Kd(kd_contact);
173
174			m_contactLF =
175			new TaskSE3Equality("contact_lfoot", *m_robot,
176			m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
177			m_contactLF->Kp(kp_contact);
178			m_contactLF->Kd(kd_contact);
179			} catch (const std::exception& e) {
180			std::cout << e.what();
181			return SEND_MSG("Init failed: Could load URDF :" + urdfFile,
182			MSG_TYPE_ERROR);
183			}
184			timestep_ = dt;
185			setStateSize(m_nj + 6);
186			m_initSucceeded = true;
187			}
188
189			void DeviceTorqueCtrl::setStateSize(const unsigned int& size) {
190			assert(size == static_cast<unsigned int>(m_nj + 6));
191			Device::setStateSize(size);
192
193			base6d_encoders_.resize(size);
194			base6d_encoders_.fill(0.0);
195			jointsVelocities_.resize(size - 6);
196			jointsVelocities_.fill(0.0);
197			jointsAccelerations_ = jointsVelocities_;
198
199			robotStateSOUT_.setConstant(base6d_encoders_);
200			jointsVelocitiesSOUT_.setConstant(jointsVelocities_);
201			jointsAccelerationsSOUT_.setConstant(jointsAccelerations_);
202			}
203
204			void DeviceTorqueCtrl::setState(const dynamicgraph::Vector& q) {
205			assert(q.size() == m_nj + 6);
206			if (!m_initSucceeded) {
207			SEND_WARNING_STREAM_MSG("Cannot setState before initialization!");
208			return;
209			}
210			Device::setState(q);
211			m_q_sot = q;
212			m_robot_util->config_sot_to_urdf(m_q_sot, m_q);
213
214			m_robot->computeAllTerms(*m_data, m_q, m_v);
215			pinocchio::SE3 H_lf = m_robot->position(
216			*m_data, m_robot->model().getJointId(
217			m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
218			tsid::trajectories::TrajectorySample s(12, 6);
219			tsid::math::SE3ToVector(H_lf, s.pos);
220			m_contactLF->setReference(s);
221			SEND_MSG("Setting left foot reference to " + toString(H_lf), MSG_TYPE_DEBUG);
222
223			pinocchio::SE3 H_rf = m_robot->position(
224			*m_data, m_robot->model().getJointId(
225			m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
226			tsid::math::SE3ToVector(H_rf, s.pos);
227			m_contactRF->setReference(s);
228			SEND_MSG("Setting right foot reference to " + toString(H_rf), MSG_TYPE_DEBUG);
229			setVelocity(m_v_sot);
230			}
231
232			void DeviceTorqueCtrl::setVelocity(const dynamicgraph::Vector& v) {
233			assert(v.size() == m_nj + 6);
234			if (!m_initSucceeded) {
235			SEND_WARNING_STREAM_MSG("Cannot setVelocity before initialization!");
236			return;
237			}
238			Device::setVelocity(v);
239			m_v_sot = v;
240			m_robot_util->velocity_sot_to_urdf(m_q, m_v_sot, m_v);
241			}
242
243			void DeviceTorqueCtrl::setControlInputType(const std::string& cit) {
244			if (cit == "torque") {
245			m_isTorqueControlled = true;
246			return;
247			}
248			m_isTorqueControlled = false;
249			return Device::setControlInputType(cit);
250			}
251
252			void DeviceTorqueCtrl::computeForwardDynamics() {
253			const Vector& tauDes = controlSIN.accessCopy();
254			assert(tauDes.size() == m_nj);
255
256			// compute mass matrix
257			m_robot->computeAllTerms(*m_data, m_q, m_v);
258
259			const ConstraintBase& constrRF = m_contactRF->compute(0.0, m_q, m_v, *m_data);
260			const ConstraintBase& constrLF = m_contactLF->compute(0.0, m_q, m_v, *m_data);
261			assert(constrRF.matrix().rows() == 6 && constrRF.matrix().cols() == m_nj + 6);
262			assert(constrLF.matrix().rows() == 6 && constrLF.matrix().cols() == m_nj + 6);
263			assert(constrRF.vector().size() == 6 && constrLF.vector().size() == 6);
264			assert(m_Jc.rows() == 12 && m_Jc.cols() == m_nj + 6);
265			assert(m_K.rows() == m_nj + 6 + 12 && m_K.cols() == m_nj + 6 + 12);
266			m_Jc.topRows<6>() = constrRF.matrix();
267			m_Jc.bottomRows<6>() = constrLF.matrix();
268			Matrix JcT = m_Jc.transpose();
269			m_dJcv.head<6>() = constrRF.vector();
270			m_dJcv.tail<6>() = constrLF.vector();
271
272			// compute constraint solution: ddqBar = - Jc^+ * dJc * dq
273			m_Jc_svd.compute(m_Jc, Eigen::ComputeThinU \| Eigen::ComputeFullV);
274			tsid::math::solveWithDampingFromSvd(m_Jc_svd, m_dJcv, m_dvBar,
275			m_numericalDamping);
276
277			// compute base of null space of constraint Jacobian
278			Eigen::VectorXd::Index r = (m_Jc_svd.singularValues().array() > 1e-8).count();
279			m_Z = m_Jc_svd.matrixV().rightCols(m_nj + 6 - r);
280
281			// compute constrained accelerations ddq_c = (Z^TMZ)^{-1}Z^T(S^T*tau - h -
282			// M*ddqBar)
283			const Matrix& M = m_robot->mass(*m_data);
284			const Vector& h = m_robot->nonLinearEffects(*m_data);
285			m_ZMZ = m_Z.transpose() * M * m_Z;
286			m_robot_util->joints_sot_to_urdf(tauDes, m_tau_np6.tail(m_nj));
287			m_dv_c = m_Z.transpose() * (m_tau_np6 - h - M * m_dvBar);
288			Vector rhs = m_dv_c;
289			// m_ZMZ_chol.compute(m_ZMZ);
290			// m_ZMZ_chol.solveInPlace(m_dv_c);
291			tsid::math::svdSolveWithDamping(m_ZMZ, rhs, m_dv_c, m_numericalDamping);
292
293			if ((m_ZMZ * m_dv_c - rhs).norm() > 1e-10)
294			SEND_MSG("ZMZdv - ZT(tau-h-Mdv_bar) = " +
295			toString((m_ZMZ * m_dv_c - rhs).norm()),
296			MSG_TYPE_DEBUG);
297
298			// compute joint accelerations
299			m_dv = m_dvBar + m_Z * m_dv_c;
300
301			// compute contact forces
302			Vector b = M * m_dv + h - m_tau_np6;
303			// const double d2 = m_numericalDamping*m_numericalDamping;
304			// const unsigned int nzsv = m_Jc_svd.nonzeroSingularValues();
305			// Eigen::VectorXd tmp(m_Jc_svd.rows());
306			// tmp.noalias() = m_Jc_svd.matrixV().leftCols(nzsv).adjoint() * b;
307			// double sv;
308			// for(int i=0; i<nzsv; i++)
309			// {
310			// sv = m_Jc_svd.singularValues()(i);
311			// tmp(i) = sv/(svsv + d2);
312			// }
313			// m_f = m_Jc_svd.matrixU().leftCols(nzsv) * tmp;
314			tsid::math::svdSolveWithDamping(JcT, b, m_f, m_numericalDamping);
315
316			// SEND_MSG("dv = "+toString(m_dv.norm()), MSG_TYPE_DEBUG);
317			// SEND_MSG("f = "+toString(m_f), MSG_TYPE_DEBUG);
318			if ((JcT * m_f - b).norm() > 1e-10)
319			SEND_MSG("Mdv +h - JTf - tau = " + toString((JcT * m_f - b).norm()),
320			MSG_TYPE_DEBUG);
321			if ((m_Jc * m_dv - m_dJcv).norm() > 1e-10)
322			SEND_MSG("Jcdv - dJcv = " + toString((m_Jc * m_dv - m_dJcv).norm()),
323			MSG_TYPE_DEBUG);
324			}
325
326			void DeviceTorqueCtrl::integrate(const double& dt) {
327			if (!m_initSucceeded) {
328			SEND_WARNING_STREAM_MSG("Cannot integrate before initialization!");
329			return;
330			}
331
332			if (m_isTorqueControlled) {
333			computeForwardDynamics();
334			// integrate
335			m_q = pinocchio::integrate(m_robot->model(), m_q, dt * m_v);
336			m_v += dt * m_dv;
337
338			m_robot_util->config_urdf_to_sot(m_q, m_q_sot);
339			m_robot_util->velocity_urdf_to_sot(m_q, m_v, m_v_sot);
340			m_robot_util->velocity_urdf_to_sot(m_q, m_dv, m_dv_sot);
341
342			state_ = m_q_sot;
343			velocity_ = m_v_sot;
344			jointsAccelerations_ = m_dv_sot.tail(m_nj);
345			} else {
346			Device::integrate(dt);
347			if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION)
348			jointsAccelerations_ = controlSIN.accessCopy().tail(m_nj);
349			else
350			jointsAccelerations_.setZero(m_nj);
351			}
352
353			base6d_encoders_ = state_;
354			jointsVelocities_ = velocity_.tail(m_nj);
355
356			// set the value for the encoders, joints velocities and accelerations output
357			// signal
358			robotStateSOUT_.setConstant(base6d_encoders_);
359			jointsVelocitiesSOUT_.setConstant(jointsVelocities_);
360			jointsAccelerationsSOUT_.setConstant(jointsAccelerations_);
361
362			int time = robotStateSOUT_.getTime() + 1;
363			for (int i = 0; i < 4; i++) {
364			// read input force (if any)
365			if (forcesSIN_[i]->isPlugged() &&
366			forcesSIN_[i]->operator()(time).size() == 6)
367			wrenches_[i] = forcesSIN_[i]->accessCopy();
368			// add random noise
369			for (int j = 0; j < 6; j++)
370			temp6_(j) = wrenches_[i](j) + normalRandomNumberGenerator_();
371			// set output force
372			forcesSOUT[i]->setConstant(temp6_);
373			}
374
375			robotStateSOUT_.setTime(time);
376			jointsVelocitiesSOUT_.setTime(time);
377			jointsAccelerationsSOUT_.setTime(time);
378			accelerometerSOUT_.setTime(time);
379			gyrometerSOUT_.setTime(time);
380			for (int i = 0; i < 4; ++i) forcesSOUT[i]->setTime(time);
381			}