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__