Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/tools/robot-utils.cpp	Lines:	237	330	71.8 %
Date:	2023-03-13 12:09:37	Branches:	217	584	37.2 %


/*
 * Copyright 2017, 2019
 * LAAS-CNRS
 * A. Del Prete, T. Flayols, O. Stasse, F. Bailly
 *
 */

/** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
    This happen to be not working when including the boost property_tree
   library. For this reason if defined, the current value of
   BOOST_MPL_LIMIT_VECTOR_SIZE is saved in the preprocessor stack and unset.
    Once the property_tree included the pinocchio value of this variable is
    restored.
 */
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#undef BOOST_MPL_LIMIT_VECTOR_SIZE
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#else
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#endif

#include <dynamic-graph/factory.h>

#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/robot-utils.hh>
#include <sstream>

namespace dynamicgraph {
namespace sot {
namespace dg = ::dynamicgraph;
namespace pt = boost::property_tree;
using namespace dg;
using namespace dg::command;

ForceLimits VoidForceLimits;
JointLimits VoidJointLimits;
Index VoidIndex(-1);

RobotUtilShrPtr RefVoidRobotUtil() {
  return std::shared_ptr<RobotUtil>(nullptr);
}

ExtractJointMimics::ExtractJointMimics(std::string &robot_model) {
  // Parsing the model from a string.
  std::istringstream iss(robot_model);
  /// Read the XML file in the property tree.
  boost::property_tree::read_xml(iss, tree_);
  /// Start the recursive parsing.
  go_through_full();
}

const std::vector<std::string> &ExtractJointMimics::get_mimic_joints() {
  return mimic_joints_;
}

void ExtractJointMimics::go_through_full() {
  /// Root of the recursive parsing.
  current_joint_name_ = "";
  go_through(tree_, 0, 0);
}

void ExtractJointMimics::go_through(pt::ptree &pt, int level, int stage) {
  /// If pt is empty (i.e. this is a leaf)
  if (pt.empty()) {
    /// and this is a name of a joint (stage == 3) update the
    /// curret_joint_name_ variable.
    if (stage == 3) current_joint_name_ = pt.data();
  } else {
    /// This is not a leaf
    for (auto pos : pt) {
      int new_stage = stage;

      /// But this is joint
      if (pos.first == "joint")
        /// the continue the exploration.
        new_stage = 1;
      else if (pos.first == "<xmlattr>") {
        /// we are exploring the xml attributes of a joint
        /// -> continue the exploration
        if (stage == 1) new_stage = 2;
      }
      /// The xml attribute of the joint is the name
      /// next leaf is the name we are possibly looking for
      else if (pos.first == "name") {
        if (stage == 2) new_stage = 3;
      }
      /// The exploration of the tree tracback on the joint
      /// and find that this is a mimic joint.
      else if (pos.first == "mimic") {
        if (stage == 1)
          /// Save the current name of the joint
          /// in mimic_joints.
          mimic_joints_.push_back(current_joint_name_);
      } else
        new_stage = 0;

      /// Explore the subtree of the XML robot description.
      go_through(pos.second, level + 1, new_stage);
    }
  }
}

void ForceLimits::display(std::ostream &os) const {
  os << "Lower limits:" << std::endl;
  os << lower << std::endl;
  os << "Upper Limits:" << std::endl;
  os << upper << std::endl;
}

/******************** FootUtil ***************************/

void FootUtil::display(std::ostream &os) const {
  os << "Right Foot Sole XYZ " << std::endl;
  os << m_Right_Foot_Sole_XYZ << std::endl;
  os << "Left Foot Frame Name:" << m_Left_Foot_Frame_Name << std::endl;
  os << "Right Foot Frame Name:" << m_Right_Foot_Frame_Name << std::endl;
}

/******************** HandUtil ***************************/

void HandUtil::display(std::ostream &os) const {
  os << "Left Hand Frame Name:" << m_Left_Hand_Frame_Name << std::endl;
  os << "Right Hand Frame Name:" << m_Right_Hand_Frame_Name << std::endl;
}

/******************** ForceUtil ***************************/

void ForceUtil::set_name_to_force_id(const std::string &name,
                                     const Index &force_id) {
  m_name_to_force_id[name] = (Index)force_id;
  create_force_id_to_name_map();
  if (name == "rf")
    set_force_id_right_foot(m_name_to_force_id[name]);
  else if (name == "lf")
    set_force_id_left_foot(m_name_to_force_id[name]);
  else if (name == "lh")
    set_force_id_left_hand(m_name_to_force_id[name]);
  else if (name == "rh")
    set_force_id_right_hand(m_name_to_force_id[name]);
}

void ForceUtil::set_force_id_to_limits(const Index &force_id,
                                       const dg::Vector &lf,
                                       const dg::Vector &uf) {
  m_force_id_to_limits[(Index)force_id].lower = lf;
  m_force_id_to_limits[(Index)force_id].upper = uf;
}

Index ForceUtil::get_id_from_name(const std::string &name) {
  std::map<std::string, Index>::const_iterator it;
  it = m_name_to_force_id.find(name);
  if (it != m_name_to_force_id.end()) return it->second;
  return -1;
}

std::string force_default_rtn("Force name not found");
std::string joint_default_rtn("Joint name not found");

const std::string &ForceUtil::get_name_from_id(Index idx) {
  std::map<Index, std::string>::const_iterator it;
  it = m_force_id_to_name.find(idx);
  if (it != m_force_id_to_name.end()) return it->second;
  return force_default_rtn;
}

std::string ForceUtil::cp_get_name_from_id(Index idx) {
  const std::string &default_rtn = get_name_from_id(idx);
  return default_rtn;
}
void ForceUtil::create_force_id_to_name_map() {
  std::map<std::string, Index>::const_iterator it;
  for (it = m_name_to_force_id.begin(); it != m_name_to_force_id.end(); it++)
    m_force_id_to_name[it->second] = it->first;
}

const ForceLimits &ForceUtil::get_limits_from_id(Index force_id) {
  std::map<Index, ForceLimits>::const_iterator iter =
      m_force_id_to_limits.find(force_id);
  if (iter == m_force_id_to_limits.end())
    return VoidForceLimits;  // Returns void instance
  return iter->second;
}

ForceLimits ForceUtil::cp_get_limits_from_id(Index force_id) {
  std::map<Index, ForceLimits>::const_iterator iter =
      m_force_id_to_limits.find(force_id);
  if (iter == m_force_id_to_limits.end())
    return VoidForceLimits;  // Returns void instance
  return iter->second;
}

void ForceUtil::display(std::ostream &os) const {
  os << "Force Id to limits " << std::endl;
  for (std::map<Index, ForceLimits>::const_iterator it =
           m_force_id_to_limits.begin();
       it != m_force_id_to_limits.end(); ++it) {
    it->second.display(os);
  }

  os << "Name to force id:" << std::endl;
  for (std::map<std::string, Index>::const_iterator it =
           m_name_to_force_id.begin();
       it != m_name_to_force_id.end(); ++it) {
    os << "(" << it->first << "," << it->second << ") ";
  }
  os << std::endl;

  os << "Force id to Name:" << std::endl;
  for (std::map<Index, std::string>::const_iterator it =
           m_force_id_to_name.begin();
       it != m_force_id_to_name.end(); ++it) {
    os << "(" << it->first << "," << it->second << ") ";
  }
  os << std::endl;

  os << "Index for force sensors:" << std::endl;
  os << "Left Hand (" << m_Force_Id_Left_Hand << ") ,"
     << "Right Hand (" << m_Force_Id_Right_Hand << ") ,"
     << "Left Foot (" << m_Force_Id_Left_Foot << ") ,"
     << "Right Foot (" << m_Force_Id_Right_Foot << ") " << std::endl;
}

/**************** FromURDFToSot *************************/
RobotUtil::RobotUtil() {}

void RobotUtil::set_joint_limits_for_id(const Index &idx, const double &lq,
                                        const double &uq) {
  m_limits_map[(Index)idx] = JointLimits(lq, uq);
}

const JointLimits &RobotUtil::get_joint_limits_from_id(Index id) {
  std::map<Index, JointLimits>::const_iterator iter = m_limits_map.find(id);
  if (iter == m_limits_map.end()) return VoidJointLimits;
  return iter->second;
}
JointLimits RobotUtil::cp_get_joint_limits_from_id(Index id) {
  const JointLimits &rtn = get_joint_limits_from_id(id);
  return rtn;
}

void RobotUtil::set_name_to_id(const std::string &jointName,
                               const Index &jointId) {
  m_name_to_id[jointName] = (Index)jointId;
  create_id_to_name_map();
}

void RobotUtil::create_id_to_name_map() {
  std::map<std::string, Index>::const_iterator it;
  for (it = m_name_to_id.begin(); it != m_name_to_id.end(); it++)
    m_id_to_name[it->second] = it->first;
}

const Index &RobotUtil::get_id_from_name(const std::string &name) {
  std::map<std::string, Index>::const_iterator it = m_name_to_id.find(name);
  if (it == m_name_to_id.end()) return VoidIndex;
  return it->second;
}

const std::string &RobotUtil::get_name_from_id(Index id) {
  std::map<Index, std::string>::const_iterator iter = m_id_to_name.find(id);
  if (iter == m_id_to_name.end()) return joint_default_rtn;
  return iter->second;
}

void RobotUtil::set_urdf_to_sot(const std::vector<Index> &urdf_to_sot) {
  m_nbJoints = urdf_to_sot.size();
  m_urdf_to_sot.resize(urdf_to_sot.size());
  m_dgv_urdf_to_sot.resize(urdf_to_sot.size());
  for (std::size_t idx = 0; idx < urdf_to_sot.size(); idx++) {
    m_urdf_to_sot[(Index)idx] = urdf_to_sot[(Index)idx];
    m_dgv_urdf_to_sot[(Index)idx] =
        static_cast<double>(urdf_to_sot[(Index)idx]);
  }
}

void RobotUtil::set_urdf_to_sot(const dg::Vector &urdf_to_sot) {
  m_nbJoints = urdf_to_sot.size();
  m_urdf_to_sot.resize(urdf_to_sot.size());
  for (unsigned int idx = 0; idx < urdf_to_sot.size(); idx++) {
    m_urdf_to_sot[idx] = (unsigned int)urdf_to_sot[idx];
  }
  m_dgv_urdf_to_sot = urdf_to_sot;
}

bool RobotUtil::joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
  if (m_nbJoints == 0) {
    SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
    return false;
  }
  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));

  for (unsigned int idx = 0; idx < m_nbJoints; idx++)
    q_sot[m_urdf_to_sot[idx]] = q_urdf[idx];
  return true;
}

