Skip to content

Commit 694cc6a

Browse files
destoglAndyZe
destogl
and
AndyZe
committed
Ros2 control extensions rolling joint limits plugins (#5)
* Added initial structures for joint-limit plugins. * Move Ruckig limiter to package joint_limits_enforcement_plugins and make it working. Co-authored-by: AndyZe <zelenak@picknik.ai>
1 parent f4e95da commit 694cc6a

19 files changed

+1122
-1
lines changed

‎joint_limits/CMakeLists.txt

Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(joint_limits)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra)
6+
endif()
7+
8+
find_package(ament_cmake REQUIRED)
9+
find_package(pluginlib REQUIRED)
10+
find_package(rclcpp REQUIRED)
11+
find_package(rcutils REQUIRED)
12+
13+
add_library(joint_limiter_interface SHARED src/joint_limiter_interface.cpp)
14+
target_include_directories(
15+
joint_limiter_interface
16+
PRIVATE
17+
include)
18+
ament_target_dependencies(
19+
joint_limiter_interface
20+
rclcpp
21+
)
22+
# Causes the visibility macros to use dllexport rather than dllimport,
23+
# which is appropriate when building the dll but not consuming it.
24+
target_compile_definitions(joint_limiter_interface PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL")
25+
26+
add_library(
27+
simple_joint_limiter
28+
SHARED
29+
src/simple_joint_limiter.cpp
30+
)
31+
target_include_directories(
32+
simple_joint_limiter
33+
PRIVATE
34+
include
35+
)
36+
target_link_libraries(
37+
simple_joint_limiter
38+
joint_limiter_interface
39+
)
40+
ament_target_dependencies(
41+
simple_joint_limiter
42+
rclcpp
43+
rcutils
44+
)
45+
# Causes the visibility macros to use dllexport rather than dllimport,
46+
# which is appropriate when building the dll but not consuming it.
47+
target_compile_definitions(simple_joint_limiter PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL")
48+
49+
pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml)
50+
51+
install(DIRECTORY include/
52+
DESTINATION include
53+
)
54+
55+
install(
56+
TARGETS
57+
joint_limiter_interface
58+
simple_joint_limiter
59+
ARCHIVE DESTINATION lib
60+
LIBRARY DESTINATION lib
61+
RUNTIME DESTINATION bin
62+
)
63+
64+
if(BUILD_TESTING)
65+
find_package(ament_cmake_gmock REQUIRED)
66+
find_package(pluginlib REQUIRED)
67+
find_package(rclcpp REQUIRED)
68+
69+
#ament_add_gmock(joint_limits_test test/joint_limits_test.cpp)
70+
#target_include_directories(joint_limits_test PUBLIC include)
71+
#target_link_libraries(joint_limits_test joint_limits)
72+
73+
ament_add_gmock(test_simple_joint_limiter test/test_simple_joint_limiter.cpp)
74+
target_include_directories(test_simple_joint_limiter PRIVATE include)
75+
target_link_libraries(test_simple_joint_limiter joint_limiter_interface)
76+
ament_target_dependencies(
77+
test_simple_joint_limiter
78+
pluginlib
79+
rclcpp
80+
)
81+
endif()
82+
83+
ament_export_dependencies(
84+
rclcpp
85+
rcutils
86+
)
87+
88+
ament_export_include_directories(
89+
include
90+
)
91+
92+
ament_export_libraries(
93+
joint_limiter_interface
94+
simple_joint_limiter
95+
)
96+
97+
ament_package()

‎joint_limits/include/joint_limits/joint_limiter_interface.hpp

Lines changed: 93 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
/// \author Denis Stogl
16+
17+
#ifndef JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_
18+
#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_
19+
20+
#include <string>
21+
#include <vector>
22+
23+
#include "joint_limits/joint_limits.hpp"
24+
#include "joint_limits/visibility_control.h"
25+
#include "rclcpp/node.hpp"
26+
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
27+
28+
namespace joint_limits
29+
{
30+
template <typename LimitsType>
31+
class JointLimiterInterface
32+
{
33+
public:
34+
JOINT_LIMITS_PUBLIC JointLimiterInterface() = default;
35+
36+
JOINT_LIMITS_PUBLIC virtual ~JointLimiterInterface() = default;
37+
38+
/// Initialization of every JointLimiter.
39+
/**
40+
* Initialization of JointLimiter for defined joints with their names.
41+
* Robot description topic provides a topic name where URDF of the robot can be found.
42+
* This is needed to use joint limits from URDF (not implemented yet!).
43+
* Override this method only if Initialization and reading joint limits should be adapted.
44+
* Otherwise, initialize your custom limiter in `on_limit` method.
45+
*
46+
* \param[in] joint_names names of joints where limits should be applied.
47+
* \param[in] node shared pointer to the node where joint limit parameters defined.
48+
* \param[in] robot_description_topic string of a topic where robot description is accessible.
49+
*
50+
*/
51+
JOINT_LIMITS_PUBLIC virtual bool init(
52+
const std::vector<std::string> joint_names, const rclcpp::Node::SharedPtr & node,
53+
const std::string & robot_description_topic = "/robot_description");
54+
55+
JOINT_LIMITS_PUBLIC virtual bool configure(
56+
const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states)
57+
{
58+
// TODO(destogl): add checks for position
59+
return on_configure(current_joint_states);
60+
}
61+
62+
JOINT_LIMITS_PUBLIC virtual bool enforce(
63+
trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
64+
trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt)
65+
{
66+
// TODO(destogl): add checks if sizes of vectors and number of limits correspond.
67+
return on_enforce(current_joint_states, desired_joint_states, dt);
68+
}
69+
70+
// TODO(destogl): Make those protected?
71+
// Methods that each limiter implementation has to implement
72+
JOINT_LIMITS_PUBLIC virtual bool on_init() { return true; }
73+
74+
JOINT_LIMITS_PUBLIC virtual bool on_configure(
75+
const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/)
76+
{
77+
return true;
78+
}
79+
80+
JOINT_LIMITS_PUBLIC virtual bool on_enforce(
81+
trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
82+
trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states,
83+
const rclcpp::Duration & dt) = 0;
84+
85+
protected:
86+
size_t number_of_joints_;
87+
std::vector<LimitsType> joint_limits_;
88+
rclcpp::Node::SharedPtr node_;
89+
};
90+
91+
} // namespace joint_limits
92+
93+
#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_

‎hardware_interface/include/joint_limits/joint_limits.hpp renamed to ‎joint_limits/include/joint_limits/joint_limits.hpp

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,8 @@
1818
#define JOINT_LIMITS__JOINT_LIMITS_HPP_
1919

2020
#include <limits>
21+
#include <sstream>
22+
#include <string>
2123

2224
namespace joint_limits
2325
{
@@ -52,6 +54,62 @@ struct JointLimits
5254
bool has_jerk_limits;
5355
bool has_effort_limits;
5456
bool angle_wraparound;
57+
58+
std::string to_string()
59+
{
60+
std::stringstream ss_output;
61+
62+
if (has_position_limits)
63+
{
64+
ss_output << " position limits: "
65+
<< "[" << min_position << ", " << max_position << "]\n";
66+
}
67+
if (has_velocity_limits)
68+
{
69+
ss_output << " velocity limit: "
70+
<< "[" << max_velocity << "]\n";
71+
}
72+
if (has_acceleration_limits)
73+
{
74+
ss_output << " acceleration limit: "
75+
<< "[" << max_acceleration << "]\n";
76+
}
77+
if (has_jerk_limits)
78+
{
79+
ss_output << " jerk limit: "
80+
<< "[" << max_acceleration << "]\n";
81+
}
82+
if (has_effort_limits)
83+
{
84+
ss_output << " effort limit: "
85+
<< "[" << max_acceleration << "]\n";
86+
}
87+
if (angle_wraparound)
88+
{
89+
ss_output << " angle wraparound is active.";
90+
}
91+
92+
return ss_output.str();
93+
}
94+
95+
std::string debug_to_string()
96+
{
97+
std::stringstream ss_output;
98+
99+
ss_output << " has position limits: " << (has_position_limits ? "true" : "false") << "["
100+
<< min_position << ", " << max_position << "]\n";
101+
ss_output << " has velocity limits: " << (has_velocity_limits ? "true" : "false") << "["
102+
<< max_velocity << "]\n";
103+
ss_output << " has acceleration limits: " << (has_acceleration_limits ? "true" : "false")
104+
<< " [" << max_acceleration << "]\n";
105+
ss_output << " has jerk limits: " << (has_jerk_limits ? "true" : "false") << "[" << max_jerk
106+
<< "]\n";
107+
ss_output << " has effort limits: " << (has_effort_limits ? "true" : "false") << "["
108+
<< max_effort << "]\n";
109+
ss_output << " angle wraparound: " << (angle_wraparound ? "true" : "false");
110+
111+
return ss_output.str();
112+
}
55113
};
56114

57115
struct SoftJointLimits

‎hardware_interface/include/joint_limits/joint_limits_rosparam.hpp renamed to ‎joint_limits/include/joint_limits/joint_limits_rosparam.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -259,7 +259,7 @@ inline bool get_joint_limits(
259259
* \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and
260260
* \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise.
261261
*/
262-
inline bool get_soft_joint_limits(
262+
inline bool get_joint_limits(
263263
const std::string & joint_name, const rclcpp::Node::SharedPtr & node,
264264
SoftJointLimits & soft_limits)
265265
{

‎joint_limits/include/joint_limits/simple_joint_limiter.hpp

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
// Copyright (c) 2021, PickNik Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
/// \author Denis Stogl
16+
17+
#ifndef JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_
18+
#define JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_
19+
20+
#include <string>
21+
22+
#include "joint_limits/joint_limiter_interface.hpp"
23+
#include "joint_limits/joint_limits.hpp"
24+
25+
namespace joint_limits
26+
{
27+
template <typename LimitsType>
28+
class SimpleJointLimiter : public JointLimiterInterface<JointLimits>
29+
{
30+
public:
31+
JOINT_LIMITS_PUBLIC SimpleJointLimiter();
32+
33+
JOINT_LIMITS_PUBLIC bool on_enforce(
34+
trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
35+
trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states,
36+
const rclcpp::Duration & dt) override;
37+
};
38+
39+
} // namespace joint_limits
40+
41+
#endif // JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_

‎joint_limits/include/joint_limits/visibility_control.h

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef JOINT_LIMITS__VISIBILITY_CONTROL_H_
16+
#define JOINT_LIMITS__VISIBILITY_CONTROL_H_
17+
18+
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
19+
// https://gcc.gnu.org/wiki/Visibility
20+
21+
#if defined _WIN32 || defined __CYGWIN__
22+
#ifdef __GNUC__
23+
#define JOINT_LIMITS_EXPORT __attribute__((dllexport))
24+
#define JOINT_LIMITS_IMPORT __attribute__((dllimport))
25+
#else
26+
#define JOINT_LIMITS_EXPORT __declspec(dllexport)
27+
#define JOINT_LIMITS_IMPORT __declspec(dllimport)
28+
#endif
29+
#ifdef JOINT_LIMITS_BUILDING_DLL
30+
#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_EXPORT
31+
#else
32+
#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_IMPORT
33+
#endif
34+
#define JOINT_LIMITS_PUBLIC_TYPE JOINT_LIMITS_PUBLIC
35+
#define JOINT_LIMITS_LOCAL
36+
#else
37+
#define JOINT_LIMITS_EXPORT __attribute__((visibility("default")))
38+
#define JOINT_LIMITS_IMPORT
39+
#if __GNUC__ >= 4
40+
#define JOINT_LIMITS_PUBLIC __attribute__((visibility("default")))
41+
#define JOINT_LIMITS_LOCAL __attribute__((visibility("hidden")))
42+
#else
43+
#define JOINT_LIMITS_PUBLIC
44+
#define JOINT_LIMITS_LOCAL
45+
#endif
46+
#define JOINT_LIMITS_PUBLIC_TYPE
47+
#endif
48+
49+
#endif // JOINT_LIMITS__VISIBILITY_CONTROL_H_

‎joint_limits/joint_limiters.xml

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<library path="simple_joint_limiter">
2+
<class name="joint_limits/SimpleJointLimiter"
3+
type="joint_limits::SimpleJointLimiter<joint_limits::JointLimits>"
4+
base_class_type="joint_limits::JointLimiterInterface<joint_limits::JointLimits>">
5+
<description>
6+
Simple joint limiter using clamping approach.
7+
</description>
8+
</class>
9+
</library>

0 commit comments

Comments
 (0)