Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/free-flyer-locator.cpp	Lines:	0	125	0.0 %
Date:	2023-06-05 17:45:50	Branches:	0	432	0.0 %


/*
 * Copyright 2017, Thomas Flayols, LAAS-CNRS
 *
 */

#include <dynamic-graph/factory.h>

#include <sot/core/debug.hh>
#include <sot/core/stop-watch.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/free-flyer-locator.hh>

#include "pinocchio/algorithm/frames.hpp"

namespace dynamicgraph {
namespace sot {
namespace torque_control {
namespace dynamicgraph = ::dynamicgraph;
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace std;
using namespace pinocchio;

typedef dynamicgraph::sot::Vector6d Vector6;

#define PROFILE_FREE_FLYER_COMPUTATION "Free-flyer position computation"
#define PROFILE_FREE_FLYER_VELOCITY_COMPUTATION \
  "Free-flyer velocity computation"

#define INPUT_SIGNALS m_base6d_encodersSIN << m_joint_velocitiesSIN
#define OUTPUT_SIGNALS \
  m_freeflyer_aaSOUT << m_base6dFromFoot_encodersSOUT << m_vSOUT

/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef FreeFlyerLocator EntityClassName;

/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FreeFlyerLocator, "FreeFlyerLocator");

/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
FreeFlyerLocator::FreeFlyerLocator(const std::string& name)
    : Entity(name),
      CONSTRUCT_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(joint_velocities, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector,
                             INPUT_SIGNALS),
      CONSTRUCT_SIGNAL_OUT(freeflyer_aa, dynamicgraph::Vector,
                           m_base6dFromFoot_encodersSOUT),
      CONSTRUCT_SIGNAL_OUT(base6dFromFoot_encoders, dynamicgraph::Vector,
                           m_kinematics_computationsSINNER),
      CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector,
                           m_kinematics_computationsSINNER),
      m_initSucceeded(false),
      m_model(0),
      m_data(0),
      m_robot_util(RefVoidRobotUtil()) {
  Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);

  /* Commands. */
  addCommand("init",
             makeCommandVoid1(*this, &FreeFlyerLocator::init,
                              docCommandVoid1("Initialize the entity.",
                                              "Robot reference (string)")));

  addCommand(
      "displayRobotUtil",
      makeCommandVoid0(*this, &FreeFlyerLocator::displayRobotUtil,
                       docCommandVoid0("Display the robot util data set linked "
                                       "with this free flyer locator.")));
}
FreeFlyerLocator::~FreeFlyerLocator() {
  if (m_model) delete m_model;
  if (m_data) delete m_data;
}

void FreeFlyerLocator::init(const std::string& robotRef) {
  try {
    /* Retrieve m_robot_util  informations */
    std::string localName(robotRef);
    if (isNameInRobotUtil(localName)) {
      m_robot_util = getRobotUtil(localName);
      std::cerr << "m_robot_util:" << m_robot_util << std::endl;
    } else {
      SEND_MSG(
          "You should have an entity controller manager initialized before",
          MSG_TYPE_ERROR);
      return;
    }

    m_model = new pinocchio::Model();
    m_model->name.assign("EmptyRobot");

    pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename,
                                pinocchio::JointModelFreeFlyer(), *m_model);
    assert(m_model->nv == static_cast<int>(m_robot_util->m_nbJoints + 6));
    assert(
        m_model->existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
    assert(
        m_model->existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
    m_left_foot_id =
        m_model->getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
    m_right_foot_id =
        m_model->getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
    m_q_pin.setZero(m_model->nq);
    m_q_pin[6] = 1.;  // for quaternion
    m_q_sot.setZero(m_robot_util->m_nbJoints + 6);
    m_v_pin.setZero(m_robot_util->m_nbJoints + 6);
    m_v_sot.setZero(m_robot_util->m_nbJoints + 6);
  } catch (const std::exception& e) {
    std::cout << e.what();
    return SEND_MSG(
        "Init failed: Could load URDF :" + m_robot_util->m_urdf_filename,
        MSG_TYPE_ERROR);
  }
  m_data = new pinocchio::Data(*m_model);
  m_initSucceeded = true;
}

/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */

DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot compute signal kinematics_computations before initialization!");
    return s;
  }

  const Eigen::VectorXd& q = m_base6d_encodersSIN(iter);  // n+6
  const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
  assert(q.size() == static_cast<Eigen::VectorXd::Index>(
                         m_robot_util->m_nbJoints + 6) &&
         "Unexpected size of signal base6d_encoder");
  assert(dq.size() ==
             static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
         "Unexpected size of signal joint_velocities");

  /* convert sot to pinocchio joint order */
  m_robot_util->joints_sot_to_urdf(q.tail(m_robot_util->m_nbJoints),
                                   m_q_pin.tail(m_robot_util->m_nbJoints));
  m_robot_util->joints_sot_to_urdf(dq, m_v_pin.tail(m_robot_util->m_nbJoints));

  /* Compute kinematic and return q with freeflyer */
  pinocchio::forwardKinematics(*m_model, *m_data, m_q_pin, m_v_pin);
  pinocchio::framesForwardKinematics(*m_model, *m_data);

  return s;
}

