Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add mode of control in talos_control_manager to check the corresponding limits. #22

Open
wants to merge 3 commits into
base: devel
Choose a base branch
from
Open

Add mode of control in talos_control_manager to check the corresponding limits. #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
85721f1
[ctrl_manager] Add an handle for the freeflyer
NoelieRamuzat Mar 8, 2021
f00071e
[ctrl_manager] Add control output mode to check the corresponding limits
Aug 5, 2021
227eadc
[base_estim] Need real positions and velocities for torque control
Aug 5, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 12 additions & 4 deletions include/sot/talos_balance/talos-control-manager.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,13 +52,19 @@ namespace talos_balance {
/// Number of time step to transition from one ctrl mode to another
#define CTRL_MODE_TRANSITION_TIME_STEP 1000.0

enum ControlOutput { CONTROL_OUTPUT_POSITION = 0, CONTROL_OUTPUT_VELOCITY = 1,
CONTROL_OUTPUT_TORQUE = 2, NO_CONTROL = 3, CONTROL_OUTPUT_SIZE = 4 };

const std::string m_controlOutput_s[] = {"position", "velocity", "torque", "freeflyer"};

class CtrlMode {
public:
int id;
std::string name;
ControlOutput ctrl;

CtrlMode() : id(-1), name("None") {}
CtrlMode(int id, const std::string& name) : id(id), name(name) {}
CtrlMode() : id(-1), name("None"), ctrl(CONTROL_OUTPUT_POSITION) {}
CtrlMode(int id, const std::string& name, const ControlOutput& ctrl) : id(id), name(name), ctrl(ctrl) {}
};

std::ostream& operator<<(std::ostream& os, const CtrlMode& s) {
Expand Down Expand Up @@ -87,6 +93,8 @@ class TALOS_CONTROL_MANAGER_EXPORT TalosControlManager : public ::dynamicgraph::
std::vector<dynamicgraph::Signal<dynamicgraph::Vector, int>*> m_jointsCtrlModesSOUT;

DECLARE_SIGNAL_IN(u_max, dynamicgraph::Vector); /// max motor control
DECLARE_SIGNAL_IN(q_predicted, dynamicgraph::Vector); // (optional) position predicted from torque command
DECLARE_SIGNAL_IN(tau_predicted, dynamicgraph::Vector); // (optional) torque predicted from position command
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); /// raw motor control
DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector); /// safe motor control

Expand All @@ -96,7 +104,7 @@ class TALOS_CONTROL_MANAGER_EXPORT TalosControlManager : public ::dynamicgraph::
void addCtrlMode(const std::string& name);
void ctrlModes();
void getCtrlMode(const std::string& jointName);
void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
void setCtrlMode(const std::string& jointName, const std::string& ctrlName, const std::string& ctrlMode);
void setCtrlMode(const int jid, const CtrlMode& cm);

void resetProfiler();
Expand Down Expand Up @@ -132,7 +140,7 @@ class TALOS_CONTROL_MANAGER_EXPORT TalosControlManager : public ::dynamicgraph::
std::vector<CtrlMode> m_jointCtrlModes_previous; /// previous control mode of the joints
std::vector<int> m_jointCtrlModesCountDown; /// counters used for the transition between two ctrl modes

bool convertStringToCtrlMode(const std::string& name, CtrlMode& cm);
bool convertStringToCtrlMode(const std::string& name, const std::string& mode, CtrlMode& cm);
bool convertJointNameToJointId(const std::string& name, unsigned int& id);
// bool isJointInRange(unsigned int id, double q);
void updateJointCtrlModesOutputSignal();
Expand Down
4 changes: 2 additions & 2 deletions src/dynamic_graph/sot_talos_balance/create_entities_utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -258,11 +258,11 @@ def create_ankle_admittance_controller(gains, robot, side, name):
def create_device_filters(robot, dt):
robot.pselec = Selec_of_vector("pselec")
robot.pselec.selec(6, 6 + N_JOINTS)
plug(robot.device.state, robot.pselec.sin)
plug(robot.device.robotState, robot.pselec.sin)

robot.vselec = Selec_of_vector("vselec")
robot.vselec.selec(6, 6 + N_JOINTS)
plug(robot.device.velocity, robot.vselec.sin)
plug(robot.device.robotVelocity, robot.vselec.sin)

