sot-torque-control  1.5.3
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);
87  DECLARE_SIGNAL_IN(u_max,
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;
143  bool m_is_first_iter;
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__
dynamicgraph::sot::torque_control::ControlManager::m_emergency_stop_triggered
bool m_emergency_stop_triggered
control loop time period
Definition: control-manager.hh:142
dynamicgraph::sot::torque_control::ControlManager::m_jointsCtrlModesSOUT
std::vector< dynamicgraph::Signal< dynamicgraph::Vector, int > * > m_jointsCtrlModesSOUT
emergency stop inputs. If one is true, control is set to zero forever
Definition: control-manager.hh:81
dynamicgraph::sot::torque_control::ControlManager::m_jointCtrlModesCountDown
std::vector< int > m_jointCtrlModesCountDown
previous control mode of the joints
Definition: control-manager.hh:151
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::ControlManager::m_sleep_time
double m_sleep_time
Definition: control-manager.hh:146
vector-conversions.hh
dynamicgraph::sot::torque_control::CtrlMode::CtrlMode
CtrlMode()
Definition: control-manager.hh:55
dynamicgraph::sot::torque_control::ControlManager::m_initSucceeded
bool m_initSucceeded
Definition: control-manager.hh:140
dynamicgraph::sot::torque_control::CtrlMode::id
int id
Definition: control-manager.hh:52
dynamicgraph::sot::torque_control::ControlManager::m_iter
int m_iter
true at the first iteration, false otherwise
Definition: control-manager.hh:145
dynamicgraph::sot::torque_control::ControlManager::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: control-manager.hh:133
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::ControlManager::m_dt
double m_dt
true if the entity has been successfully initialized
Definition: control-manager.hh:141
dynamicgraph::sot::torque_control::CtrlMode::CtrlMode
CtrlMode(int id, const std::string &name)
Definition: control-manager.hh:56
dynamicgraph::sot::torque_control::ControlManager::m_jointCtrlModes_previous
std::vector< CtrlMode > m_jointCtrlModes_previous
control mode of the joints
Definition: control-manager.hh:150
dynamicgraph::sot::torque_control::ControlManager::m_emergencyStopSIN
std::vector< dynamicgraph::SignalPtr< bool, int > * > m_emergencyStopSIN
Definition: control-manager.hh:80
dynamicgraph::sot::torque_control::CtrlMode::name
std::string name
Definition: control-manager.hh:53
dynamicgraph::sot::torque_control::CtrlMode
Definition: control-manager.hh:50
dynamicgraph::sot::torque_control::ControlManager::m_jointCtrlModes_current
std::vector< CtrlMode > m_jointCtrlModes_current
existing control modes
Definition: control-manager.hh:149
dynamicgraph::sot::torque_control::ControlManager::m_ctrlModes
std::vector< std::string > m_ctrlModes
time to sleep at every iteration (to slow down simulation)
Definition: control-manager.hh:148
dynamicgraph::sot::torque_control::ControlManager
Definition: control-manager.hh:64
SOTCONTROLMANAGER_EXPORT
#define SOTCONTROLMANAGER_EXPORT
Definition: control-manager.hh:20
dynamicgraph::sot::torque_control::ControlManager::m_ctrlInputsSIN
std::vector< dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * > m_ctrlInputsSIN
Definition: control-manager.hh:78
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::ControlManager::m_robot_util
RobotUtilShrPtr m_robot_util
Definition: control-manager.hh:138
dynamicgraph::sot::torque_control::ControlManager::m_robot
tsid::robots::RobotWrapper * m_robot
Definition: control-manager.hh:139
dynamicgraph::sot::torque_control::operator<<
std::ostream & operator<<(std::ostream &os, const CtrlMode &s)
Definition: control-manager.hh:59