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