8 #ifndef __sot_torque_control_common_H__ 9 #define __sot_torque_control_common_H__ 23 #ifdef BOOST_MPL_LIMIT_VECTOR_SIZE 24 #pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE") 25 #define UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE 26 #undef BOOST_MPL_LIMIT_VECTOR_SIZE 29 #ifdef BOOST_MPL_LIMIT_LIST_SIZE 30 #pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE") 31 #define UNDEF_BOOST_MPL_LIMIT_LIST_SIZE 32 #undef BOOST_MPL_LIMIT_LIST_SIZE 35 #include <boost/property_tree/ptree.hpp> 37 #ifdef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE 38 #pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE") 39 #undef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE 42 #ifdef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE 43 #pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE") 44 #undef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE 47 #include "boost/assign.hpp" 48 #include <dynamic-graph/linear-algebra.h> 49 #include <dynamic-graph/logger.h> 50 #include <dynamic-graph/signal-helper.h> 75 const std::vector<std::string> &get_mimic_joints();
78 void go_through(boost::property_tree::ptree &pt,
int level,
int stage);
81 boost::property_tree::ptree tree_;
82 std::vector<std::string> mimic_joints_;
83 std::string current_joint_name_;
84 void go_through_full();
91 ForceLimits() : upper(Vector6d::Zero()), lower(Vector6d::Zero()) {}
93 ForceLimits(
const Eigen::VectorXd &l,
const Eigen::VectorXd &u)
94 : upper(u), lower(l) {}
96 void display(std::ostream &os)
const;
105 m_Force_Id_Right_Foot;
107 void set_name_to_force_id(
const std::string &name,
const Index &force_id);
109 void set_force_id_to_limits(
const Index &force_id,
110 const dynamicgraph::Vector &lf,
111 const dynamicgraph::Vector &uf);
113 void create_force_id_to_name_map();
115 Index get_id_from_name(
const std::string &name);
117 const std::string &get_name_from_id(Index idx);
118 std::string cp_get_name_from_id(Index idx);
120 const ForceLimits &get_limits_from_id(Index force_id);
139 void display(std::ostream &out)
const;
152 void display(std::ostream &os)
const;
158 void display(std::ostream &os)
const;
195 void create_id_to_name_map();
206 const Index &get_id_from_name(
const std::string &name);
214 const std::string &get_name_from_id(Index
id);
218 void set_name_to_id(
const std::string &jointName,
const Index &jointId);
221 void set_urdf_to_sot(
const std::vector<Index> &urdf_to_sot);
222 void set_urdf_to_sot(
const dynamicgraph::Vector &urdf_to_sot);
225 void set_joint_limits_for_id(
const Index &idx,
const double &lq,
249 const JointLimits &get_joint_limits_from_id(Index
id);
254 void sendMsg(
const std::string &msg, MsgType t = MSG_TYPE_INFO,
257 const std::string &lineId =
"");
265 void display(std::ostream &os)
const;
272 template <
typename Type>
274 const Type ¶meter_value) {
276 typedef boost::property_tree::ptree::path_type path;
277 path apath(parameter_name,
'/');
278 property_tree_.put<Type>(apath, parameter_value);
279 }
catch (
const boost::property_tree::ptree_error &e) {
280 std::ostringstream oss;
281 oss <<
"Robot utils: parameter path is invalid " <<
'\n' 282 <<
" for set_parameter(" << parameter_name <<
")\n" 283 << e.what() << std::endl;
284 sendMsg(oss.str(), MSG_TYPE_ERROR);
295 template <
typename Type>
298 boost::property_tree::ptree::path_type apath(parameter_name,
'/');
299 const Type &res = property_tree_.get<Type>(apath);
302 }
catch (
const boost::property_tree::ptree_error &e) {
303 std::ostringstream oss;
304 oss <<
"Robot utils: parameter path is invalid " <<
'\n' 305 <<
" for get_parameter(" << parameter_name <<
")\n" 306 << e.what() << std::endl;
307 sendMsg(oss.str(), MSG_TYPE_ERROR);
313 boost::property_tree::ptree &get_property_tree();
339 #endif // sot_torque_control_common_h_ JointLimits(double l, double u)
Definition: robot-utils.hh:63
Definition: robot-utils.hh:99
std::map< Index, JointLimits > m_limits_map
The joint limits map.
Definition: robot-utils.hh:187
const Eigen::Ref< const Eigen::VectorXd > & ConstRefVector
Definition: matrix-geometry.hh:69
Index m_Force_Id_Right_Hand
Definition: robot-utils.hh:104
Definition: robot-utils.hh:161
ForceUtil m_force_util
Forces data.
Definition: robot-utils.hh:166
void set_force_id_right_hand(Index anId)
Definition: robot-utils.hh:129
bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot)
Eigen::VectorXd::Index Index
Definition: robot-utils.hh:66
Definition: robot-utils.hh:155
RobotUtilShrPtr createRobotUtil(std::string &robotName)
double lower
Definition: robot-utils.hh:59
std::string m_Left_Hand_Frame_Name
Definition: robot-utils.hh:156
void set_parameter(const std::string ¶meter_name, const Type ¶meter_value)
Set a parameter of type string. If parameter_name already exists the value is overwritten. If not it is inserted.
Definition: robot-utils.hh:273
Eigen::VectorXd upper
Definition: robot-utils.hh:88
void set_force_id_right_foot(Index anId)
Definition: robot-utils.hh:137
std::map< std::string, Index > m_name_to_force_id
Definition: robot-utils.hh:101
dynamicgraph::Vector m_dgv_urdf_to_sot
Definition: robot-utils.hh:200
Index get_force_id_right_foot()
Definition: robot-utils.hh:135
#define SOT_CORE_EXPORT
Definition: api.hh:20
void set_force_id_left_foot(Index anId)
Definition: robot-utils.hh:133
JointLimits()
Definition: robot-utils.hh:61
ForceLimits()
Definition: robot-utils.hh:91
void setLoggerVerbosityLevel(LoggerVerbosity lv)
Specify the verbosity level of the logger.
Definition: robot-utils.hh:260
const Eigen::Ref< const Eigen::MatrixXd > ConstRefMatrix
Definition: matrix-geometry.hh:71
std::string m_imu_joint_name
The name of the joint IMU is attached to.
Definition: robot-utils.hh:190
std::map< Index, ForceLimits > m_force_id_to_limits
Definition: robot-utils.hh:100
std::shared_ptr< std::vector< std::string > > getListOfRobots()
Eigen::VectorXd lower
Definition: robot-utils.hh:89
Definition: robot-utils.hh:87
std::shared_ptr< RobotUtil > RobotUtilShrPtr
Accessors - This should be changed to RobotUtilPtrShared.
Definition: robot-utils.hh:326
ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
Definition: robot-utils.hh:93
double upper
Definition: robot-utils.hh:58
std::size_t m_nbJoints
Nb of Dofs for the robot.
Definition: robot-utils.hh:178
void set_force_id_left_hand(Index anId)
Definition: robot-utils.hh:125
RobotUtilShrPtr RefVoidRobotUtil()
LoggerVerbosity getLoggerVerbosityLevel()
Get the logger's verbosity level.
Definition: robot-utils.hh:263
Index get_force_id_right_hand()
Definition: robot-utils.hh:127
std::vector< Index > m_urdf_to_sot
Map from the urdf index to the SoT index.
Definition: robot-utils.hh:175
std::string m_Right_Hand_Frame_Name
Definition: robot-utils.hh:157
RobotUtilShrPtr getRobotUtil(std::string &robotName)
FootUtil m_foot_util
Foot information.
Definition: robot-utils.hh:169
boost::property_tree::ptree property_tree_
Property tree.
Definition: robot-utils.hh:322
Index get_force_id_left_hand()
Definition: robot-utils.hh:123
std::map< std::string, std::string > parameters_strings_
Map of the parameters: map of strings.
Definition: robot-utils.hh:319
std::map< std::string, Index > m_name_to_id
Map from the name to the id.
Definition: robot-utils.hh:181
Type get_parameter(const std::string ¶meter_name)
Get a parameter of type string. If parameter_name already exists the value is overwritten. If not it is inserted.
Definition: robot-utils.hh:296
HandUtil m_hand_util
Hand information.
Definition: robot-utils.hh:172
Eigen::Ref< Eigen::VectorXd > RefVector
Definition: matrix-geometry.hh:68
Logger logger_
Definition: robot-utils.hh:316
std::map< Index, std::string > m_force_id_to_name
Definition: robot-utils.hh:102
Index get_force_id_left_foot()
Definition: robot-utils.hh:131
Definition: robot-utils.hh:57
bool isNameInRobotUtil(std::string &robotName)
std::map< Index, std::string > m_id_to_name
The map between id and name.
Definition: robot-utils.hh:184
Definition: abstract-sot-external-interface.hh:17
std::string m_urdf_filename
URDF file path.
Definition: robot-utils.hh:198