-
Notifications
You must be signed in to change notification settings - Fork 58
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #170 from lbr-stack/dev-humble-cart-vel-demo
Additional demos
- Loading branch information
Showing
27 changed files
with
834 additions
and
260 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
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
8 changes: 8 additions & 0 deletions
8
lbr_demos/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control.yaml
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,8 @@ | ||
| admittance_control: | ||
| ros__parameters: | ||
| base_link: "link_0" | ||
| end_effector_link: "link_ee" | ||
| f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] | ||
| dq_gains: [20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0] | ||
| dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] | ||
| exp_smooth: 0.95 |
7 changes: 0 additions & 7 deletions
7
lbr_demos/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control_node.yaml
This file was deleted.
Oops, something went wrong.
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
47 changes: 0 additions & 47 deletions
47
lbr_demos/lbr_demos_fri_ros2_advanced_cpp/launch/admittance_control_node.launch.py
This file was deleted.
Oops, something went wrong.
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
64 changes: 29 additions & 35 deletions
64
lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp
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 |
|---|---|---|
| @@ -1,78 +1,72 @@ | ||
| #include <algorithm> | ||
| #include <vector> | ||
|
|
||
| #include "rclcpp/rclcpp.hpp" | ||
|
|
||
| #include "lbr_fri_msgs/msg/lbr_position_command.hpp" | ||
| #include "lbr_fri_msgs/msg/lbr_state.hpp" | ||
| #include "lbr_fri_ros2/app.hpp" | ||
|
|
||
| #include "admittance_controller.hpp" | ||
| #include "lbr_base_position_command_node.hpp" | ||
|
|
||
| namespace lbr_fri_ros2 { | ||
| class AdmittanceControlNode : public rclcpp::Node { | ||
| namespace lbr_demos { | ||
| class AdmittanceControlNode : public LBRBasePositionCommandNode { | ||
| public: | ||
| AdmittanceControlNode(const rclcpp::NodeOptions &options) | ||
| : rclcpp::Node("admittance_control_node", options) { | ||
| this->declare_parameter<std::string>("robot_description"); | ||
| : LBRBasePositionCommandNode("admittance_control", options) { | ||
| this->declare_parameter<std::string>("base_link", "link_0"); | ||
| this->declare_parameter<std::string>("end_effector_link", "link_ee"); | ||
| this->declare_parameter<std::vector<double>>("f_ext_th", {2., 2., 2., 0.5, 0.5, 0.5}); | ||
| this->declare_parameter<std::vector<double>>("dq_gains", {0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8}); | ||
| this->declare_parameter<std::vector<double>>("dx_gains", {4.0, 4.0, 4.0, 40., 40., 40.}); | ||
| this->declare_parameter<std::vector<double>>("dq_gains", {20., 20., 20., 20., 20., 20., 20.}); | ||
| this->declare_parameter<std::vector<double>>("dx_gains", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); | ||
| this->declare_parameter<double>("exp_smooth", 0.95); | ||
|
|
||
| admittance_controller_ = | ||
| std::make_unique<AdmittanceController>(this->get_parameter("robot_description").as_string(), | ||
| this->get_parameter("base_link").as_string(), | ||
| this->get_parameter("end_effector_link").as_string(), | ||
| this->get_parameter("f_ext_th").as_double_array(), | ||
| this->get_parameter("dq_gains").as_double_array(), | ||
| this->get_parameter("dx_gains").as_double_array()); | ||
| exp_smooth_ = this->get_parameter("exp_smooth").as_double(); | ||
| if (exp_smooth_ < 0. || exp_smooth_ > 1.) { | ||
| throw std::runtime_error("Invalid exponential smoothing factor."); | ||
| } | ||
|
|
||
| lbr_position_command_pub_ = | ||
| create_publisher<lbr_fri_msgs::msg::LBRPositionCommand>("/lbr/command/joint_position", 1); | ||
| lbr_state_sub_ = create_subscription<lbr_fri_msgs::msg::LBRState>( | ||
| "/lbr/state", 1, | ||
| std::bind(&AdmittanceControlNode::on_lbr_state, this, std::placeholders::_1)); | ||
| admittance_controller_ = std::make_unique<AdmittanceController>( | ||
| this->robot_description_, this->get_parameter("base_link").as_string(), | ||
| this->get_parameter("end_effector_link").as_string(), | ||
| this->get_parameter("f_ext_th").as_double_array(), | ||
| this->get_parameter("dq_gains").as_double_array(), | ||
| this->get_parameter("dx_gains").as_double_array()); | ||
| } | ||
|
|
||
| protected: | ||
| void on_lbr_state(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) { | ||
| void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) override { | ||
| if (!lbr_state) { | ||
| return; | ||
| } | ||
|
|
||
| smooth_lbr_state_(lbr_state, 0.95); | ||
| smooth_lbr_state_(lbr_state); | ||
|
|
||
| auto lbr_command = admittance_controller_->update(lbr_state_); | ||
| auto lbr_command = admittance_controller_->update(lbr_state_, dt_); | ||
| lbr_position_command_pub_->publish(lbr_command); | ||
| }; | ||
|
|
||
| void smooth_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state, double alpha) { | ||
| void smooth_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) { | ||
| if (!init_) { | ||
| lbr_state_ = *lbr_state; | ||
| init_ = true; | ||
| return; | ||
| } | ||
|
|
||
| for (int i = 0; i < 7; i++) { | ||
| lbr_state_.measured_joint_position[i] = lbr_state->measured_joint_position[i] * (1 - alpha) + | ||
| lbr_state_.measured_joint_position[i] * alpha; | ||
| lbr_state_.external_torque[i] = | ||
| lbr_state->external_torque[i] * (1 - alpha) + lbr_state_.external_torque[i] * alpha; | ||
| lbr_state_.measured_joint_position[i] = | ||
| lbr_state->measured_joint_position[i] * (1 - exp_smooth_) + | ||
| lbr_state_.measured_joint_position[i] * exp_smooth_; | ||
| lbr_state_.external_torque[i] = lbr_state->external_torque[i] * (1 - exp_smooth_) + | ||
| lbr_state_.external_torque[i] * exp_smooth_; | ||
| } | ||
| } | ||
|
|
||
| double exp_smooth_; | ||
| bool init_{false}; | ||
| lbr_fri_msgs::msg::LBRState lbr_state_; | ||
|
|
||
| rclcpp::Publisher<lbr_fri_msgs::msg::LBRPositionCommand>::SharedPtr lbr_position_command_pub_; | ||
| rclcpp::Subscription<lbr_fri_msgs::msg::LBRState>::SharedPtr lbr_state_sub_; | ||
|
|
||
| std::unique_ptr<AdmittanceController> admittance_controller_; | ||
| }; | ||
| } // end of namespace lbr_fri_ros2 | ||
| } // end of namespace lbr_demos | ||
|
|
||
| #include "rclcpp_components/register_node_macro.hpp" | ||
|
|
||
| RCLCPP_COMPONENTS_REGISTER_NODE(lbr_fri_ros2::AdmittanceControlNode) | ||
| RCLCPP_COMPONENTS_REGISTER_NODE(lbr_demos::AdmittanceControlNode) |
Oops, something went wrong.