GCC Code Coverage Report

Directory: ./
File: include/sot/core/parameter-server.hh
Date: 2024-12-13 12:22:33
Exec Total Coverage
Lines: 6 16 37.5%
Branches: 1 20 5.0%
Line Branch Exec Source
1 /*
2 * Copyright 2015, Andrea Del Prete, LAAS-CNRS
3 *
4 * This file is part of sot-torque-control.
5 * sot-torque-control is free software: you can redistribute it and/or
6 * modify it under the terms of the GNU Lesser General Public License
7 * as published by the Free Software Foundation, either version 3 of
8 * the License, or (at your option) any later version.
9 * sot-torque-control is distributed in the hope that it will be
10 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU Lesser General Public License for more details. You should
13 * have received a copy of the GNU Lesser General Public License along
14 * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
15 */
16
17 #ifndef __sot_torque_control_parameter_server_H__
18 #define __sot_torque_control_parameter_server_H__
19
20 /* --------------------------------------------------------------------- */
21 /* --- API ------------------------------------------------------------- */
22 /* --------------------------------------------------------------------- */
23
24 #if defined(WIN32)
25 #if defined(__sot_torque_parameter_server_H__)
26 #define SOTParameterServer_EXPORT __declspec(dllexport)
27 #else
28 #define SOTParameterServer_EXPORT __declspec(dllimport)
29 #endif
30 #else
31 #define SOTParameterServer_EXPORT
32 #endif
33
34 /* --------------------------------------------------------------------- */
35 /* --- INCLUDE --------------------------------------------------------- */
36 /* --------------------------------------------------------------------- */
37
38 #include <dynamic-graph/signal-helper.h>
39
40 #include <map>
41 #include <sot/core/matrix-geometry.hh>
42 #include <sot/core/robot-utils.hh>
43
44 #include "boost/assign.hpp"
45
46 namespace dynamicgraph {
47 namespace sot {
48
49 /* --------------------------------------------------------------------- */
50 /* --- CLASS ----------------------------------------------------------- */
51 /* --------------------------------------------------------------------- */
52
53 /// Number of time step to transition from one ctrl mode to another
54 #define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
55
56 class SOTParameterServer_EXPORT ParameterServer
57 : public ::dynamicgraph::Entity {
58 typedef ParameterServer EntityClassName;
59 ✗ DYNAMIC_GRAPH_ENTITY_DECL();
60
61 public:
62 /* --- CONSTRUCTOR ---- */
63 ParameterServer(const std::string &name);
64
65 ✗ ~ParameterServer(){};
66
67 /// Initialize
68 /// @param dt: control interval
69 /// @param urdfFile: path to the URDF model of the robot
70 void init(const double &dt, const std::string &urdfFile,
71 const std::string &robotRef);
72
73 /// Initialize
74 /// @param dt: control interval provided by the device.
75 /// The urdf model is found by reading /robot_description
76 /// The robot name is found using the name inside robot_description
77 void init_simple(const double &dt);
78 /* --- SIGNALS --- */
79
80 /* --- COMMANDS --- */
81
82 /// Commands related to joint name and joint id
83 void setNameToId(const std::string &jointName, const double &jointId);
84 void setJointLimitsFromId(const double &jointId, const double &lq,
85 const double &uq);
86
87 /// Command related to ForceUtil
88 void setForceLimitsFromId(const double &jointId,
89 const dynamicgraph::Vector &lq,
90 const dynamicgraph::Vector &uq);
91 void setForceNameToForceId(const std::string &forceName,
92 const double &forceId);
93
94 /// \name Commands related to FootUtil
95 /// @{
96 void setRightFootSoleXYZ(const dynamicgraph::Vector &);
97 void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
98 void setFootFrameName(const std::string &, const std::string &);
99 void setHandFrameName(const std::string &, const std::string &);
100 void setImuJointName(const std::string &);
101 void displayRobotUtil();
102 /// @}
103 /// \name Commands related to the model
104 /// @{
105 template <typename Type>
106 4 void setParameter(const std::string &ParameterName,
107 const Type &ParameterValue) {
108
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 4 if (!m_initSucceeded) {
109 ✗ DYNAMIC_GRAPH_ENTITY_WARNING(*this)
110 ✗ << "Cannot set parameter " << ParameterName << " to "
111 ✗ << ParameterValue << " before initialization!\n";
112 ✗ return;
113 }
114
115 4 m_robot_util->set_parameter<Type>(ParameterName, ParameterValue);
116 }
117
118 template <typename Type>
119 2 Type getParameter(const std::string &ParameterName) {
120 2 if (!m_initSucceeded) {
121 ✗ DYNAMIC_GRAPH_ENTITY_WARNING(*this)
122 ✗ << "Cannot get parameter " << ParameterName
123 ✗ << " before initialization!\n";
124 ✗ throw std::runtime_error("Cannot get parameter before initialization");
125 }
126 2 return m_robot_util->get_parameter<Type>(ParameterName);
127 }
128
129 /// @}
130 /// Set the mapping between urdf and sot.
131 void setJoints(const dynamicgraph::Vector &);
132
133 /* --- ENTITY INHERITANCE --- */
134 virtual void display(std::ostream &os) const;
135
136 protected:
137 RobotUtilShrPtr m_robot_util;
138 bool
139 m_initSucceeded; /// true if the entity has been successfully initialized
140 double m_dt; /// control loop time period
141 bool m_emergency_stop_triggered; /// true if an emergency condition as been
142 /// triggered either by an other entity, or
143 /// by control limit violation
144 bool m_is_first_iter; /// true at the first iteration, false otherwise
145 int m_iter;
146 double m_sleep_time; /// time to sleep at every iteration (to slow down
147 /// simulation)
148
149 bool convertJointNameToJointId(const std::string &name, unsigned int &id);
150 bool isJointInRange(unsigned int id, double q);
151 void updateJointCtrlModesOutputSignal();
152
153 }; // class ParameterServer
154
155 } // namespace sot
156 } // namespace dynamicgraph
157
158 #endif // #ifndef __sot_torque_control_control_manager_H__
159