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>*>
88 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';
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();
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
RobotUtilShrPtr m_robot_util
std::vector< CtrlMode > m_jointCtrlModes_previous
control mode of the joints
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector)
max torque allowed before stopping the controller
DECLARE_SIGNAL_IN(u_max, dynamicgraph::Vector)
max current allowed before stopping the controller (in Ampers)
double m_dt
true if the entity has been successfully initialized
tsid::robots::RobotWrapper * m_robot
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< std::string > m_ctrlModes
time to sleep at every iteration (to slow down simulation)
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
emergency stop inputs. If one is true, control is set to zero forever
bool m_emergency_stop_triggered
control loop time period
DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector)
max desired current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_IN(tau_predicted, dynamicgraph::Vector)
estimated joint torques (using dynamic robot model + F/T sensors)
DECLARE_SIGNAL_IN(i_measured, dynamicgraph::Vector)
CtrlMode(int id, const std::string &name)
#define SOTCONTROLMANAGER_EXPORT
AdmittanceController EntityClassName
std::ostream & operator<<(std::ostream &os, const CtrlMode &s)