diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp
index d9652a28f9..c49925b701 100644
--- a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp
+++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp
@@ -27,7 +27,6 @@ constexpr char HW_IF_VELOCITY[] = "velocity";
constexpr char HW_IF_ACCELERATION[] = "acceleration";
/// Constant defining effort interface name
constexpr char HW_IF_EFFORT[] = "effort";
-
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_
diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml
index 75589df5a3..0d3ffdbe04 100644
--- a/hardware_interface/package.xml
+++ b/hardware_interface/package.xml
@@ -9,7 +9,6 @@
ament_cmake
- backward_ros
control_msgs
lifecycle_msgs
pluginlib
diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp
index 85425ff8a1..79fd40a047 100644
--- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp
+++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -49,7 +49,7 @@ class JointLimiterInterface
*
*/
JOINT_LIMITS_PUBLIC virtual bool init(
- const std::vector joint_names, const rclcpp::Node::SharedPtr & node,
+ const std::vector & joint_names, const rclcpp::Node::SharedPtr & node,
const std::string & robot_description_topic = "/robot_description");
JOINT_LIMITS_PUBLIC virtual bool configure(
@@ -67,7 +67,7 @@ class JointLimiterInterface
return on_enforce(current_joint_states, desired_joint_states, dt);
}
- // TODO(destogl): Make those protected?
+protected:
// Methods that each limiter implementation has to implement
JOINT_LIMITS_PUBLIC virtual bool on_init() { return true; }
@@ -78,11 +78,10 @@ class JointLimiterInterface
}
JOINT_LIMITS_PUBLIC virtual bool on_enforce(
- trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
+ const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states,
const rclcpp::Duration & dt) = 0;
-protected:
size_t number_of_joints_;
std::vector joint_names_;
std::vector joint_limits_;
diff --git a/joint_limits/include/joint_limits/simple_joint_limiter.hpp b/joint_limits/include/joint_limits/simple_joint_limiter.hpp
index 2011e0d48e..aa2e0a8d6c 100644
--- a/joint_limits/include/joint_limits/simple_joint_limiter.hpp
+++ b/joint_limits/include/joint_limits/simple_joint_limiter.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2021, PickNik Inc.
+// Copyright (c) 2023, PickNik Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-/// \author Denis Stogl
+/// \author Dr. Denis Stogl
#ifndef JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_
#define JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_
@@ -31,7 +31,7 @@ class SimpleJointLimiter : public JointLimiterInterface
JOINT_LIMITS_PUBLIC SimpleJointLimiter();
JOINT_LIMITS_PUBLIC bool on_enforce(
- trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
+ const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states,
const rclcpp::Duration & dt) override;
};
diff --git a/joint_limits/include/joint_limits/visibility_control.h b/joint_limits/include/joint_limits/visibility_control.h
index 0dcc0193a1..9c957a3cf9 100644
--- a/joint_limits/include/joint_limits/visibility_control.h
+++ b/joint_limits/include/joint_limits/visibility_control.h
@@ -1,4 +1,4 @@
-// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml
index ff45611198..a25667d2c1 100644
--- a/joint_limits/joint_limiters.xml
+++ b/joint_limits/joint_limiters.xml
@@ -3,7 +3,7 @@
type="joint_limits::SimpleJointLimiter<joint_limits::JointLimits>"
base_class_type="joint_limits::JointLimiterInterface<joint_limits::JointLimits>">
- Simple joint limiter using clamping approach.
+ Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities.
diff --git a/joint_limits/package.xml b/joint_limits/package.xml
index 9a8996a84c..0e0b401814 100644
--- a/joint_limits/package.xml
+++ b/joint_limits/package.xml
@@ -1,7 +1,7 @@
joint_limits
3.12.0
- Package for with interfaces for handling of joint limits for use in controllers or in hardware. The package also implements a simple, default joint limit strategy by clamping the values.
+ Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements a simple, default joint limit strategy by clamping the values.
Bence Magyar
Denis Štogl
diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp
index b3b4d65bb6..290bee9842 100644
--- a/joint_limits/src/joint_limiter_interface.cpp
+++ b/joint_limits/src/joint_limiter_interface.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-/// \author Denis Stogl
+/// \author Dr. Denis Stogl
#include "joint_limits/joint_limiter_interface.hpp"
@@ -39,7 +39,7 @@ bool JointLimiterInterface::init(
// TODO(destogl): get limits from URDF
// Initialize and get joint limits from parameter server
- for (auto i = 0ul; i < number_of_joints_; ++i)
+ for (size_t i = 0; i < number_of_joints_; ++i)
{
if (!declare_parameters(joint_names[i], node))
{
diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp
index 8ebcfde77b..9ebbefbea8 100644
--- a/joint_limits/src/simple_joint_limiter.cpp
+++ b/joint_limits/src/simple_joint_limiter.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2021, PickNik Inc.
+// Copyright (c) 2023, PickNik Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -62,7 +62,7 @@ bool SimpleJointLimiter::on_enforce(
bool position_limit_triggered = false;
- for (auto index = 0u; index < num_joints; ++index)
+ for (size_t index = 0; index < num_joints; ++index)
{
desired_pos[index] = desired_joint_states.positions[index];
@@ -143,8 +143,7 @@ bool SimpleJointLimiter::on_enforce(
if (position_limit_triggered)
{
- std::ostringstream ostr;
- for (auto index = 0u; index < num_joints; ++index)
+ for (size_t index = 0; index < num_joints; ++index)
{
// Compute accel to stop
// Here we aren't explicitly maximally decelerating, but for joints near their limits this
@@ -171,7 +170,7 @@ bool SimpleJointLimiter::on_enforce(
pos_limit_trig_jnts.begin(), pos_limit_trig_jnts.end(), [](bool trig) { return trig; }) > 0)
{
std::ostringstream ostr;
- for (auto index = 0u; index < num_joints; ++index)
+ for (size_t index = 0; index < num_joints; ++index)
{
if (pos_limit_trig_jnts[index]) ostr << joint_names_[index] << " ";
}
diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp
index e025ac0b9f..e5bd3697ed 100644
--- a/joint_limits/test/test_simple_joint_limiter.cpp
+++ b/joint_limits/test/test_simple_joint_limiter.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-/// \author Denis Stogl
+/// \author Dr. Denis Stogl
#include
diff --git a/joint_limits_interface/CMakeLists.txt b/joint_limits_interface/CMakeLists.txt
index d48085177c..a5d4577343 100644
--- a/joint_limits_interface/CMakeLists.txt
+++ b/joint_limits_interface/CMakeLists.txt
@@ -5,7 +5,6 @@ find_package(ament_cmake REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(rclcpp REQUIRED)
find_package(urdf REQUIRED)
-find_package(backward_ros REQUIRED)
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
diff --git a/joint_limits_interface/package.xml b/joint_limits_interface/package.xml
index 199d53120b..badbb51773 100644
--- a/joint_limits_interface/package.xml
+++ b/joint_limits_interface/package.xml
@@ -18,7 +18,6 @@
rclcpp
urdf
- backward_ros
hardware_interface