Skip to content

Commit

Permalink
Merge branch 'master' into jtc/angle_wraparound_trajectory
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich authored Nov 15, 2023
2 parents 43d03c1 + 3fc0f50 commit 6201fd7
Show file tree
Hide file tree
Showing 4 changed files with 239 additions and 113 deletions.
10 changes: 9 additions & 1 deletion ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -262,8 +262,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
const rclcpp::Time & time, const JointTrajectoryPoint & desired_state,
const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error);

void read_state_from_hardware(JointTrajectoryPoint & state);
void read_state_from_state_interfaces(JointTrajectoryPoint & state);

/** Assign values from the command interfaces as state.
* This is only possible if command AND state interfaces exist for the same type,
* therefore needs check for both.
* @param[out] state to be filled with values from command interfaces.
* @return true if all interfaces exists and contain non-NaN values, false otherwise.
*/
bool read_state_from_command_interfaces(JointTrajectoryPoint & state);
bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands);

Expand All @@ -272,6 +278,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
std::shared_ptr<control_msgs::srv::QueryTrajectoryState::Response> response);

private:
void update_pids();

bool contains_interface_type(
const std::vector<std::string> & interface_type_list, const std::string & interface_type);

Expand Down
114 changes: 74 additions & 40 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,17 @@ controller_interface::return_type JointTrajectoryController::update(
{
return controller_interface::return_type::OK;
}
// update dynamic parameters
if (param_listener_->is_old(params_))
{
params_ = param_listener_->get_params();
// use_closed_loop_pid_adapter_ is updated in on_configure only
if (use_closed_loop_pid_adapter_)
{
update_pids();
default_tolerances_ = get_segment_tolerances(params_);
}
}

auto compute_error_for_joint = [&](
JointTrajectoryPoint & error, size_t index,
Expand Down Expand Up @@ -175,7 +186,7 @@ controller_interface::return_type JointTrajectoryController::update(

// current state update
state_current_.time_from_start.set__sec(0);
read_state_from_hardware(state_current_);
read_state_from_state_interfaces(state_current_);

// currently carrying out a trajectory
if (has_active_trajectory())
Expand Down Expand Up @@ -399,7 +410,7 @@ controller_interface::return_type JointTrajectoryController::update(
return controller_interface::return_type::OK;
}

void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state)
void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state)
{
auto assign_point_from_interface =
[&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
Expand Down Expand Up @@ -706,15 +717,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
ff_velocity_scale_.resize(dof_);
tmp_command_.resize(dof_, 0.0);

// Init PID gains from ROS parameters
for (size_t i = 0; i < dof_; ++i)
{
const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
pids_[i] = std::make_shared<control_toolbox::Pid>(
gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);

ff_velocity_scale_[i] = gains.ff_velocity_scale;
}
update_pids();
}

// Configure joint position error normalization from ROS parameters (angle_wraparound)
Expand Down Expand Up @@ -789,8 +792,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
get_interface_list(params_.command_interfaces).c_str(),
get_interface_list(params_.state_interfaces).c_str());

default_tolerances_ = get_segment_tolerances(params_);

// parse remaining parameters
const std::string interpolation_string =
get_node()->get_parameter("interpolation_method").as_string();
interpolation_method_ = interpolation_methods::from_string(interpolation_string);
Expand Down Expand Up @@ -876,32 +878,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
std::string(get_node()->get_name()) + "/query_state",
std::bind(&JointTrajectoryController::query_state_service, this, _1, _2));

if (params_.cmd_timeout > 0.0)
{
if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance)
{
cmd_timeout_ = params_.cmd_timeout;
}
else
{
// deactivate timeout
RCLCPP_WARN(
logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)",
params_.cmd_timeout, default_tolerances_.goal_time_tolerance);
cmd_timeout_ = 0.0;
}
}
else
{
cmd_timeout_ = 0.0;
}

return CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn JointTrajectoryController::on_activate(
const rclcpp_lifecycle::State &)
{
// update the dynamic map parameters
param_listener_->refresh_dynamic_parameters();

// get parameters from the listener in case they were updated
params_ = param_listener_->get_params();

// parse remaining parameters
default_tolerances_ = get_segment_tolerances(params_);

// order all joints in the storage
for (const auto & interface : params_.command_interfaces)
{
Expand Down Expand Up @@ -953,20 +944,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(

subscriber_is_active_ = true;

// Initialize current state storage if hardware state has tracking offset
read_state_from_hardware(state_current_);
read_state_from_hardware(state_desired_);
read_state_from_hardware(last_commanded_state_);
// Handle restart of controller by reading from commands if
// those are not nan
// Handle restart of controller by reading from commands if those are not NaN (a controller was
// running already)
trajectory_msgs::msg::JointTrajectoryPoint state;
resize_joint_trajectory_point(state, dof_);
if (read_state_from_command_interfaces(state))
{
state_current_ = state;
state_desired_ = state;
last_commanded_state_ = state;
}
else
{
// Initialize current state storage from hardware
read_state_from_state_interfaces(state_current_);
read_state_from_state_interfaces(last_commanded_state_);
}

// Should the controller start by holding position at the beginning of active state?
if (params_.start_with_holding)
Expand All @@ -975,6 +967,28 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
}
rt_is_holding_.writeFromNonRT(true);

// parse timeout parameter
if (params_.cmd_timeout > 0.0)
{
if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance)
{
cmd_timeout_ = params_.cmd_timeout;
}
else
{
// deactivate timeout
RCLCPP_WARN(
get_node()->get_logger(),
"Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout,
default_tolerances_.goal_time_tolerance);
cmd_timeout_ = 0.0;
}
}
else
{
cmd_timeout_ = 0.0;
}

return CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -1531,6 +1545,26 @@ bool JointTrajectoryController::has_active_trajectory() const
return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg();
}

void JointTrajectoryController::update_pids()
{
for (size_t i = 0; i < dof_; ++i)
{
const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
if (pids_[i])
{
// update PIDs with gains from ROS parameters
pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
}
else
{
// Init PIDs with gains from ROS parameters
pids_[i] = std::make_shared<control_toolbox::Pid>(
gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
}
ff_velocity_scale_[i] = gains.ff_velocity_scale;
}
}

void JointTrajectoryController::init_hold_position_msg()
{
hold_position_msg_ptr_ = std::make_shared<trajectory_msgs::msg::JointTrajectory>();
Expand Down
Loading

0 comments on commit 6201fd7

Please sign in to comment.