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] Accept larger number of joints than command_joints #809

Open
wants to merge 22 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
22 commits
Select commit Hold shift + click to select a range
09fd49a
First changes to JTC accepting num_cmd_joints<dof
christophfroehlich Nov 28, 2023
eb64893
Map command joints to dof
christophfroehlich Nov 28, 2023
d1d55d3
Remove not needed included
christophfroehlich Nov 28, 2023
1327fd9
Add some comments to the tests
christophfroehlich Nov 28, 2023
474b04a
Fix typos
christophfroehlich Nov 28, 2023
28bfa80
Update comments
christophfroehlich Nov 28, 2023
aa67b0b
Use a function to reuse for tests
christophfroehlich Nov 28, 2023
7d62e83
Fix includes
christophfroehlich Nov 28, 2023
141f686
Use correct ff_velocity_scale parameter
christophfroehlich Nov 28, 2023
88ddb8f
Fix tests due to allow_nonzero..
christophfroehlich Nov 28, 2023
c5c2f01
Remove unnecessary warning
christophfroehlich Nov 28, 2023
23146d8
Use new update method for added tests
christophfroehlich Nov 28, 2023
ea50f0f
Fix rqt_jtc with num_cmd<dof
christophfroehlich Dec 10, 2023
bc606f7
Merge branch 'master' into jtc/dof_independent
christophfroehlich Jan 2, 2024
44ef204
Merge branch 'master' into jtc/dof_independent
christophfroehlich Jan 11, 2024
2456246
Merge branch 'master' into jtc/dof_independent
christophfroehlich Feb 17, 2024
f983bcf
Merge branch 'master' into jtc/dof_independent
christophfroehlich Mar 7, 2024
fe8ed5a
Merge branch 'master' into jtc/dof_independent
christophfroehlich Feb 3, 2025
94a7435
Fix pre-commit
christophfroehlich Feb 3, 2025
617afc7
Fix compile errors
christophfroehlich Feb 3, 2025
0e8f70b
Fix the tests from the latest dT change
christophfroehlich Feb 3, 2025
49715d0
Merge branch 'master' into jtc/dof_independent
christophfroehlich Feb 17, 2025
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
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa

// Degrees of freedom
size_t dof_;
size_t num_cmd_joints_;
std::vector<size_t> map_cmd_to_joints_;

// Storing command joint names for interfaces
std::vector<std::string> command_joint_names_;
Expand Down Expand Up @@ -267,9 +269,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa

void init_hold_position_msg();
void resize_joint_trajectory_point(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0);
void resize_joint_trajectory_point_command(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0);

/**
* @brief Assigns the values from a trajectory point interface to a joint interface.
Expand All @@ -283,9 +285,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
void assign_interface_from_point(
const T & joint_interface, const std::vector<double> & trajectory_point_interface)
{
for (size_t index = 0; index < dof_; ++index)
for (size_t index = 0; index < num_cmd_joints_; ++index)
{
joint_interface[index].get().set_value(trajectory_point_interface[index]);
joint_interface[index].get().set_value(trajectory_point_interface[map_cmd_to_joints_[index]]);
}
}
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -169,7 +169,7 @@ class Trajectory
/**
* \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2
* indices. If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated
* mapping vector is <tt>"{2, 1}"</tt>.
* mapping vector is <tt>"{2, 1}"</tt>. return empty vector if \p t1 is not a subset of \p t2.
*/
template <class T>
inline std::vector<size_t> mapping(const T & t1, const T & t2)
Expand Down
116 changes: 73 additions & 43 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
#include <chrono>
#include <functional>
#include <memory>

