sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
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 <pinocchio/fwd.hpp>
28
29// include pinocchio first
30
31#include <dynamic-graph/signal-helper.h>
32
33#include <map>
34#include <pinocchio/multibody/model.hpp>
35#include <pinocchio/parsers/urdf.hpp>
36#include <sot/core/matrix-geometry.hh>
37#include <sot/core/robot-utils.hh>
39#include <tsid/robots/robot-wrapper.hpp>
40
41#include "boost/assign.hpp"
42
43namespace dynamicgraph {
44namespace sot {
45namespace torque_control {
46
47/* --------------------------------------------------------------------- */
48/* --- CLASS ----------------------------------------------------------- */
49/* --------------------------------------------------------------------- */
50
52#define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
53
54class CtrlMode {
55 public:
56 int id;
57 std::string name;
58
59 CtrlMode() : id(-1), name("None") {}
60 CtrlMode(int id, const std::string& name) : id(id), name(name) {}
61};
62
63std::ostream& operator<<(std::ostream& os, const CtrlMode& s) {
64 os << s.id << "_" << s.name;
65 return os;
66}
67
68class SOTCONTROLMANAGER_EXPORT ControlManager : public ::dynamicgraph::Entity {
70 DYNAMIC_GRAPH_ENTITY_DECL();
71
72 public:
73 /* --- CONSTRUCTOR ---- */
74 ControlManager(const std::string& name);
75
79 void init(const double& dt, const std::string& urdfFile,
80 const std::string& robotRef);
81
82 /* --- SIGNALS --- */
83 std::vector<dynamicgraph::SignalPtr<dynamicgraph::Vector, int>*>
85 std::vector<dynamicgraph::SignalPtr<bool, int>*>
88 std::vector<dynamicgraph::Signal<dynamicgraph::Vector, int>*>
90
91 DECLARE_SIGNAL_IN(i_measured, dynamicgraph::Vector);
93 tau, dynamicgraph::Vector);
96 tau_predicted,
97 dynamicgraph::Vector);
99 i_max, dynamicgraph::Vector);
102 u_max,
103 dynamicgraph::Vector);
106 dynamicgraph::Vector);
108
109 DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
111 u_safe,
112 dynamicgraph::Vector);
113
114 /* --- COMMANDS --- */
115
117 void addCtrlMode(const std::string& name);
118 void ctrlModes();
119 void getCtrlMode(const std::string& jointName);
120 void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
121 void setCtrlMode(const int jid, const CtrlMode& cm);
122
123 void resetProfiler();
124
126 void setNameToId(const std::string& jointName, const double& jointId);
127 void setJointLimitsFromId(const double& jointId, const double& lq,
128 const double& uq);
129
131 void setForceLimitsFromId(const double& jointId,
132 const dynamicgraph::Vector& lq,
133 const dynamicgraph::Vector& uq);
134 void setForceNameToForceId(const std::string& forceName,
135 const double& forceId);
136
138 void setRightFootSoleXYZ(const dynamicgraph::Vector&);
139 void setRightFootForceSensorXYZ(const dynamicgraph::Vector&);
140 void setFootFrameName(const std::string&, const std::string&);
141 void setImuJointName(const std::string&);
142 void displayRobotUtil();
143
145 void setHandFrameName(const std::string&, const std::string&);
146
148 void setJoints(const dynamicgraph::Vector&);
149
150 void setStreamPrintPeriod(const double& s);
151 void setSleepTime(const double& seconds);
152 void addEmergencyStopSIN(const std::string& name);
153
154 /* --- ENTITY INHERITANCE --- */
155 virtual void display(std::ostream& os) const;
156
157 void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
158 const char* = "", int = 0) {
159 logger_.stream(t) << ("[ControlManager-" + name + "] " + msg) << '\n';
160 }
161
162 protected:
163 RobotUtilShrPtr m_robot_util;
164 tsid::robots::RobotWrapper* m_robot;
165 bool
167 double m_dt;
175
176 std::vector<std::string> m_ctrlModes;
177 std::vector<CtrlMode>
179 std::vector<CtrlMode>
181 std::vector<int>
184
185 bool convertStringToCtrlMode(const std::string& name, CtrlMode& cm);
186 bool convertJointNameToJointId(const std::string& name, unsigned int& id);
187 bool isJointInRange(unsigned int id, double q);
188 void updateJointCtrlModesOutputSignal();
189
190}; // class ControlManager
191
192} // namespace torque_control
193} // namespace sot
194} // namespace dynamicgraph
195
196#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_IN(u_max, dynamicgraph::Vector)
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< dynamicgraph::SignalPtr< bool, int > * > m_emergencyStopSIN
std::vector< CtrlMode > m_jointCtrlModes_current
existing control modes
std::vector< dynamicgraph::Signal< dynamicgraph::Vector, int > * > m_jointsCtrlModesSOUT
bool m_emergency_stop_triggered
control loop time period
DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(tau_predicted, dynamicgraph::Vector)
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