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.

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

[JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain. #380

Merged
merged 3 commits into from
Aug 21, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
27 changes: 25 additions & 2 deletions joint_trajectory_controller/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,26 @@ Other features

Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments.

ros2_control interfaces
------------------------

References
^^^^^^^^^^^
(the controller is not yet implemented as chainable controller)

States
^^^^^^^
The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``<joint>/<state_interface>``.
Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the [hardware_interface/hardware_interface_type_values.hpp](https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp).
Legal combinations of state interfaces are:
- ``position``
- ``position`` and ``velocity``
- ``position``, ``velocity`` and ``acceleration``
- ``effort``

Commands
^^^^^^^^^


Using Joint Trajectory Controller(s)
------------------------------------
Expand Down Expand Up @@ -84,8 +104,11 @@ A yaml file for using it could be:
Details about parameters
^^^^^^^^^^^^^^^^^^^^^^^^

joint (list(string)):
Joint names to control.
joints (list(string)):
Joint names to control and listen to.
destogl marked this conversation as resolved.
Show resolved Hide resolved

command_joints (list(string)):
Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names.
destogl marked this conversation as resolved.
Show resolved Hide resolved

command_interface (list(string)):
Command interfaces provided by the hardware interface for all joints.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa

protected:
std::vector<std::string> joint_names_;
std::vector<std::string> command_joint_names_;
std::vector<std::string> command_interface_types_;
std::vector<std::string> state_interface_types_;

Expand Down
32 changes: 26 additions & 6 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init()
{
// with the lifecycle node being initialized, we can declare parameters
joint_names_ = auto_declare<std::vector<std::string>>("joints", joint_names_);
command_joint_names_ =
auto_declare<std::vector<std::string>>("command_joints", command_joint_names_);
command_interface_types_ =
auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
state_interface_types_ =
Expand Down Expand Up @@ -97,7 +99,7 @@ JointTrajectoryController::command_interface_configuration() const
std::exit(EXIT_FAILURE);
}
conf.names.reserve(dof_ * command_interface_types_.size());
for (const auto & joint_name : joint_names_)
for (const auto & joint_name : command_joint_names_)
{
for (const auto & interface_type : command_interface_types_)
{
Expand Down Expand Up @@ -493,16 +495,33 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(

dof_ = joint_names_.size();

// TODO(destogl): why is this here? Add comment or move
if (!reset())
{
return CallbackReturn::FAILURE;
}
destogl marked this conversation as resolved.
Show resolved Hide resolved

if (joint_names_.empty())
{
// TODO(destogl): is this correct? Can we really move-on if no joint names are not provided?
destogl marked this conversation as resolved.
Show resolved Hide resolved
RCLCPP_WARN(logger, "'joints' parameter is empty.");
}

command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array();

if (command_joint_names_.empty())
{
command_joint_names_ = joint_names_;
RCLCPP_INFO(
logger, "No specific joint names are used for command interfaces. Using 'joints' parameter.");
}
else if (command_joint_names_.size() != joint_names_.size())
{
RCLCPP_ERROR(
logger, "'command_joints' parameter has to have the same size as 'joints' parameter.");
return CallbackReturn::FAILURE;
}

// Specialized, child controllers set interfaces before calling configure function.
if (command_interface_types_.empty())
{
Expand Down Expand Up @@ -592,12 +611,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
// Init PID gains from ROS parameter server
for (size_t i = 0; i < pids_.size(); ++i)
{
const std::string prefix = "gains." + joint_names_[i];
const std::string prefix = "gains." + command_joint_names_[i];
const auto k_p = auto_declare<double>(prefix + ".p", 0.0);
const auto k_i = auto_declare<double>(prefix + ".i", 0.0);
const auto k_d = auto_declare<double>(prefix + ".d", 0.0);
const auto i_clamp = auto_declare<double>(prefix + ".i_clamp", 0.0);
ff_velocity_scale_[i] = auto_declare<double>("ff_velocity_scale/" + joint_names_[i], 0.0);
ff_velocity_scale_[i] =
auto_declare<double>("ff_velocity_scale/" + command_joint_names_[i], 0.0);
// Initialize PID
pids_[i] = std::make_shared<control_toolbox::Pid>(k_p, k_i, k_d, i_clamp, -i_clamp);
}
Expand Down Expand Up @@ -702,7 +722,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
get_interface_list(command_interface_types_).c_str(),
get_interface_list(state_interface_types_).c_str());

default_tolerances_ = get_segment_tolerances(*get_node(), joint_names_);
default_tolerances_ = get_segment_tolerances(*get_node(), command_joint_names_);

// Read parameters customizing controller for special cases
open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value<bool>();
Expand Down Expand Up @@ -738,7 +758,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
state_publisher_ = std::make_unique<StatePublisher>(publisher_);

state_publisher_->lock();
state_publisher_->msg_.joint_names = joint_names_;
state_publisher_->msg_.joint_names = command_joint_names_;
state_publisher_->msg_.desired.positions.resize(dof_);
state_publisher_->msg_.desired.velocities.resize(dof_);
state_publisher_->msg_.desired.accelerations.resize(dof_);
Expand Down Expand Up @@ -797,7 +817,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface);
auto index = std::distance(allowed_interface_types_.begin(), it);
if (!controller_interface::get_ordered_interfaces(
command_interfaces_, joint_names_, interface, joint_command_interface_[index]))
command_interfaces_, command_joint_names_, interface, joint_command_interface_[index]))
{
RCLCPP_ERROR(
get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <gmock/gmock.h>
#include <memory>

#include "gmock/gmock.h"

#include "controller_manager/controller_manager.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/executor.hpp"
Expand Down
81 changes: 81 additions & 0 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,87 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
executor.cancel();
}

TEST_P(TrajectoryControllerTestParameterized, check_interface_names)
{
SetUpTrajectoryController();

const auto state = traj_controller_->get_node()->configure();
destogl marked this conversation as resolved.
Show resolved Hide resolved
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);

std::vector<std::string> state_interface_names;
state_interface_names.reserve(joint_names_.size() * state_interface_types_.size());
for (const auto & joint : joint_names_)
{
for (const auto & interface : state_interface_types_)
{
state_interface_names.push_back(joint + "/" + interface);
}
}
auto state_interfaces = traj_controller_->state_interface_configuration();
EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size());
ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names));

