GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/sot/core/parameter-server.hh Lines: 6 16 37.5 %
Date: 2023-03-13 12:09:37 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
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__