6 #ifndef __sot_torque_control_control_manager_H__ 7 #define __sot_torque_control_control_manager_H__ 14 #if defined(__sot_torque_control_control_manager_H__) 15 #define SOTCONTROLMANAGER_EXPORT __declspec(dllexport) 17 #define SOTCONTROLMANAGER_EXPORT __declspec(dllimport) 20 #define SOTCONTROLMANAGER_EXPORT 28 #include "boost/assign.hpp" 30 #include <pinocchio/multibody/model.hpp> 31 #include <pinocchio/parsers/urdf.hpp> 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> 48 #define CTRL_MODE_TRANSITION_TIME_STEP 1000.0 60 os << s.
id <<
"_" << s.
name;
66 DYNAMIC_GRAPH_ENTITY_DECL();
75 void init(
const double& dt,
const std::string& urdfFile,
const std::string& robotRef);
79 std::vector<dynamicgraph::SignalPtr<bool, int>*>
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);
91 DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
92 DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector);
97 void addCtrlMode(
const std::string& name);
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);
103 void resetProfiler();
106 void setNameToId(
const std::string& jointName,
const double& jointId);
107 void setJointLimitsFromId(
const double& jointId,
const double& lq,
const double& uq);
110 void setForceLimitsFromId(
const double& jointId,
const dynamicgraph::Vector& lq,
const dynamicgraph::Vector& uq);
111 void setForceNameToForceId(
const std::string& forceName,
const double& forceId);
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();
121 void setHandFrameName(
const std::string&,
const std::string&);
124 void setJoints(
const dynamicgraph::Vector&);
126 void setStreamPrintPeriod(
const double& s);
127 void setSleepTime(
const double& seconds);
128 void addEmergencyStopSIN(
const std::string& name);
131 virtual void display(std::ostream& os)
const;
133 void sendMsg(
const std::string& msg, MsgType t = MSG_TYPE_INFO,
const char* =
"",
int = 0) {
134 logger_.stream(t) << (
"[ControlManager-" + name +
"] " + msg) <<
'\n';
143 bool m_is_first_iter;
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();
164 #endif // #ifndef __sot_torque_control_control_manager_H__
tsid::robots::RobotWrapper * m_robot
int m_iter
true at the first iteration, false otherwise
RobotUtilShrPtr m_robot_util
#define SOTCONTROLMANAGER_EXPORT
std::vector< dynamicgraph::Signal< dynamicgraph::Vector, int > * > m_jointsCtrlModesSOUT
emergency stop inputs. If one is true, control is set to zero forever
std::vector< std::string > m_ctrlModes
time to sleep at every iteration (to slow down simulation)
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
std::vector< dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * > m_ctrlInputsSIN
std::vector< CtrlMode > m_jointCtrlModes_current
existing control modes
std::vector< dynamicgraph::SignalPtr< bool, int > * > m_emergencyStopSIN
bool m_emergency_stop_triggered
control loop time period
std::vector< int > m_jointCtrlModesCountDown
previous control mode of the joints
std::vector< CtrlMode > m_jointCtrlModes_previous
control mode of the joints
double m_dt
true if the entity has been successfully initialized
AdmittanceController EntityClassName
std::ostream & operator<<(std::ostream &os, const CtrlMode &s)
CtrlMode(int id, const std::string &name)