DEFINE_SIGNAL_OUT_FUNCTION(base6dFromFoot_encoders, dynamicgraph::Vector) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot compute signal base6dFromFoot_encoders before initialization!");
    return s;
  }
  if (s.size() !=
      static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
    s.resize(m_robot_util->m_nbJoints + 6);

  m_kinematics_computationsSINNER(iter);

  getProfiler().start(PROFILE_FREE_FLYER_COMPUTATION);
  {
    const Eigen::VectorXd& q = m_base6d_encodersSIN(iter);  // n+6
    assert(q.size() == static_cast<Eigen::VectorXd::Index>(
                           m_robot_util->m_nbJoints + 6) &&
           "Unexpected size of signal base6d_encoder");

    /* Compute kinematic and return q with freeflyer */
    const pinocchio::SE3 iMo1(m_data->oMf[m_left_foot_id].inverse());
    const pinocchio::SE3 iMo2(m_data->oMf[m_right_foot_id].inverse());
    // Average in SE3
    const pinocchio::SE3::Vector3 w(0.5 * (pinocchio::log3(iMo1.rotation()) +
                                           pinocchio::log3(iMo2.rotation())));
    m_Mff = pinocchio::SE3(pinocchio::exp3(w),
                           0.5 * (iMo1.translation() + iMo2.translation()));

    // due to distance from ankle to ground
    Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(
        &m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ[0]);

    m_q_sot.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
    base_se3_to_sot(m_Mff.translation() - righ_foot_sole_xyz, m_Mff.rotation(),
                    m_q_sot.head<6>());

    s = m_q_sot;
  }
  getProfiler().stop(PROFILE_FREE_FLYER_COMPUTATION);

  return s;
}

DEFINE_SIGNAL_OUT_FUNCTION(freeflyer_aa, dynamicgraph::Vector) {
  m_base6dFromFoot_encodersSOUT(iter);
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot compute signal freeflyer_aa before initialization!");
    return s;
  }
  // oMi is has been calulated before since we depend on base6dFromFoot_encoders
  // signal. just read the data, convert to axis angle
  if (s.size() != 6) s.resize(6);
  //~ const pinocchio::SE3 & iMo = m_data->oMi[31].inverse();
  const Eigen::AngleAxisd aa(m_Mff.rotation());
  dynamicgraph::sot::Vector6d freeflyer;
  freeflyer << m_Mff.translation(), aa.axis() * aa.angle();

  // due to distance from ankle to ground
  Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(
      &m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ[0]);
  freeflyer.head<3>() -= righ_foot_sole_xyz;

  s = freeflyer;
  return s;
}

