sot-core  4.11.6
Hierarchical task solver plug-in for dynamic-graph.
parameter-server.hh
Go to the documentation of this file.
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 "boost/assign.hpp"
39 #include <dynamic-graph/signal-helper.h>
40 #include <map>
42 #include <sot/core/robot-utils.hh>
43 
44 namespace dynamicgraph {
45 namespace sot {
46 
47 /* --------------------------------------------------------------------- */
48 /* --- CLASS ----------------------------------------------------------- */
49 /* --------------------------------------------------------------------- */
50 
52 #define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
53 
55  : public ::dynamicgraph::Entity {
57  DYNAMIC_GRAPH_ENTITY_DECL();
58 
59 public:
60  /* --- CONSTRUCTOR ---- */
61  ParameterServer(const std::string &name);
62 
64 
68  void init(const double &dt, const std::string &urdfFile,
69  const std::string &robotRef);
70 
75  void init_simple(const double &dt);
76  /* --- SIGNALS --- */
77 
78  /* --- COMMANDS --- */
79 
81  void setNameToId(const std::string &jointName, const double &jointId);
82  void setJointLimitsFromId(const double &jointId, const double &lq,
83  const double &uq);
84 
86  void setForceLimitsFromId(const double &jointId,
87  const dynamicgraph::Vector &lq,
88  const dynamicgraph::Vector &uq);
89  void setForceNameToForceId(const std::string &forceName,
90  const double &forceId);
91 
94  void setRightFootSoleXYZ(const dynamicgraph::Vector &);
95  void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
96  void setFootFrameName(const std::string &, const std::string &);
97  void setImuJointName(const std::string &);
98  void displayRobotUtil();
102  template <typename Type>
103  void setParameter(const std::string &ParameterName,
104  const Type &ParameterValue) {
105  if (!m_initSucceeded) {
106  DYNAMIC_GRAPH_ENTITY_WARNING(*this)
107  << "Cannot set parameter " << ParameterName << " to "
108  << ParameterValue << " before initialization!\n";
109  return;
110  }
111 
112  m_robot_util->set_parameter<Type>(ParameterName, ParameterValue);
113  }
114 
115  template <typename Type> Type getParameter(const std::string &ParameterName) {
116 
117  if (!m_initSucceeded) {
118  DYNAMIC_GRAPH_ENTITY_WARNING(*this)
119  << "Cannot get parameter " << ParameterName
120  << " before initialization!\n";
121  Type ParameterValue;
122  return ParameterValue;
123  }
124  return m_robot_util->get_parameter<Type>(ParameterName);
125  }
126 
129  void setJoints(const dynamicgraph::Vector &);
130 
131  /* --- ENTITY INHERITANCE --- */
132  virtual void display(std::ostream &os) const;
133 
134 protected:
137  double m_dt;
139  bool m_is_first_iter;
142  int m_iter;
143  double m_sleep_time;
144 
146  bool convertJointNameToJointId(const std::string &name, unsigned int &id);
147  bool isJointInRange(unsigned int id, double q);
148  void updateJointCtrlModesOutputSignal();
149 
150 }; // class ParameterServer
151 
152 } // namespace sot
153 } // namespace dynamicgraph
154 
155 #endif // #ifndef __sot_torque_control_control_manager_H__
int m_iter
true at the first iteration, false otherwise
Definition: parameter-server.hh:142
bool m_emergency_stop_triggered
control loop time period
Definition: parameter-server.hh:138
double m_dt
true if the entity has been successfully initialized
Definition: parameter-server.hh:137
~ParameterServer()
Definition: parameter-server.hh:63
double m_sleep_time
Definition: parameter-server.hh:143
#define SOTParameterServer_EXPORT
Definition: parameter-server.hh:31
Definition: parameter-server.hh:54
RobotUtilShrPtr m_robot_util
Definition: parameter-server.hh:135
std::shared_ptr< RobotUtil > RobotUtilShrPtr
Accessors - This should be changed to RobotUtilPtrShared.
Definition: robot-utils.hh:299
Type getParameter(const std::string &ParameterName)
Definition: parameter-server.hh:115
void setParameter(const std::string &ParameterName, const Type &ParameterValue)
Definition: parameter-server.hh:103
Definition: abstract-sot-external-interface.hh:17
bool m_initSucceeded
Definition: parameter-server.hh:136