ros2_control - rolling
admittance_controller.hpp
1 // Copyright (c) 2022, 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 //
16 
17 #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_
18 #define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_
19 
20 #include <memory>
21 #include <string>
22 #include <vector>
23 
24 // auto-generated by generate_parameter_library
25 #include "admittance_controller_parameters.hpp"
26 
27 #include "admittance_controller/admittance_rule.hpp"
28 #include "admittance_controller/visibility_control.h"
29 #include "control_msgs/msg/admittance_controller_state.hpp"
30 #include "controller_interface/chainable_controller_interface.hpp"
31 #include "hardware_interface/types/hardware_interface_type_values.hpp"
32 #include "rclcpp/duration.hpp"
33 #include "rclcpp/time.hpp"
34 #include "rclcpp_lifecycle/state.hpp"
35 #include "realtime_tools/realtime_buffer.hpp"
36 #include "realtime_tools/realtime_publisher.hpp"
37 #include "semantic_components/force_torque_sensor.hpp"
38 
40 {
41 using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState;
42 
44 {
45 public:
46  ADMITTANCE_CONTROLLER_PUBLIC
47  controller_interface::CallbackReturn on_init() override;
48 
50 
54  ADMITTANCE_CONTROLLER_PUBLIC
56 
58 
62  ADMITTANCE_CONTROLLER_PUBLIC
64 
65  ADMITTANCE_CONTROLLER_PUBLIC
66  controller_interface::CallbackReturn on_configure(
67  const rclcpp_lifecycle::State & previous_state) override;
68 
69  ADMITTANCE_CONTROLLER_PUBLIC
70  controller_interface::CallbackReturn on_activate(
71  const rclcpp_lifecycle::State & previous_state) override;
72 
73  ADMITTANCE_CONTROLLER_PUBLIC
74  controller_interface::CallbackReturn on_deactivate(
75  const rclcpp_lifecycle::State & previous_state) override;
76 
77  ADMITTANCE_CONTROLLER_PUBLIC
78  controller_interface::CallbackReturn on_cleanup(
79  const rclcpp_lifecycle::State & previous_state) override;
80 
81  ADMITTANCE_CONTROLLER_PUBLIC
82  controller_interface::CallbackReturn on_error(
83  const rclcpp_lifecycle::State & previous_state) override;
84 
85  ADMITTANCE_CONTROLLER_PUBLIC
86  controller_interface::return_type update_and_write_commands(
87  const rclcpp::Time & time, const rclcpp::Duration & period) override;
88 
89 protected:
90  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
91 
92  controller_interface::return_type update_reference_from_subscribers(
93  const rclcpp::Time & time, const rclcpp::Duration & period) override;
94 
95  size_t num_joints_ = 0;
96  std::vector<std::string> command_joint_names_;
97 
98  // The interfaces are defined as the types in 'allowed_interface_types_' member.
99  // For convenience, for each type the interfaces are ordered so that i-th position
100  // matches i-th index in joint_names_
101  template <typename T>
102  using InterfaceReferences = std::vector<std::vector<std::reference_wrapper<T>>>;
103 
104  InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
105  InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;
106 
107  bool has_position_state_interface_ = false;
108  bool has_velocity_state_interface_ = false;
109  bool has_acceleration_state_interface_ = false;
110  bool has_position_command_interface_ = false;
111  bool has_velocity_command_interface_ = false;
112  bool has_acceleration_command_interface_ = false;
113  bool has_effort_command_interface_ = false;
114 
115  // To reduce number of variables and to make the code shorter the interfaces are ordered in types
116  // as the following constants
117  const std::vector<std::string> allowed_interface_types_ = {
120 
121  // internal reference values
122  const std::vector<std::string> allowed_reference_interfaces_types_ = {
124  std::vector<std::reference_wrapper<double>> position_reference_;
125  std::vector<std::reference_wrapper<double>> velocity_reference_;
126 
127  // Admittance rule and dependent variables;
128  std::unique_ptr<admittance_controller::AdmittanceRule> admittance_;
129 
130  // force torque sensor
131  std::unique_ptr<semantic_components::ForceTorqueSensor> force_torque_sensor_;
132 
133  // ROS subscribers
134  rclcpp::Subscription<trajectory_msgs::msg::JointTrajectoryPoint>::SharedPtr
135  input_joint_command_subscriber_;
136  rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr
137  input_wrench_command_subscriber_;
138  rclcpp::Publisher<control_msgs::msg::AdmittanceControllerState>::SharedPtr s_publisher_;
139 
140  // admittance parameters
141  std::shared_ptr<admittance_controller::ParamListener> parameter_handler_;
142 
143  // ROS messages
144  std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> joint_command_msg_;
145 
146  // real-time buffer
148  input_joint_command_;
150  std::unique_ptr<realtime_tools::RealtimePublisher<ControllerStateMsg>> state_publisher_;
151 
152  trajectory_msgs::msg::JointTrajectoryPoint last_commanded_;
153  trajectory_msgs::msg::JointTrajectoryPoint last_reference_;
154 
155  // control loop data
156  // reference_: reference value read by the controller
157  // joint_state_: current joint readings from the hardware
158  // reference_admittance_: reference value used by the controller after the admittance values are
159  // applied ft_values_: values read from the force torque sensor
160  trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_;
161  geometry_msgs::msg::Wrench ft_values_;
162 
168  trajectory_msgs::msg::JointTrajectoryPoint & state_current,
169  geometry_msgs::msg::Wrench & ft_values);
170 
175  void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state);
176 
180  void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_command);
181 };
182 
183 } // namespace admittance_controller
184 
185 #endif // ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_
Definition: admittance_controller.hpp:44
controller_interface::return_type update_reference_from_subscribers(const rclcpp::Time &time, const rclcpp::Duration &period) override
Update reference from input topics when not in chained mode.
Definition: admittance_controller.cpp:395
std::vector< hardware_interface::CommandInterface > on_export_reference_interfaces() override
Definition: admittance_controller.cpp:118
ADMITTANCE_CONTROLLER_PUBLIC controller_interface::return_type update_and_write_commands(const rclcpp::Time &time, const rclcpp::Duration &period) override
Execute calculations of the controller and update command interfaces.
Definition: admittance_controller.cpp:422
ADMITTANCE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override
Export configuration of required state interfaces.
Definition: admittance_controller.cpp:77
void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint &state_current, geometry_msgs::msg::Wrench &ft_values)
Read values from hardware interfaces and set corresponding fields of state_current and ft_values.
Definition: admittance_controller.cpp:499
void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint &state_command)
Write values from state_command to claimed hardware interfaces.
Definition: admittance_controller.cpp:557
ADMITTANCE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition: admittance_controller.cpp:47
void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state)
Set fields of state_reference with values from controllers exported position and velocity references.
Definition: admittance_controller.cpp:585
ADMITTANCE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override
Export configuration of required state interfaces.
Definition: admittance_controller.cpp:95
Definition: chainable_controller_interface.hpp:38
Definition: realtime_buffer.hpp:44
Definition: admittance_controller.hpp:40
constexpr char HW_IF_ACCELERATION[]
Constant defining acceleration interface name.
Definition: hardware_interface_type_values.hpp:25
constexpr char HW_IF_VELOCITY[]
Constant defining velocity interface name.
Definition: hardware_interface_type_values.hpp:23
constexpr char HW_IF_POSITION[]
Constant defining position interface name.
Definition: hardware_interface_type_values.hpp:21
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58