Skip to content

Commit f6699aa

Browse files
mcbedThibault Poignonectpoignonec
authored
added cartesian velocity controller (#8)
* added cartesian velocity controller * test for naN * add general parameters to template * Update package.xml --------- Co-authored-by: Thibault Poignonec <tpoignonec@unistra.fr> Co-authored-by: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com>
1 parent e91a406 commit f6699aa

File tree

9 files changed

+666
-0
lines changed

9 files changed

+666
-0
lines changed

‎cartesian_velocity_controller/CHANGELOG.rst

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
Changelog for package cartesian_velocity_controller
2+
===================================================
3+
0.0.0 (2023-09-15)
4+
------------------

‎cartesian_velocity_controller/CMakeLists.txt

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(cartesian_velocity_controller)
3+
4+
# Default to C++14
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 14)
7+
endif()
8+
9+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10+
add_compile_options(-Wall -Wextra -Wpedantic)
11+
endif()
12+
13+
# find dependencies
14+
set(THIS_PACKAGE_INCLUDE_DEPENDS
15+
hardware_interface
16+
pluginlib
17+
rclcpp
18+
rclcpp_lifecycle
19+
generate_parameter_library
20+
controller_interface
21+
realtime_tools
22+
kinematics_interface
23+
geometry_msgs
24+
Eigen3
25+
)
26+
27+
# find dependencies
28+
find_package(ament_cmake REQUIRED)
29+
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
30+
find_package(${Dependency} REQUIRED)
31+
endforeach()
32+
33+
include_directories(${EIGEN3_INCLUDE_DIR})
34+
35+
add_library(
36+
cartesian_velocity_controller
37+
SHARED
38+
src/cartesian_velocity_controller.cpp
39+
)
40+
target_include_directories(
41+
cartesian_velocity_controller
42+
PUBLIC
43+
include
44+
)
45+
46+
# Add library of the controller and export it
47+
generate_parameter_library(cartesian_velocity_controller_parameters
48+
src/cartesian_velocity_controller_parameters.yaml
49+
)
50+
51+
target_link_libraries(cartesian_velocity_controller PUBLIC cartesian_velocity_controller_parameters)
52+
ament_target_dependencies(cartesian_velocity_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
53+
# Causes the visibility macros to use dllexport rather than dllimport,
54+
# which is appropriate when building the dll but not consuming it.
55+
target_compile_definitions(cartesian_velocity_controller PRIVATE "EXTERNAL_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL")
56+
# prevent pluginlib from using boost
57+
target_compile_definitions(cartesian_velocity_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
58+
59+
pluginlib_export_plugin_description_file(controller_interface controller_plugin.xml)
60+
61+
install(
62+
TARGETS
63+
cartesian_velocity_controller
64+
RUNTIME DESTINATION bin
65+
ARCHIVE DESTINATION lib
66+
LIBRARY DESTINATION lib
67+
)
68+
69+
install(
70+
DIRECTORY include/
71+
DESTINATION include
72+
)
73+
74+
if(BUILD_TESTING)
75+
find_package(ament_lint_auto REQUIRED)
76+
find_package(controller_manager REQUIRED)
77+
find_package(hardware_interface REQUIRED)
78+
find_package(ros2_control_test_assets REQUIRED)
79+
80+
ament_lint_auto_find_test_dependencies()
81+
82+
ament_add_gmock(
83+
test_load_cartesian_velocity_controller
84+
test/test_load_cartesian_velocity_controller.cpp
85+
)
86+
87+
target_include_directories(
88+
test_load_cartesian_velocity_controller
89+
PRIVATE
90+
include
91+
)
92+
93+
ament_target_dependencies(
94+
test_load_cartesian_velocity_controller
95+
controller_manager
96+
hardware_interface
97+
ros2_control_test_assets
98+
)
99+
endif()
100+
101+
ament_export_include_directories(
102+
include
103+
)
104+
105+
ament_export_libraries(
106+
cartesian_velocity_controller
107+
)
108+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
109+
110+
ament_package()

‎cartesian_velocity_controller/controller_plugin.xml

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<library path="cartesian_velocity_controller">
2+
<class name="cartesian_velocity_controller/CartesianVelocityController"
3+
type="cartesian_velocity_controller::CartesianVelocityController"
4+
base_class_type="controller_interface::ChainableControllerInterface">
5+
<description>
6+
This ros2_control controller commands the Cartesian velocity of the robot end-effector.
7+
</description>
8+
</class>
9+
</library>

‎cartesian_velocity_controller/include/cartesian_velocity_controller/cartesian_velocity_controller.hpp

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
// Copyright 2023, ICube Laboratory, University of Strasbourg
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 CARTESIAN_VELOCITY_CONTROLLER__CARTESIAN_VELOCITY_CONTROLLER_HPP_
16+
#define CARTESIAN_VELOCITY_CONTROLLER__CARTESIAN_VELOCITY_CONTROLLER_HPP_
17+
18+
#include <Eigen/Geometry>
19+
20+
#include <memory>
21+
#include <string>
22+
#include <vector>
23+
24+
#include "controller_interface/chainable_controller_interface.hpp"
25+
#include "cartesian_velocity_controller/visibility_control.h"
26+
#include "geometry_msgs/msg/twist_stamped.hpp"
27+
#include "realtime_tools/realtime_buffer.h"
28+
29+
// auto-generated by generate_parameter_library
30+
#include "cartesian_velocity_controller_parameters.hpp"
31+
32+
// kinematics plugins
33+
#include "kinematics_interface/kinematics_interface.hpp"
34+
#include "pluginlib/class_loader.hpp"
35+
36+
/**
37+
* CartesianVelocityController is a chainable controller that converts end-effector
38+
* twist commands to joint velocity commands using the logic implemented by the
39+
* kinematics interface plugin.
40+
*/
41+
42+
namespace cartesian_velocity_controller
43+
{
44+
using CallbackReturn = controller_interface::CallbackReturn;
45+
using DataType = geometry_msgs::msg::TwistStamped;
46+
47+
class CartesianVelocityController : public controller_interface::ChainableControllerInterface
48+
{
49+
public:
50+
CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
51+
CallbackReturn on_init() override;
52+
53+
CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
54+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
55+
56+
CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
57+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
58+
59+
CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
60+
CallbackReturn on_configure(
61+
const rclcpp_lifecycle::State & previous_state) override;
62+
63+
CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
64+
CallbackReturn on_activate(
65+
const rclcpp_lifecycle::State & previous_state) override;
66+
67+
CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
68+
CallbackReturn on_deactivate(
69+
const rclcpp_lifecycle::State & previous_state) override;
70+
71+
// CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
72+
// controller_interface::CallbackReturn on_cleanup(
73+
// const rclcpp_lifecycle::State & previous_state) override;
74+
75+
// CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
76+
// controller_interface::CallbackReturn on_error(
77+
// const rclcpp_lifecycle::State & previous_state) override;
78+
79+
CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
80+
bool on_set_chained_mode(bool chained_mode) override;
81+
82+
CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
83+
controller_interface::return_type update_and_write_commands(
84+
const rclcpp::Time & time,
85+
const rclcpp::Duration & period) override;
86+
87+
protected:
88+
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
89+
90+
controller_interface::return_type update_reference_from_subscribers() override;
91+
92+
std::shared_ptr<ParamListener> param_listener_;
93+
Params params_;
94+
95+
realtime_tools::RealtimeBuffer<std::shared_ptr<DataType>> rt_buffer_ptr_;
96+
rclcpp::Subscription<DataType>::SharedPtr twist_cmd_sub_;
97+
98+
std::vector<std::string> reference_joint_names_;
99+
std::vector<std::string> reference_interface_names_;
100+
std::vector<std::string> command_joint_names_;
101+
std::vector<std::string> command_interface_names_;
102+
std::vector<std::string> state_interface_names_;
103+
104+
Eigen::VectorXd joint_positions_, joint_velocities_, cart_velocities_;
105+
106+
// Kinematics interface plugin loader
107+
std::shared_ptr<pluginlib::ClassLoader<kinematics_interface::KinematicsInterface>>
108+
kinematics_loader_;
109+
std::unique_ptr<kinematics_interface::KinematicsInterface> kinematics_;
110+
};
111+
112+
} // namespace cartesian_velocity_controller
113+
114+
#endif // CARTESIAN_VELOCITY_CONTROLLER__CARTESIAN_VELOCITY_CONTROLLER_HPP_

‎cartesian_velocity_controller/include/cartesian_velocity_controller/visibility_control.h

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
// Copyright 2022, ICube Laboratory, University of Strasbourg
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 CARTESIAN_VELOCITY_CONTROLLER__VISIBILITY_CONTROL_H_
16+
#define CARTESIAN_VELOCITY_CONTROLLER__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 CARTESIAN_VELOCITY_CONTROLLER_EXPORT __attribute__((dllexport))
24+
#define CARTESIAN_VELOCITY_CONTROLLER_IMPORT __attribute__((dllimport))
25+
#else
26+
#define CARTESIAN_VELOCITY_CONTROLLER_EXPORT __declspec(dllexport)
27+
#define CARTESIAN_VELOCITY_CONTROLLER_IMPORT __declspec(dllimport)
28+
#endif
29+
#ifdef CARTESIAN_VELOCITY_CONTROLLER_BUILDING_DLL
30+
#define CARTESIAN_VELOCITY_CONTROLLER_PUBLIC CARTESIAN_VELOCITY_CONTROLLER_EXPORT
31+
#else
32+
#define CARTESIAN_VELOCITY_CONTROLLER_PUBLIC CARTESIAN_VELOCITY_CONTROLLER_IMPORT
33+
#endif
34+
#define CARTESIAN_VELOCITY_CONTROLLER_PUBLIC_TYPE CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
35+
#define CARTESIAN_VELOCITY_CONTROLLER_LOCAL
36+
#else
37+
#define CARTESIAN_VELOCITY_CONTROLLER_EXPORT __attribute__((visibility("default")))
38+
#define CARTESIAN_VELOCITY_CONTROLLER_IMPORT
39+
#if __GNUC__ >= 4
40+
#define CARTESIAN_VELOCITY_CONTROLLER_PUBLIC __attribute__((visibility("default")))
41+
#define CARTESIAN_VELOCITY_CONTROLLER_LOCAL __attribute__((visibility("hidden")))
42+
#else
43+
#define CARTESIAN_VELOCITY_CONTROLLER_PUBLIC
44+
#define CARTESIAN_VELOCITY_CONTROLLER_LOCAL
45+
#endif
46+
#define CARTESIAN_VELOCITY_CONTROLLER_PUBLIC_TYPE
47+
#endif
48+
49+
#endif // CARTESIAN_VELOCITY_CONTROLLER__VISIBILITY_CONTROL_H_

‎cartesian_velocity_controller/package.xml

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>cartesian_velocity_controller</name>
5+
<version>0.0.0</version>
6+
<description>Cartesian velocity controller package for ros2_control</description>
7+
<maintainer email="mcbed.robotics@gmail.com">Maciej Bednarczyk</maintainer>
8+
<license>Apache 2.0</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>controller_interface</depend>
13+
<depend>hardware_interface</depend>
14+
<depend>pluginlib</depend>
15+
<depend>rclcpp</depend>
16+
<depend>rclcpp_lifecycle</depend>
17+
<depend>realtime_tools</depend>
18+
<depend>kinematics_interface</depend>
19+
<depend>geometry_msgs</depend>
20+
<depend>generate_parameter_library</depend>
21+
22+
<test_depend>ament_cmake_gmock</test_depend>
23+
<test_depend>controller_manager</test_depend>
24+
<test_depend>hardware_interface</test_depend>
25+
<test_depend>hardware_interface_testing</test_depend>
26+
<test_depend>ros2_control_test_assets</test_depend>
27+
<test_depend>kinematics_interface_kdl</test_depend>
28+
29+
<export>
30+
<build_type>ament_cmake</build_type>
31+
</export>
32+
</package>

0 commit comments

Comments
 (0)