diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt
index c92d61b3f3..4b0ca82df8 100644
--- a/joint_trajectory_controller/CMakeLists.txt
+++ b/joint_trajectory_controller/CMakeLists.txt
@@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
rsl
tl_expected
trajectory_msgs
+ urdf
)
find_package(ament_cmake REQUIRED)
diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml
index 2ffe8aed36..4e4dacf202 100644
--- a/joint_trajectory_controller/doc/parameters_context.yaml
+++ b/joint_trajectory_controller/doc/parameters_context.yaml
@@ -9,5 +9,10 @@ gains: |
.. math:: u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v)
- with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below),
+ with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below),
the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively.
+
+ If the joint is of continuous type, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`,
+ i.e., the shortest rotation to the target position is the desired motion.
+ Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured
+ position :math:`s` from the state interface.
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
index 111837cc17..ae370df0a6 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -43,6 +43,7 @@
#include "realtime_tools/realtime_server_goal_handle.h"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+#include "urdf/model.h"
// auto-generated by generate_parameter_library
#include "joint_trajectory_controller_parameters.hpp"
@@ -298,6 +299,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
void resize_joint_trajectory_point_command(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
+ urdf::Model model_;
+
/**
* @brief Assigns the values from a trajectory point interface to a joint interface.
*
diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml
index 5aad4cb86e..8457c02aeb 100644
--- a/joint_trajectory_controller/package.xml
+++ b/joint_trajectory_controller/package.xml
@@ -25,6 +25,7 @@
rsl
tl_expected
trajectory_msgs
+ urdf
ament_cmake_gmock
controller_manager
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 46544bf875..6cae29e083 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -36,6 +36,7 @@
#include "rclcpp_action/create_server.hpp"
#include "rclcpp_action/server_goal_handle.hpp"
#include "rclcpp_lifecycle/state.hpp"
+#include "urdf/model.h"
namespace joint_trajectory_controller
{
@@ -46,6 +47,23 @@ JointTrajectoryController::JointTrajectoryController()
controller_interface::CallbackReturn JointTrajectoryController::on_init()
{
+ if (!urdf_.empty())
+ {
+ if (!model_.initString(urdf_))
+ {
+ RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse URDF file");
+ }
+ else
+ {
+ RCLCPP_DEBUG(get_node()->get_logger(), "Successfully parsed URDF file");
+ }
+ }
+ else
+ {
+ // empty URDF is used for some tests
+ RCLCPP_DEBUG(get_node()->get_logger(), "No URDF file given");
+ }
+
try
{
// Create the parameter listener and get the parameters
@@ -684,12 +702,33 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
update_pids();
}
- // Configure joint position error normalization from ROS parameters (angle_wraparound)
+ // Configure joint position error normalization (angle_wraparound)
joints_angle_wraparound_.resize(dof_);
for (size_t i = 0; i < dof_; ++i)
{
const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
- joints_angle_wraparound_[i] = gains.angle_wraparound;
+ if (gains.angle_wraparound)
+ {
+ // TODO(christophfroehlich): remove this warning in a future release (ROS-J)
+ RCLCPP_WARN(
+ logger,
+ "[Deprecated] Parameter 'gains..angle_wraparound' is deprecated. The "
+ "angle_wraparound is now used if a continuous joint is configured in the URDF.");
+ joints_angle_wraparound_[i] = true;
+ }
+
+ if (!urdf_.empty())
+ {
+ auto urdf_joint = model_.getJoint(params_.joints[i]);
+ if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS)
+ {
+ RCLCPP_DEBUG(
+ logger, "joint '%s' is of type continuous, use angle_wraparound.",
+ params_.joints[i].c_str());
+ joints_angle_wraparound_[i] = true;
+ }
+ // do nothing if joint is not found in the URDF
+ }
}
if (params_.state_interfaces.empty())
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
index ded5bb7ca3..b17229cab8 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
+++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
@@ -125,11 +125,8 @@ joint_trajectory_controller:
angle_wraparound: {
type: bool,
default_value: false,
- description: 'For joints that wrap around (without end stop, ie. are continuous),
- where the shortest rotation to the target position is the desired motion.
- If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`.
- Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured
- position :math:`s` from the state interface.'
+ description: "[deprecated] For joints that wrap around (ie. are continuous).
+ Normalizes position-error to -pi to pi."
}
constraints:
stopped_velocity_tolerance: {
diff --git a/joint_trajectory_controller/test/test_assets.hpp b/joint_trajectory_controller/test/test_assets.hpp
new file mode 100644
index 0000000000..ccdbaf4c8a
--- /dev/null
+++ b/joint_trajectory_controller/test/test_assets.hpp
@@ -0,0 +1,278 @@
+// Copyright 2023 ros2_control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TEST_ASSETS_HPP_
+#define TEST_ASSETS_HPP_
+
+#include
+
+namespace test_trajectory_controllers
+{
+const auto urdf_rrrbot_revolute =
+ R"(
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+)";
+
+const auto urdf_rrrbot_continuous =
+ R"(
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+)";
+
+} // namespace test_trajectory_controllers
+
+#endif // TEST_ASSETS_HPP_
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 2bfe6f150d..67eb959df2 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -45,6 +45,7 @@
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+#include "test_assets.hpp"
#include "test_trajectory_controller_utils.hpp"
using lifecycle_msgs::msg::State;
@@ -463,14 +464,15 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
const double EPS = 1e-6;
/**
- * @brief check if calculated trajectory error is correct with angle wraparound=true
+ * @brief check if calculated trajectory error is correct (angle wraparound) for continuous joints
*/
TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_true)
{
rclcpp::executors::MultiThreadedExecutor executor;
std::vector params = {};
SetUpAndActivateTrajectoryController(
- executor, params, true, 0.0, 1.0, true /* angle_wraparound */);
+ executor, params, true, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS,
+ INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_continuous);
size_t n_joints = joint_names_.size();
@@ -554,14 +556,15 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru
}
/**
- * @brief check if calculated trajectory error is correct with angle wraparound=false
+ * @brief check if calculated trajectory error is correct (no angle wraparound) for revolute joints
*/
TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_false)
{
rclcpp::executors::MultiThreadedExecutor executor;
std::vector params = {};
SetUpAndActivateTrajectoryController(
- executor, params, true, 0.0, 1.0, false /* angle_wraparound */);
+ executor, params, true, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS,
+ INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute);
size_t n_joints = joint_names_.size();
@@ -636,15 +639,16 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal
}
/**
- * @brief check if position error of revolute joints are wrapped around if not configured so
+ * @brief check if position error of revolute joints aren't wrapped around (state topic)
*/
TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound)
{
rclcpp::executors::MultiThreadedExecutor executor;
constexpr double k_p = 10.0;
std::vector params = {};
- bool angle_wraparound = false;
- SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound);
+ SetUpAndActivateTrajectoryController(
+ executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS,
+ INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute);
subscribeToState();
size_t n_joints = joint_names_.size();
@@ -737,14 +741,16 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
}
/**
- * @brief check if position error of revolute joints are wrapped around if configured so
+ * @brief check if position error of continuous joints are wrapped around (state topic)
*/
TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
{
rclcpp::executors::MultiThreadedExecutor executor;
constexpr double k_p = 10.0;
std::vector params = {};
- SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true);
+ SetUpAndActivateTrajectoryController(
+ executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS,
+ INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_continuous);
size_t n_joints = joint_names_.size();
@@ -1844,7 +1850,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co
std::vector initial_acc_cmd{3, std::numeric_limits::quiet_NaN()};
SetUpAndActivateTrajectoryController(
- executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd,
+ executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd,
initial_acc_cmd);
// no call of update method, so the values should be read from state interfaces
@@ -1888,7 +1894,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co
initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0);
}
SetUpAndActivateTrajectoryController(
- executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd,
+ executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd,
initial_acc_cmd);
// no call of update method, so the values should be read from command interfaces
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index 6978d0e452..baa4111239 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -215,9 +215,10 @@ class TrajectoryControllerTest : public ::testing::Test
}
void SetUpTrajectoryController(
- rclcpp::Executor & executor, const std::vector & parameters = {})
+ rclcpp::Executor & executor, const std::vector & parameters = {},
+ const std::string & urdf = "")
{
- auto ret = SetUpTrajectoryControllerLocal(parameters);
+ auto ret = SetUpTrajectoryControllerLocal(parameters, urdf);
if (ret != controller_interface::return_type::OK)
{
FAIL();
@@ -226,7 +227,7 @@ class TrajectoryControllerTest : public ::testing::Test
}
controller_interface::return_type SetUpTrajectoryControllerLocal(
- const std::vector & parameters = {})
+ const std::vector & parameters = {}, const std::string & urdf = "")
{
traj_controller_ = std::make_shared();
@@ -241,11 +242,10 @@ class TrajectoryControllerTest : public ::testing::Test
traj_controller_->set_node_options(node_options);
return traj_controller_->init(
- controller_name_, "", 0, "", traj_controller_->define_custom_node_options());
+ controller_name_, urdf, 0, "", traj_controller_->define_custom_node_options());
}
- void SetPidParameters(
- double p_value = 0.0, double ff_value = 1.0, bool angle_wraparound_value = false)
+ void SetPidParameters(double p_value = 0.0, double ff_value = 1.0)
{
traj_controller_->trigger_declare_parameters();
auto node = traj_controller_->get_node();
@@ -258,20 +258,18 @@ class TrajectoryControllerTest : public ::testing::Test
const rclcpp::Parameter k_d(prefix + ".d", 0.0);
const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0);
const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value);
- const rclcpp::Parameter angle_wraparound(
- prefix + ".angle_wraparound", angle_wraparound_value);
- node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound});
+ node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale});
}
}
void SetUpAndActivateTrajectoryController(
rclcpp::Executor & executor, const std::vector & parameters = {},
bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0,
- bool angle_wraparound = false,
const std::vector initial_pos_joints = INITIAL_POS_JOINTS,
const std::vector initial_vel_joints = INITIAL_VEL_JOINTS,
const std::vector initial_acc_joints = INITIAL_ACC_JOINTS,
- const std::vector initial_eff_joints = INITIAL_EFF_JOINTS)
+ const std::vector initial_eff_joints = INITIAL_EFF_JOINTS,
+ const std::string & urdf = "")
{
auto has_nonzero_vel_param =
std::find_if(
@@ -287,10 +285,10 @@ class TrajectoryControllerTest : public ::testing::Test
parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true);
}
// read-only parameters have to be set before init -> won't be read otherwise
- SetUpTrajectoryController(executor, parameters_local);
+ SetUpTrajectoryController(executor, parameters_local, urdf);
// set pid parameters before configure
- SetPidParameters(k_p, ff, angle_wraparound);
+ SetPidParameters(k_p, ff);
traj_controller_->get_node()->configure();
ActivateTrajectoryController(