bool RobotUtil::joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));

  if (m_nbJoints == 0) {
    SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
    return false;
  }

  for (unsigned int idx = 0; idx < m_nbJoints; idx++)
    q_urdf[idx] = q_sot[m_urdf_to_sot[idx]];
  return true;
}

bool RobotUtil::velocity_urdf_to_sot(ConstRefVector q_urdf,
                                     ConstRefVector v_urdf, RefVector v_sot) {
  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
  assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
  assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));

  if (m_nbJoints == 0) {
    SEND_MSG("velocity_urdf_to_sot should be called", MSG_TYPE_ERROR);
    return false;
  }
  const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
  Eigen::Matrix3d oRb = q.toRotationMatrix();
  v_sot.head<3>() = oRb * v_urdf.head<3>();
  v_sot.segment<3>(3) = oRb * v_urdf.segment<3>(3);
  //        v_sot.head<6>() = v_urdf.head<6>();
  joints_urdf_to_sot(v_urdf.tail(m_nbJoints), v_sot.tail(m_nbJoints));
  return true;
}

bool RobotUtil::velocity_sot_to_urdf(ConstRefVector q_urdf,
                                     ConstRefVector v_sot, RefVector v_urdf) {
  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
  assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
  assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));

  if (m_nbJoints == 0) {
    SEND_MSG("velocity_sot_to_urdf should be called", MSG_TYPE_ERROR);
    return false;
  }
  // compute rotation from world to base frame
  const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
  Eigen::Matrix3d oRb = q.toRotationMatrix();
  v_urdf.head<3>() = oRb.transpose() * v_sot.head<3>();
  v_urdf.segment<3>(3) = oRb.transpose() * v_sot.segment<3>(3);
  //        v_urdf.head<6>() = v_sot.head<6>();
  joints_sot_to_urdf(v_sot.tail(m_nbJoints), v_urdf.tail(m_nbJoints));
  return true;
}

