robot-utils.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017, 2019
3  * LAAS-CNRS
4  * A. Del Prete, T. Flayols, O. Stasse, F. Bailly
5  *
6  */
7 
15 #ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
16 #pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
17 #undef BOOST_MPL_LIMIT_VECTOR_SIZE
18 #include <boost/property_tree/ptree.hpp>
19 #include <boost/property_tree/xml_parser.hpp>
20 #pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
21 #else
22 #include <boost/property_tree/ptree.hpp>
23 #include <boost/property_tree/xml_parser.hpp>
24 #endif
25 
26 #include <dynamic-graph/factory.h>
27 
28 #include <iostream>
29 #include <sot/core/debug.hh>
30 #include <sot/core/robot-utils.hh>
31 #include <sstream>
32 
33 namespace dynamicgraph {
34 namespace sot {
35 namespace dg = ::dynamicgraph;
36 namespace pt = boost::property_tree;
37 using namespace dg;
38 using namespace dg::command;
39 
42 Index VoidIndex(-1);
43 
45  return std::shared_ptr<RobotUtil>(nullptr);
46 }
47 
48 ExtractJointMimics::ExtractJointMimics(std::string &robot_model) {
49  // Parsing the model from a string.
50  std::istringstream iss(robot_model);
52  boost::property_tree::read_xml(iss, tree_);
54  go_through_full();
55 }
56 
57 const std::vector<std::string> &ExtractJointMimics::get_mimic_joints() {
58  return mimic_joints_;
59 }
60 
63  current_joint_name_ = "";
64  go_through(tree_, 0, 0);
65 }
66 
67 void ExtractJointMimics::go_through(pt::ptree &pt, size_type level,
68  size_type stage) {
70  if (pt.empty()) {
73  if (stage == 3) current_joint_name_ = pt.data();
74  } else {
76  for (auto pos : pt) {
77  size_type new_stage = stage;
78 
80  if (pos.first == "joint")
82  new_stage = 1;
83  else if (pos.first == "<xmlattr>") {
86  if (stage == 1) new_stage = 2;
87  }
90  else if (pos.first == "name") {
91  if (stage == 2) new_stage = 3;
92  }
95  else if (pos.first == "mimic") {
96  if (stage == 1)
99  mimic_joints_.push_back(current_joint_name_);
100  } else
101  new_stage = 0;
102 
104  go_through(pos.second, level + 1, new_stage);
105  }
106  }
107 }
108 
109 void ForceLimits::display(std::ostream &os) const {
110  os << "Lower limits:" << std::endl;
111  os << lower << std::endl;
112  os << "Upper Limits:" << std::endl;
113  os << upper << std::endl;
114 }
115 
116 /******************** FootUtil ***************************/
117 
118 void FootUtil::display(std::ostream &os) const {
119  os << "Right Foot Sole XYZ " << std::endl;
120  os << m_Right_Foot_Sole_XYZ << std::endl;
121  os << "Left Foot Frame Name:" << m_Left_Foot_Frame_Name << std::endl;
122  os << "Right Foot Frame Name:" << m_Right_Foot_Frame_Name << std::endl;
123 }
124 
125 /******************** HandUtil ***************************/
126 
127 void HandUtil::display(std::ostream &os) const {
128  os << "Left Hand Frame Name:" << m_Left_Hand_Frame_Name << std::endl;
129  os << "Right Hand Frame Name:" << m_Right_Hand_Frame_Name << std::endl;
130 }
131 
132 /******************** ForceUtil ***************************/
133 
134 void ForceUtil::set_name_to_force_id(const std::string &name,
135  const Index &force_id) {
136  m_name_to_force_id[name] = (Index)force_id;
137  create_force_id_to_name_map();
138  if (name == "rf")
139  set_force_id_right_foot(m_name_to_force_id[name]);
140  else if (name == "lf")
141  set_force_id_left_foot(m_name_to_force_id[name]);
142  else if (name == "lh")
143  set_force_id_left_hand(m_name_to_force_id[name]);
144  else if (name == "rh")
145  set_force_id_right_hand(m_name_to_force_id[name]);
146 }
147 
149  const dg::Vector &lf,
150  const dg::Vector &uf) {
151  m_force_id_to_limits[(Index)force_id].lower = lf;
152  m_force_id_to_limits[(Index)force_id].upper = uf;
153 }
154 
156  std::map<std::string, Index>::const_iterator it;
157  it = m_name_to_force_id.find(name);
158  if (it != m_name_to_force_id.end()) return it->second;
159  return -1;
160 }
161 
162 std::string force_default_rtn("Force name not found");
163 std::string joint_default_rtn("Joint name not found");
164 
165 const std::string &ForceUtil::get_name_from_id(Index idx) {
166  std::map<Index, std::string>::const_iterator it;
167  it = m_force_id_to_name.find(idx);
168  if (it != m_force_id_to_name.end()) return it->second;
169  return force_default_rtn;
170 }
171 
173  const std::string &default_rtn = get_name_from_id(idx);
174  return default_rtn;
175 }
177  std::map<std::string, Index>::const_iterator it;
178  for (it = m_name_to_force_id.begin(); it != m_name_to_force_id.end(); it++)
179  m_force_id_to_name[it->second] = it->first;
180 }
181 
183  std::map<Index, ForceLimits>::const_iterator iter =
184  m_force_id_to_limits.find(force_id);
185  if (iter == m_force_id_to_limits.end())
186  return VoidForceLimits; // Returns void instance
187  return iter->second;
188 }
189 
191  std::map<Index, ForceLimits>::const_iterator iter =
192  m_force_id_to_limits.find(force_id);
193  if (iter == m_force_id_to_limits.end())
194  return VoidForceLimits; // Returns void instance
195  return iter->second;
196 }
197 
198 void ForceUtil::display(std::ostream &os) const {
199  os << "Force Id to limits " << std::endl;
200  for (std::map<Index, ForceLimits>::const_iterator it =
201  m_force_id_to_limits.begin();
202  it != m_force_id_to_limits.end(); ++it) {
203  it->second.display(os);
204  }
205 
206  os << "Name to force id:" << std::endl;
207  for (std::map<std::string, Index>::const_iterator it =
208  m_name_to_force_id.begin();
209  it != m_name_to_force_id.end(); ++it) {
210  os << "(" << it->first << "," << it->second << ") ";
211  }
212  os << std::endl;
213 
214  os << "Force id to Name:" << std::endl;
215  for (std::map<Index, std::string>::const_iterator it =
216  m_force_id_to_name.begin();
217  it != m_force_id_to_name.end(); ++it) {
218  os << "(" << it->first << "," << it->second << ") ";
219  }
220  os << std::endl;
221 
222  os << "Index for force sensors:" << std::endl;
223  os << "Left Hand (" << m_Force_Id_Left_Hand << ") ,"
224  << "Right Hand (" << m_Force_Id_Right_Hand << ") ,"
225  << "Left Foot (" << m_Force_Id_Left_Foot << ") ,"
226  << "Right Foot (" << m_Force_Id_Right_Foot << ") " << std::endl;
227 }
228 
229 /**************** FromURDFToSot *************************/
231 
232 void RobotUtil::set_joint_limits_for_id(const Index &idx, const double &lq,
233  const double &uq) {
234  m_limits_map[(Index)idx] = JointLimits(lq, uq);
235 }
236 
238  std::map<Index, JointLimits>::const_iterator iter = m_limits_map.find(id);
239  if (iter == m_limits_map.end()) return VoidJointLimits;
240  return iter->second;
241 }
243  const JointLimits &rtn = get_joint_limits_from_id(id);
244  return rtn;
245 }
246 
247 void RobotUtil::set_name_to_id(const std::string &jointName,
248  const Index &jointId) {
249  m_name_to_id[jointName] = (Index)jointId;
250  create_id_to_name_map();
251 }
252 
254  std::map<std::string, Index>::const_iterator it;
255  for (it = m_name_to_id.begin(); it != m_name_to_id.end(); it++)
256  m_id_to_name[it->second] = it->first;
257 }
258 
259 const Index &RobotUtil::get_id_from_name(const std::string &name) {
260  std::map<std::string, Index>::const_iterator it = m_name_to_id.find(name);
261  if (it == m_name_to_id.end()) return VoidIndex;
262  return it->second;
263 }
264 
265 const std::string &RobotUtil::get_name_from_id(Index id) {
266  std::map<Index, std::string>::const_iterator iter = m_id_to_name.find(id);
267  if (iter == m_id_to_name.end()) return joint_default_rtn;
268  return iter->second;
269 }
270 
271 void RobotUtil::set_urdf_to_sot(const std::vector<Index> &urdf_to_sot) {
272  m_nbJoints = urdf_to_sot.size();
273  m_urdf_to_sot.resize(urdf_to_sot.size());
274  m_dgv_urdf_to_sot.resize(urdf_to_sot.size());
275  for (std::size_t idx = 0; idx < urdf_to_sot.size(); idx++) {
276  m_urdf_to_sot[(Index)idx] = urdf_to_sot[(Index)idx];
277  m_dgv_urdf_to_sot[(Index)idx] =
278  static_cast<double>(urdf_to_sot[(Index)idx]);
279  }
280 }
281 
282 void RobotUtil::set_urdf_to_sot(const dg::Vector &urdf_to_sot) {
283  m_nbJoints = urdf_to_sot.size();
284  m_urdf_to_sot.resize(urdf_to_sot.size());
285  for (std::size_t idx = 0; idx < (std::size_t)urdf_to_sot.size(); idx++) {
286  m_urdf_to_sot[idx] = (std::size_t)urdf_to_sot[idx];
287  }
288  m_dgv_urdf_to_sot = urdf_to_sot;
289 }
290 
292  if (m_nbJoints == 0) {
293  SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
294  return false;
295  }
296  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
297  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
298 
299  for (std::size_t idx = 0; idx < m_nbJoints; idx++)
300  q_sot[m_urdf_to_sot[idx]] = q_urdf[idx];
301  return true;
302 }
303 
305  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
306  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
307 
308  if (m_nbJoints == 0) {
309  SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
310  return false;
311  }
312 
313  for (std::size_t idx = 0; idx < m_nbJoints; idx++)
314  q_urdf[idx] = q_sot[m_urdf_to_sot[idx]];
315  return true;
316 }
317 
319  ConstRefVector v_urdf, RefVector v_sot) {
320  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
321  assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
322  assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
323 
324  if (m_nbJoints == 0) {
325  SEND_MSG("velocity_urdf_to_sot should be called", MSG_TYPE_ERROR);
326  return false;
327  }
328  const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
329  Eigen::Matrix3d oRb = q.toRotationMatrix();
330  v_sot.head<3>() = oRb * v_urdf.head<3>();
331  v_sot.segment<3>(3) = oRb * v_urdf.segment<3>(3);
332  // v_sot.head<6>() = v_urdf.head<6>();
333  joints_urdf_to_sot(v_urdf.tail(m_nbJoints), v_sot.tail(m_nbJoints));
334  return true;
335 }
336 
338  ConstRefVector v_sot, RefVector v_urdf) {
339  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
340  assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
341  assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
342 
343  if (m_nbJoints == 0) {
344  SEND_MSG("velocity_sot_to_urdf should be called", MSG_TYPE_ERROR);
345  return false;
346  }
347  // compute rotation from world to base frame
348  const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
349  Eigen::Matrix3d oRb = q.toRotationMatrix();
350  v_urdf.head<3>() = oRb.transpose() * v_sot.head<3>();
351  v_urdf.segment<3>(3) = oRb.transpose() * v_sot.segment<3>(3);
352  // v_urdf.head<6>() = v_sot.head<6>();
353  joints_sot_to_urdf(v_sot.tail(m_nbJoints), v_urdf.tail(m_nbJoints));
354  return true;
355 }
356 
358  assert(q_urdf.size() == 7);
359  assert(q_sot.size() == 6);
360 
361  // ********* Quat to RPY *********
362  const double W = q_urdf[6];
363  const double X = q_urdf[3];
364  const double Y = q_urdf[4];
365  const double Z = q_urdf[5];
366  const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
367  return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
368 }
369 
371  assert(q_urdf.size() == 7);
372  assert(q_sot.size() == 6);
373  // ********* RPY to Quat *********
374  const double r = q_sot[3];
375  const double p = q_sot[4];
376  const double y = q_sot[5];
377  const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
378  const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
379  const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
380  const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
381 
382  q_urdf[0] = q_sot[0]; // BASE
383  q_urdf[1] = q_sot[1];
384  q_urdf[2] = q_sot[2];
385  q_urdf[3] = quat.x();
386  q_urdf[4] = quat.y();
387  q_urdf[5] = quat.z();
388  q_urdf[6] = quat.w();
389 
390  return true;
391 }
392 
394  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
395  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
396 
397  base_urdf_to_sot(q_urdf.head<7>(), q_sot.head<6>());
398  joints_urdf_to_sot(q_urdf.tail(m_nbJoints), q_sot.tail(m_nbJoints));
399 
400  return true;
401 }
402 
404  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
405  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
406  base_sot_to_urdf(q_sot.head<6>(), q_urdf.head<7>());
407  joints_sot_to_urdf(q_sot.tail(m_nbJoints), q_urdf.tail(m_nbJoints));
408  return true;
409 }
410 void RobotUtil::display(std::ostream &os) const {
411  m_force_util.display(os);
412  m_foot_util.display(os);
413  m_hand_util.display(os);
414  os << "Nb of joints: " << m_nbJoints << std::endl;
415  os << "Urdf file name: " << m_urdf_filename << std::endl;
416 
417  // Display m_urdf_to_sot
418  os << "Map from urdf index to the Sot Index " << std::endl;
419  for (std::size_t i = 0; i < m_urdf_to_sot.size(); i++)
420  os << "(" << i << " : " << m_urdf_to_sot[i] << ") ";
421  os << std::endl;
422 
423  os << "Joint name to joint id:" << std::endl;
424  for (std::map<std::string, Index>::const_iterator it = m_name_to_id.begin();
425  it != m_name_to_id.end(); ++it) {
426  os << "(" << it->first << "," << it->second << ") ";
427  }
428  os << std::endl;
429 
430  os << "Joint id to joint Name:" << std::endl;
431  for (std::map<Index, std::string>::const_iterator it = m_id_to_name.begin();
432  it != m_id_to_name.end(); ++it) {
433  os << "(" << it->first << "," << it->second << ") ";
434  }
435  os << std::endl;
436 
437  boost::property_tree::write_xml(os, property_tree_);
438 }
439 
440 void RobotUtil::sendMsg(const std::string &msg, MsgType t,
441  const std::string &lineId) {
442  logger_.stream(t, lineId) << "[RobotUtil]" << msg << '\n';
443 }
444 
446  assert(q_sot.size() == 6);
447  assert(pos.size() == 3);
448  assert(R.rows() == 3);
449  assert(R.cols() == 3);
450  // ********* Quat to RPY *********
451  double r, p, y, m;
452  m = sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
453  p = atan2(-R(2, 0), m);
454  if (fabs(fabs(p) - M_PI / 2) < 0.001) {
455  r = 0.0;
456  y = -atan2(R(0, 1), R(1, 1));
457  } else {
458  y = atan2(R(1, 0), R(0, 0));
459  r = atan2(R(2, 1), R(2, 2));
460  }
461  // *********************************
462  q_sot[0] = pos[0];
463  q_sot[1] = pos[1];
464  q_sot[2] = pos[2];
465  q_sot[3] = r;
466  q_sot[4] = p;
467  q_sot[5] = y;
468  return true;
469 }
470 
472  assert(q_urdf.size() == 7);
473  assert(q_sot.size() == 6);
474  // ********* Quat to RPY *********
475  const double W = q_urdf[6];
476  const double X = q_urdf[3];
477  const double Y = q_urdf[4];
478  const double Z = q_urdf[5];
479  const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
480  return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
481 }
482 
484  assert(q_urdf.size() == 7);
485  assert(q_sot.size() == 6);
486  // ********* RPY to Quat *********
487  const double r = q_sot[3];
488  const double p = q_sot[4];
489  const double y = q_sot[5];
490  const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
491  const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
492  const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
493  const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
494 
495  q_urdf[0] = q_sot[0]; // BASE
496  q_urdf[1] = q_sot[1];
497  q_urdf[2] = q_sot[2];
498  q_urdf[3] = quat.x();
499  q_urdf[4] = quat.y();
500  q_urdf[5] = quat.z();
501  q_urdf[6] = quat.w();
502 
503  return true;
504 }
505 
506 static std::map<std::string, RobotUtilShrPtr> sgl_map_name_to_robot_util;
507 
508 std::shared_ptr<std::vector<std::string> > getListOfRobots() {
509  std::shared_ptr<std::vector<std::string> > res =
510  std::make_shared<std::vector<std::string> >();
511 
512  std::map<std::string, RobotUtilShrPtr>::iterator it =
514  while (it != sgl_map_name_to_robot_util.end()) {
515  res->push_back(it->first);
516  it++;
517  }
518 
519  return res;
520 }
521 
522 RobotUtilShrPtr getRobotUtil(std::string &robotName) {
523  std::map<std::string, RobotUtilShrPtr>::iterator it =
524  sgl_map_name_to_robot_util.find(robotName);
525  if (it != sgl_map_name_to_robot_util.end()) return it->second;
526  return RefVoidRobotUtil();
527 }
528 
529 bool isNameInRobotUtil(std::string &robotName) {
530  std::map<std::string, RobotUtilShrPtr>::iterator it =
531  sgl_map_name_to_robot_util.find(robotName);
532  if (it != sgl_map_name_to_robot_util.end()) return true;
533  return false;
534 }
535 
536 RobotUtilShrPtr createRobotUtil(std::string &robotName) {
537  std::map<std::string, RobotUtilShrPtr>::iterator it =
538  sgl_map_name_to_robot_util.find(robotName);
539  if (it == sgl_map_name_to_robot_util.end()) {
540  sgl_map_name_to_robot_util[robotName] = std::make_shared<RobotUtil>();
541  it = sgl_map_name_to_robot_util.find(robotName);
542  return it->second;
543  }
544  return RefVoidRobotUtil();
545 }
546 } // namespace sot
547 } // namespace dynamicgraph
SEND_MSG
#define SEND_MSG(msg, type)
dynamicgraph::sot::RobotUtil::config_sot_to_urdf
bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf)
Definition: robot-utils.cpp:403
m
float m
dynamicgraph::sot::RobotUtil::create_id_to_name_map
void create_id_to_name_map()
Definition: robot-utils.cpp:253
dynamicgraph::sot::RobotUtil::display
void display(std::ostream &os) const
Definition: robot-utils.cpp:410
dynamicgraph::sot::ForceUtil::display
void display(std::ostream &out) const
Definition: robot-utils.cpp:198
dynamicgraph::sot::base_sot_to_urdf
bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf)
Definition: robot-utils.cpp:483
dynamicgraph::sot::createRobotUtil
RobotUtilShrPtr createRobotUtil(std::string &robotName)
Definition: robot-utils.cpp:536
quat
quat
dynamicgraph::sot::base_se3_to_sot
bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot)
Definition: robot-utils.cpp:445
dynamicgraph
y
y
dynamicgraph::sot::RobotUtil::base_urdf_to_sot
bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot)
Definition: robot-utils.cpp:357
dynamicgraph::sot::VoidForceLimits
ForceLimits VoidForceLimits
Definition: robot-utils.cpp:40
dynamicgraph::sot::RobotUtil::config_urdf_to_sot
bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot)
Definition: robot-utils.cpp:393
i
int i
dynamicgraph::sot::Index
Eigen::VectorXd::Index Index
Definition: robot-utils.hh:39
dynamicgraph::sot::VoidIndex
Index VoidIndex(-1)
dynamicgraph::sot::RobotUtil::joints_sot_to_urdf
bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf)
Definition: robot-utils.cpp:304
dynamicgraph::sot::ConstRefMatrix
const typedef Eigen::Ref< const Eigen::MatrixXd > ConstRefMatrix
Definition: matrix-geometry.hh:72
dynamicgraph::sot::ForceUtil::get_name_from_id
const std::string & get_name_from_id(Index idx)
Definition: robot-utils.cpp:165
R
R
dynamicgraph::sot::RobotUtil::velocity_sot_to_urdf
bool velocity_sot_to_urdf(ConstRefVector q_urdf, ConstRefVector v_sot, RefVector v_urdf)
Definition: robot-utils.cpp:337
dynamicgraph::sot::ForceUtil::set_force_id_to_limits
void set_force_id_to_limits(const Index &force_id, const dynamicgraph::Vector &lf, const dynamicgraph::Vector &uf)
Definition: robot-utils.cpp:148
robot-utils.hh
dynamicgraph::sot::RobotUtil::velocity_urdf_to_sot
bool velocity_urdf_to_sot(ConstRefVector q_urdf, ConstRefVector v_urdf, RefVector v_sot)
Definition: robot-utils.cpp:318
dynamicgraph::sot::ExtractJointMimics::get_mimic_joints
const std::vector< std::string > & get_mimic_joints()
Get mimic joints.
Definition: robot-utils.cpp:57
dynamicgraph::sot::getListOfRobots
std::shared_ptr< std::vector< std::string > > getListOfRobots()
Definition: robot-utils.cpp:508
dynamicgraph::MSG_TYPE_ERROR
MSG_TYPE_ERROR
r
FCL_REAL r
dynamicgraph::sot::ForceUtil::cp_get_name_from_id
std::string cp_get_name_from_id(Index idx)
Definition: robot-utils.cpp:172
dynamicgraph::sot::ConstRefVector
const typedef Eigen::Ref< const Eigen::VectorXd > & ConstRefVector
Definition: matrix-geometry.hh:70
debug.hh
p
p
res
res
dynamicgraph::sot::RobotUtil::set_joint_limits_for_id
void set_joint_limits_for_id(const Index &idx, const double &lq, const double &uq)
Set the limits (lq,uq) for joint idx.
Definition: robot-utils.cpp:232
dynamicgraph::sot::RobotUtilShrPtr
std::shared_ptr< RobotUtil > RobotUtilShrPtr
Accessors - This should be changed to RobotUtilPtrShared.
Definition: robot-utils.hh:300
dynamicgraph::sot::RobotUtil::cp_get_joint_limits_from_id
JointLimits cp_get_joint_limits_from_id(Index id)
Definition: robot-utils.cpp:242
dynamicgraph::sot::force_default_rtn
std::string force_default_rtn("Force name not found")
dynamicgraph::sot::RobotUtil::get_id_from_name
const Index & get_id_from_name(const std::string &name)
Definition: robot-utils.cpp:259
dynamicgraph::sot::ForceUtil::set_name_to_force_id
void set_name_to_force_id(const std::string &name, const Index &force_id)
Definition: robot-utils.cpp:134
dynamicgraph::sot::HandUtil::display
void display(std::ostream &os) const
Definition: robot-utils.cpp:127
dynamicgraph::sot::RefVoidRobotUtil
RobotUtilShrPtr RefVoidRobotUtil()
Definition: robot-utils.cpp:44
dynamicgraph::sot::ExtractJointMimics::go_through_full
void go_through_full()
Definition: robot-utils.cpp:61
Index
std::size_t Index
dynamicgraph::sot::RobotUtil::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const std::string &lineId="")
Send messages msg with level t. Add string file and line to message.
Definition: robot-utils.cpp:440
dynamicgraph::sot::RobotUtil::set_urdf_to_sot
void set_urdf_to_sot(const std::vector< Index > &urdf_to_sot)
Set the map between urdf index and sot index.
Definition: robot-utils.cpp:271
dynamicgraph::sot::ForceUtil::get_limits_from_id
const ForceLimits & get_limits_from_id(Index force_id)
Definition: robot-utils.cpp:182
pos
pos
dynamicgraph::sot::JointLimits
Definition: robot-utils.hh:30
dynamicgraph::sot::RobotUtil::joints_urdf_to_sot
bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot)
Definition: robot-utils.cpp:291
q
q
dynamicgraph::size_type
Matrix::Index size_type
dynamicgraph::sot::RobotUtil::get_name_from_id
const std::string & get_name_from_id(Index id)
Get the joint name from its index.
Definition: robot-utils.cpp:265
dynamicgraph::Vector
Eigen::VectorXd Vector
dynamicgraph::sot::RobotUtil::set_name_to_id
void set_name_to_id(const std::string &jointName, const Index &jointId)
Set relation between the name and the SoT id.
Definition: robot-utils.cpp:247
dynamicgraph::sot::RobotUtil::get_joint_limits_from_id
const JointLimits & get_joint_limits_from_id(Index id)
Definition: robot-utils.cpp:237
dynamicgraph::MsgType
MsgType
dynamicgraph::sot::ExtractJointMimics::ExtractJointMimics
ExtractJointMimics(std::string &robot_model)
Constructor.
Definition: robot-utils.cpp:48
X
X
Y
Y
dynamicgraph::sot::RobotUtil::base_sot_to_urdf
bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf)
Definition: robot-utils.cpp:370
dynamicgraph::sot::sgl_map_name_to_robot_util
static std::map< std::string, RobotUtilShrPtr > sgl_map_name_to_robot_util
Definition: robot-utils.cpp:506
dynamicgraph::sot::RefVector
Eigen::Ref< Eigen::VectorXd > RefVector
Definition: matrix-geometry.hh:69
M_PI
#define M_PI
dynamicgraph::sot::getRobotUtil
RobotUtilShrPtr getRobotUtil(std::string &robotName)
Definition: robot-utils.cpp:522
dynamicgraph::sot::FootUtil::display
void display(std::ostream &os) const
Definition: robot-utils.cpp:118
dynamicgraph::sot::ExtractJointMimics::go_through
void go_through(boost::property_tree::ptree &pt, size_type level, size_type stage)
Definition: robot-utils.cpp:67
dynamicgraph::sot::ForceUtil::cp_get_limits_from_id
ForceLimits cp_get_limits_from_id(Index force_id)
Definition: robot-utils.cpp:190
dynamicgraph::sot::base_urdf_to_sot
bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot)
Definition: robot-utils.cpp:471
dynamicgraph::sot::ForceUtil::create_force_id_to_name_map
void create_force_id_to_name_map()
Definition: robot-utils.cpp:176
t
Transform3f t
dynamicgraph::sot::VoidJointLimits
JointLimits VoidJointLimits
Definition: robot-utils.cpp:41
dynamicgraph::sot::RobotUtil::RobotUtil
RobotUtil()
Definition: robot-utils.cpp:230
dynamicgraph::sot::joint_default_rtn
std::string joint_default_rtn("Joint name not found")
dynamicgraph::sot::ForceLimits
Definition: robot-utils.hh:60
dynamicgraph::sot::ForceUtil::get_id_from_name
Index get_id_from_name(const std::string &name)
Definition: robot-utils.cpp:155
compile.name
name
Definition: compile.py:23
dynamicgraph::sot::ForceLimits::display
void display(std::ostream &os) const
Definition: robot-utils.cpp:109
Z
Z
dynamicgraph::sot::isNameInRobotUtil
bool isNameInRobotUtil(std::string &robotName)
Definition: robot-utils.cpp:529


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Tue Oct 24 2023 02:26:31