|
1 | | -#include <algorithm> |
2 | 1 | #include <vector> |
3 | 2 |
|
4 | 3 | #include "rclcpp/rclcpp.hpp" |
5 | 4 |
|
6 | | -#include "lbr_fri_msgs/msg/lbr_position_command.hpp" |
7 | 5 | #include "lbr_fri_msgs/msg/lbr_state.hpp" |
8 | | -#include "lbr_fri_ros2/app.hpp" |
9 | 6 |
|
10 | 7 | #include "admittance_controller.hpp" |
| 8 | +#include "lbr_base_position_command_node.hpp" |
11 | 9 |
|
12 | | -namespace lbr_fri_ros2 { |
13 | | -class AdmittanceControlNode : public rclcpp::Node { |
| 10 | +namespace lbr_demos { |
| 11 | +class AdmittanceControlNode : public LBRBasePositionCommandNode { |
14 | 12 | public: |
15 | 13 | AdmittanceControlNode(const rclcpp::NodeOptions &options) |
16 | | - : rclcpp::Node("admittance_control_node", options) { |
17 | | - this->declare_parameter<std::string>("robot_description"); |
| 14 | + : LBRBasePositionCommandNode("admittance_control", options) { |
18 | 15 | this->declare_parameter<std::string>("base_link", "link_0"); |
19 | 16 | this->declare_parameter<std::string>("end_effector_link", "link_ee"); |
20 | 17 | this->declare_parameter<std::vector<double>>("f_ext_th", {2., 2., 2., 0.5, 0.5, 0.5}); |
21 | | - this->declare_parameter<std::vector<double>>("dq_gains", {0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8}); |
22 | | - this->declare_parameter<std::vector<double>>("dx_gains", {4.0, 4.0, 4.0, 40., 40., 40.}); |
| 18 | + this->declare_parameter<std::vector<double>>("dq_gains", {20., 20., 20., 20., 20., 20., 20.}); |
| 19 | + this->declare_parameter<std::vector<double>>("dx_gains", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); |
| 20 | + this->declare_parameter<double>("exp_smooth", 0.95); |
23 | 21 |
|
24 | | - admittance_controller_ = |
25 | | - std::make_unique<AdmittanceController>(this->get_parameter("robot_description").as_string(), |
26 | | - this->get_parameter("base_link").as_string(), |
27 | | - this->get_parameter("end_effector_link").as_string(), |
28 | | - this->get_parameter("f_ext_th").as_double_array(), |
29 | | - this->get_parameter("dq_gains").as_double_array(), |
30 | | - this->get_parameter("dx_gains").as_double_array()); |
| 22 | + exp_smooth_ = this->get_parameter("exp_smooth").as_double(); |
| 23 | + if (exp_smooth_ < 0. || exp_smooth_ > 1.) { |
| 24 | + throw std::runtime_error("Invalid exponential smoothing factor."); |
| 25 | + } |
31 | 26 |
|
32 | | - lbr_position_command_pub_ = |
33 | | - create_publisher<lbr_fri_msgs::msg::LBRPositionCommand>("/lbr/command/joint_position", 1); |
34 | | - lbr_state_sub_ = create_subscription<lbr_fri_msgs::msg::LBRState>( |
35 | | - "/lbr/state", 1, |
36 | | - std::bind(&AdmittanceControlNode::on_lbr_state, this, std::placeholders::_1)); |
| 27 | + admittance_controller_ = std::make_unique<AdmittanceController>( |
| 28 | + this->robot_description_, this->get_parameter("base_link").as_string(), |
| 29 | + this->get_parameter("end_effector_link").as_string(), |
| 30 | + this->get_parameter("f_ext_th").as_double_array(), |
| 31 | + this->get_parameter("dq_gains").as_double_array(), |
| 32 | + this->get_parameter("dx_gains").as_double_array()); |
37 | 33 | } |
38 | 34 |
|
39 | 35 | protected: |
40 | | - void on_lbr_state(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) { |
| 36 | + void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) override { |
41 | 37 | if (!lbr_state) { |
42 | 38 | return; |
43 | 39 | } |
44 | 40 |
|
45 | | - smooth_lbr_state_(lbr_state, 0.95); |
| 41 | + smooth_lbr_state_(lbr_state); |
46 | 42 |
|
47 | | - auto lbr_command = admittance_controller_->update(lbr_state_); |
| 43 | + auto lbr_command = admittance_controller_->update(lbr_state_, dt_); |
48 | 44 | lbr_position_command_pub_->publish(lbr_command); |
49 | 45 | }; |
50 | 46 |
|
51 | | - void smooth_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state, double alpha) { |
| 47 | + void smooth_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) { |
52 | 48 | if (!init_) { |
53 | 49 | lbr_state_ = *lbr_state; |
54 | 50 | init_ = true; |
55 | 51 | return; |
56 | 52 | } |
57 | 53 |
|
58 | 54 | for (int i = 0; i < 7; i++) { |
59 | | - lbr_state_.measured_joint_position[i] = lbr_state->measured_joint_position[i] * (1 - alpha) + |
60 | | - lbr_state_.measured_joint_position[i] * alpha; |
61 | | - lbr_state_.external_torque[i] = |
62 | | - lbr_state->external_torque[i] * (1 - alpha) + lbr_state_.external_torque[i] * alpha; |
| 55 | + lbr_state_.measured_joint_position[i] = |
| 56 | + lbr_state->measured_joint_position[i] * (1 - exp_smooth_) + |
| 57 | + lbr_state_.measured_joint_position[i] * exp_smooth_; |
| 58 | + lbr_state_.external_torque[i] = lbr_state->external_torque[i] * (1 - exp_smooth_) + |
| 59 | + lbr_state_.external_torque[i] * exp_smooth_; |
63 | 60 | } |
64 | 61 | } |
65 | 62 |
|
| 63 | + double exp_smooth_; |
66 | 64 | bool init_{false}; |
67 | 65 | lbr_fri_msgs::msg::LBRState lbr_state_; |
68 | 66 |
|
69 | | - rclcpp::Publisher<lbr_fri_msgs::msg::LBRPositionCommand>::SharedPtr lbr_position_command_pub_; |
70 | | - rclcpp::Subscription<lbr_fri_msgs::msg::LBRState>::SharedPtr lbr_state_sub_; |
71 | | - |
72 | 67 | std::unique_ptr<AdmittanceController> admittance_controller_; |
73 | 68 | }; |
74 | | -} // end of namespace lbr_fri_ros2 |
| 69 | +} // end of namespace lbr_demos |
75 | 70 |
|
76 | 71 | #include "rclcpp_components/register_node_macro.hpp" |
77 | | - |
78 | | -RCLCPP_COMPONENTS_REGISTER_NODE(lbr_fri_ros2::AdmittanceControlNode) |
| 72 | +RCLCPP_COMPONENTS_REGISTER_NODE(lbr_demos::AdmittanceControlNode) |
0 commit comments