bool RobotUtil::base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
  assert(q_urdf.size() == 7);
  assert(q_sot.size() == 6);

  // ********* Quat to RPY *********
  const double W = q_urdf[6];
  const double X = q_urdf[3];
  const double Y = q_urdf[4];
  const double Z = q_urdf[5];
  const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
  return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
}

bool RobotUtil::base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
  assert(q_urdf.size() == 7);
  assert(q_sot.size() == 6);
  // *********  RPY to Quat *********
  const double r = q_sot[3];
  const double p = q_sot[4];
  const double y = q_sot[5];
  const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
  const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;

  q_urdf[0] = q_sot[0];  // BASE
  q_urdf[1] = q_sot[1];
  q_urdf[2] = q_sot[2];
  q_urdf[3] = quat.x();
  q_urdf[4] = quat.y();
  q_urdf[5] = quat.z();
  q_urdf[6] = quat.w();

  return true;
}

bool RobotUtil::config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));

  base_urdf_to_sot(q_urdf.head<7>(), q_sot.head<6>());
  joints_urdf_to_sot(q_urdf.tail(m_nbJoints), q_sot.tail(m_nbJoints));

  return true;
}

bool RobotUtil::config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
  base_sot_to_urdf(q_sot.head<6>(), q_urdf.head<7>());
  joints_sot_to_urdf(q_sot.tail(m_nbJoints), q_urdf.tail(m_nbJoints));
  return true;
}
void RobotUtil::display(std::ostream &os) const {
  m_force_util.display(os);
  m_foot_util.display(os);
  m_hand_util.display(os);
  os << "Nb of joints: " << m_nbJoints << std::endl;
  os << "Urdf file name: " << m_urdf_filename << std::endl;

  // Display m_urdf_to_sot
  os << "Map from urdf index to the Sot Index " << std::endl;
  for (unsigned int i = 0; i < m_urdf_to_sot.size(); i++)
    os << "(" << i << " : " << m_urdf_to_sot[i] << ") ";
  os << std::endl;

  os << "Joint name to joint id:" << std::endl;
  for (std::map<std::string, Index>::const_iterator it = m_name_to_id.begin();
       it != m_name_to_id.end(); ++it) {
    os << "(" << it->first << "," << it->second << ") ";
  }
  os << std::endl;

  os << "Joint id to joint Name:" << std::endl;
  for (std::map<Index, std::string>::const_iterator it = m_id_to_name.begin();
       it != m_id_to_name.end(); ++it) {
    os << "(" << it->first << "," << it->second << ") ";
  }
  os << std::endl;

  boost::property_tree::write_xml(os, property_tree_);
}