std::vector<std::string> command_interface_names;
command_interface_names.reserve(joint_names_.size() * command_interface_types_.size());
for (const auto & joint : joint_names_)
{
for (const auto & interface : command_interface_types_)
{
command_interface_names.push_back(joint + "/" + interface);
}
}
auto command_interfaces = traj_controller_->command_interface_configuration();
EXPECT_EQ(
command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
EXPECT_EQ(command_interfaces.names.size(), joint_names_.size() * command_interface_types_.size());
ASSERT_THAT(
command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names));
}

TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints)
{
SetUpTrajectoryController();

// set command_joints parameter
const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_);
traj_controller_->get_node()->set_parameter({command_joint_names_param});

const auto state = traj_controller_->get_node()->configure();
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);

std::vector<std::string> state_interface_names;
state_interface_names.reserve(joint_names_.size() * state_interface_types_.size());
for (const auto & joint : joint_names_)
{
for (const auto & interface : state_interface_types_)
{
state_interface_names.push_back(joint + "/" + interface);
}
}
auto state_interfaces = traj_controller_->state_interface_configuration();
EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size());
ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names));

std::vector<std::string> command_interface_names;
command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size());
for (const auto & joint : command_joint_names_)
{
for (const auto & interface : command_interface_types_)
{
command_interface_names.push_back(joint + "/" + interface);
}
}
auto command_interfaces = traj_controller_->command_interface_configuration();
EXPECT_EQ(
command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
EXPECT_EQ(
command_interfaces.names.size(), command_joint_names_.size() * command_interface_types_.size());
ASSERT_THAT(
command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names));
}

TEST_P(TrajectoryControllerTestParameterized, activate)
{
SetUpTrajectoryController();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,11 @@ class TestableJointTrajectoryController

void set_joint_names(const std::vector<std::string> & joint_names) { joint_names_ = joint_names; }

void set_command_joint_names(const std::vector<std::string> & command_joint_names)
{
command_joint_names_ = command_joint_names;
}

void set_command_interfaces(const std::vector<std::string> & command_interfaces)
{
command_interface_types_ = command_interfaces;
Expand Down Expand Up @@ -115,6 +120,8 @@ class TrajectoryControllerTest : public ::testing::Test
controller_name_ = "test_joint_trajectory_controller";

joint_names_ = {"joint1", "joint2", "joint3"};
command_joint_names_ = {
"following_controller/joint1", "following_controller/joint2", "following_controller/joint3"};
joint_pos_.resize(joint_names_.size(), 0.0);
joint_state_pos_.resize(joint_names_.size(), 0.0);
joint_vel_.resize(joint_names_.size(), 0.0);
Expand Down Expand Up @@ -396,6 +403,7 @@ class TrajectoryControllerTest : public ::testing::Test
std::string controller_name_;

std::vector<std::string> joint_names_;
std::vector<std::string> command_joint_names_;
std::vector<std::string> command_interface_types_;
std::vector<std::string> state_interface_types_;

Expand Down