18 #include <sot/core/debug.hh> 19 #include <dynamic-graph/factory.h> 21 #include <dynamic-graph/all-commands.h> 22 #include <sot/core/stop-watch.hh> 32 namespace talos_balance
42 #define PROFILE_PWM_DESIRED_COMPUTATION "Control manager " 43 #define PROFILE_DYNAMIC_GRAPH_PERIOD "Control period " 45 #define INPUT_SIGNALS m_u_maxSIN 46 #define OUTPUT_SIGNALS m_uSOUT << m_u_safeSOUT 54 "TalosControlManager");
65 ,m_robot_util(RefVoidRobotUtil())
66 ,m_initSucceeded(false)
67 ,m_emergency_stop_triggered(false)
68 ,m_is_first_iter(true)
77 docCommandVoid2(
"Initialize the entity.",
78 "Time period in seconds (double)",
79 "Robot reference (string)")));
80 addCommand(
"addCtrlMode",
82 docCommandVoid1(
"Create an input signal with name 'ctrl_x' where x is the specified name.",
85 addCommand(
"ctrlModes",
87 docCommandVoid0(
"Get a list of all the available control modes.")));
89 addCommand(
"setCtrlMode",
91 docCommandVoid2(
"Set the control mode for a joint.",
92 "(string) joint name",
93 "(string) control mode")));
95 addCommand(
"getCtrlMode",
97 docCommandVoid1(
"Get the control mode of a joint.",
98 "(string) joint name")));
100 addCommand(
"resetProfiler",
102 docCommandVoid0(
"Reset the statistics computed by the profiler (print this entity to see them).")));
104 addCommand(
"addEmergencyStopSIN",
106 docCommandVoid1(
"Add emergency signal input from another entity that can stop the control if necessary.",
107 "(string) signal name : 'emergencyStop_' + name")));
109 addCommand(
"isEmergencyStopTriggered", makeDirectGetter(*
this,&
m_emergency_stop_triggered, docDirectGetter(
"Check whether emergency stop is triggered",
"bool")));
113 const std::string &robotRef)
116 return SEND_MSG(
"Timestep must be positive", MSG_TYPE_ERROR);
120 vector<string> package_dirs;
121 std::string localName(robotRef);
122 if (!isNameInRobotUtil(localName))
124 SEND_MSG(
"You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
147 SEND_MSG(
"Cannot compute signal u before initialization!",MSG_TYPE_WARNING);
161 int cm_id, cm_id_prev;
167 SEND_MSG(
"You forgot to set the control mode of joint "+toString(i), MSG_TYPE_ERROR_STREAM);
185 s(i) = alpha*ctrl_prev(i) + (1-alpha)*ctrl(i);
190 SEND_MSG(
"Joint "+toString(i)+
" changed ctrl mode from "+toString(cm_id_prev)+
191 " to "+toString(cm_id),MSG_TYPE_INFO);
215 SEND_WARNING_STREAM_MSG(
"Cannot compute signal u_safe before initialization!");
227 SEND_MSG(
"Emergency Stop has been triggered by an external entity", MSG_TYPE_ERROR);
236 if(fabs(u(i)) > ctrl_max(i))
239 SEND_MSG(
"Joint " + toString(i) +
" desired control is too large: "+ toString(u(i))+
" > "+toString(ctrl_max(i)), MSG_TYPE_ERROR_STREAM);
260 return SEND_MSG(
"It already exists a control mode with name "+name, MSG_TYPE_ERROR);
263 m_ctrlInputsSIN.push_back(
new SignalPtr<dynamicgraph::Vector, int>(NULL,
264 getClassName()+
"("+getName()+
")::input(dynamicgraph::Vector)::ctrl_"+name));
269 getClassName()+
"("+getName()+
")::output(dynamicgraph::Vector)::joints_ctrl_mode_"+name));
303 std::stringstream ss(jointName);
305 std::list<int> jIdList;
307 while (std::getline(ss, item,
'-'))
309 SEND_MSG(
"parsed joint : "+item, MSG_TYPE_INFO);
311 jIdList.push_back(i);
313 for (std::list<int>::iterator it=jIdList.begin(); it != jIdList.end(); ++it)
322 return SEND_MSG(
"Cannot change control mode of joint "+toString(jid)+
323 " because its previous ctrl mode transition has not terminated yet: "+
327 return SEND_MSG(
"Cannot change control mode of joint "+toString(jid)+
328 " because it has already the specified ctrl mode", MSG_TYPE_ERROR);
351 SEND_MSG(ss.str(),MSG_TYPE_INFO);
363 getProfiler().reset_all();
375 return SEND_MSG(
"Sleep time cannot be negative!", MSG_TYPE_ERROR);
381 SEND_MSG(
"New emergency signal input emergencyStop_" + name +
" created",MSG_TYPE_INFO);
383 m_emergencyStopVector.push_back(
new SignalPtr<bool, int>(NULL, getClassName()+
"("+getName()+
")::input(bool)::emergencyStop_"+name));
397 SEND_MSG(
"You should call init first. The size of the vector is unknown.", MSG_TYPE_ERROR);
428 SEND_MSG(
"The specified control mode does not exist", MSG_TYPE_ERROR);
429 SEND_MSG(
"Possible control modes are: "+toString(
m_ctrlModes), MSG_TYPE_INFO);
440 SEND_MSG(
"The specified joint name does not exist: "+name, MSG_TYPE_ERROR);
441 std::stringstream ss;
443 ss<< toString(it) <<
", ";
444 SEND_MSG(
"Possible joint names are: "+ss.str(), MSG_TYPE_INFO);
447 id = (
unsigned int )jid;
480 os <<
"TalosControlManager "<<getName();
483 getProfiler().report_all(3, os);
485 catch (ExceptionSignal e) {}
TalosControlManager(const std::string &name)
std::vector< CtrlMode > m_jointCtrlModes_current
existing control modes
#define CTRL_MODE_TRANSITION_TIME_STEP
Number of time step to transition from one ctrl mode to another.
void getCtrlMode(const std::string &jointName)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
std::vector< CtrlMode > m_jointCtrlModes_previous
control mode of the joints
std::vector< int > m_jointCtrlModesCountDown
previous control mode of the joints
virtual void display(std::ostream &os) const
std::vector< dynamicgraph::SignalPtr< bool, int > *> m_emergencyStopVector
void setSleepTime(const double &seconds)
Commands related to joint name and joint id.
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)
void addEmergencyStopSIN(const std::string &name)
void init(const double &dt, const std::string &robotRef)
void updateJointCtrlModesOutputSignal()
bool m_emergency_stop_triggered
control loop time period
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TalosControlManager, "TalosControlManager")
Statistics & getStatistics()
bool convertJointNameToJointId(const std::string &name, unsigned int &id)
bool convertStringToCtrlMode(const std::string &name, CtrlMode &cm)
counters used for the transition between two ctrl modes
void addCtrlMode(const std::string &name)
safe motor control
std::vector< dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * > m_ctrlInputsSIN
RobotUtilShrPtr m_robot_util
void setCtrlMode(const std::string &jointName, const std::string &ctrlMode)
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)