Skip to content

Commit

Permalink
Merge pull request fkanehiro#926 from orikuma/fix-torque-controller-p…
Browse files Browse the repository at this point in the history 
…ass-qref-mode

Fix torque controller pass qref mode
  • Loading branch information
@fkanehiro
fkanehiro committed Dec 26, 2015
2 parents af1f02e + 59418ed commit a6d0bdc
Show file tree
Hide file tree
Showing 3 changed files with 96 additions and 39 deletions.
35 changes: 27 additions & 8 deletions rtc/TorqueController/MotorTorqueController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,8 @@
#define TRANSITION_TIME 2.0 // [sec]
#define MAX_TRANSITION_COUNT (TRANSITION_TIME/m_dt)
#define TORQUE_MARGIN 10.0 // [Nm]
#define DEFAULT_MIN_MAX_DQ (0.17 * m_dt) // default min/max is 10[deg/sec] = 0.17[rad/sec]
#define DEFAULT_MIN_MAX_DQ 0.26 // default min/max is 15[deg] = 0.26[rad]
#define DEFAULT_MIN_MAX_TRANSITION_DQ (0.17 * m_dt) // default min/max is 10[deg/sec] = 0.17[rad/sec]

MotorTorqueController::MotorTorqueController()
{
Expand All @@ -30,7 +31,9 @@ MotorTorqueController::MotorTorqueController()
param.ke = 0.0; param.tc = 0.0; param.dt = 0.0;
setupController(param);
setupControllerCommon("", param.dt);
setupMotorControllerMinMaxDq(0.0, 0.0);
setupMotorControllerControlMinMaxDq(0.0, 0.0);
setupMotorControllerTransitionMinMaxDq(0.0, 0.0);

}

