-
Notifications
You must be signed in to change notification settings - Fork 324
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add generic admittance controller for TCP wrenches (#370)
Co-authored-by: AndyZe <[email protected]> Co-authored-by: Denis Štogl <[email protected]>
- Loading branch information
1 parent
34da157
commit 9be3bfe
Showing
21 changed files
with
3,098 additions
and
19 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,122 @@ | ||
cmake_minimum_required(VERSION 3.5) | ||
project(admittance_controller) | ||
|
||
if(NOT CMAKE_CXX_STANDARD) | ||
set(CMAKE_CXX_STANDARD 17) | ||
endif() | ||
|
||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
add_compile_options(-Wall -Wextra -Wpedantic) | ||
endif() | ||
|
||
# find dependencies | ||
set(THIS_PACKAGE_INCLUDE_DEPENDS | ||
angles | ||
control_msgs | ||
control_toolbox | ||
controller_interface | ||
kinematics_interface | ||
Eigen3 | ||
generate_parameter_library | ||
geometry_msgs | ||
hardware_interface | ||
joint_trajectory_controller | ||
pluginlib | ||
rclcpp | ||
rclcpp_lifecycle | ||
realtime_tools | ||
tf2 | ||
tf2_eigen | ||
tf2_geometry_msgs | ||
tf2_kdl | ||
tf2_ros | ||
trajectory_msgs | ||
) | ||
|
||
find_package(ament_cmake REQUIRED) | ||
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) | ||
find_package(${Dependency} REQUIRED) | ||
endforeach() | ||
|
||
add_library(${PROJECT_NAME} SHARED src/admittance_controller.cpp) | ||
target_include_directories(${PROJECT_NAME} PRIVATE include) | ||
generate_parameter_library(${PROJECT_NAME}_parameters src/admittance_controller_parameters.yaml) | ||
target_link_libraries(${PROJECT_NAME} admittance_controller_parameters) | ||
ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) | ||
|
||
# Causes the visibility macros to use dllexport rather than dllimport, | ||
# which is appropriate when building the dll but not consuming it. | ||
target_compile_definitions(${PROJECT_NAME} PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") | ||
|
||
pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) | ||
|
||
install(DIRECTORY include/ | ||
DESTINATION include | ||
) | ||
|
||
install(TARGETS ${PROJECT_NAME} | ||
RUNTIME DESTINATION bin | ||
ARCHIVE DESTINATION lib | ||
LIBRARY DESTINATION lib | ||
) | ||
|
||
if(BUILD_TESTING) | ||
find_package(ament_cmake_gmock REQUIRED) | ||
find_package(control_msgs REQUIRED) | ||
find_package(controller_manager REQUIRED) | ||
find_package(controller_interface REQUIRED) | ||
find_package(hardware_interface REQUIRED) | ||
find_package(ros2_control_test_assets REQUIRED) | ||
|
||
## create custom test function to pass yaml file into test main | ||
#function(add_test_with_yaml_input TARGET SOURCES YAML_FILE) | ||
#add_executable(${TARGET} ${SOURCES}) | ||
#_ament_cmake_gmock_find_gmock() | ||
#target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}") | ||
#target_link_libraries(${TARGET} ${GMOCK_LIBRARIES}) | ||
#set(executable "$<TARGET_FILE:${TARGET}>") | ||
#set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml") | ||
#ament_add_test( | ||
#${TARGET} | ||
#COMMAND ${executable} --ros-args --params-file ${YAML_FILE} | ||
#--gtest_output=xml:${result_file} | ||
#OUTPUT_FILE ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt | ||
#RESULT_FILE ${result_file} | ||
#) | ||
#endfunction() | ||
|
||
# test loading admittance controller | ||
add_rostest_with_parameters_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp | ||
${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) | ||
target_include_directories(test_load_admittance_controller PUBLIC ${GMOCK_INCLUDE_DIRS}) | ||
target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) | ||
ament_target_dependencies( | ||
test_load_admittance_controller | ||
controller_manager | ||
hardware_interface | ||
ros2_control_test_assets | ||
) | ||
# test admittance controller function | ||
add_rostest_with_parameters_gmock(test_admittance_controller test/test_admittance_controller.cpp | ||
${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) | ||
target_include_directories(test_admittance_controller PRIVATE include) | ||
target_link_libraries(test_admittance_controller admittance_controller) | ||
ament_target_dependencies( | ||
test_admittance_controller | ||
control_msgs | ||
controller_interface | ||
hardware_interface | ||
ros2_control_test_assets | ||
) | ||
endif() | ||
|
||
ament_export_include_directories( | ||
include | ||
) | ||
ament_export_dependencies( | ||
${THIS_PACKAGE_INCLUDE_DEPENDS} | ||
) | ||
ament_export_libraries( | ||
${PROJECT_NAME} | ||
) | ||
ament_package() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,9 @@ | ||
<library path="admittance_controller"> | ||
<class name="admittance_controller/AdmittanceController" | ||
type="admittance_controller::AdmittanceController" | ||
base_class_type="controller_interface::ChainableControllerInterface"> | ||
<description> | ||
AdmittanceController ros2_control controller. | ||
</description> | ||
</class> | ||
</library> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,58 @@ | ||
.. _joint_trajectory_controller_userdoc: | ||
|
||
Admittance Controller | ||
====================== | ||
|
||
Admittance controller enables you do zero-force control from a force measured on your TCP. | ||
The controller implements ``ChainedControllerInterface``, so it is possible to add another controllers in front of it, e.g., ``JointTrajectoryController``. | ||
|
||
The controller requires an external kinematics plugin to function. The `kinematics_interface <https://github.com/ros-controls/kinematics_interface>`_ repository provides an interface and an implementation that the admittance controller can use. | ||
|
||
Parameters: | ||
|
||
|
||
|
||
ROS2 interface of the controller | ||
--------------------------------- | ||
|
||
Parameters | ||
^^^^^^^^^^^ | ||
|
||
The admittance controller's uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters. | ||
The parameter `definition file located in the src folder <https://github.com/ros-controls/ros2_controllers/blob/master/admittance_controller/src/admittance_controller_parameters.yaml>`_ contains descriptions for all the parameters used by the controller. | ||
An example parameter file can be found in the `test folder of the controller <https://github.com/ros-controls/ros2_controllers/blob/master/admittance_controller/test/test_params.yaml>`_ | ||
|
||
|
||
Topics | ||
^^^^^^^ | ||
|
||
~/joint_references (input topic) [trajectory_msgs::msg::JointTrajectoryPoint] | ||
Target joint commands when controller is not in chained mode. | ||
|
||
~/state (output topic) [control_msgs::msg::AdmittanceControllerState] | ||
Topic publishing internal states. | ||
|
||
|
||
ros2_control interfaces | ||
------------------------ | ||
|
||
References | ||
^^^^^^^^^^^ | ||
The controller has ``position`` and ``velocity`` reference interfaces exported in the format: | ||
``<controller_name>/<joint_name>/[position|velocity]`` | ||
|
||
|
||
States | ||
^^^^^^^ | ||
The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``<joint>/<state_interface>``. | ||
Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp <https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp>`_. | ||
If some interface is not provided, the last commanded interface will be used for calculation. | ||
|
||
For handling TCP wrenches `*Force Torque Sensor* semantic component (from package *controller_interface*) <https://github.com/ros-controls/ros2_control/blob/master/controller_interface/include/semantic_components/force_torque_sensor.hpp>`_ is used. | ||
The interfaces have prefix ``ft_sensor.name``, building the interfaces: ``<sensor_name>/[force.x|force.y|force.z|torque.x|torque.y|torque.z]``. | ||
|
||
|
||
Commands | ||
^^^^^^^^^ | ||
The command interfaces are defined with ``joints`` and ``command_interfaces`` parameters as follows: ``<joint>/<command_interface>``. | ||
Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp <https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp>`_. |
187 changes: 187 additions & 0 deletions
187
admittance_controller/include/admittance_controller/admittance_controller.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,187 @@ | ||
// Copyright (c) 2022, PickNik, Inc. | ||
// | ||
// 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. | ||
// | ||
/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel | ||
|
||
#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ | ||
#define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ | ||
|
||
#include <chrono> | ||
#include <memory> | ||
#include <string> | ||
#include <vector> | ||
|
||
// include generated parameter library | ||
#include "admittance_controller_parameters.hpp" | ||
|
||
#include "admittance_controller/admittance_rule.hpp" | ||
#include "admittance_controller/visibility_control.h" | ||
#include "control_msgs/msg/admittance_controller_state.hpp" | ||
#include "controller_interface/chainable_controller_interface.hpp" | ||
#include "geometry_msgs/msg/pose_stamped.hpp" | ||
#include "geometry_msgs/msg/transform_stamped.hpp" | ||
#include "geometry_msgs/msg/wrench_stamped.hpp" | ||
#include "hardware_interface/types/hardware_interface_type_values.hpp" | ||
#include "pluginlib/class_loader.hpp" | ||
#include "rclcpp/duration.hpp" | ||
#include "rclcpp/time.hpp" | ||
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" | ||
#include "rclcpp_lifecycle/state.hpp" | ||
#include "realtime_tools/realtime_buffer.h" | ||
#include "realtime_tools/realtime_publisher.h" | ||
#include "semantic_components/force_torque_sensor.hpp" | ||
|
||
#include "trajectory_msgs/msg/joint_trajectory.hpp" | ||
|
||
namespace admittance_controller | ||
{ | ||
using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; | ||
|
||
class AdmittanceController : public controller_interface::ChainableControllerInterface | ||
{ | ||
public: | ||
ADMITTANCE_CONTROLLER_PUBLIC | ||
controller_interface::CallbackReturn on_init() override; | ||
|
||
/// Export configuration of required state interfaces. | ||
/** | ||
* Allowed types of state interfaces are \ref hardware_interface::POSITION, | ||
* \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. | ||
*/ | ||
ADMITTANCE_CONTROLLER_PUBLIC | ||
controller_interface::InterfaceConfiguration command_interface_configuration() const override; | ||
|
||
/// Export configuration of required state interfaces. | ||
/** | ||
* Allowed types of state interfaces are \ref hardware_interface::POSITION, | ||
* \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. | ||
*/ | ||
ADMITTANCE_CONTROLLER_PUBLIC | ||
controller_interface::InterfaceConfiguration state_interface_configuration() const override; | ||
|
||
ADMITTANCE_CONTROLLER_PUBLIC | ||
controller_interface::CallbackReturn on_configure( | ||
const rclcpp_lifecycle::State & previous_state) override; | ||
|
||
ADMITTANCE_CONTROLLER_PUBLIC | ||
controller_interface::CallbackReturn on_activate( | ||
const rclcpp_lifecycle::State & previous_state) override; | ||
|
||
ADMITTANCE_CONTROLLER_PUBLIC | ||
controller_interface::CallbackReturn on_deactivate( | ||
const rclcpp_lifecycle::State & previous_state) override; | ||
|
||
ADMITTANCE_CONTROLLER_PUBLIC | ||
controller_interface::CallbackReturn on_cleanup( | ||
const rclcpp_lifecycle::State & previous_state) override; | ||
|
||
ADMITTANCE_CONTROLLER_PUBLIC | ||
controller_interface::CallbackReturn on_error( | ||
const rclcpp_lifecycle::State & previous_state) override; | ||
|
||
ADMITTANCE_CONTROLLER_PUBLIC | ||
controller_interface::return_type update_and_write_commands( | ||
const rclcpp::Time & time, const rclcpp::Duration & period) override; | ||
|
||
protected: | ||
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override; | ||
|
||
controller_interface::return_type update_reference_from_subscribers() override; | ||
|
||
size_t num_joints_ = 0; | ||
std::vector<std::string> command_joint_names_; | ||
|
||
// The interfaces are defined as the types in 'allowed_interface_types_' member. | ||
// For convenience, for each type the interfaces are ordered so that i-th position | ||
// matches i-th index in joint_names_ | ||
template <typename T> | ||
using InterfaceReferences = std::vector<std::vector<std::reference_wrapper<T>>>; | ||
|
||
InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_; | ||
InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_; | ||
|
||
bool has_position_state_interface_ = false; | ||
bool has_velocity_state_interface_ = false; | ||
bool has_acceleration_state_interface_ = false; | ||
bool has_position_command_interface_ = false; | ||
bool has_velocity_command_interface_ = false; | ||
bool has_acceleration_command_interface_ = false; | ||
bool has_effort_command_interface_ = false; | ||
|
||
// To reduce number of variables and to make the code shorter the interfaces are ordered in types | ||
// as the following constants | ||
const std::vector<std::string> allowed_interface_types_ = { | ||
hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, | ||
hardware_interface::HW_IF_ACCELERATION}; | ||
|
||
// internal reference values | ||
const std::vector<std::string> allowed_reference_interfaces_types_ = { | ||
hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY}; | ||
std::vector<std::reference_wrapper<double>> position_reference_; | ||
std::vector<std::reference_wrapper<double>> velocity_reference_; | ||
|
||
// Admittance rule and dependent variables; | ||
std::unique_ptr<admittance_controller::AdmittanceRule> admittance_; | ||
|
||
// force torque sensor | ||
std::unique_ptr<semantic_components::ForceTorqueSensor> force_torque_sensor_; | ||
|
||
// ROS subscribers | ||
rclcpp::Subscription<trajectory_msgs::msg::JointTrajectoryPoint>::SharedPtr | ||
input_joint_command_subscriber_; | ||
rclcpp::Publisher<control_msgs::msg::AdmittanceControllerState>::SharedPtr s_publisher_; | ||
|
||
// admittance parameters | ||
std::shared_ptr<admittance_controller::ParamListener> parameter_handler_; | ||
|
||
// ROS messages | ||
std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> joint_command_msg_; | ||
|
||
// real-time buffer | ||
realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint>> | ||
input_joint_command_; | ||
std::unique_ptr<realtime_tools::RealtimePublisher<ControllerStateMsg>> state_publisher_; | ||
|
||
trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; | ||
trajectory_msgs::msg::JointTrajectoryPoint last_reference_; | ||
|
||
// control loop data | ||
// reference_: reference value read by the controller | ||
// joint_state_: current joint readings from the hardware | ||
// reference_admittance_: reference value used by the controller after the admittance values are | ||
// applied ft_values_: values read from the force torque sensor | ||
trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_; | ||
geometry_msgs::msg::Wrench ft_values_; | ||
|
||
/** | ||
* @brief Read values from hardware interfaces and set corresponding fields of state_current and ft_values | ||
*/ | ||
void read_state_from_hardware( | ||
trajectory_msgs::msg::JointTrajectoryPoint & state_current, | ||
geometry_msgs::msg::Wrench & ft_values); | ||
|
||
/** | ||
* @brief Set fields of state_reference with values from controllers exported position and velocity references | ||
*/ | ||
void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); | ||
|
||
/** | ||
* @brief Write values from state_command to claimed hardware interfaces | ||
*/ | ||
void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_command); | ||
}; | ||
|
||
} // namespace admittance_controller | ||
|
||
#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ |
Oops, something went wrong.