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
-
+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:
-
-
-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 |
+|:--:|:---|
+| | This work was supported by core and project funding from the Wellcome/EPSRC [WT203148/Z/16/Z; NS/A000049/1; WT101957; NS/A000027/1]. |
+| | This project has received funding from the European Union's Horizon 2020 research and innovation programme under grant agreement No 101016985 (FAROS project). |
+| | Built at [RViMLab](https://rvim.online/). |
+| | Built at [CAI4CAI](https://cai4cai.ml/). |
+| | 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)