MotorTorqueController::~MotorTorqueController(void)
Expand All @@ -42,7 +45,8 @@ MotorTorqueController::MotorTorqueController(std::string _jname, TwoDofControlle
{
setupController(_param);
setupControllerCommon(_jname, _param.dt);
setupMotorControllerMinMaxDq(-DEFAULT_MIN_MAX_DQ, DEFAULT_MIN_MAX_DQ);
setupMotorControllerControlMinMaxDq(-DEFAULT_MIN_MAX_DQ, DEFAULT_MIN_MAX_DQ);
setupMotorControllerTransitionMinMaxDq(-DEFAULT_MIN_MAX_TRANSITION_DQ, DEFAULT_MIN_MAX_TRANSITION_DQ);
}

void MotorTorqueController::setupController(TwoDofController::TwoDofControllerParam &_param)
Expand Down Expand Up @@ -70,7 +74,9 @@ MotorTorqueController::MotorTorqueController(std::string _jname, TwoDofControlle
{
setupController(_param);
setupControllerCommon(_jname, _param.dt);
setupMotorControllerMinMaxDq(-DEFAULT_MIN_MAX_DQ, DEFAULT_MIN_MAX_DQ);
setupMotorControllerControlMinMaxDq(-DEFAULT_MIN_MAX_DQ, DEFAULT_MIN_MAX_DQ);
setupMotorControllerTransitionMinMaxDq(-DEFAULT_MIN_MAX_TRANSITION_DQ, DEFAULT_MIN_MAX_TRANSITION_DQ);

}
void MotorTorqueController::setupController(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param)
{
Expand All @@ -96,7 +102,8 @@ MotorTorqueController::MotorTorqueController(std::string _jname, TwoDofControlle
{
setupController(_param);
setupControllerCommon(_jname, _param.dt);
setupMotorControllerMinMaxDq(-DEFAULT_MIN_MAX_DQ, DEFAULT_MIN_MAX_DQ);
setupMotorControllerControlMinMaxDq(-DEFAULT_MIN_MAX_DQ, DEFAULT_MIN_MAX_DQ);
setupMotorControllerTransitionMinMaxDq(-DEFAULT_MIN_MAX_TRANSITION_DQ, DEFAULT_MIN_MAX_TRANSITION_DQ);
}

void MotorTorqueController::setupController(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param)
Expand Down Expand Up @@ -141,12 +148,23 @@ bool MotorTorqueController::disable(void)
return retval; // return result of changing mode
}

void MotorTorqueController::setupMotorControllerMinMaxDq(double _min_dq, double _max_dq)
void MotorTorqueController::setupMotorControllerControlMinMaxDq(double _min_dq, double _max_dq)
{
m_normalController.min_dq = _min_dq;
m_emergencyController.min_dq = _min_dq;
m_normalController.max_dq = _max_dq;
m_emergencyController.max_dq = _max_dq;

return;
}

void MotorTorqueController::setupMotorControllerTransitionMinMaxDq(double _min_transition_dq, double _max_transition_dq)
{
m_normalController.min_transition_dq = _min_transition_dq;
m_emergencyController.min_transition_dq = _min_transition_dq;
m_normalController.max_transition_dq = _max_transition_dq;
m_emergencyController.max_transition_dq = _max_transition_dq;

return;
}

Expand Down Expand Up @@ -218,7 +236,7 @@ double MotorTorqueController::execute (double _tau, double _tauMax)
#if BOOST_VERSION >= 103500
limitedTauRef = boost::math::copysign(_tauMax, _tau);
#else
limitedTauRef = std::fabs(_tauMax) * ((_tau == 0) ? 0 : (_tau > 0) ? 1 : -1);
limitedTauRef = std::fabs(_tauMax) * ((_tau < 0) ? -1 : 1);
#endif
updateController(_tau, limitedTauRef, m_emergencyController);
dq = m_emergencyController.getMotorControllerDq();
Expand Down Expand Up @@ -304,7 +322,7 @@ void MotorTorqueController::prepareStop(MotorTorqueController::MotorController &
_mc.transition_dq = _mc.getMotorControllerDq();

// determine transition in 1 cycle
_mc.recovery_dq = std::min(std::max(_mc.transition_dq / MAX_TRANSITION_COUNT, _mc.min_dq), _mc.max_dq); // transition in 1 cycle
_mc.recovery_dq = std::min(std::max(_mc.transition_dq / MAX_TRANSITION_COUNT, _mc.min_transition_dq), _mc.max_transition_dq); // transition in 1 cycle
std::cerr << _mc.recovery_dq << std::endl;

_mc.dq = 0; // dq must be reseted after recovery_dq setting(used in getMotoroControllerDq)
Expand All @@ -317,6 +335,7 @@ void MotorTorqueController::updateController(double _tau, double _tauRef, MotorT
switch (_mc.state) {
case ACTIVE:
_mc.dq += _mc.controller->update(_tau, _tauRef);
_mc.dq = std::min(std::max(_mc.min_dq, _mc.dq), _mc.max_dq);
break;
case STOP:
if (std::abs(_mc.recovery_dq) > std::abs(_mc.transition_dq)){
Expand Down
10 changes: 7 additions & 3 deletions rtc/TorqueController/MotorTorqueController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,8 @@ class MotorTorqueController {
bool disable(void); // disable torque controller (emergency controller is also ignored)

// for normal torque controller
void setupMotorControllerMinMaxDq(double _min_dq, double _max_dq); // set min/max dq for transition
void setupMotorControllerControlMinMaxDq(double _min_dq, double _max_dq); // set min/max dq for control
void setupMotorControllerTransitionMinMaxDq(double _min_transition_dq, double _max_transition_dq); // set min/max dq for transition
bool activate(void); // set state of torque controller to ACTIVE
bool deactivate(void); // set state of torque controller to STOP -> INACTIVE
bool setReferenceTorque(double _tauRef); // set reference torque (does not activate controller)
Expand All @@ -80,8 +81,11 @@ class MotorTorqueController {
double dq; //difference of joint angle from base(qRef) from tdc. it is calcurated by dq = integrate(qd * dt), dq*dt is output of tdc
double transition_dq; // for transition. first value is last difference of joint angle from qRef (dq + transition_dq) when state was changed to STOP
double recovery_dq; // difference of joint angle in 1 cycle to be recoverd
double min_dq; // min dq when transition
double max_dq; // max dq when transition
double min_dq; // min total dq when control
double max_dq; // max total dq when control
double min_transition_dq; // min dq when transition
double max_transition_dq; // max dq when transition

// for TwoDofController
void setupTwoDofController(TwoDofController::TwoDofControllerParam &_param);
bool updateTwoDofControllerParam(TwoDofController::TwoDofControllerParam &_param);
Expand Down
90 changes: 62 additions & 28 deletions rtc/TorqueController/TorqueController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -201,9 +201,23 @@ RTC::ReturnCode_t TorqueController::onInitialize()
}

// parameter setttings for torque controller
bool dq_min_max_from_conf_is_valid = false;
coil::vstring dqMinMaxFromConf = coil::split(prop["torque_controler_min_max_dq"], ",");
if (dqMinMaxFromConf.size() == 2 * m_robot->numJoints()) {
dq_min_max_from_conf_is_valid = true;
} else {
std::cerr << "[" << m_profile.instance_name << "]" << "size of torque_controller_min_max_dq " << dqMinMaxFromConf.size() << " is not correct number " << 2 * m_robot->numJoints() << ". Use default values." << std::endl;
dq_min_max_from_conf_is_valid = false;
}
for (int i = 0; i < m_robot->numJoints(); i++) {
m_motorTorqueControllers[i].setErrorPrefix(std::string(m_profile.instance_name));
m_motorTorqueControllers[i].setupMotorControllerMinMaxDq(m_robot->joint(i)->lvlimit * m_dt, m_robot->joint(i)->uvlimit * m_dt);
m_motorTorqueControllers[i].setupMotorControllerTransitionMinMaxDq(m_robot->joint(i)->lvlimit * m_dt, m_robot->joint(i)->uvlimit * m_dt);
if(dq_min_max_from_conf_is_valid) {
double tmp_dq_min, tmp_dq_max;
coil::stringTo(tmp_dq_min, dqMinMaxFromConf[2 * i].c_str());
coil::stringTo(tmp_dq_max, dqMinMaxFromConf[2 * i + 1].c_str());
m_motorTorqueControllers[i].setupMotorControllerControlMinMaxDq(tmp_dq_min, tmp_dq_max);
}
}

// allocate memory for outPorts
Expand Down Expand Up @@ -266,41 +280,52 @@ RTC::ReturnCode_t TorqueController::onExecute(RTC::UniqueId ec_id)
m_qRefInIn.read();
}

if (m_qRefIn.data.length() == m_robot->numJoints() &&
m_tauCurrentIn.data.length() == m_robot->numJoints() &&
m_qCurrentIn.data.length() == m_robot->numJoints()) {
if (m_qRefIn.data.length() == m_robot->numJoints()) {
if (m_tauCurrentIn.data.length() == m_robot->numJoints() &&
m_qCurrentIn.data.length() == m_robot->numJoints()) {

// update model
for ( int i = 0; i < m_robot->numJoints(); i++ ){
// update model
for ( int i = 0; i < m_robot->numJoints(); i++ ){
m_robot->joint(i)->q = m_qCurrentIn.data[i];
}
m_robot->calcForwardKinematics();
}
m_robot->calcForwardKinematics();

// calculate dq by torque controller
executeTorqueControl(dq);
// calculate dq by torque controller
executeTorqueControl(dq);

// output restricted qRef
for (int i = 0; i < m_robot->numJoints(); i++) {
m_qRefOut.data[i] = std::min(std::max(m_qRefIn.data[i] + dq[i], m_robot->joint(i)->llimit), m_robot->joint(i)->ulimit);
// check range of motion and insert to output
for (int i = 0; i < m_robot->numJoints(); i++) {
if (m_motorTorqueControllers[i].isEnabled()) {
m_qRefOut.data[i] = std::min(std::max(m_qRefIn.data[i] + dq[i], m_robot->joint(i)->llimit), m_robot->joint(i)->ulimit);
} else {
m_qRefOut.data[i] = m_qRefIn.data[i]; // pass joint angle when controller is disabled
}
}

} else {
if (isDebug()) {
std::cerr << "[" << m_profile.instance_name << "]" << "TorqueController input is not correct" << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << " numJoints: " << m_robot->numJoints() << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << " qCurrent: " << m_qCurrentIn.data.length() << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << "tauCurrent: " << m_tauCurrentIn.data.length() << std::endl;
std::cerr << std::endl;
}
// pass qRefIn to qRefOut
for (int i = 0; i < m_robot->numJoints(); i++) {
m_qRefOut.data[i] = m_qRefIn.data[i];
}
}
m_qRefOut.tm = m_qRefIn.tm;
m_qRefOutOut.write();
} else {
if (isDebug()) {
std::cerr << "[" << m_profile.instance_name << "]" << "TorqueController input is not correct" << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << " numJoints: " << m_robot->numJoints() << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << " qCurrent: " << m_qCurrentIn.data.length() << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << " qRefIn: " << m_qRefIn.data.length() << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << "tauCurrent: " << m_tauCurrentIn.data.length() << std::endl;
std::cerr << std::endl;
}
// pass qRefIn to qRefOut
for (int i = 0; i < m_robot->numJoints(); i++) {
m_qRefOut.data[i] = m_qRefIn.data[i];
}
if (isDebug()) {
std::cerr << "[" << m_profile.instance_name << "]" << "TorqueController has incorrect qRefIn" << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << " numJoints: " << m_robot->numJoints() << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << " qRefIn: " << m_qRefIn.data.length() << std::endl;
std::cerr << std::endl;
}
}

m_qRefOut.tm = m_qRefIn.tm;
m_qRefOutOut.write();

return RTC::RTC_OK;
}

Expand Down Expand Up @@ -453,6 +478,15 @@ bool TorqueController::startTorqueControl(std::string jname)
if (m_debugLevel > 0) {
std::cerr << "[" << m_profile.instance_name << "]" << "Start torque control in " << jname << std::endl;
}
if (!(*it).isEnabled()) {
succeed = enableTorqueController(jname);
if (!succeed) {
if (m_debugLevel > 0) {
std::cerr << "[" << m_profile.instance_name << "]" << "Failed to enable torque control in " << jname << std::endl;
}
return succeed;
}
}
succeed = (*it).activate();
}
}
Expand Down

0 comments on commit a6d0bdc

Please sign in to comment.