void RobotUtil::sendMsg(const std::string &msg, MsgType t,
                        const std::string &lineId) {
  logger_.stream(t, lineId) << "[RobotUtil]" << msg << '\n';
}

bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot) {
  assert(q_sot.size() == 6);
  assert(pos.size() == 3);
  assert(R.rows() == 3);
  assert(R.cols() == 3);
  // ********* Quat to RPY *********
  double r, p, y, m;
  m = sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
  p = atan2(-R(2, 0), m);
  if (fabs(fabs(p) - M_PI / 2) < 0.001) {
    r = 0.0;
    y = -atan2(R(0, 1), R(1, 1));
  } else {
    y = atan2(R(1, 0), R(0, 0));
    r = atan2(R(2, 1), R(2, 2));
  }
  // *********************************
  q_sot[0] = pos[0];
  q_sot[1] = pos[1];
  q_sot[2] = pos[2];
  q_sot[3] = r;
  q_sot[4] = p;
  q_sot[5] = y;
  return true;
}

bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
  assert(q_urdf.size() == 7);
  assert(q_sot.size() == 6);
  // ********* Quat to RPY *********
  const double W = q_urdf[6];
  const double X = q_urdf[3];
  const double Y = q_urdf[4];
  const double Z = q_urdf[5];
  const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
  return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
}

bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
  assert(q_urdf.size() == 7);
  assert(q_sot.size() == 6);
  // *********  RPY to Quat *********
  const double r = q_sot[3];
  const double p = q_sot[4];
  const double y = q_sot[5];
  const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
  const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;

  q_urdf[0] = q_sot[0];  // BASE
  q_urdf[1] = q_sot[1];
  q_urdf[2] = q_sot[2];
  q_urdf[3] = quat.x();
  q_urdf[4] = quat.y();
  q_urdf[5] = quat.z();
  q_urdf[6] = quat.w();

  return true;
}

static std::map<std::string, RobotUtilShrPtr> sgl_map_name_to_robot_util;

std::shared_ptr<std::vector<std::string> > getListOfRobots() {
  std::shared_ptr<std::vector<std::string> > res =
      std::make_shared<std::vector<std::string> >();

  std::map<std::string, RobotUtilShrPtr>::iterator it =
      sgl_map_name_to_robot_util.begin();
  while (it != sgl_map_name_to_robot_util.end()) {
    res->push_back(it->first);
    it++;
  }

  return res;
}

RobotUtilShrPtr getRobotUtil(std::string &robotName) {
  std::map<std::string, RobotUtilShrPtr>::iterator it =
      sgl_map_name_to_robot_util.find(robotName);
  if (it != sgl_map_name_to_robot_util.end()) return it->second;
  return RefVoidRobotUtil();
}

bool isNameInRobotUtil(std::string &robotName) {
  std::map<std::string, RobotUtilShrPtr>::iterator it =
      sgl_map_name_to_robot_util.find(robotName);
  if (it != sgl_map_name_to_robot_util.end()) return true;
  return false;
}

RobotUtilShrPtr createRobotUtil(std::string &robotName) {
  std::map<std::string, RobotUtilShrPtr>::iterator it =
      sgl_map_name_to_robot_util.find(robotName);
  if (it == sgl_map_name_to_robot_util.end()) {
    sgl_map_name_to_robot_util[robotName] = std::make_shared<RobotUtil>();
    it = sgl_map_name_to_robot_util.find(robotName);
    return it->second;
  }
  return RefVoidRobotUtil();
}
}  // namespace sot
}  // namespace dynamicgraph


Generated by: GCOVR (Version 4.2)

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