Additional demos

README.md

@@ -80,10 +80,14 @@ If you enjoyed using this repository for your work, we would really appreciate 
 ```
 
 ## Acknowledgements
-<img src="https://medicalengineering.org.uk/wp-content/themes/aalto-child/_assets/images/medicalengineering-logo.svg" alt="wellcome" height="45" width="65" align="left">
+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:
 
-<img src="https://upload.wikimedia.org/wikipedia/commons/thumb/b/b7/Flag_of_Europe.svg/1920px-Flag_of_Europe.svg.png" alt="eu_flag" height="45" width="65" align="left" >
-
-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 |
+|:--:|:---|
+| <img src="https://medicalengineering.org.uk/wp-content/themes/aalto-child/_assets/images/medicalengineering-logo.svg" alt="wellcome" width="200" align="left">  | This work was supported by core and project funding from the Wellcome/EPSRC [WT203148/Z/16/Z; NS/A000049/1; WT101957; NS/A000027/1]. |
+| <img src="https://upload.wikimedia.org/wikipedia/commons/thumb/b/b7/Flag_of_Europe.svg/1920px-Flag_of_Europe.svg.png" alt="eu_flag" width="200" align="left"> | This project has received funding from the European Union's Horizon 2020 research and innovation programme under grant agreement No 101016985 (FAROS project). |
+| <img src="https://rvim.online/author/avatar_hu8970a6942005977dc117387facf47a75_62303_270x270_fill_lanczos_center_2.png" alt="RViMLab" width="200" align="left"> | Built at [RViMLab](https://rvim.online/). |
+| <img src="https://avatars.githubusercontent.com/u/75276868?s=200&v=4" alt="King's College London" width="200" align="left"> | Built at [CAI4CAI](https://cai4cai.ml/). |
+| <img src="https://upload.wikimedia.org/wikipedia/commons/1/14/King%27s_College_London_logo.svg" alt="King's College London" width="200" align="left"> | Built at [King's College London](https://www.kcl.ac.uk/). |

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()

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

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 <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp>`_:
+#. 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 <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp>`_:
+
+.. 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

lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml

@@ -12,6 +12,7 @@
   <exec_depend>lbr_description</exec_depend>
 
   <depend>fri_vendor</depend>
+  <depend>geometry_msgs</depend>
   <depend>kdl_parser</depend>
   <depend>lbr_fri_ros2</depend>
   <depend>lbr_fri_msgs</depend>

lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp

@@ -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)