sot-talos-balance  1.5.0
talos-control-manager.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_control_manager_H__
18 #define __sot_torque_control_control_manager_H__
19 
20 /* --------------------------------------------------------------------- */
21 /* --- API ------------------------------------------------------------- */
22 /* --------------------------------------------------------------------- */
23 
24 #if defined (WIN32)
25 # if defined (__sot_torque_control_control_manager_H__)
26 # define TALOS_CONTROL_MANAGER_EXPORT __declspec(dllexport)
27 # else
28 # define TALOS_CONTROL_MANAGER_EXPORT __declspec(dllimport)
29 # endif
30 #else
31 # define TALOS_CONTROL_MANAGER_EXPORT
32 #endif
33 
34 
35 /* --------------------------------------------------------------------- */
36 /* --- INCLUDE --------------------------------------------------------- */
37 /* --------------------------------------------------------------------- */
38 
39 #include <dynamic-graph/signal-helper.h>
40 #include <sot/core/matrix-geometry.hh>
41 #include <sot/core/robot-utils.hh>
42 #include <map>
43 #include "boost/assign.hpp"
44 
45 namespace dynamicgraph {
46  namespace sot {
47  namespace talos_balance {
48 
49  /* --------------------------------------------------------------------- */
50  /* --- CLASS ----------------------------------------------------------- */
51  /* --------------------------------------------------------------------- */
52 
54 #define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
55 
56  class CtrlMode
57  {
58  public:
59  int id;
60  std::string name;
61 
62  CtrlMode(): id(-1), name("None"){}
63  CtrlMode(int id, const std::string& name):
64  id(id), name(name) {}
65  };
66 
67  std::ostream& operator<<( std::ostream& os, const CtrlMode& s )
68  {
69  os<<s.id<<"_"<<s.name;
70  return os;
71  }
72 
74  :public::dynamicgraph::Entity
75  {
78  DYNAMIC_GRAPH_ENTITY_DECL();
79 
80  public:
81  /* --- CONSTRUCTOR ---- */
82  TalosControlManager( const std::string & name);
83 
87  void init(const double & dt,
88  const std::string & robotRef);
89 
90  /* --- SIGNALS --- */
91  std::vector<dynamicgraph::SignalPtr<dynamicgraph::Vector,int>*> m_ctrlInputsSIN;
92  std::vector< dynamicgraph::SignalPtr<bool,int>* > m_emergencyStopVector;
93  std::vector<dynamicgraph::Signal<dynamicgraph::Vector,int>*> m_jointsCtrlModesSOUT;
94 
95  DECLARE_SIGNAL_IN(u_max, dynamicgraph::Vector);
96  DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
97  DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector);
98 
99  /* --- COMMANDS --- */
100 
102  void addCtrlMode(const std::string& name);
103  void ctrlModes();
104  void getCtrlMode(const std::string& jointName);
105  void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
106  void setCtrlMode(const int jid, const CtrlMode& cm);
107 
108  void resetProfiler();
109 
111  // void setNameToId(const std::string& jointName, const double & jointId);
112  // void setJointLimitsFromId(const double &jointId, const double &lq, const double &uq);
113 
115  // void setJoints(const dynamicgraph::Vector &);
116 
117  // void setStreamPrintPeriod(const double & s);
118 
119  void setSleepTime(const double &seconds);
120  void addEmergencyStopSIN(const std::string& name);
121 
122  /* --- ENTITY INHERITANCE --- */
123  virtual void display( std::ostream& os ) const;
124 
125  protected:
126  RobotUtilShrPtr m_robot_util;
127  size_t m_numDofs;
129  double m_dt;
132  int m_iter;
133  double m_sleep_time;
134 
135  std::vector<std::string> m_ctrlModes;
136  std::vector<CtrlMode> m_jointCtrlModes_current;
137  std::vector<CtrlMode> m_jointCtrlModes_previous;
138  std::vector<int> m_jointCtrlModesCountDown;
139 
140  bool convertStringToCtrlMode(const std::string& name, CtrlMode& cm);
141  bool convertJointNameToJointId(const std::string& name, unsigned int& id);
142  //bool isJointInRange(unsigned int id, double q);
143  void updateJointCtrlModesOutputSignal();
144 
145  }; // class TalosControlManager
146 
147  } // namespace talos_balance
148  } // namespace sot
149 } // namespace dynamicgraph
150 
151 
152 
153 #endif // #ifndef __sot_torque_control_control_manager_H__
CtrlMode(int id, const std::string &name)
std::vector< CtrlMode > m_jointCtrlModes_current
existing control modes
#define TALOS_CONTROL_MANAGER_EXPORT
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
std::vector< CtrlMode > m_jointCtrlModes_previous
control mode of the joints
std::vector< int > m_jointCtrlModesCountDown
previous control mode of the joints
std::vector< dynamicgraph::SignalPtr< bool, int > *> m_emergencyStopVector
int m_iter
true at the first iteration, false otherwise
double m_dt
true if the entity has been successfully initialized
std::vector< dynamicgraph::Signal< dynamicgraph::Vector, int > * > m_jointsCtrlModesSOUT
emergency stop inputs. If one is true, control is set to zero forever
bool m_is_first_iter
true if an emergency condition as been triggered either by an other entity, or by control limit viola...
std::vector< std::string > m_ctrlModes
time to sleep at every iteration (to slow down simulation)
std::ostream & operator<<(std::ostream &os, const CtrlMode &s)
std::vector< dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * > m_ctrlInputsSIN