sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
control-manager.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2015, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_control_manager_H__
7 #define __sot_torque_control_control_manager_H__
8 
9 /* --------------------------------------------------------------------- */
10 /* --- API ------------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12 
13 #if defined(WIN32)
14 #if defined(__sot_torque_control_control_manager_H__)
15 #define SOTCONTROLMANAGER_EXPORT __declspec(dllexport)
16 #else
17 #define SOTCONTROLMANAGER_EXPORT __declspec(dllimport)
18 #endif
19 #else
20 #define SOTCONTROLMANAGER_EXPORT
21 #endif
22 
23 /* --------------------------------------------------------------------- */
24 /* --- INCLUDE --------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 
27 #include <map>
28 #include "boost/assign.hpp"
29 
30 #include <pinocchio/multibody/model.hpp>
31 #include <pinocchio/parsers/urdf.hpp>
32 
33 #include <tsid/robots/robot-wrapper.hpp>
34 #include <dynamic-graph/signal-helper.h>
35 #include <sot/core/matrix-geometry.hh>
36 #include <sot/core/robot-utils.hh>
38 
39 namespace dynamicgraph {
40 namespace sot {
41 namespace torque_control {
42 
43 /* --------------------------------------------------------------------- */
44 /* --- CLASS ----------------------------------------------------------- */
45 /* --------------------------------------------------------------------- */
46 
48 #define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
49 
50 class CtrlMode {
51  public:
52  int id;
53  std::string name;
54 
55  CtrlMode() : id(-1), name("None") {}
56  CtrlMode(int id, const std::string& name) : id(id), name(name) {}
57 };
58 
59 std::ostream& operator<<(std::ostream& os, const CtrlMode& s) {
60  os << s.id << "_" << s.name;
61  return os;
62 }
63 
64 class SOTCONTROLMANAGER_EXPORT ControlManager : public ::dynamicgraph::Entity {
66  DYNAMIC_GRAPH_ENTITY_DECL();
67 
68  public:
69  /* --- CONSTRUCTOR ---- */
70  ControlManager(const std::string& name);
71 
75  void init(const double& dt, const std::string& urdfFile, const std::string& robotRef);
76 
77  /* --- SIGNALS --- */
78  std::vector<dynamicgraph::SignalPtr<dynamicgraph::Vector, int>*> m_ctrlInputsSIN;
79  std::vector<dynamicgraph::SignalPtr<bool, int>*>
81  std::vector<dynamicgraph::Signal<dynamicgraph::Vector, int>*> m_jointsCtrlModesSOUT;
82 
83  DECLARE_SIGNAL_IN(i_measured, dynamicgraph::Vector);
84  DECLARE_SIGNAL_IN(tau, dynamicgraph::Vector);
85  DECLARE_SIGNAL_IN(tau_predicted, dynamicgraph::Vector);
86  DECLARE_SIGNAL_IN(i_max, dynamicgraph::Vector);
88  dynamicgraph::Vector);
89  DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector);
90 
91  DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
92  DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector);
93 
94  /* --- COMMANDS --- */
95 
97  void addCtrlMode(const std::string& name);
98  void ctrlModes();
99  void getCtrlMode(const std::string& jointName);
100  void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
101  void setCtrlMode(const int jid, const CtrlMode& cm);
102 
103  void resetProfiler();
104 
106  void setNameToId(const std::string& jointName, const double& jointId);
107  void setJointLimitsFromId(const double& jointId, const double& lq, const double& uq);
108 
110  void setForceLimitsFromId(const double& jointId, const dynamicgraph::Vector& lq, const dynamicgraph::Vector& uq);
111  void setForceNameToForceId(const std::string& forceName, const double& forceId);
112 
114  void setRightFootSoleXYZ(const dynamicgraph::Vector&);
115  void setRightFootForceSensorXYZ(const dynamicgraph::Vector&);
116  void setFootFrameName(const std::string&, const std::string&);
117  void setImuJointName(const std::string&);
118  void displayRobotUtil();
119 
121  void setHandFrameName(const std::string&, const std::string&);
122 
124  void setJoints(const dynamicgraph::Vector&);
125 
126  void setStreamPrintPeriod(const double& s);
127  void setSleepTime(const double& seconds);
128  void addEmergencyStopSIN(const std::string& name);
129 
130  /* --- ENTITY INHERITANCE --- */
131  virtual void display(std::ostream& os) const;
132 
133  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
134  logger_.stream(t) << ("[ControlManager-" + name + "] " + msg) << '\n';
135  }
136 
137  protected:
138  RobotUtilShrPtr m_robot_util;
139  tsid::robots::RobotWrapper* m_robot;
141  double m_dt;
145  int m_iter;
146  double m_sleep_time;
147 
148  std::vector<std::string> m_ctrlModes;
149  std::vector<CtrlMode> m_jointCtrlModes_current;
150  std::vector<CtrlMode> m_jointCtrlModes_previous;
151  std::vector<int> m_jointCtrlModesCountDown;
152 
153  bool convertStringToCtrlMode(const std::string& name, CtrlMode& cm);
154  bool convertJointNameToJointId(const std::string& name, unsigned int& id);
155  bool isJointInRange(unsigned int id, double q);
156  void updateJointCtrlModesOutputSignal();
157 
158 }; // class ControlManager
159 
160 } // namespace torque_control
161 } // namespace sot
162 } // namespace dynamicgraph
163 
164 #endif // #ifndef __sot_torque_control_control_manager_H__
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
DECLARE_SIGNAL_IN(i_max, dynamicgraph::Vector)
predicted joint torques (using motor model)
DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(tau, dynamicgraph::Vector)
motor currents
std::vector< dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * > m_ctrlInputsSIN
std::vector< CtrlMode > m_jointCtrlModes_previous
control mode of the joints
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector)
max torque allowed before stopping the controller
DECLARE_SIGNAL_IN(u_max, dynamicgraph::Vector)
max current allowed before stopping the controller (in Ampers)
double m_dt
true if the entity has been successfully initialized
void setCtrlMode(const std::string &jointName, const std::string &ctrlMode)
int m_iter
true at the first iteration, false otherwise
std::vector< int > m_jointCtrlModesCountDown
previous control mode of the joints
std::vector< std::string > m_ctrlModes
time to sleep at every iteration (to slow down simulation)
std::vector< dynamicgraph::SignalPtr< bool, int > * > m_emergencyStopSIN
std::vector< CtrlMode > m_jointCtrlModes_current
existing control modes
std::vector< dynamicgraph::Signal< dynamicgraph::Vector, int > * > m_jointsCtrlModesSOUT
emergency stop inputs. If one is true, control is set to zero forever
bool m_emergency_stop_triggered
control loop time period
DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector)
max desired current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_IN(tau_predicted, dynamicgraph::Vector)
estimated joint torques (using dynamic robot model + F/T sensors)
DECLARE_SIGNAL_IN(i_measured, dynamicgraph::Vector)
CtrlMode(int id, const std::string &name)
#define SOTCONTROLMANAGER_EXPORT
std::ostream & operator<<(std::ostream &os, const CtrlMode &s)
to read text file
Definition: treeview.dox:22