GCC Code Coverage Report

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			#ifndef __sot_core_robot_utils_H__
9			#define __sot_core_robot_utils_H__
10
11			/* --------------------------------------------------------------------- */
12			/* --- INCLUDE --------------------------------------------------------- */
13			/* --------------------------------------------------------------------- */
14
15			#include <map>
16			#include <pinocchio/fwd.hpp>
17			// keep pinocchio before boost
18
19			#include <dynamic-graph/linear-algebra.h>
20			#include <dynamic-graph/logger.h>
21			#include <dynamic-graph/signal-helper.h>
22
23			#include <boost/assign.hpp>
24			#include <boost/property_tree/ptree.hpp>
25			#include <sot/core/matrix-geometry.hh>
26
27			namespace dynamicgraph {
28			namespace sot {
29
30			struct SOT_CORE_EXPORT JointLimits {
31			double upper;
32			double lower;
33
34		24	JointLimits() : upper(0.0), lower(0.0) {}
35
36		1	JointLimits(double l, double u) : upper(u), lower(l) {}
37			};
38
39			typedef Eigen::VectorXd::Index Index;
40
41			class SOT_CORE_EXPORT ExtractJointMimics {
42			public:
43			/// Constructor
44			ExtractJointMimics(std::string &robot_model);
45
46			/// Get mimic joints.
47			const std::vector<std::string> &get_mimic_joints();
48
49			private:
50			void go_through(boost::property_tree::ptree &pt, int level, int stage);
51
52			// Create empty property tree object
53			boost::property_tree::ptree tree_;
54			std::vector<std::string> mimic_joints_;
55			std::string current_joint_name_;
56			void go_through_full();
57			};
58
59			struct SOT_CORE_EXPORT ForceLimits {
60			Eigen::VectorXd upper;
61			Eigen::VectorXd lower;
62
63	3/6 ✓ Branch 2 taken 24 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 24 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 24 times. ✗ Branch 9 not taken.	24	ForceLimits() : upper(Vector6d::Zero()), lower(Vector6d::Zero()) {}
64
65		✗	ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
66		✗	: upper(u), lower(l) {}
67
68			void display(std::ostream &os) const;
69			};
70
71			struct SOT_CORE_EXPORT ForceUtil {
72			std::map<Index, ForceLimits> m_force_id_to_limits;
73			std::map<std::string, Index> m_name_to_force_id;
74			std::map<Index, std::string> m_force_id_to_name;
75
76			Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand, m_Force_Id_Left_Foot,
77			m_Force_Id_Right_Foot;
78
79			void set_name_to_force_id(const std::string &name, const Index &force_id);
80
81			void set_force_id_to_limits(const Index &force_id,
82			const dynamicgraph::Vector &lf,
83			const dynamicgraph::Vector &uf);
84
85			void create_force_id_to_name_map();
86
87			Index get_id_from_name(const std::string &name);
88
89			const std::string &get_name_from_id(Index idx);
90			std::string cp_get_name_from_id(Index idx);
91
92			const ForceLimits &get_limits_from_id(Index force_id);
93			ForceLimits cp_get_limits_from_id(Index force_id);
94
95		✗	Index get_force_id_left_hand() { return m_Force_Id_Left_Hand; }
96
97		1	void set_force_id_left_hand(Index anId) { m_Force_Id_Left_Hand = anId; }
98
99		✗	Index get_force_id_right_hand() { return m_Force_Id_Right_Hand; }
100
101		1	void set_force_id_right_hand(Index anId) { m_Force_Id_Right_Hand = anId; }
102
103		✗	Index get_force_id_left_foot() { return m_Force_Id_Left_Foot; }
104
105		1	void set_force_id_left_foot(Index anId) { m_Force_Id_Left_Foot = anId; }
106
107		✗	Index get_force_id_right_foot() { return m_Force_Id_Right_Foot; }
108
109		1	void set_force_id_right_foot(Index anId) { m_Force_Id_Right_Foot = anId; }
110
111			void display(std::ostream &out) const;
112
113			}; // struct ForceUtil
114
115			struct SOT_CORE_EXPORT FootUtil {
116			/// Position of the foot soles w.r.t. the frame of the foot
117			dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
118
119			/// Position of the force/torque sensors w.r.t. the frame of the hosting link
120			dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
121
122			std::string m_Left_Foot_Frame_Name;
123			std::string m_Right_Foot_Frame_Name;
124			void display(std::ostream &os) const;
125			};
126
127			struct SOT_CORE_EXPORT HandUtil {
128			std::string m_Left_Hand_Frame_Name;
129			std::string m_Right_Hand_Frame_Name;
130			void display(std::ostream &os) const;
131			};
132
133			struct SOT_CORE_EXPORT RobotUtil {
134			public:
135			RobotUtil();
136
137			/// Forces data
138			ForceUtil m_force_util;
139
140			/// Foot information
141			FootUtil m_foot_util;
142
143			/// Hand information
144			HandUtil m_hand_util;
145
146			/// Map from the urdf index to the SoT index.
147			std::vector<Index> m_urdf_to_sot;
148
149			/// Nb of Dofs for the robot.
150			std::size_t m_nbJoints;
151
152			/// Map from the name to the id.
153			std::map<std::string, Index> m_name_to_id;
154
155			/// The map between id and name
156			std::map<Index, std::string> m_id_to_name;
157
158			/// The joint limits map.
159			std::map<Index, JointLimits> m_limits_map;
160
161			/// The name of the joint IMU is attached to
162			std::string m_imu_joint_name;
163
164			/// This method creates the map between id and name.
165			/// It is called each time a new link between id and name is inserted
166			/// (i.e. when set_name_to_id is called).
167			void create_id_to_name_map();
168
169			/// URDF file path
170			std::string m_urdf_filename;
171
172			dynamicgraph::Vector m_dgv_urdf_to_sot;
173
174			/** Given a joint name it finds the associated joint id.
175			* If the specified joint name is not found it returns -1;
176			* @param name Name of the joint to find.
177			* @return The id of the specified joint, -1 if not found. */
178			const Index &get_id_from_name(const std::string &name);
179
180			/** Given a joint id it finds the associated joint name.
181			* If the specified joint is not found it returns "Joint name not found";
182			* @param id Id of the joint to find.
183			* @return The name of the specified joint, "Joint name not found" if not
184			* found. */
185
186			/// Get the joint name from its index
187			const std::string &get_name_from_id(Index id);
188
189			/// Set relation between the name and the SoT id
190			void set_name_to_id(const std::string &jointName, const Index &jointId);
191
192			/// Set the map between urdf index and sot index
193			void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
194			void set_urdf_to_sot(const dynamicgraph::Vector &urdf_to_sot);
195
196			/// Set the limits (lq,uq) for joint idx
197			void set_joint_limits_for_id(const Index &idx, const double &lq,
198			const double &uq);
199
200			bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
201
202			bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
203
204			bool velocity_urdf_to_sot(ConstRefVector q_urdf, ConstRefVector v_urdf,
205			RefVector v_sot);
206
207			bool velocity_sot_to_urdf(ConstRefVector q_urdf, ConstRefVector v_sot,
208			RefVector v_urdf);
209
210			bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
211			bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
212
213			bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
214			bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
215
216			/** Given a joint id it finds the associated joint limits.
217			* If the specified joint is not found it returns JointLimits(0,0).
218			* @param id Id of the joint to find.
219			* @return The limits of the specified joint, JointLimits(0,0) if not found.
220			*/
221			const JointLimits &get_joint_limits_from_id(Index id);
222			JointLimits cp_get_joint_limits_from_id(Index id);
223
224			/** \name Logger related methods */
225			/** \{*/
226			/// \brief Send messages \c msg with level \c t. Add string \c file and \c
227			/// line to message.
228			void sendMsg(const std::string &msg, MsgType t = MSG_TYPE_INFO,
229			const std::string &lineId = "");
230
231			/// \brief Specify the verbosity level of the logger.
232			void setLoggerVerbosityLevel(LoggerVerbosity lv) { logger_.setVerbosity(lv); }
233
234			/// \brief Get the logger's verbosity level.
235			LoggerVerbosity getLoggerVerbosityLevel() { return logger_.getVerbosity(); };
236
237			void display(std::ostream &os) const;
238
239			/*{ \name Handling general parameters /
240			/** \brief Set a parameter of type string.
241			If parameter_name already exists the value is overwritten.
242			If not it is inserted.
243			*/
244			template <typename Type>
245		4	void set_parameter(const std::string &parameter_name,
246			const Type &parameter_value) {
247			try {
248			typedef boost::property_tree::ptree::path_type path;
249	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	4	path apath(parameter_name, '/');
250	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	4	property_tree_.put<Type>(apath, parameter_value);
251	0/2 ✗ Branch 2 not taken. ✗ Branch 3 not taken.	4	} catch (const boost::property_tree::ptree_error &e) {
252		✗	std::ostringstream oss;
253			oss << "Robot utils: parameter path is invalid " << '\n'
254			<< " for set_parameter(" << parameter_name << ")\n"
255		✗	<< e.what() << std::endl;
256		✗	sendMsg(oss.str(), MSG_TYPE_ERROR);
257		✗	return;
258			}
259			}
260
261			/** \brief Get a parameter of type string.
262			If parameter_name already exists the value is overwritten.
263			If not it is inserted.
264			@param parameter_name: Name of the parameter
265			Return false if the parameter is not found.
266			*/
267			template <typename Type>
268		2	Type get_parameter(const std::string &parameter_name) {
269			try {
270		2	boost::property_tree::ptree::path_type apath(parameter_name, '/');
271		2	const Type &res = property_tree_.get<Type>(apath);
272
273		3	return res;
274		2	} catch (const boost::property_tree::ptree_error &e) {
275		✗	std::ostringstream oss;
276			oss << "Robot utils: parameter path is invalid " << '\n'
277			<< " for get_parameter(" << parameter_name << ")\n"
278		✗	<< e.what() << std::endl;
279		✗	sendMsg(oss.str(), MSG_TYPE_ERROR);
280		✗	throw;
281			}
282			}
283			/** @} */
284
285			/** Access to property tree directly */
286			boost::property_tree::ptree &get_property_tree();
287
288			protected:
289			Logger logger_;
290
291			/** \brief Map of the parameters: map of strings. */
292			std::map<std::string, std::string> parameters_strings_;
293
294			/** \brief Property tree */
295			boost::property_tree::ptree property_tree_;
296			}; // struct RobotUtil
297
298			/// Accessors - This should be changed to RobotUtilPtrShared
299			typedef std::shared_ptr<RobotUtil> RobotUtilShrPtr;
300
301			RobotUtilShrPtr RefVoidRobotUtil();
302			RobotUtilShrPtr getRobotUtil(std::string &robotName);
303			bool isNameInRobotUtil(std::string &robotName);
304			RobotUtilShrPtr createRobotUtil(std::string &robotName);
305			std::shared_ptr<std::vector<std::string> > getListOfRobots();
306
307			bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot);
308
309			} // namespace sot
310			} // namespace dynamicgraph
311
312			#endif // sot_torque_control_common_h_
313