#include <numeric>
#include <stdexcept>
#include <string>
#include <vector>
Expand All @@ -44,7 +44,7 @@
namespace joint_trajectory_controller
{
JointTrajectoryController::JointTrajectoryController()
: controller_interface::ControllerInterface(), dof_(0)
: controller_interface::ControllerInterface(), dof_(0), num_cmd_joints_(0)
{
}

Expand Down Expand Up @@ -105,16 +105,7 @@ JointTrajectoryController::command_interface_configuration() const
{
controller_interface::InterfaceConfiguration conf;
conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
if (dof_ == 0)
{
fprintf(
stderr,
"During ros2_control interface configuration, degrees of freedom is not valid;"
" it should be positive. Actual DOF is %zu\n",
dof_);
std::exit(EXIT_FAILURE);
}
conf.names.reserve(dof_ * params_.command_interfaces.size());
conf.names.reserve(num_cmd_joints_ * params_.command_interfaces.size());
for (const auto & joint_name : command_joint_names_)
{
for (const auto & interface_type : params_.command_interfaces)
Expand Down Expand Up @@ -291,11 +282,14 @@ controller_interface::return_type JointTrajectoryController::update(
if (use_closed_loop_pid_adapter_)
{
// Update PIDs
for (auto i = 0ul; i < dof_; ++i)
for (auto i = 0ul; i < num_cmd_joints_; ++i)
{
tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) +
pids_[i]->compute_command(
state_error_.positions[i], state_error_.velocities[i], period);
size_t index_cmd_joint = map_cmd_to_joints_[i];
tmp_command_[index_cmd_joint] =
(command_next_.velocities[index_cmd_joint] * ff_velocity_scale_[i]) +
pids_[i]->compute_command(
state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint],
period);
}
}

Expand Down Expand Up @@ -468,9 +462,10 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto
auto assign_point_from_interface =
[&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
{
for (size_t index = 0; index < dof_; ++index)
for (size_t index = 0; index < num_cmd_joints_; ++index)
{
trajectory_point_interface[index] = joint_interface[index].get().get_value();
trajectory_point_interface[map_cmd_to_joints_[index]] =
joint_interface[index].get().get_value();
}
};

Expand Down Expand Up @@ -538,9 +533,10 @@ bool JointTrajectoryController::read_commands_from_command_interfaces(
auto assign_point_from_interface =
[&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
{
for (size_t index = 0; index < dof_; ++index)
for (size_t index = 0; index < num_cmd_joints_; ++index)
{
trajectory_point_interface[index] = joint_interface[index].get().get_value();
trajectory_point_interface[map_cmd_to_joints_[index]] =
joint_interface[index].get().get_value();
}
};

Expand Down Expand Up @@ -672,8 +668,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(

if (params_.joints.empty())
{
// TODO(destogl): is this correct? Can we really move-on if no joint names are not provided?
RCLCPP_WARN(logger, "'joints' parameter is empty.");
return CallbackReturn::FAILURE;
}

command_joint_names_ = params_.command_joints;
Expand All @@ -684,12 +680,40 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
RCLCPP_INFO(
logger, "No specific joint names are used for command interfaces. Using 'joints' parameter.");
}
else if (command_joint_names_.size() != params_.joints.size())
num_cmd_joints_ = command_joint_names_.size();

if (num_cmd_joints_ > dof_)
{
RCLCPP_ERROR(
logger, "'command_joints' parameter has to have the same size as 'joints' parameter.");
logger, "'command_joints' parameter must not have greater size as 'joints' parameter.");
return CallbackReturn::FAILURE;
}
else if (num_cmd_joints_ < dof_)
{
// create a map for the command joints
map_cmd_to_joints_ = mapping(command_joint_names_, params_.joints);
if (map_cmd_to_joints_.size() != num_cmd_joints_)
{
RCLCPP_ERROR(
logger,
"'command_joints' parameter must be a subset of 'joints' parameter, if their size is not "
"equal.");
return CallbackReturn::FAILURE;
}
for (size_t i = 0; i < command_joint_names_.size(); i++)
{
RCLCPP_DEBUG(
logger, "Command joint %lu: '%s' maps to joint %lu: '%s'.", i,
command_joint_names_[i].c_str(), map_cmd_to_joints_[i],
params_.joints.at(map_cmd_to_joints_[i]).c_str());
}
}
else
{
// create a map for the command joints, trivial if the size is the same
map_cmd_to_joints_.resize(num_cmd_joints_);
std::iota(map_cmd_to_joints_.begin(), map_cmd_to_joints_.end(), 0);
}

if (params_.command_interfaces.empty())
{
Expand Down Expand Up @@ -719,9 +743,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(

if (use_closed_loop_pid_adapter_)
{
pids_.resize(dof_);
ff_velocity_scale_.resize(dof_);
tmp_command_.resize(dof_, 0.0);
pids_.resize(num_cmd_joints_);
ff_velocity_scale_.resize(num_cmd_joints_);
tmp_command_.resize(dof_, std::numeric_limits<double>::quiet_NaN());

update_pids();
}
Expand Down Expand Up @@ -869,10 +893,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1));

resize_joint_trajectory_point(state_current_, dof_);
resize_joint_trajectory_point_command(command_current_, dof_);
resize_joint_trajectory_point_command(
command_current_, dof_, std::numeric_limits<double>::quiet_NaN());
resize_joint_trajectory_point(state_desired_, dof_);
resize_joint_trajectory_point(state_error_, dof_);
resize_joint_trajectory_point(last_commanded_state_, dof_);
resize_joint_trajectory_point(
last_commanded_state_, dof_, std::numeric_limits<double>::quiet_NaN());

query_state_srv_ = get_node()->create_service<control_msgs::srv::QueryTrajectoryState>(
std::string(get_node()->get_name()) + "/query_state",
Expand Down Expand Up @@ -912,8 +938,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
command_interfaces_, command_joint_names_, interface, joint_command_interface_[index]))
{
RCLCPP_ERROR(
logger, "Expected %zu '%s' command interfaces, got %zu.", dof_, interface.c_str(),
joint_command_interface_[index].size());
logger, "Expected %zu '%s' command interfaces, got %zu.", num_cmd_joints_,
interface.c_str(), joint_command_interface_[index].size());
return CallbackReturn::ERROR;
}
}
Expand Down Expand Up @@ -941,8 +967,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
// running already)
trajectory_msgs::msg::JointTrajectoryPoint state;
resize_joint_trajectory_point(state, dof_);
// read from cmd joints only if all joints have command interface
// otherwise it leaves the entries of joints without command interface NaN.
// if no open_loop control, state_current_ is then used for `set_point_before_trajectory_msg` and
// future trajectory sampling will always give NaN for these joints
if (
params_.set_last_command_interface_value_as_state_on_activation &&
params_.set_last_command_interface_value_as_state_on_activation && dof_ == num_cmd_joints_ &&
read_state_from_command_interfaces(state))
{
state_current_ = state;
Expand Down Expand Up @@ -998,7 +1028,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
}

for (size_t index = 0; index < dof_; ++index)
for (size_t index = 0; index < num_cmd_joints_; ++index)
{
if (has_position_command_interface_)
{
Expand Down Expand Up @@ -1548,37 +1578,37 @@ bool JointTrajectoryController::contains_interface_type(
}

void JointTrajectoryController::resize_joint_trajectory_point(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value)
{
point.positions.resize(size, 0.0);
point.positions.resize(size, value);
if (has_velocity_state_interface_)
{
point.velocities.resize(size, 0.0);
point.velocities.resize(size, value);
}
if (has_acceleration_state_interface_)
{
point.accelerations.resize(size, 0.0);
point.accelerations.resize(size, value);
}
}

void JointTrajectoryController::resize_joint_trajectory_point_command(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value)
{
if (has_position_command_interface_)
{
point.positions.resize(size, 0.0);
point.positions.resize(size, value);
}
if (has_velocity_command_interface_)
{
point.velocities.resize(size, 0.0);
point.velocities.resize(size, value);
}
if (has_acceleration_command_interface_)
{
point.accelerations.resize(size, 0.0);
point.accelerations.resize(size, value);
}
if (has_effort_command_interface_)
{
point.effort.resize(size, 0.0);
point.effort.resize(size, value);
}
}

Expand All @@ -1589,9 +1619,9 @@ bool JointTrajectoryController::has_active_trajectory() const

void JointTrajectoryController::update_pids()
{
for (size_t i = 0; i < dof_; ++i)
for (size_t i = 0; i < num_cmd_joints_; ++i)
{
const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
const auto & gains = params_.gains.joints_map.at(params_.joints.at(map_cmd_to_joints_[i]));
if (pids_[i])
{
// update PIDs with gains from ROS parameters
Expand Down
5 changes: 4 additions & 1 deletion joint_trajectory_controller/test/test_trajectory_actions.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,8 @@
#include <vector>

#include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp"
#include "gtest/gtest.h"
#include "controller_interface/controller_interface.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/executors/multi_threaded_executor.hpp"
Expand All @@ -36,9 +37,11 @@
#include "rclcpp_action/client.hpp"
#include "rclcpp_action/client_goal_handle.hpp"
#include "rclcpp_action/create_client.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"

#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
#include "test_trajectory_controller_utils.hpp"

using std::placeholders::_1;
Expand Down
Loading
Loading