#include <sot/torque_control/control-manager.hh>
Public Member Functions | |
ControlManager (const std::string &name) | |
void | addCtrlMode (const std::string &name) |
same as u when everything is fine, 0 otherwise More... | |
void | addEmergencyStopSIN (const std::string &name) |
void | ctrlModes () |
DECLARE_SIGNAL_IN (i_max, dynamicgraph::Vector) | |
predicted joint torques (using motor model) More... | |
DECLARE_SIGNAL_IN (i_measured, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (tau, dynamicgraph::Vector) | |
motor currents More... | |
DECLARE_SIGNAL_IN (tau_max, dynamicgraph::Vector) | |
max desired current allowed before stopping the controller (in Ampers) More... | |
DECLARE_SIGNAL_IN (tau_predicted, dynamicgraph::Vector) | |
estimated joint torques (using dynamic robot model + F/T sensors) More... | |
DECLARE_SIGNAL_IN (u_max, dynamicgraph::Vector) | |
max current allowed before stopping the controller (in Ampers) More... | |
DECLARE_SIGNAL_OUT (u, dynamicgraph::Vector) | |
max torque allowed before stopping the controller More... | |
DECLARE_SIGNAL_OUT (u_safe, dynamicgraph::Vector) | |
virtual void | display (std::ostream &os) const |
void | displayRobotUtil () |
void | getCtrlMode (const std::string &jointName) |
void | init (const double &dt, const std::string &urdfFile, const std::string &robotRef) |
void | resetProfiler () |
void | sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0) |
void | setCtrlMode (const int jid, const CtrlMode &cm) |
void | setCtrlMode (const std::string &jointName, const std::string &ctrlMode) |
void | setFootFrameName (const std::string &, const std::string &) |
void | setForceLimitsFromId (const double &jointId, const dynamicgraph::Vector &lq, const dynamicgraph::Vector &uq) |
Command related to ForceUtil. More... | |
void | setForceNameToForceId (const std::string &forceName, const double &forceId) |
void | setHandFrameName (const std::string &, const std::string &) |
Commands related to HandUtil. More... | |
void | setImuJointName (const std::string &) |
void | setJointLimitsFromId (const double &jointId, const double &lq, const double &uq) |
void | setJoints (const dynamicgraph::Vector &) |
Set the mapping between urdf and sot. More... | |
void | setNameToId (const std::string &jointName, const double &jointId) |
Commands related to joint name and joint id. More... | |
void | setRightFootForceSensorXYZ (const dynamicgraph::Vector &) |
void | setRightFootSoleXYZ (const dynamicgraph::Vector &) |
Commands related to FootUtil. More... | |
void | setSleepTime (const double &seconds) |
void | setStreamPrintPeriod (const double &s) |
Public Attributes | |
std::vector< dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * > | m_ctrlInputsSIN |
std::vector< dynamicgraph::SignalPtr< bool, int > * > | m_emergencyStopSIN |
std::vector< dynamicgraph::Signal< dynamicgraph::Vector, int > * > | m_jointsCtrlModesSOUT |
emergency stop inputs. If one is true, control is set to zero forever More... | |
Protected Member Functions | |
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 More... | |
bool | isJointInRange (unsigned int id, double q) |
void | updateJointCtrlModesOutputSignal () |
Protected Attributes | |
std::vector< std::string > | m_ctrlModes |
time to sleep at every iteration (to slow down simulation) More... | |
double | m_dt |
true if the entity has been successfully initialized More... | |
bool | m_emergency_stop_triggered |
control loop time period More... | |
bool | m_initSucceeded |
bool | m_is_first_iter |
int | m_iter |
true at the first iteration, false otherwise More... | |
std::vector< CtrlMode > | m_jointCtrlModes_current |
existing control modes More... | |
std::vector< CtrlMode > | m_jointCtrlModes_previous |
control mode of the joints More... | |
std::vector< int > | m_jointCtrlModesCountDown |
previous control mode of the joints More... | |
tsid::robots::RobotWrapper * | m_robot |
RobotUtilShrPtr | m_robot_util |
double | m_sleep_time |
Definition at line 64 of file control-manager.hh.
ControlManager | ( | const std::string & | name | ) |
Definition at line 43 of file control-manager.cpp.
References ControlManager::addCtrlMode(), ControlManager::addEmergencyStopSIN(), ControlManager::ctrlModes(), ControlManager::displayRobotUtil(), ControlManager::getCtrlMode(), ControlManager::init(), INPUT_SIGNALS, OUTPUT_SIGNALS, ControlManager::resetProfiler(), ControlManager::setCtrlMode(), ControlManager::setFootFrameName(), ControlManager::setForceLimitsFromId(), ControlManager::setForceNameToForceId(), ControlManager::setHandFrameName(), ControlManager::setImuJointName(), ControlManager::setJointLimitsFromId(), ControlManager::setJoints(), ControlManager::setNameToId(), ControlManager::setRightFootForceSensorXYZ(), ControlManager::setRightFootSoleXYZ(), ControlManager::setSleepTime(), and ControlManager::setStreamPrintPeriod().
void addCtrlMode | ( | const std::string & | name | ) |
same as u when everything is fine, 0 otherwise
Commands related to the control mode.
Definition at line 307 of file control-manager.cpp.
References ControlManager::m_ctrlInputsSIN, ControlManager::m_ctrlModes, ControlManager::m_jointsCtrlModesSOUT, and ControlManager::updateJointCtrlModesOutputSignal().
Referenced by ControlManager::ControlManager().
void addEmergencyStopSIN | ( | const std::string & | name | ) |
Definition at line 404 of file control-manager.cpp.
References ControlManager::m_emergencyStopSIN.
Referenced by ControlManager::ControlManager().
|
protected |
Definition at line 547 of file control-manager.cpp.
References ControlManager::m_robot_util.
Referenced by ControlManager::getCtrlMode(), and ControlManager::setCtrlMode().
|
protected |
counters used for the transition between two ctrl modes
Definition at line 535 of file control-manager.cpp.
References ControlManager::m_ctrlModes.
Referenced by ControlManager::setCtrlMode().
void ctrlModes | ( | ) |
Definition at line 332 of file control-manager.cpp.
References ControlManager::m_ctrlModes.
Referenced by ControlManager::ControlManager().
DECLARE_SIGNAL_IN | ( | i_max | , |
dynamicgraph::Vector | |||
) |
predicted joint torques (using motor model)
DECLARE_SIGNAL_IN | ( | i_measured | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | tau | , |
dynamicgraph::Vector | |||
) |
motor currents
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 | ( | u_max | , |
dynamicgraph::Vector | |||
) |
max current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_OUT | ( | u | , |
dynamicgraph::Vector | |||
) |
max torque allowed before stopping the controller
DECLARE_SIGNAL_OUT | ( | u_safe | , |
dynamicgraph::Vector | |||
) |
|
virtual |
Definition at line 582 of file control-manager.cpp.
void displayRobotUtil | ( | ) |
Definition at line 512 of file control-manager.cpp.
References ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void getCtrlMode | ( | const std::string & | jointName | ) |
Definition at line 378 of file control-manager.cpp.
References ControlManager::convertJointNameToJointId(), ControlManager::m_jointCtrlModes_current, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void init | ( | const double & | dt, |
const std::string & | urdfFile, | ||
const std::string & | robotRef | ||
) |
Initialize
dt | control interval |
urdfFile | path to the URDF model of the robot |
Definition at line 147 of file control-manager.cpp.
References ControlManager::m_dt, ControlManager::m_emergency_stop_triggered, ControlManager::m_initSucceeded, ControlManager::m_jointCtrlModes_current, ControlManager::m_jointCtrlModes_previous, ControlManager::m_jointCtrlModesCountDown, ControlManager::m_robot, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
|
protected |
Definition at line 562 of file control-manager.cpp.
References ControlManager::m_robot_util.
void resetProfiler | ( | ) |
Definition at line 392 of file control-manager.cpp.
Referenced by ControlManager::ControlManager().
|
inline |
Definition at line 133 of file control-manager.hh.
void setCtrlMode | ( | const int | jid, |
const CtrlMode & | cm | ||
) |
Definition at line 355 of file control-manager.cpp.
References CTRL_MODE_TRANSITION_TIME_STEP, CtrlMode::id, ControlManager::m_jointCtrlModes_current, ControlManager::m_jointCtrlModes_previous, ControlManager::m_jointCtrlModesCountDown, and ControlManager::m_robot_util.
void setCtrlMode | ( | const std::string & | jointName, |
const std::string & | ctrlMode | ||
) |
Definition at line 334 of file control-manager.cpp.
References ControlManager::convertJointNameToJointId(), ControlManager::convertStringToCtrlMode(), ControlManager::m_robot_util, and ControlManager::updateJointCtrlModesOutputSignal().
Referenced by ControlManager::ControlManager().
void setFootFrameName | ( | const std::string & | FootName, |
const std::string & | FrameName | ||
) |
Definition at line 478 of file control-manager.cpp.
References ControlManager::m_initSucceeded, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void setForceLimitsFromId | ( | const double & | jointId, |
const dynamicgraph::Vector & | lq, | ||
const dynamicgraph::Vector & | uq | ||
) |
Command related to ForceUtil.
Definition at line 433 of file control-manager.cpp.
References ControlManager::m_initSucceeded, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void setForceNameToForceId | ( | const std::string & | forceName, |
const double & | forceId | ||
) |
Definition at line 443 of file control-manager.cpp.
References ControlManager::m_initSucceeded, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void setHandFrameName | ( | const std::string & | HandName, |
const std::string & | FrameName | ||
) |
Commands related to HandUtil.
Definition at line 491 of file control-manager.cpp.
References ControlManager::m_initSucceeded, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void setImuJointName | ( | const std::string & | JointName | ) |
Definition at line 504 of file control-manager.cpp.
References ControlManager::m_initSucceeded, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void setJointLimitsFromId | ( | const double & | jointId, |
const double & | lq, | ||
const double & | uq | ||
) |
Definition at line 424 of file control-manager.cpp.
References ControlManager::m_initSucceeded, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void setJoints | ( | const dynamicgraph::Vector & | ) |
Set the mapping between urdf and sot.
Definition at line 452 of file control-manager.cpp.
References ControlManager::m_initSucceeded, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void setNameToId | ( | const std::string & | jointName, |
const double & | jointId | ||
) |
Commands related to joint name and joint id.
Definition at line 416 of file control-manager.cpp.
References ControlManager::m_initSucceeded, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void setRightFootForceSensorXYZ | ( | const dynamicgraph::Vector & | xyz | ) |
Definition at line 469 of file control-manager.cpp.
References ControlManager::m_initSucceeded, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void setRightFootSoleXYZ | ( | const dynamicgraph::Vector & | xyz | ) |
Commands related to FootUtil.
Definition at line 460 of file control-manager.cpp.
References ControlManager::m_initSucceeded, and ControlManager::m_robot_util.
Referenced by ControlManager::ControlManager().
void setSleepTime | ( | const double & | seconds | ) |
Definition at line 399 of file control-manager.cpp.
References ControlManager::m_sleep_time.
Referenced by ControlManager::ControlManager().
void setStreamPrintPeriod | ( | const double & | s | ) |
Definition at line 397 of file control-manager.cpp.
Referenced by ControlManager::ControlManager().
|
protected |
Definition at line 516 of file control-manager.cpp.
References ControlManager::m_jointCtrlModes_current, ControlManager::m_jointCtrlModes_previous, ControlManager::m_jointCtrlModesCountDown, ControlManager::m_jointsCtrlModesSOUT, and ControlManager::m_robot_util.
Referenced by ControlManager::addCtrlMode(), and ControlManager::setCtrlMode().
std::vector<dynamicgraph::SignalPtr<dynamicgraph::Vector, int>*> m_ctrlInputsSIN |
Definition at line 78 of file control-manager.hh.
Referenced by ControlManager::addCtrlMode().
|
protected |
time to sleep at every iteration (to slow down simulation)
Definition at line 148 of file control-manager.hh.
Referenced by ControlManager::addCtrlMode(), ControlManager::convertStringToCtrlMode(), and ControlManager::ctrlModes().
|
protected |
true if the entity has been successfully initialized
Definition at line 141 of file control-manager.hh.
Referenced by ControlManager::init().
|
protected |
control loop time period
Definition at line 142 of file control-manager.hh.
Referenced by ControlManager::init().
std::vector<dynamicgraph::SignalPtr<bool, int>*> m_emergencyStopSIN |
Definition at line 80 of file control-manager.hh.
Referenced by ControlManager::addEmergencyStopSIN().
|
protected |
Definition at line 140 of file control-manager.hh.
Referenced by ControlManager::init(), ControlManager::setFootFrameName(), ControlManager::setForceLimitsFromId(), ControlManager::setForceNameToForceId(), ControlManager::setHandFrameName(), ControlManager::setImuJointName(), ControlManager::setJointLimitsFromId(), ControlManager::setJoints(), ControlManager::setNameToId(), ControlManager::setRightFootForceSensorXYZ(), and ControlManager::setRightFootSoleXYZ().
|
protected |
true if an emergency condition as been triggered either by an other entity, or by control limit violation
Definition at line 144 of file control-manager.hh.
|
protected |
true at the first iteration, false otherwise
Definition at line 145 of file control-manager.hh.
|
protected |
existing control modes
Definition at line 149 of file control-manager.hh.
Referenced by ControlManager::getCtrlMode(), ControlManager::init(), ControlManager::setCtrlMode(), and ControlManager::updateJointCtrlModesOutputSignal().
|
protected |
control mode of the joints
Definition at line 150 of file control-manager.hh.
Referenced by ControlManager::init(), ControlManager::setCtrlMode(), and ControlManager::updateJointCtrlModesOutputSignal().
|
protected |
previous control mode of the joints
Definition at line 151 of file control-manager.hh.
Referenced by ControlManager::init(), ControlManager::setCtrlMode(), and ControlManager::updateJointCtrlModesOutputSignal().
std::vector<dynamicgraph::Signal<dynamicgraph::Vector, int>*> m_jointsCtrlModesSOUT |
emergency stop inputs. If one is true, control is set to zero forever
Definition at line 81 of file control-manager.hh.
Referenced by ControlManager::addCtrlMode(), and ControlManager::updateJointCtrlModesOutputSignal().
|
protected |
Definition at line 139 of file control-manager.hh.
Referenced by ControlManager::init().
|
protected |
Definition at line 138 of file control-manager.hh.
Referenced by ControlManager::convertJointNameToJointId(), ControlManager::displayRobotUtil(), ControlManager::getCtrlMode(), ControlManager::init(), ControlManager::isJointInRange(), ControlManager::setCtrlMode(), ControlManager::setFootFrameName(), ControlManager::setForceLimitsFromId(), ControlManager::setForceNameToForceId(), ControlManager::setHandFrameName(), ControlManager::setImuJointName(), ControlManager::setJointLimitsFromId(), ControlManager::setJoints(), ControlManager::setNameToId(), ControlManager::setRightFootForceSensorXYZ(), ControlManager::setRightFootSoleXYZ(), and ControlManager::updateJointCtrlModesOutputSignal().
|
protected |
Definition at line 146 of file control-manager.hh.
Referenced by ControlManager::setSleepTime().