filters = Bunch()
filters.joints_kin = filter_utils.create_chebi1_checby2_series_filter("joints_kin", dt, N_JOINTS)
Expand Down
94 changes: 75 additions & 19 deletions src/talos-control-manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ using namespace dg::sot::talos_balance;
#define PROFILE_PWM_DESIRED_COMPUTATION "Control manager "
#define PROFILE_DYNAMIC_GRAPH_PERIOD "Control period "

#define INPUT_SIGNALS m_u_maxSIN
#define INPUT_SIGNALS m_u_maxSIN << m_q_predictedSIN << m_tau_predictedSIN
#define OUTPUT_SIGNALS m_uSOUT << m_u_safeSOUT

/// Define EntityClassName here rather than in the header file
Expand All @@ -52,6 +52,8 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TalosControlManager, "TalosControlManager");
TalosControlManager::TalosControlManager(const std::string& name)
: Entity(name),
CONSTRUCT_SIGNAL_IN(u_max, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN(q_predicted, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN(tau_predicted, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, m_u_maxSIN),
CONSTRUCT_SIGNAL_OUT(u_safe, dynamicgraph::Vector, m_uSOUT << m_u_maxSIN),
m_robot_util(RefVoidRobotUtil()),
Expand All @@ -75,9 +77,10 @@ TalosControlManager::TalosControlManager(const std::string& name)
addCommand("ctrlModes", makeCommandVoid0(*this, &TalosControlManager::ctrlModes,
docCommandVoid0("Get a list of all the available control modes.")));

addCommand("setCtrlMode", makeCommandVoid2(*this, &TalosControlManager::setCtrlMode,
docCommandVoid2("Set the control mode for a joint.",
"(string) joint name", "(string) control mode")));
addCommand("setCtrlMode", makeCommandVoid3(*this, &TalosControlManager::setCtrlMode,
docCommandVoid3("Set the control mode for a joint.",
"(string) joint name", "(string) control name",
"(string) control mode")));

addCommand("getCtrlMode",
makeCommandVoid1(*this, &TalosControlManager::getCtrlMode,
Expand Down Expand Up @@ -205,13 +208,56 @@ DEFINE_SIGNAL_OUT_FUNCTION(u_safe, dynamicgraph::Vector) {
}

if (!m_emergency_stop_triggered) {
dynamicgraph::Vector tau_pred, q_pred;
if (m_tau_predictedSIN.isPlugged()){
tau_pred = m_tau_predictedSIN(iter);
}
if (m_q_predictedSIN.isPlugged()){
q_pred = m_q_predictedSIN(iter);
}

for (unsigned int i = 0; i < m_numDofs; i++) {
if (fabs(u(i)) > ctrl_max(i)) {
m_emergency_stop_triggered = true;
SEND_MSG("t = " + toString(iter) + ": Joint " + toString(i) +
" desired control is too large: " + toString(u(i)) + " > " + toString(ctrl_max(i)),
MSG_TYPE_ERROR_STREAM);
break;
if (m_jointCtrlModes_current[i].ctrl == CONTROL_OUTPUT_POSITION){
double upperLim = m_robot_util->get_joint_limits_from_id(i-6).upper; // no freeflyer in joint_limits
double lowerLim = m_robot_util->get_joint_limits_from_id(i-6).lower; // no freeflyer in joint_limits
if ((u(i) < lowerLim) || (upperLim < u(i))) {
m_emergency_stop_triggered = true;
SEND_MSG("t = " + toString(iter) + ": Joint " + toString(i) +
" desired position control is too large: " + toString(u(i)) + " > " + toString(upperLim)
+ " or " + toString(u(i)) + " < " + toString(lowerLim),
MSG_TYPE_ERROR_STREAM);
break;
}
if ((tau_pred.size() != 0) && (fabs(tau_pred[i]) > ctrl_max(i))) {
m_emergency_stop_triggered = true;
SEND_MSG("t = " + toString(iter) + ": Joint " + toString(i) +
" predicted torque from position control is too large: " +
toString(tau_pred[i]) + " > " + toString(ctrl_max(i)),
MSG_TYPE_ERROR_STREAM);
break;
}
}
// TODO: Add check on velocities -> No limits accessible in robotUtils -> Had to be added
else if (m_jointCtrlModes_current[i].ctrl == CONTROL_OUTPUT_TORQUE){
if (fabs(u(i)) > ctrl_max(i)) {
m_emergency_stop_triggered = true;
SEND_MSG("t = " + toString(iter) + ": Joint " + toString(i) +
" desired torque control is too large: " + toString(u(i)) + " > " + toString(ctrl_max(i)),
MSG_TYPE_ERROR_STREAM);
break;
}
if (q_pred.size() != 0) {
double upperLim = m_robot_util->get_joint_limits_from_id(i-6).upper; // no freeflyer in joint_limits
double lowerLim = m_robot_util->get_joint_limits_from_id(i-6).lower; // no freeflyer in joint_limits
if ((q_pred[i] < lowerLim) || (upperLim < q_pred[i])) {
m_emergency_stop_triggered = true;
SEND_MSG("t = " + toString(iter) + ": Joint " + toString(i) +
" predicted position from torque control is too large: " + toString(q_pred[i]) + " > " + toString(upperLim)
+ " or " + toString(q_pred[i]) + " < " + toString(lowerLim),
MSG_TYPE_ERROR_STREAM);
break;
}
}
}
}
}
Expand Down Expand Up @@ -253,9 +299,9 @@ void TalosControlManager::addCtrlMode(const string& name) {

void TalosControlManager::ctrlModes() { SEND_MSG(toString(m_ctrlModes), MSG_TYPE_INFO); }

void TalosControlManager::setCtrlMode(const string& jointName, const string& ctrlMode) {
void TalosControlManager::setCtrlMode(const string& jointName, const string& ctrlName, const string& ctrlMode) {
CtrlMode cm;
if (convertStringToCtrlMode(ctrlMode, cm) == false) return;
if (convertStringToCtrlMode(ctrlName, ctrlMode, cm) == false) return;

if (jointName == "all") {
for (unsigned int i = 0; i < m_numDofs; i++) setCtrlMode(i, cm);
Expand All @@ -267,7 +313,12 @@ void TalosControlManager::setCtrlMode(const string& jointName, const string& ctr
unsigned int i;
while (std::getline(ss, item, '-')) {
SEND_MSG("parsed joint : " + item, MSG_TYPE_INFO);
if (convertJointNameToJointId(item, i)) jIdList.push_back(i);
if (item == "freeflyer") {
for (unsigned int j = 0; j < 6; j++) jIdList.push_back(j);
} else {
convertJointNameToJointId(item, i);
jIdList.push_back(i);
}
}
for (std::list<int>::iterator it = jIdList.begin(); it != jIdList.end(); ++it) setCtrlMode(*it, cm);
}
Expand Down Expand Up @@ -358,15 +409,20 @@ void TalosControlManager::updateJointCtrlModesOutputSignal() {
}
}

bool TalosControlManager::convertStringToCtrlMode(const std::string& name, CtrlMode& cm) {
bool TalosControlManager::convertStringToCtrlMode(const std::string& name, const std::string& type, CtrlMode& cm) {
// Check if the ctrl mode name exists
for (unsigned int i = 0; i < m_ctrlModes.size(); i++)
for (unsigned int i = 0; i < m_ctrlModes.size(); i++) {
if (name == m_ctrlModes[i]) {
cm = CtrlMode(i, name);
return true;
const auto it = std::find(std::begin(m_controlOutput_s), std::end(m_controlOutput_s), type);
if(it != std::end(m_controlOutput_s)){
cm = CtrlMode(i, name, static_cast<ControlOutput>(std::distance(std::begin(m_controlOutput_s), it)));
return true;
}
}
SEND_MSG("The specified control mode does not exist", MSG_TYPE_ERROR);
SEND_MSG("Possible control modes are: " + toString(m_ctrlModes), MSG_TYPE_INFO);
}
SEND_MSG("The specified control name or mode does not exist", MSG_TYPE_ERROR);
SEND_MSG("Possible control names are: " + toString(m_ctrlModes), MSG_TYPE_INFO);
SEND_MSG("Possible control modes are: position, velocity, torque, freeflyer", MSG_TYPE_INFO);
return false;
}

Expand Down