DEFINE_SIGNAL_OUT_FUNCTION(v, dynamicgraph::Vector) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
    return s;
  }
  if (s.size() !=
      static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
    s.resize(m_robot_util->m_nbJoints + 6);

  m_kinematics_computationsSINNER(iter);

  getProfiler().start(PROFILE_FREE_FLYER_VELOCITY_COMPUTATION);
  {
    const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
    assert(dq.size() ==
               static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
           "Unexpected size of signal joint_velocities");

    /* Compute foot velocities */
    const Frame& f_lf = m_model->frames[m_left_foot_id];
    const Motion v_lf_local = f_lf.placement.actInv(m_data->v[f_lf.parent]);
    const Vector6 v_lf = m_data->oMf[m_left_foot_id].act(v_lf_local).toVector();

    const Frame& f_rf = m_model->frames[m_right_foot_id];
    const Motion v_rf_local = f_rf.placement.actInv(m_data->v[f_rf.parent]);
    const Vector6 v_rf =
        m_data->oMf[m_right_foot_id].act(v_rf_local).toVector();

    m_v_sot.head<6>() = -0.5 * (v_lf + v_rf);
    m_v_sot.tail(m_robot_util->m_nbJoints) = dq;

    s = m_v_sot;
  }
  getProfiler().stop(PROFILE_FREE_FLYER_VELOCITY_COMPUTATION);

  return s;
}

/* --- COMMANDS ---------------------------------------------------------- */
void FreeFlyerLocator::displayRobotUtil() { m_robot_util->display(std::cout); }

/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */

