diff --git a/README.md b/README.md index d07abd97..06e2a17e 100644 --- a/README.md +++ b/README.md @@ -80,10 +80,14 @@ If you enjoyed using this repository for your work, we would really appreciate ``` ## Acknowledgements -wellcome +We would like to acknowledge all open source contributors 🚀! -This work was supported by core and project funding from the Wellcome/EPSRC [WT203148/Z/16/Z; NS/A000049/1; WT101957; NS/A000027/1]. +We would further like to acknowledge following supporters: -eu_flag - -This project has received funding from the European Union's Horizon 2020 research and innovation programme under grant agreement No 101016985 (FAROS project). +| Logo | Notes | +|:--:|:---| +| wellcome | This work was supported by core and project funding from the Wellcome/EPSRC [WT203148/Z/16/Z; NS/A000049/1; WT101957; NS/A000027/1]. | +| eu_flag | This project has received funding from the European Union's Horizon 2020 research and innovation programme under grant agreement No 101016985 (FAROS project). | +| RViMLab | Built at [RViMLab](https://rvim.online/). | +| King's College London | Built at [CAI4CAI](https://cai4cai.ml/). | +| King's College London | Built at [King's College London](https://www.kcl.ac.uk/). | diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt index c92089b4..37bda587 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt @@ -13,6 +13,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(fri_vendor REQUIRED) find_package(FRIClient REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(kdl_parser REQUIRED) find_package(lbr_fri_ros2 REQUIRED) find_package(lbr_fri_msgs REQUIRED) @@ -30,8 +31,7 @@ target_include_directories(admittance_control_component PRIVATE src ) -ament_target_dependencies( - admittance_control_component +ament_target_dependencies(admittance_control_component kdl_parser lbr_fri_ros2 lbr_fri_msgs @@ -45,19 +45,70 @@ target_link_libraries(admittance_control_component ) rclcpp_components_register_node(admittance_control_component - PLUGIN lbr_fri_ros2::AdmittanceControlNode - EXECUTABLE admittance_control_node + PLUGIN lbr_demos::AdmittanceControlNode + EXECUTABLE admittance_control ) +# pose planning node +add_library(pose_planning_component + SHARED + src/pose_planning_node.cpp +) + +target_include_directories(pose_planning_component + PRIVATE src +) + +ament_target_dependencies(pose_planning_component + geometry_msgs + rclcpp + rclcpp_components +) + +rclcpp_components_register_node(pose_planning_component + PLUGIN lbr_demos::PosePlanningNode + EXECUTABLE pose_planning +) + +# pose contol node +add_library(pose_control_component + SHARED + src/pose_control_node.cpp +) + +target_include_directories(pose_control_component + PRIVATE src +) + +ament_target_dependencies(pose_control_component + fri_vendor + geometry_msgs + kdl_parser + lbr_fri_msgs + orocos_kdl_vendor + rclcpp + rclcpp_components +) + +target_link_libraries(pose_control_component + FRIClient::FRIClient +) + +rclcpp_components_register_node(pose_control_component + PLUGIN lbr_demos::PoseControlNode + EXECUTABLE pose_control +) + +# install install( - TARGETS admittance_control_component + TARGETS admittance_control_component pose_planning_component pose_control_component LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/lbr_demos_fri_ros2_advanced_cpp + RUNTIME DESTINATION lib/${PROJECT_NAME} ) install( - DIRECTORY config launch - DESTINATION share/lbr_demos_fri_ros2_advanced_cpp + DIRECTORY config + DESTINATION share/${PROJECT_NAME} ) ament_package() diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control.yaml b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control.yaml new file mode 100644 index 00000000..4291d202 --- /dev/null +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control.yaml @@ -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 diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control_node.yaml b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control_node.yaml deleted file mode 100644 index 3c4c86aa..00000000 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control_node.yaml +++ /dev/null @@ -1,7 +0,0 @@ -admittance_control_node: - 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: [0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8] - dx_gains: [4.0, 4.0, 4.0, 40., 40., 40.] diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst index d443528f..51886e5f 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst @@ -14,10 +14,53 @@ Admittance Controller .. thumbnail:: ../../doc/img/applications_lbr_server.png -#. Launch the `admittance_control_node `_: +#. Launch the robot driver: .. code-block:: bash - ros2 launch lbr_demos_fri_ros2_advanced_cpp admittance_control_node.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_bringup bringup.launch.py \ + sim:=false \ + ctrl:=forward_lbr_position_command_controller \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Launch the `admittance_control `_: + +.. code-block:: bash + + ros2 run lbr_demos_fri_ros2_advanced_cpp admittance_control --ros-args \ + -r __ns:=/lbr \ + --params-file `ros2 pkg prefix lbr_demos_fri_ros2_advanced_cpp`/share/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control.yaml #. Now gently move the robot at the end-effector. + +Pose Controller +--------------- +This demo uses ``KDL`` to calculate forward kinematics and inverse +kinematics to move the robot's end-effector along the z-axis in Cartesian space. + +#. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` + + .. thumbnail:: ../../doc/img/applications_lbr_server.png + +#. Launch the robot driver: + +.. code-block:: bash + + ros2 launch lbr_bringup bringup.launch.py \ + sim:=false \ + ctrl:=forward_lbr_position_command_controller \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Launch the pose control + +.. code-block:: bash + + ros2 run lbr_demos_fri_ros2_advanced_cpp pose_control --ros-args \ + -r __ns:=/lbr + +#. Launch the path planning + +.. code-block:: bash + + ros2 run lbr_demos_fri_ros2_advanced_cpp pose_planning --ros-args \ + -r __ns:=/lbr diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/launch/admittance_control_node.launch.py b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/launch/admittance_control_node.launch.py deleted file mode 100644 index 39b0dad6..00000000 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/launch/admittance_control_node.launch.py +++ /dev/null @@ -1,47 +0,0 @@ -import os - -from ament_index_python import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - -from lbr_description import LBRDescriptionMixin -from lbr_fri_ros2 import LBRFRIROS2Mixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRFRIROS2Mixin.arg_port_id()) - ld.add_action(LBRDescriptionMixin.arg_model()) - robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - config = os.path.join( - get_package_share_directory("lbr_demos_fri_ros2_advanced_cpp"), - "config", - "admittance_control_node.yaml", - ) - ld.add_action( - ComposableNodeContainer( - name="admittance_container", - namespace="", - package="rclcpp_components", - executable="component_container", - composable_node_descriptions=[ - ComposableNode( - package="lbr_fri_ros2", - plugin="lbr_fri_ros2::AppComponent", - name="app", - namespace="lbr", - parameters=[robot_description, LBRFRIROS2Mixin.param_port_id()], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ComposableNode( - package="lbr_demos_fri_ros2_advanced_cpp", - plugin="lbr_fri_ros2::AdmittanceControlNode", - name="admittance_control_node", - parameters=[robot_description, config], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ], - ) - ) - return ld diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml index 4698f37f..9b0d828e 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml @@ -12,6 +12,7 @@ lbr_description fri_vendor + geometry_msgs kdl_parser lbr_fri_ros2 lbr_fri_msgs diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp index 9dd60a7f..7463adfb 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp @@ -1,54 +1,50 @@ -#include #include #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("robot_description"); + : LBRBasePositionCommandNode("admittance_control", options) { this->declare_parameter("base_link", "link_0"); this->declare_parameter("end_effector_link", "link_ee"); this->declare_parameter>("f_ext_th", {2., 2., 2., 0.5, 0.5, 0.5}); - this->declare_parameter>("dq_gains", {0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8}); - this->declare_parameter>("dx_gains", {4.0, 4.0, 4.0, 40., 40., 40.}); + this->declare_parameter>("dq_gains", {20., 20., 20., 20., 20., 20., 20.}); + this->declare_parameter>("dx_gains", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); + this->declare_parameter("exp_smooth", 0.95); - admittance_controller_ = - std::make_unique(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/command/joint_position", 1); - lbr_state_sub_ = create_subscription( - "/lbr/state", 1, - std::bind(&AdmittanceControlNode::on_lbr_state, this, std::placeholders::_1)); + admittance_controller_ = std::make_unique( + 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; @@ -56,23 +52,21 @@ class AdmittanceControlNode : public rclcpp::Node { } 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::SharedPtr lbr_position_command_pub_; - rclcpp::Subscription::SharedPtr lbr_state_sub_; - std::unique_ptr 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) diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp index 7d8a8c0f..cbe056dc 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp @@ -16,7 +16,7 @@ #include "lbr_fri_msgs/msg/lbr_state.hpp" #include "lbr_fri_ros2/pinv.hpp" -namespace lbr_fri_ros2 { +namespace lbr_demos { class AdmittanceController { using joint_vector_t = Eigen::Vector; using cartesian_vector_t = Eigen::Vector; @@ -26,8 +26,8 @@ class AdmittanceController { const std::string &base_link = "link_0", const std::string &end_effector_link = "link_ee", const std::vector &f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}, - const std::vector &dq_gains = {0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8}, - const std::vector &dx_gains = {4.0, 4.0, 4.0, 40., 40., 40.}) + const std::vector &dq_gains = {20., 20., 20., 20., 20., 20., 20.}, + const std::vector &dx_gains = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}) : dq_gains_(dq_gains.data()), dx_gains_(dx_gains.data()), f_ext_th_(f_ext_th.data()) { if (!kdl_parser::treeFromString(robot_description, tree_)) { throw std::runtime_error("Failed to construct kdl tree from robot description."); @@ -41,8 +41,8 @@ class AdmittanceController { q_.resize(chain_.getNrOfJoints()); }; - const lbr_fri_msgs::msg::LBRPositionCommand & - update(const lbr_fri_msgs::msg::LBRState &lbr_state) { + const lbr_fri_msgs::msg::LBRPositionCommand &update(const lbr_fri_msgs::msg::LBRState &lbr_state, + const double &dt) { std::memcpy(q_.data.data(), lbr_state.measured_joint_position.data(), sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); std::memcpy(tau_ext_.data(), lbr_state.external_torque.data(), @@ -50,7 +50,7 @@ class AdmittanceController { jacobian_solver_->JntToJac(q_, jacobian_); - jacobian_inv_ = pinv(jacobian_.data); + jacobian_inv_ = lbr_fri_ros2::pinv(jacobian_.data); f_ext_ = jacobian_inv_.transpose() * tau_ext_; for (int i = 0; i < 6; i++) { @@ -64,8 +64,7 @@ class AdmittanceController { dq_ = dq_gains_.asDiagonal() * jacobian_inv_ * f_ext_; for (int i = 0; i < 7; i++) { - lbr_position_command_.joint_position[i] = - lbr_state.measured_joint_position[i] + dq_[i] * lbr_state.sample_time; + lbr_position_command_.joint_position[i] = lbr_state.measured_joint_position[i] + dq_[i] * dt; } return lbr_position_command_; @@ -86,5 +85,5 @@ class AdmittanceController { cartesian_vector_t f_ext_; cartesian_vector_t f_ext_th_; }; -} // end of namespace lbr_fri_ros2 +} // end of namespace lbr_demos #endif // LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_ diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/lbr_base_position_command_node.hpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/lbr_base_position_command_node.hpp new file mode 100644 index 00000000..82dd552f --- /dev/null +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/lbr_base_position_command_node.hpp @@ -0,0 +1,67 @@ +#ifndef LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__LBR_BASE_POSITION_COMMAND_NODE_HPP_ +#define LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__LBR_BASE_POSITION_COMMAND_NODE_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "lbr_fri_msgs/msg/lbr_position_command.hpp" +#include "lbr_fri_msgs/msg/lbr_state.hpp" + +namespace lbr_demos { +class LBRBasePositionCommandNode : public rclcpp::Node { +public: + LBRBasePositionCommandNode(const std::string &node_name, const rclcpp::NodeOptions &options) + : Node(node_name, options) { + // retrieve parameters + robot_description_ = + this->retrieve_parameter_("controller_manager", "robot_description").as_string(); + update_rate_ = this->retrieve_parameter_("controller_manager", "update_rate").as_int(); + dt_ = 1.0 / static_cast(update_rate_); + + // publishers and subscribers + lbr_position_command_pub_ = + create_publisher("command/joint_position", 1); + + lbr_state_sub_ = create_subscription( + "state", 1, + std::bind(&LBRBasePositionCommandNode::on_lbr_state_, this, std::placeholders::_1)); + } + +protected: + rclcpp::Publisher::SharedPtr lbr_position_command_pub_; + rclcpp::Subscription::SharedPtr lbr_state_sub_; + + std::string robot_description_; + int64_t update_rate_; + double dt_; + +protected: + virtual void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) = 0; + + rclcpp::Parameter retrieve_parameter_(const std::string &remote_node_name, + const std::string ¶meter_name) { + rclcpp::AsyncParametersClient robot_description_client(this, remote_node_name); + while (!robot_description_client.wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + std::string err = "Interrupted while waiting for the service. Exiting."; + RCLCPP_ERROR(this->get_logger(), err.c_str()); + throw std::runtime_error(err); + } + RCLCPP_INFO(this->get_logger(), "Wating for '%s' service...", remote_node_name.c_str()); + } + auto future = robot_description_client.get_parameters({parameter_name}); + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != + rclcpp::FutureReturnCode::SUCCESS) { + std::string err = "Failed to retrieve '" + parameter_name + "'."; + RCLCPP_ERROR(this->get_logger(), parameter_name.c_str()); + throw std::runtime_error(err); + } + RCLCPP_INFO(this->get_logger(), "Received '%s' from '%s'.", parameter_name.c_str(), + remote_node_name.c_str()); + return future.get()[0]; + } +}; +} // end of namespace lbr_demos +#endif // LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__LBR_BASE_POSITION_COMMAND_NODE_HPP_ diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/pose_control_node.cpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/pose_control_node.cpp new file mode 100644 index 00000000..dc8b24c9 --- /dev/null +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/pose_control_node.cpp @@ -0,0 +1,144 @@ +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "kdl/chainfksolverpos_recursive.hpp" +#include "kdl/chainiksolverpos_lma.hpp" +#include "kdl_parser/kdl_parser.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "friClientIf.h" +#include "friLBRState.h" +#include "lbr_fri_msgs/msg/lbr_state.hpp" + +#include "lbr_base_position_command_node.hpp" + +namespace lbr_demos { +class PoseControlNode : public LBRBasePositionCommandNode { +protected: + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Subscription::SharedPtr pose_sub_; + + lbr_fri_msgs::msg::LBRState current_lbr_state_; // robot state, including joint positions + geometry_msgs::msg::Pose current_pose_; // current pose of robot + + KDL::Chain chain_; // robot kinematics chain exetracted from robot URDF file + +public: + PoseControlNode(const rclcpp::NodeOptions &options) + : LBRBasePositionCommandNode("pose_control", options) { + KDL::Tree robot_tree; + if (!kdl_parser::treeFromString(this->robot_description_, robot_tree)) { + RCLCPP_ERROR(this->get_logger(), "Failed to construct kdl tree."); + return; + } + + this->declare_parameter("base_link", "link_0"); + this->declare_parameter("end_effector_link", "link_ee"); + + std::string root_link = this->get_parameter("base_link").as_string(); + std::string tip_link = this->get_parameter("end_effector_link").as_string(); + + if (!robot_tree.getChain(root_link, tip_link, chain_)) { + RCLCPP_ERROR(this->get_logger(), "Failed to get chain from tree."); + } else { + RCLCPP_INFO(this->get_logger(), "Get chain from tree successfully."); + } + + pose_pub_ = this->create_publisher("state/pose", 1); + pose_sub_ = this->create_subscription( + "command/pose", 1, std::bind(&PoseControlNode::on_pose_, this, std::placeholders::_1)); + } + +protected: + void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) override { + current_lbr_state_ = *lbr_state; + + double joint_position[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; + for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_position[i] = current_lbr_state_.measured_joint_position[i]; + } + current_pose_ = compute_fk_(joint_position); + pose_pub_->publish(current_pose_); + } + + void on_pose_(const geometry_msgs::msg::Pose &msg) { + if (current_lbr_state_.session_state == KUKA::FRI::COMMANDING_ACTIVE) { + KDL::JntArray current_joint_positions(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + + for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + current_joint_positions(i) = current_lbr_state_.measured_joint_position[i]; + } + + lbr_fri_msgs::msg::LBRPositionCommand joint_position_command = + compute_ik_(msg, current_joint_positions); + lbr_position_command_pub_->publish(joint_position_command); + } + } + + geometry_msgs::msg::Pose compute_fk_(double *position_array_ptr) { + KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain_); + KDL::JntArray joint_positions = KDL::JntArray(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + + for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_positions(i) = position_array_ptr[i]; + } + + KDL::Frame pose_tmp; // pose described in data type KDL::Frame + geometry_msgs::msg::Pose pose; // described in geometry_msgs::msg::Pose + + if (fk_solver.JntToCart(joint_positions, pose_tmp) < 0) { + RCLCPP_ERROR(this->get_logger(), "FK Solver to calculate JointToCartesian failed."); + } else { + // Position + pose.position.x = pose_tmp.p.x(); + pose.position.y = pose_tmp.p.y(); + pose.position.z = pose_tmp.p.z(); + + // Orientation + double x, y, z, w; + pose_tmp.M.GetQuaternion(x, y, z, w); // get quaternion + pose.orientation.x = x; + pose.orientation.y = y; + pose.orientation.z = z; + pose.orientation.w = w; + } + + return pose; + } + + lbr_fri_msgs::msg::LBRPositionCommand compute_ik_(const geometry_msgs::msg::Pose &desired_pose, + KDL::JntArray ¤t_joint_positions) { + KDL::ChainIkSolverPos_LMA ik_solver(chain_); + KDL::JntArray result_joint_positions = KDL::JntArray(chain_.getNrOfJoints()); + lbr_fri_msgs::msg::LBRPositionCommand joint_position_command; + + // transfer data type 'geometry::msg::Pose' to be 'KDL::Frame' + KDL::Vector position(desired_pose.position.x, desired_pose.position.y, desired_pose.position.z); + KDL::Rotation rotation = + KDL::Rotation::Quaternion(desired_pose.orientation.x, desired_pose.orientation.y, + desired_pose.orientation.z, desired_pose.orientation.w); + KDL::Frame desired_pose_tmp(rotation, position); + + auto start = std::chrono::high_resolution_clock::now(); + int ik_result = + ik_solver.CartToJnt(current_joint_positions, desired_pose_tmp, result_joint_positions); + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration execution_time = end - start; + RCLCPP_DEBUG(this->get_logger(), "IK solver execution time: %f ms", execution_time.count()); + + if (ik_result < 0) // if solving failed, 'ik_result' would be less than 0 + { + RCLCPP_ERROR(this->get_logger(), "Inverse kinematics failed."); + } else { + for (unsigned int i = 0; i < result_joint_positions.data.size(); i++) { + joint_position_command.joint_position[i] = result_joint_positions(i); + } + } + + return joint_position_command; + } +}; +} // namespace lbr_demos + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(lbr_demos::PoseControlNode) diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/pose_planning_node.cpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/pose_planning_node.cpp new file mode 100644 index 00000000..05161ec3 --- /dev/null +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/pose_planning_node.cpp @@ -0,0 +1,51 @@ +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace lbr_demos { +class PosePlanningNode : public rclcpp::Node { +protected: + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Subscription::SharedPtr pose_sub_; + + geometry_msgs::msg::Pose initial_pose_; // robot starts from this pose + bool is_init_; + + double amplitude_; // rad + double frequency_; // Hz + double sampling_time_; // sampling time for sending position command + double phase_; // initial phase + +public: + PosePlanningNode(const rclcpp::NodeOptions &options) : Node("pose_planning", options) { + is_init_ = false; + amplitude_ = 0.05; + frequency_ = 0.5; + sampling_time_ = 0.01; + phase_ = 0.0; + + pose_pub_ = this->create_publisher("command/pose", 1); + pose_sub_ = this->create_subscription( + "state/pose", 1, std::bind(&PosePlanningNode::on_pose_, this, std::placeholders::_1)); + } + +protected: + void on_pose_(const geometry_msgs::msg::Pose &msg) { + if (!is_init_) { + initial_pose_ = msg; + is_init_ = true; + } else { + geometry_msgs::msg::Pose cartesian_pose_command = initial_pose_; + + phase_ = phase_ + 2 * M_PI * frequency_ * sampling_time_; + cartesian_pose_command.position.z += amplitude_ * sin(phase_); + + pose_pub_->publish(cartesian_pose_command); + } + } +}; +} // namespace lbr_demos + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(lbr_demos::PosePlanningNode) diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/config/admittance_control.yaml b/lbr_demos/lbr_demos_fri_ros2_advanced_python/config/admittance_control.yaml new file mode 100644 index 00000000..31e57382 --- /dev/null +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/config/admittance_control.yaml @@ -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: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0] + dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + exp_smooth: 0.95 diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/config/admittance_rcm_control.yaml b/lbr_demos/lbr_demos_fri_ros2_advanced_python/config/admittance_rcm_control.yaml new file mode 100644 index 00000000..84a25e4e --- /dev/null +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/config/admittance_rcm_control.yaml @@ -0,0 +1,8 @@ +/**/admittance_rcm_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: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0] + dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + exp_smooth: 0.95 diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/doc/lbr_demos_fri_ros2_advanced_python.rst b/lbr_demos/lbr_demos_fri_ros2_advanced_python/doc/lbr_demos_fri_ros2_advanced_python.rst index 98d7338d..9c8777f6 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/doc/lbr_demos_fri_ros2_advanced_python.rst +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/doc/lbr_demos_fri_ros2_advanced_python.rst @@ -1,14 +1,43 @@ LBR Demos FRI ROS 2 Advanced Python =================================== -Collection of advanced usage examples for the ``lbr_fri_ros2`` package through Python. +Collection of advanced usage examples for the ``lbr_ros2_control`` package through Python. .. warning:: Do always execute in ``T1`` mode first. Admittance Controller --------------------- +This demo implements a simple admittance controller. + .. warning:: - Not well behaved around singularities, put the robot in a well-behaved configuration first, e.g. ``A1 = 0°``, ``A2 = -30°``, ``A3 = 0°``, ``A4 = 60°``, ``A5 = 0°``, ``A6 = -90°``, ``A7 = 0°``. This can be done using the ``smartPAD`` in ``T1`` mode. + May not be well behaved around singularities, put the robot in a well-behaved configuration first, e.g. ``A1 = 0°``, ``A2 = -30°``, ``A3 = 0°``, ``A4 = 60°``, ``A5 = 0°``, ``A6 = -90°``, ``A7 = 0°``. This can be done using the ``smartPAD`` in ``T1`` mode. + +#. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` + + .. thumbnail:: ../../doc/img/applications_lbr_server.png + +#. Launch the robot driver: + +.. code-block:: bash + + ros2 launch lbr_bringup bringup.launch.py \ + sim:=false \ + ctrl:=forward_lbr_position_command_controller \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Run the `admittance_control `_ with remapping and parameter file: + +.. code-block:: bash + + ros2 run lbr_demos_fri_ros2_advanced_python admittance_control --ros-args \ + -r __ns:=/lbr \ + --params-file `ros2 pkg prefix lbr_demos_fri_ros2_advanced_python`/share/lbr_demos_fri_ros2_advanced_python/config/admittance_control.yaml + +#. Now gently move the robot at the end-effector. + +Admittance Controller with Remote Center of Motion +-------------------------------------------------- +This demo implements an admittance controller with a remote center of motion (RCM). #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` @@ -18,12 +47,17 @@ Admittance Controller .. code-block:: bash - ros2 launch lbr_fri_ros2 app.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_bringup bringup.launch.py \ + sim:=false \ + ctrl:=forward_lbr_position_command_controller \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `admittance_control_node `_: +#. Run the `admittance_rcm_control `_ with remapping and parameter file: .. code-block:: bash - ros2 launch lbr_demos_fri_ros2_advanced_python admittance_control_node.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 run lbr_demos_fri_ros2_advanced_python admittance_rcm_control --ros-args \ + -r __ns:=/lbr \ + --params-file `ros2 pkg prefix lbr_demos_fri_ros2_advanced_python`/share/lbr_demos_fri_ros2_advanced_python/config/admittance_rcm_control.yaml #. Now gently move the robot at the end-effector. diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/launch/admittance_control_node.launch.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/launch/admittance_control_node.launch.py deleted file mode 100644 index 78fd5f95..00000000 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/launch/admittance_control_node.launch.py +++ /dev/null @@ -1,20 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -from lbr_description import LBRDescriptionMixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - ld.add_action( - Node( - package="lbr_demos_fri_ros2_advanced_python", - executable="admittance_control_node", - output="screen", - parameters=[robot_description], - ) - ) - return ld diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py index ef99295f..68d09652 100755 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py @@ -1,64 +1,67 @@ import numpy as np import rclpy -from rclpy.node import Node -from lbr_fri_msgs.msg import LBRPositionCommand, LBRState +from lbr_fri_msgs.msg import LBRState from .admittance_controller import AdmittanceController +from .lbr_base_position_command_node import LBRBasePositionCommandNode -class AdmittanceControlNode(Node): - def __init__(self, node_name="admittance_control_node") -> None: +class AdmittanceControlNode(LBRBasePositionCommandNode): + def __init__(self, node_name: str = "admittance_control") -> None: super().__init__(node_name=node_name) # parameters - self.declare_parameter("robot_description", "") self.declare_parameter("base_link", "link_0") self.declare_parameter("end_effector_link", "link_ee") + self.declare_parameter("f_ext_th", [2.0, 2.0, 2.0, 0.5, 0.5, 0.5]) + self.declare_parameter("dq_gain", [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0]) + self.declare_parameter("dx_gain", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + self.declare_parameter("exp_smooth", 0.95) - self.init_ = False - self.lbr_state_ = LBRState() - - self.controller_ = AdmittanceController( - robot_description=str(self.get_parameter("robot_description").value), - base_link=str(self.get_parameter("base_link").value), - end_effector_link=str(self.get_parameter("end_effector_link").value), + self._init = False + self._lbr_state = LBRState() + self._exp_smooth = ( + self.get_parameter("exp_smooth").get_parameter_value().double_value ) + if self._exp_smooth < 0.0 or self._exp_smooth > 1.0: + raise ValueError("Exponential smoothing factor must be in [0, 1].") - # publishers and subscribers - self.lbr_state_sub_ = self.create_subscription( - LBRState, "/lbr/state", self.on_lbr_state_, 1 - ) - self.lbr_position_command_pub_ = self.create_publisher( - LBRPositionCommand, - "/lbr/command/joint_position", - 1, + self._controller = AdmittanceController( + robot_description=self._robot_description, + base_link=self.get_parameter("base_link") + .get_parameter_value() + .string_value, + end_effector_link=self.get_parameter("end_effector_link") + .get_parameter_value() + .string_value, ) - def on_lbr_state_(self, lbr_state: LBRState) -> None: - self.smooth_lbr_state_(lbr_state, 0.95) + def _on_lbr_state(self, lbr_state: LBRState) -> None: + self._smooth_lbr_state(lbr_state) - lbr_command = self.controller_(self.lbr_state_) - self.lbr_position_command_pub_.publish(lbr_command) + lbr_command = self._controller(self._lbr_state, self._dt) + self._lbr_position_command_pub.publish(lbr_command) - def smooth_lbr_state_(self, lbr_state: LBRState, alpha: float): - if not self.init_: - self.lbr_state_ = lbr_state - self.init_ = True + def _smooth_lbr_state(self, lbr_state: LBRState) -> None: + if not self._init: + self._lbr_state = lbr_state + self._init = True return - self.lbr_state_.measured_joint_position = ( - (1 - alpha) * np.array(self.lbr_state_.measured_joint_position.tolist()) - + alpha * np.array(lbr_state.measured_joint_position.tolist()) + self._lbr_state.measured_joint_position = ( + (1 - self._exp_smooth) + * np.array(self._lbr_state.measured_joint_position.tolist()) + + self._exp_smooth * np.array(lbr_state.measured_joint_position.tolist()) ).data - self.lbr_state_.external_torque = ( - (1 - alpha) * np.array(self.lbr_state_.external_torque.tolist()) - + alpha * np.array(lbr_state.external_torque.tolist()) + self._lbr_state.external_torque = ( + (1 - self._exp_smooth) * np.array(self._lbr_state.external_torque.tolist()) + + self._exp_smooth * np.array(lbr_state.external_torque.tolist()) ).data -def main(args=None): +def main(args=None) -> None: rclpy.init(args=args) admittance_control_node = AdmittanceControlNode() rclpy.spin(admittance_control_node) diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_controller.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_controller.py index 688f8919..c95738e3 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_controller.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_controller.py @@ -1,5 +1,5 @@ -import kinpy import numpy as np +import optas from lbr_fri_msgs.msg import LBRPositionCommand, LBRState @@ -11,53 +11,51 @@ def __init__( base_link: str = "link_0", end_effector_link: str = "link_ee", f_ext_th: np.ndarray = np.array([2.0, 2.0, 2.0, 0.5, 0.5, 0.5]), - dq_gain: np.ndarray = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), - dx_gain: np.ndarray = np.array([1.0, 1.0, 1.0, 20.0, 40.0, 60.0]), + dq_gain: np.ndarray = np.array([10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0]), + dx_gain: np.ndarray = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]), ) -> None: - self.lbr_position_command_ = LBRPositionCommand() + self._lbr_position_command = LBRPositionCommand() - self.chain_ = kinpy.build_serial_chain_from_urdf( - data=robot_description, - root_link_name=base_link, - end_link_name=end_effector_link, + self._robot = optas.RobotModel(urdf_string=robot_description) + + self._jacobian_func = self._robot.get_link_geometric_jacobian_function( + link=end_effector_link, base_link=base_link, numpy_output=True ) - self.dof_ = len(self.chain_.get_joint_parameter_names()) - self.jacobian_ = np.zeros((6, self.dof_)) - self.jacobian_inv_ = np.zeros((self.dof_, 6)) - self.q = np.zeros(self.dof_) - self.dq_ = np.zeros(self.dof_) - self.tau_ext_ = np.zeros(6) - self.dq_gain_ = np.diag(dq_gain) - self.dx_gain_ = np.diag(dx_gain) - self.f_ext_ = np.zeros(6) - self.f_ext_th_ = f_ext_th - self.alpha_ = 0.99 - - def __call__(self, lbr_state: LBRState) -> LBRPositionCommand: - self.q_ = np.array(lbr_state.measured_joint_position.tolist()) - self.tau_ext_ = np.array(lbr_state.external_torque.tolist()) - - self.jacobian_ = self.chain_.jacobian(self.q_) - - self.jacobian_inv_ = np.linalg.pinv(self.jacobian_, rcond=0.1) - self.f_ext_ = self.jacobian_inv_.T @ self.tau_ext_ - - self.f_ext_ = np.where( - abs(self.f_ext_) > self.f_ext_th_, - self.dx_gain_ @ np.sign(self.f_ext_) * (abs(self.f_ext_) - self.f_ext_th_), + self._dof = self._robot.ndof + self._jacobian = np.zeros((6, self._dof)) + self._jacobian_inv = np.zeros((self._dof, 6)) + self._q = np.zeros(self._dof) + self._dq = np.zeros(self._dof) + self._tau_ext = np.zeros(6) + self._dq_gain = np.diag(dq_gain) + self._dx_gain = np.diag(dx_gain) + self._f_ext = np.zeros(6) + self._f_ext_th = f_ext_th + self._alpha = 0.95 + + def __call__(self, lbr_state: LBRState, dt: float) -> LBRPositionCommand: + self._q = np.array(lbr_state.measured_joint_position.tolist()) + self._tau_ext = np.array(lbr_state.external_torque.tolist()) + + self._jacobian = self._jacobian_func(self._q) + self._jacobian_inv = np.linalg.pinv(self._jacobian, rcond=0.1) + self._f_ext = self._jacobian_inv.T @ self._tau_ext + + dx = np.where( + abs(self._f_ext) > self._f_ext_th, + self._dx_gain @ np.sign(self._f_ext) * (abs(self._f_ext) - self._f_ext_th), 0.0, ) # additional smoothing required in python - self.dq_ = ( - self.alpha_ * self.dq_ - + (1 - self.alpha_) * self.dq_gain_ @ self.jacobian_inv_ @ self.f_ext_ + self._dq = ( + self._alpha * self._dq + + (1 - self._alpha) * self._dq_gain @ self._jacobian_inv @ dx ) - self.lbr_position_command_.joint_position = ( - np.array(lbr_state.measured_joint_position.tolist()) - + lbr_state.sample_time * self.dq_ + self._lbr_position_command.joint_position = ( + np.array(lbr_state.measured_joint_position.tolist()) + dt * self._dq ).data - return self.lbr_position_command_ + return self._lbr_position_command diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_rcm_control_node.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_rcm_control_node.py new file mode 100644 index 00000000..ac29bc3b --- /dev/null +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_rcm_control_node.py @@ -0,0 +1,89 @@ +import numpy as np +import rclpy + +from lbr_fri_msgs.msg import LBRPositionCommand, LBRState + +from .admittance_rcm_controller import AdmittanceRCMController +from .lbr_base_position_command_node import LBRBasePositionCommandNode + + +class LBRAdmittanceControlRCMNode(LBRBasePositionCommandNode): + def __init__(self, node_name: str = "admittance_rcm_control") -> None: + super().__init__(node_name) + + # declare and get parameters + self.declare_parameter("base_link", "link_0") + self.declare_parameter("end_effector_link", "link_ee") + self.declare_parameter("f_ext_th", [4.0, 4.0, 4.0, 1.0, 1.0, 1.0]) + self.declare_parameter("dq_gain", [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0]) + self.declare_parameter("dx_gain", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + self.declare_parameter("exp_smooth", 0.95) + + self._f_ext_th = np.array( + self.get_parameter("f_ext_th").get_parameter_value().double_array_value + ) + self._dq_gain = np.diag( + self.get_parameter("dq_gain").get_parameter_value().double_array_value + ) + self._dx_gain = np.diag( + self.get_parameter("dx_gain").get_parameter_value().double_array_value + ) + + self._exp_smooth = ( + self.get_parameter("exp_smooth").get_parameter_value().double_value + ) + if self._exp_smooth < 0.0 or self._exp_smooth > 1.0: + raise ValueError("Exponential smoothing factor must be in [0, 1].") + + self._dq = np.zeros(7) + self._controller = AdmittanceRCMController( + robot_description=self._robot_description, + base_link=self.get_parameter("base_link") + .get_parameter_value() + .string_value, + end_effector_link=self.get_parameter("end_effector_link") + .get_parameter_value() + .string_value, + ) + + def _command(self, q) -> None: + lbr_command = LBRPositionCommand() + lbr_command.joint_position = q + self._lbr_position_command_pub.publish(lbr_command) + + def _admittance(self, tau_ext, qc) -> None: + J = self._controller.jacobian_func(qc) + Jinv = np.linalg.pinv(J, rcond=0.1) + f_ext = Jinv.T @ tau_ext + dx = np.where( + abs(f_ext) > self._f_ext_th, + self._dx_gain @ np.sign(f_ext) * (abs(f_ext) - self._f_ext_th), + 0.0, + ).flatten() + dx = np.clip(dx, -1.0, 1.0) + dq = Jinv @ dx + self._dq = self._exp_smooth * self._dq + (1.0 - self._exp_smooth) * dq + return self._dq + + def _on_lbr_state(self, msg: LBRState) -> None: + if self._controller._rcm is None: + self._controller.set_start(msg.measured_joint_position) + return + + dq_goal = self._admittance(msg.external_torque, msg.measured_joint_position) + self._controller.reset(msg.measured_joint_position, dq_goal) + + if self._controller.solve(): + qc = np.array(msg.measured_joint_position) + dq = self._controller.get_qd_target() + qn = qc + self._dt * self._dq_gain @ dq + self._command(qn.tolist()) + else: + self.get_logger().error("Solver failed!") + + +def main(args=None) -> None: + rclpy.init(args=args) + node = LBRAdmittanceControlRCMNode() + rclpy.spin(node) + rclpy.shutdown() diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_rcm_controller.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_rcm_controller.py new file mode 100644 index 00000000..95fd3b18 --- /dev/null +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_rcm_controller.py @@ -0,0 +1,83 @@ +import casadi as cs +import optas + + +class AdmittanceRCMController: + def __init__( + self, + robot_description: str, + base_link: str = "link_0", + end_effector_link: str = "link_ee", + ): + self._robot = optas.RobotModel( + urdf_string=robot_description, time_derivs=[0, 1] + ) + + self.jacobian_func = self._robot.get_link_geometric_jacobian_function( + link=end_effector_link, base_link=base_link, numpy_output=True + ) + + T = 2 + builder = optas.OptimizationBuilder(T, robots=[self._robot]) + self._name = self._robot.get_name() + + rcm = builder.add_parameter("rcm", 3) + + q0 = builder.get_model_state(self._name, 0, time_deriv=0) + qF = builder.get_model_state(self._name, 1, time_deriv=0) + qd = builder.get_model_state(self._name, 0, time_deriv=1) + + qc = builder.add_parameter("qc", self._robot.ndof) + qd_goal = builder.add_parameter("qd_goal", self._robot.ndof) + + _q = optas.SX.sym("q", self._robot.ndof) + _rcm = optas.SX.sym("rcm", 3) + Tf = self._robot.get_global_link_transform(end_effector_link, _q) + zf = Tf[:3, 2] + pf = Tf[:3, 3] + alpha = zf.T @ (_rcm - pf) + _dist_sqr = optas.sumsqr(pf + alpha * zf - _rcm) + self._dist_sqr = optas.Function("dist_sqr", [_q, _rcm], [_dist_sqr]) + + builder.add_equality_constraint("rcm", self._dist_sqr(qF, rcm)) + + c1 = cs.sumsqr(qd - qd_goal) + builder.add_cost_term("match_qd_goal", c1) + + builder.add_equality_constraint("qinit", q0, qc) + builder.add_equality_constraint("dynamics", q0 + qd, qF) + + self._eff_transform = self._robot.get_global_link_transform_function( + end_effector_link + ) + + q = cs.SX.sym("q", self._robot.ndof) + p = self._robot.get_global_link_position(end_effector_link, q) + self._xpos = cs.Function("xpos", [q], [p[0]]) + + self._solver = optas.ScipyMinimizeSolver(builder.build()).setup("SLSQP") + + self._rcm = None + + def set_start(self, q): + T = self._eff_transform(q) + z = T[:3, 2] + p = T[:3, 3] + self._rcm = p + 0.2 * z + + def reset(self, qc, qd_goal): + params = {"qc": qc, "qd_goal": qd_goal, "rcm": self._rcm} + self._solver.reset_parameters(params) + self._solver.reset_initial_seed( + { + f"{self._name}/q": cs.horzcat(cs.DM(qc), cs.DM(qc)), + f"{self._name}/dq": qd_goal, + } + ) + + def solve(self): + self.sol = self._solver.solve() + return self._solver.did_solve() + + def get_qd_target(self): + return self.sol[f"{self._name}/dq"].toarray().flatten() diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/lbr_base_position_command_node.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/lbr_base_position_command_node.py new file mode 100644 index 00000000..e27020b2 --- /dev/null +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/lbr_base_position_command_node.py @@ -0,0 +1,60 @@ +import rclpy +import rclpy.parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import GetParameters +from rclpy.node import Node + +from lbr_fri_msgs.msg import LBRPositionCommand, LBRState + + +class LBRBasePositionCommandNode(Node): + r"""Utility class for creating a base node for sending position commands to the KUKA LBRs. + Retrieves update rate and robot description from the parameter servers. + """ + + _update_rate: int + _dt: float + _robot_description: str + + def __init__(self, node_name: str) -> None: + super().__init__(node_name) + + # retrieve parameters + self._robot_description = self._retrieve_parameter( + "controller_manager/get_parameters", "robot_description" + ).string_value + self._update_rate = self._retrieve_parameter( + "controller_manager/get_parameters", "update_rate" + ).integer_value + self._dt = 1.0 / float(self._update_rate) + + # publishers and subscribers + self._lbr_state_sub = self.create_subscription( + LBRState, "state", self._on_lbr_state, 1 + ) + self._lbr_position_command_pub = self.create_publisher( + LBRPositionCommand, + "command/joint_position", + 1, + ) + + def _retrieve_parameter(self, service: str, parameter_name: str) -> ParameterValue: + parameter_client = self.create_client(GetParameters, service) + while not parameter_client.wait_for_service(timeout_sec=1.0): + if not rclpy.ok(): + self.get_logger().error( + "Interrupted while waiting for the service. Exiting." + ) + return None + self.get_logger().info(f"Waiting for '{service}' service...") + request = GetParameters.Request(names=[parameter_name]) + future = parameter_client.call_async(request=request) + rclpy.spin_until_future_complete(self, future) + if future.result() is None: + self.get_logger().error(f"Failed to retrieve '{parameter_name}'.") + return None + self.get_logger().info(f"Received '{parameter_name}' from '{service}'.") + return future.result().values[0] + + def _on_lbr_state(self, lbr_state: LBRState) -> None: + raise NotImplementedError diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml b/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml index 6205e56f..def43c22 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml @@ -11,6 +11,7 @@ lbr_fri_msgs lbr_fri_ros2 rclpy + rcl_interfaces ament_copyright ament_flake8 diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py index f37331f4..7bb66286 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py @@ -11,6 +11,7 @@ data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/config", glob.glob("config/*.yaml")), ("share/" + package_name + "/launch", glob.glob("launch/*.py")), ], install_requires=["setuptools"], @@ -22,7 +23,8 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "admittance_control_node = lbr_demos_fri_ros2_advanced_python.admittance_control_node:main", + "admittance_control = lbr_demos_fri_ros2_advanced_python.admittance_control_node:main", + "admittance_rcm_control = lbr_demos_fri_ros2_advanced_python.admittance_rcm_control_node:main", ], }, ) diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py index 3dc65d51..f6c80141 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py @@ -11,37 +11,37 @@ class JointSineOverlayNode(Node): def __init__(self, node_name: str) -> None: super().__init__(node_name) - self.amplitude_ = 0.04 # rad - self.frequency_ = 0.25 # Hz - self.phase_ = 0.0 - self.lbr_position_command_ = LBRPositionCommand() + self._amplitude = 0.04 # rad + self._frequency = 0.25 # Hz + self._phase = 0.0 + self._lbr_position_command = LBRPositionCommand() # create publisher to /lbr/command/joint_position - self.lbr_position_command_pub_ = self.create_publisher( + self._lbr_position_command_pub = self.create_publisher( LBRPositionCommand, "/lbr/command/joint_position", 1, ) # create subscription to /lbr_state - self.lbr_state_sub_ = self.create_subscription( - LBRState, "/lbr/state", self.on_lbr_state_, 1 + self._lbr_state_sub_ = self.create_subscription( + LBRState, "/lbr/state", self._on_lbr_state, 1 ) - def on_lbr_state_(self, lbr_state: LBRState) -> None: - self.lbr_position_command_.joint_position = lbr_state.ipo_joint_position + def _on_lbr_state(self, lbr_state: LBRState) -> None: + self._lbr_position_command.joint_position = lbr_state.ipo_joint_position if lbr_state.session_state == 4: # KUKA::FRI::COMMANDING_ACTIVE == 4 # overlay sine wave on 4th joint - self.lbr_position_command_.joint_position[3] += self.amplitude_ * math.sin( - self.phase_ + self._lbr_position_command.joint_position[3] += self._amplitude * math.sin( + self._phase ) - self.phase_ += 2 * math.pi * self.frequency_ * lbr_state.sample_time + self._phase += 2 * math.pi * self._frequency * lbr_state.sample_time - self.lbr_position_command_pub_.publish(self.lbr_position_command_) + self._lbr_position_command_pub.publish(self._lbr_position_command) else: # reset phase - self.phase_ = 0.0 + self._phase = 0.0 def main(args: list = None) -> None: diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py index ae46d7c9..6c71f75c 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py @@ -11,33 +11,33 @@ class TorqueSineOverlayNode(Node): def __init__(self, node_name: str) -> None: super().__init__(node_name) - self.amplitude_ = 15.0 # Nm - self.frequency_ = 0.25 # Hz - self.phase_ = 0.0 - self.lbr_torque_command_ = LBRTorqueCommand() + self._amplitude = 15.0 # Nm + self._frequency = 0.25 # Hz + self._phase = 0.0 + self._lbr_torque_command = LBRTorqueCommand() # create publisher to /lbr/command/torque - self.lbr_torque_command_pub_ = self.create_publisher( + self._lbr_torque_command_pub = self.create_publisher( LBRTorqueCommand, "/lbr/command/torque", 1 ) # create subscription to /lbr_state - self.lbr_state_sub_ = self.create_subscription( - LBRState, "/lbr/state", self.on_lbr_state_, 1 + self._lbr_state_sub = self.create_subscription( + LBRState, "/lbr/state", self._on_lbr_state, 1 ) - def on_lbr_state_(self, lbr_state: LBRState) -> None: - self.lbr_torque_command_.joint_position = lbr_state.ipo_joint_position + def _on_lbr_state(self, lbr_state: LBRState) -> None: + self._lbr_torque_command.joint_position = lbr_state.ipo_joint_position if lbr_state.session_state == 4: # KUKA::FRI::COMMANDING_ACTIVE == 4 # overlay torque sine wave on 4th joint - self.lbr_torque_command_.torque[3] = self.amplitude_ * math.sin(self.phase_) - self.phase_ += 2 * math.pi * self.frequency_ * lbr_state.sample_time + self._lbr_torque_command.torque[3] = self._amplitude * math.sin(self._phase) + self._phase += 2 * math.pi * self._frequency * lbr_state.sample_time - self.lbr_torque_command_pub_.publish(self.lbr_torque_command_) + self._lbr_torque_command_pub.publish(self._lbr_torque_command) else: # reset phase - self.phase_ = 0.0 + self._phase = 0.0 def main(args: list = None) -> None: diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/wrench_sine_overlay_node.py b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/wrench_sine_overlay_node.py index d5567023..a636f34c 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/wrench_sine_overlay_node.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/wrench_sine_overlay_node.py @@ -11,39 +11,39 @@ class WrenchSineOverlayNode(Node): def __init__(self, node_name: str) -> None: super().__init__(node_name) - self.amplitude_x_, self.amplitude_y_ = 5.0, 5.0 # N - self.frequency_x_, self.frequency_y_ = 0.25, 0.25 # Hz - self.phase_x_, self.phase_y_ = 0.0, 0.0 - self.lbr_wrench_command_ = LBRWrenchCommand() + self._amplitude_x, self._amplitude_y = 5.0, 5.0 # N + self._frequency_x, self._frequency_y = 0.25, 0.25 # Hz + self._phase_x, self._phase_y = 0.0, 0.0 + self._lbr_wrench_command = LBRWrenchCommand() # create publisher to /lbr/command/wrench - self.lbr_wrench_command_pub_ = self.create_publisher( + self._lbr_wrench_command_pub = self.create_publisher( LBRWrenchCommand, "/lbr/command/wrench", 1 ) # create subscription to /lbr_state - self.lbr_state_sub_ = self.create_subscription( - LBRState, "/lbr/state", self.on_lbr_state_, 1 + self._lbr_state_sub = self.create_subscription( + LBRState, "/lbr/state", self._on_lbr_state, 1 ) - def on_lbr_state_(self, lbr_state: LBRState) -> None: - self.lbr_wrench_command_.joint_position = lbr_state.ipo_joint_position + def _on_lbr_state(self, lbr_state: LBRState) -> None: + self._lbr_wrench_command.joint_position = lbr_state.ipo_joint_position if lbr_state.session_state == 4: # KUKA::FRI::COMMANDING_ACTIVE == 4 # overlay wrench sine wave on x / y direction - self.lbr_wrench_command_.wrench[0] = self.amplitude_x_ * math.sin( - self.phase_x_ + self._lbr_wrench_command.wrench[0] = self._amplitude_x * math.sin( + self._phase_x ) - self.lbr_wrench_command_.wrench[1] = self.amplitude_y_ * math.sin( - self.phase_y_ + self._lbr_wrench_command.wrench[1] = self._amplitude_y * math.sin( + self._phase_y ) - self.phase_x_ += 2 * math.pi * self.frequency_x_ * lbr_state.sample_time - self.phase_y_ += 2 * math.pi * self.frequency_y_ * lbr_state.sample_time + self._phase_x += 2 * math.pi * self._frequency_x * lbr_state.sample_time + self._phase_y += 2 * math.pi * self._frequency_y * lbr_state.sample_time - self.lbr_wrench_command_pub_.publish(self.lbr_wrench_command_) + self._lbr_wrench_command_pub.publish(self._lbr_wrench_command) else: # reset phase - self.phase_x_, self.phase_y_ = 0.0, 0.0 + self._phase_x, self._phase_y = 0.0, 0.0 def main(args: list = None) -> None: diff --git a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/joint_trajectory_executioner_node.py b/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/joint_trajectory_executioner_node.py index 203ace45..76abde97 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/joint_trajectory_executioner_node.py +++ b/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/joint_trajectory_executioner_node.py @@ -11,12 +11,12 @@ def __init__( node_name: str, ) -> None: super().__init__(node_name=node_name) - self.joint_trajectory_action_client_ = ActionClient( + self._joint_trajectory_action_client = ActionClient( node=self, action_type=FollowJointTrajectory, action_name="/lbr/joint_trajectory_controller/follow_joint_trajectory", ) - while not self.joint_trajectory_action_client_.wait_for_server(1): + while not self._joint_trajectory_action_client.wait_for_server(1): self.get_logger().info("Waiting for action server to become available...") self.get_logger().info("Action server available.") @@ -40,7 +40,7 @@ def execute(self, positions: list, sec_from_start: int = 15): joint_trajectory_goal.trajectory.points.append(point) # send goal - goal_future = self.joint_trajectory_action_client_.send_goal_async( + goal_future = self._joint_trajectory_action_client.send_goal_async( joint_trajectory_goal ) rclpy.spin_until_future_complete(self, goal_future)