예제 #1
0
void Pid::setGains(const Gains &gains)
{
  gains_buffer_.writeFromNonRT(gains);

  // Update dynamic reconfigure with the new gains
  updateDynamicReconfig(gains);
}
예제 #2
0
void Pid::updateDynamicReconfig()
{
  // Make sure dynamic reconfigure is initialized
  if(!dynamic_reconfig_initialized_)
    return;

  // Get starting values 
  control_toolbox::ParametersConfig config;

  // Get starting values   
  getGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min);

  updateDynamicReconfig(config);
}
예제 #3
0
void Pid::initDynamicReconfig(ros::NodeHandle &node)
{
  ROS_DEBUG_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace " 
    << node.getNamespace());

  // Start dynamic reconfigure server
  param_reconfig_server_.reset(new DynamicReconfigServer(param_reconfig_mutex_, node));
  dynamic_reconfig_initialized_ = true;
 
  // Set Dynamic Reconfigure's gains to Pid's values
  updateDynamicReconfig();

  // Set callback
  param_reconfig_callback_ = boost::bind(&Pid::dynamicReconfigCallback, this, _1, _2);
  param_reconfig_server_->setCallback(param_reconfig_callback_); 
}
예제 #4
0
void Pid::updateDynamicReconfig(Gains gains_config)
{
  // Make sure dynamic reconfigure is initialized
  if(!dynamic_reconfig_initialized_)
    return;

  control_toolbox::ParametersConfig config;

  // Convert to dynamic reconfigure format
  config.p_gain = gains_config.p_gain_;
  config.i_gain = gains_config.i_gain_;
  config.d_gain = gains_config.d_gain_;
  config.i_clamp_max = gains_config.i_max_;
  config.i_clamp_min = gains_config.i_min_;

  updateDynamicReconfig(config);
}
/**
 * @brief AdmittanceParamManager::setAdmittanceParams
 * Sends changes of the admittance parameters to the controller.
 * @param active
 * @param inertia
 * @param damping
 * @param stiffness
 * @param dead_zone
 */
void AdmittanceParamManager::setParams(compliant_ros_controller::AdmittanceParamsConfig &config) {
    controller_->activate(config.active);
    controller_->setInertia(config.inertia);
    controller_->setDamping(config.damping);
    controller_->setStiffness(config.stiffness);
    controller_->setTransDeadZone(config.dead_zone_trans);
    controller_->setRotDeadZone(config.dead_zone_rot);
    controller_->setMode(config.mode);
    controller_->setTransSpeedLimit(config.speed_limit_trans);
    controller_->setRotSpeedLimit(config.speed_limit_rot);

    if (config.active) {
        ROS_INFO_STREAM("Admittance params changed to: Active with "
                        << config.inertia << ", " << config.damping << ", " << config.stiffness
                        << ". Dead Zone: " << config.dead_zone_trans << " | " << config.dead_zone_rot
                        << ", Speed Limit: " << config.speed_limit_trans << " | " << config.speed_limit_rot
                        << " Mode: " << config.mode << ".");
    } else {
        ROS_INFO_STREAM("Admittance params changed to: Not active");
    }
    updateDynamicReconfig(config);
}