void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t level)
{
ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved.");
// Set the gains
setGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min);
}
bool Pid::init(const ros::NodeHandle &node)
{
ros::NodeHandle nh(node);
Gains gains;
// Load PID gains from parameter server
if (!nh.getParam("p", gains.p_gain_))
{
ROS_ERROR("No p gain specified for pid. Namespace: %s", nh.getNamespace().c_str());
return false;
}
// Only the P gain is required, the I and D gains are optional and default to 0:
nh.param("i", gains.i_gain_, 0.0);
nh.param("d", gains.d_gain_, 0.0);
// Load integral clamp from param server or default to 0
double i_clamp;
nh.param("i_clamp", i_clamp, 0.0);
gains.i_max_ = std::abs(i_clamp);
gains.i_min_ = -std::abs(i_clamp);
setGains(gains);
reset();
initDynamicReconfig(nh);
return true;
}
Pid::Pid(double p, double i, double d, double i_max, double i_min)
: dynamic_reconfig_initialized_(false)
{
setGains(p,i,d,i_max,i_min);
reset();
}
ITG3200::ITG3200() {
setGains(1.0,1.0,1.0);
setOffsets(0,0,0);
setRevPolarity(0,0,0);
//Wire.begin(); //Normally this code is called from setup() at user code
//but some people reported that joining I2C bus earlier
//apparently solved problems with master/slave conditions.
//Uncomment if needed.
}
/*!*****************************************************************************
*******************************************************************************
\note setGainsSim
\date August 7, 1992
\remarks
adjust the gains, with interface for simulation
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
setGainsSim(void)
{
static int jID=0;
if (!get_int("Which joint?",jID,&jID))
return;
setGains(jID);
}
bool Pid::init(const ros::NodeHandle &node, const bool quiet)
{
ros::NodeHandle nh(node);
Gains gains;
// Load PID gains from parameter server
if (!nh.getParam("p", gains.p_gain_))
{
if (!quiet) {
ROS_ERROR("No p gain specified for pid. Namespace: %s", nh.getNamespace().c_str());
}
return false;
}
// Only the P gain is required, the I and D gains are optional and default to 0:
nh.param("i", gains.i_gain_, 0.0);
nh.param("d", gains.d_gain_, 0.0);
// Load integral clamp from param server or default to 0
double i_clamp;
nh.param("i_clamp", i_clamp, 0.0);
gains.i_max_ = std::abs(i_clamp);
gains.i_min_ = -std::abs(i_clamp);
if(nh.hasParam("i_clamp_min"))
{
nh.param("i_clamp_min", gains.i_min_, gains.i_min_); // use i_clamp_min parameter, otherwise keep -i_clamp
gains.i_min_ = -std::abs(gains.i_min_); // make sure the value is <= 0
}
if(nh.hasParam("i_clamp_max"))
{
nh.param("i_clamp_max", gains.i_max_, gains.i_max_); // use i_clamp_max parameter, otherwise keep i_clamp
gains.i_max_ = std::abs(gains.i_max_); // make sure the value is >= 0
}
setGains(gains);
reset();
initDynamicReconfig(nh);
return true;
}
bool Pid::initXml(TiXmlElement *config)
{
// Create node handle for dynamic reconfigure
ros::NodeHandle nh(DEFAULT_NAMESPACE);
double i_clamp;
i_clamp = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
setGains(
config->Attribute("p") ? atof(config->Attribute("p")) : 0.0,
config->Attribute("i") ? atof(config->Attribute("i")) : 0.0,
config->Attribute("d") ? atof(config->Attribute("d")) : 0.0,
std::abs(i_clamp),
-std::abs(i_clamp)
);
reset();
initDynamicReconfig(nh);
return true;
}
void Pid::initPid(double p, double i, double d, double i_max, double i_min)
{
setGains(p,i,d,i_max,i_min);
reset();
}
void Pid::setGains(double p, double i, double d, double i_max, double i_min)
{
Gains gains(p,i,d,i_max,i_min);
setGains(gains);
}
/*! @brief Sets the leg gains stored in this WalkParameter class
@param leggains the gains: [first phase, second phase, third phase, etc] where each phase [hip roll, hip pitch, hip yaw, knee, ankle roll, ankle pitch]
*/
void WalkParameters::setLegGains(const vector<vector<float> >& leggains)
{
setGains(m_leg_gains, m_num_torso_gains, leggains);
}
/*! @brief Sets the torso gains stored in this WalkParameter class
@param torsogains the gains: [first phase, second phase, third phase, etc] where each phase [torso roll, torso pitch, torso yaw]
*/
void WalkParameters::setTorsoGains(const vector<vector<float> >& torsogains)
{
setGains(m_torso_gains, m_num_torso_gains, torsogains);
}
/*! @brief Sets the arm gains
@param armgains the gains: [first phase, second phase, third phase, etc] where each phase [shoulder roll, shouler pitch, shouler yaw, elbow roll, elbow pitch]
*/
void WalkParameters::setArmGains(const vector<vector<float> >& armgains)
{
setGains(m_arm_gains, m_num_arm_gains, armgains);
}