void FreeFlyerLocator::display(std::ostream& os) const {
  os << "FreeFlyerLocator " << getName();
  try {
    getProfiler().report_all(3, os);
  } catch (ExceptionSignal e) {
  }
}
}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2017, Thomas Flayols, LAAS-CNRS
3			*
4			*/
5
6			#include <dynamic-graph/factory.h>
7
8			#include <sot/core/debug.hh>
9			#include <sot/core/stop-watch.hh>
10			#include <sot/torque_control/commands-helper.hh>
11			#include <sot/torque_control/free-flyer-locator.hh>
12
13			#include "pinocchio/algorithm/frames.hpp"
14
15			namespace dynamicgraph {
16			namespace sot {
17			namespace torque_control {
18			namespace dynamicgraph = ::dynamicgraph;
19			using namespace dynamicgraph;
20			using namespace dynamicgraph::command;
21			using namespace std;
22			using namespace pinocchio;
23
24			typedef dynamicgraph::sot::Vector6d Vector6;
25
26			#define PROFILE_FREE_FLYER_COMPUTATION "Free-flyer position computation"
27			#define PROFILE_FREE_FLYER_VELOCITY_COMPUTATION \
28			"Free-flyer velocity computation"
29
30			#define INPUT_SIGNALS m_base6d_encodersSIN << m_joint_velocitiesSIN
31			#define OUTPUT_SIGNALS \
32			m_freeflyer_aaSOUT << m_base6dFromFoot_encodersSOUT << m_vSOUT
33
34			/// Define EntityClassName here rather than in the header file
35			/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
36			typedef FreeFlyerLocator EntityClassName;
37
38			/* --- DG FACTORY ---------------------------------------------------- */
39			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FreeFlyerLocator, "FreeFlyerLocator");
40
41			/* ------------------------------------------------------------------- */
42			/* --- CONSTRUCTION -------------------------------------------------- */
43			/* ------------------------------------------------------------------- */
44			FreeFlyerLocator::FreeFlyerLocator(const std::string& name)
45			: Entity(name),
46			CONSTRUCT_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector),
47			CONSTRUCT_SIGNAL_IN(joint_velocities, dynamicgraph::Vector),
48			CONSTRUCT_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector,
49			INPUT_SIGNALS),
50			CONSTRUCT_SIGNAL_OUT(freeflyer_aa, dynamicgraph::Vector,
51			m_base6dFromFoot_encodersSOUT),
52			CONSTRUCT_SIGNAL_OUT(base6dFromFoot_encoders, dynamicgraph::Vector,
53			m_kinematics_computationsSINNER),
54			CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector,
55			m_kinematics_computationsSINNER),
56			m_initSucceeded(false),
57			m_model(0),
58			m_data(0),
59			m_robot_util(RefVoidRobotUtil()) {
60			Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
61
62			/* Commands. */
63			addCommand("init",
64			makeCommandVoid1(*this, &FreeFlyerLocator::init,
65			docCommandVoid1("Initialize the entity.",
66			"Robot reference (string)")));
67
68			addCommand(
69			"displayRobotUtil",
70			makeCommandVoid0(*this, &FreeFlyerLocator::displayRobotUtil,
71			docCommandVoid0("Display the robot util data set linked "
72			"with this free flyer locator.")));
73			}
74			FreeFlyerLocator::~FreeFlyerLocator() {
75			if (m_model) delete m_model;
76			if (m_data) delete m_data;
77			}
78
79			void FreeFlyerLocator::init(const std::string& robotRef) {
80			try {
81			/* Retrieve m_robot_util informations */
82			std::string localName(robotRef);
83			if (isNameInRobotUtil(localName)) {
84			m_robot_util = getRobotUtil(localName);
85			std::cerr << "m_robot_util:" << m_robot_util << std::endl;
86			} else {
87			SEND_MSG(
88			"You should have an entity controller manager initialized before",
89			MSG_TYPE_ERROR);
90			return;
91			}
92
93			m_model = new pinocchio::Model();
94			m_model->name.assign("EmptyRobot");
95
96			pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename,
97			pinocchio::JointModelFreeFlyer(), *m_model);
98			assert(m_model->nv == static_cast<int>(m_robot_util->m_nbJoints + 6));
99			assert(
100			m_model->existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
101			assert(
102			m_model->existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
103			m_left_foot_id =
104			m_model->getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
105			m_right_foot_id =
106			m_model->getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
107			m_q_pin.setZero(m_model->nq);
108			m_q_pin[6] = 1.; // for quaternion
109			m_q_sot.setZero(m_robot_util->m_nbJoints + 6);
110			m_v_pin.setZero(m_robot_util->m_nbJoints + 6);
111			m_v_sot.setZero(m_robot_util->m_nbJoints + 6);
112			} catch (const std::exception& e) {
113			std::cout << e.what();
114			return SEND_MSG(
115			"Init failed: Could load URDF :" + m_robot_util->m_urdf_filename,
116			MSG_TYPE_ERROR);
117			}
118			m_data = new pinocchio::Data(*m_model);
119			m_initSucceeded = true;
120			}
121
122			/* ------------------------------------------------------------------- */
123			/* --- SIGNALS ------------------------------------------------------- */
124			/* ------------------------------------------------------------------- */
125
126			DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector) {
127			if (!m_initSucceeded) {
128			SEND_WARNING_STREAM_MSG(
129			"Cannot compute signal kinematics_computations before initialization!");
130			return s;
131			}
132
133			const Eigen::VectorXd& q = m_base6d_encodersSIN(iter); // n+6
134			const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
135			assert(q.size() == static_cast<Eigen::VectorXd::Index>(
136			m_robot_util->m_nbJoints + 6) &&
137			"Unexpected size of signal base6d_encoder");
138			assert(dq.size() ==
139			static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
140			"Unexpected size of signal joint_velocities");
141
142			/* convert sot to pinocchio joint order */
143			m_robot_util->joints_sot_to_urdf(q.tail(m_robot_util->m_nbJoints),
144			m_q_pin.tail(m_robot_util->m_nbJoints));
145			m_robot_util->joints_sot_to_urdf(dq, m_v_pin.tail(m_robot_util->m_nbJoints));
146
147			/* Compute kinematic and return q with freeflyer */
148			pinocchio::forwardKinematics(m_model, m_data, m_q_pin, m_v_pin);
149			pinocchio::framesForwardKinematics(m_model, m_data);
150
151			return s;
152			}
153
154			DEFINE_SIGNAL_OUT_FUNCTION(base6dFromFoot_encoders, dynamicgraph::Vector) {
155			if (!m_initSucceeded) {
156			SEND_WARNING_STREAM_MSG(
157			"Cannot compute signal base6dFromFoot_encoders before initialization!");
158			return s;
159			}
160			if (s.size() !=
161			static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
162			s.resize(m_robot_util->m_nbJoints + 6);
163
164			m_kinematics_computationsSINNER(iter);
165
166			getProfiler().start(PROFILE_FREE_FLYER_COMPUTATION);
167			{
168			const Eigen::VectorXd& q = m_base6d_encodersSIN(iter); // n+6
169			assert(q.size() == static_cast<Eigen::VectorXd::Index>(
170			m_robot_util->m_nbJoints + 6) &&
171			"Unexpected size of signal base6d_encoder");
172
173			/* Compute kinematic and return q with freeflyer */
174			const pinocchio::SE3 iMo1(m_data->oMf[m_left_foot_id].inverse());
175			const pinocchio::SE3 iMo2(m_data->oMf[m_right_foot_id].inverse());
176			// Average in SE3
177			const pinocchio::SE3::Vector3 w(0.5 * (pinocchio::log3(iMo1.rotation()) +
178			pinocchio::log3(iMo2.rotation())));
179			m_Mff = pinocchio::SE3(pinocchio::exp3(w),
180			0.5 * (iMo1.translation() + iMo2.translation()));
181
182			// due to distance from ankle to ground
183			Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(
184			&m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ[0]);
185
186			m_q_sot.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
187			base_se3_to_sot(m_Mff.translation() - righ_foot_sole_xyz, m_Mff.rotation(),
188			m_q_sot.head<6>());
189
190			s = m_q_sot;
191			}
192			getProfiler().stop(PROFILE_FREE_FLYER_COMPUTATION);
193
194			return s;
195			}
196
197			DEFINE_SIGNAL_OUT_FUNCTION(freeflyer_aa, dynamicgraph::Vector) {
198			m_base6dFromFoot_encodersSOUT(iter);
199			if (!m_initSucceeded) {
200			SEND_WARNING_STREAM_MSG(
201			"Cannot compute signal freeflyer_aa before initialization!");
202			return s;
203			}
204			// oMi is has been calulated before since we depend on base6dFromFoot_encoders
205			// signal. just read the data, convert to axis angle
206			if (s.size() != 6) s.resize(6);
207			//~ const pinocchio::SE3 & iMo = m_data->oMi[31].inverse();
208			const Eigen::AngleAxisd aa(m_Mff.rotation());
209			dynamicgraph::sot::Vector6d freeflyer;
210			freeflyer << m_Mff.translation(), aa.axis() * aa.angle();
211
212			// due to distance from ankle to ground
213			Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(
214			&m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ[0]);
215			freeflyer.head<3>() -= righ_foot_sole_xyz;
216
217			s = freeflyer;
218			return s;
219			}
220
221			DEFINE_SIGNAL_OUT_FUNCTION(v, dynamicgraph::Vector) {
222			if (!m_initSucceeded) {
223			SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
224			return s;
225			}
226			if (s.size() !=
227			static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
228			s.resize(m_robot_util->m_nbJoints + 6);
229
230			m_kinematics_computationsSINNER(iter);
231
232			getProfiler().start(PROFILE_FREE_FLYER_VELOCITY_COMPUTATION);
233			{
234			const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
235			assert(dq.size() ==
236			static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
237			"Unexpected size of signal joint_velocities");
238
239			/* Compute foot velocities */
240			const Frame& f_lf = m_model->frames[m_left_foot_id];
241			const Motion v_lf_local = f_lf.placement.actInv(m_data->v[f_lf.parent]);
242			const Vector6 v_lf = m_data->oMf[m_left_foot_id].act(v_lf_local).toVector();
243
244			const Frame& f_rf = m_model->frames[m_right_foot_id];
245			const Motion v_rf_local = f_rf.placement.actInv(m_data->v[f_rf.parent]);
246			const Vector6 v_rf =
247			m_data->oMf[m_right_foot_id].act(v_rf_local).toVector();
248
249			m_v_sot.head<6>() = -0.5 * (v_lf + v_rf);
250			m_v_sot.tail(m_robot_util->m_nbJoints) = dq;
251
252			s = m_v_sot;
253			}
254			getProfiler().stop(PROFILE_FREE_FLYER_VELOCITY_COMPUTATION);
255
256			return s;
257			}
258
259			/* --- COMMANDS ---------------------------------------------------------- */
260			void FreeFlyerLocator::displayRobotUtil() { m_robot_util->display(std::cout); }
261
262			/* ------------------------------------------------------------------- */
263			/* --- ENTITY -------------------------------------------------------- */
264			/* ------------------------------------------------------------------- */
265
266			void FreeFlyerLocator::display(std::ostream& os) const {
267			os << "FreeFlyerLocator " << getName();
268			try {
269			getProfiler().report_all(3, os);
270			} catch (ExceptionSignal e) {
271			}
272			}
273			} // namespace torque_control
274			} // namespace sot
275			} // namespace dynamicgraph