Merge pull request #316 from lbr-stack/backport-315-to-jazzy

Backport 315 to jazzy

Author: mhubii

lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt

@@ -76,9 +76,4 @@ install(
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-install(
-  DIRECTORY config
-  DESTINATION share/${PROJECT_NAME}
-)
-
 ament_package()

lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml

@@ -62,14 +62,12 @@ This demo uses the admittance controller.
         - ``FRI control mode``: ``POSITION_CONTROL``
         - ``FRI client command mode``: ``POSITION``
 
-#. Launch the robot driver (please note that a different system configuration file is used with heavier smoothing!):
+#. Launch the robot driver:
 
     .. code-block:: bash
 
         ros2 launch lbr_bringup hardware.launch.py \
             ctrl:=admittance_controller \
-            sys_cfg_pkg:=lbr_demos_advanced_cpp \
-            sys_cfg:=config/lbr_system_config.yaml \
             model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
 
 #. Now gently move the robot at the end-effector.

lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst

@@ -108,7 +108,7 @@
       chain_tip: lbr_link_ee
       damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
       max_linear_velocity: 2.0 # maximum linear velocity
-      max_angular_velocity: 2.0 # maximum linear acceleration
+      max_angular_velocity: 2.0 # maximum angular velocity
       joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains
       cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains
     filter:
@@ -121,9 +121,9 @@
       chain_root: lbr_link_0
       chain_tip: lbr_link_ee
       twist_in_tip_frame: true # if true, the twist command is expressed in the tip frame, otherwise in the root frame
-      damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
-      max_linear_velocity: 0.1 # maximum linear velocity
-      max_angular_velocity: 0.1 # maximum linear acceleration
+      damping: 0.02 # damping factor for the pseudo-inverse of the Jacobian
+      max_linear_velocity: 0.2 # maximum linear velocity
+      max_angular_velocity: 0.2 # maximum angular velocity
       joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains
       cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains
     timeout: 0.2 # zeros the twist command when no command is received within timeout [s]

lbr_description/ros2_control/lbr_controllers.yaml

@@ -3,8 +3,12 @@
 
 #include <array>
 
+#include "lbr_fri_ros2/types.hpp"
+
 namespace lbr_fri_ros2 {
 bool norm_in_bounds(const double &x0, const double &x1, const double &x2, const double &max);
 bool norm_in_bounds(const std::array<double, 3> &vec, const double &max);
+bool all_jnts_in_bounds(const_jnt_array_t_ref vals, const_jnt_array_t_ref lower,
+                        const_jnt_array_t_ref upper);
 } // namespace lbr_fri_ros2
 #endif // LBR_FRI_ROS2__MATH_HPP_

lbr_fri_ros2/include/lbr_fri_ros2/math.hpp

@@ -101,10 +101,10 @@ void AdmittanceImpl::log_info() const {
 
 void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) {
   // clip velocity
-  twist_target_.head(3).unaryExpr([&](double v) {
+  twist_target_.head(3) = twist_target_.head(3).unaryExpr([&](double v) {
     return std::clamp(v, -parameters_.max_linear_velocity, parameters_.max_linear_velocity);
   });
-  twist_target_.tail(3).unaryExpr([&](double v) {
+  twist_target_.tail(3) = twist_target_.tail(3).unaryExpr([&](double v) {
     return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity);
   });
 

lbr_fri_ros2/src/control.cpp

@@ -8,4 +8,13 @@ bool norm_in_bounds(const double &x0, const double &x1, const double &x2, const
 bool norm_in_bounds(const std::array<double, 3> &vec, const double &max) {
   return norm_in_bounds(vec[0], vec[1], vec[2], max);
 }
+bool all_jnts_in_bounds(const_jnt_array_t_ref vals, const_jnt_array_t_ref lower,
+                        const_jnt_array_t_ref upper) {
+  for (std::size_t i = 0; i < vals.size(); ++i) {
+    if (vals[i] < lower[i] || upper[i] < vals[i]) {
+      return false;
+    }
+  }
+  return true;
+}
 } // namespace lbr_fri_ros2

lbr_fri_ros2/src/math.cpp

@@ -21,6 +21,7 @@
 #include "lbr_fri_ros2/control.hpp"
 #include "lbr_fri_ros2/formatting.hpp"
 #include "lbr_fri_ros2/kinematics.hpp"
+#include "lbr_fri_ros2/math.hpp"
 #include "lbr_fri_ros2/types.hpp"
 #include "lbr_ros2_control/system_interface_type_values.hpp"
 
@@ -53,6 +54,7 @@ class TwistController : public controller_interface::ControllerInterface {
   void reset_command_buffer_();
   void zero_joint_velocity_command_();
   void configure_joint_names_();
+  void configure_joint_limits_();
   void configure_inv_jac_ctrl_impl_();
   void log_info_() const;
 
@@ -62,10 +64,11 @@ class TwistController : public controller_interface::ControllerInterface {
 
   // joint veloctiy computation
   std::unique_ptr<lbr_fri_ros2::InvJacCtrlImpl> inv_jac_ctrl_impl_ptr_;
-  lbr_fri_ros2::jnt_array_t q_, dq_;
+  lbr_fri_ros2::jnt_array_t q_, q_target_, dq_;
 
   // interfaces
   lbr_fri_ros2::jnt_name_array_t joint_names_;
+  lbr_fri_ros2::jnt_array_t lower_joint_limits_, upper_joint_limits_;
   std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
       joint_position_state_interfaces_;
   std::unique_ptr<std::reference_wrapper<hardware_interface::LoanedStateInterface>>

lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp

@@ -183,6 +183,7 @@ AdmittanceController::on_configure(const rclcpp_lifecycle::State & /*previous_st
 
 controller_interface::CallbackReturn
 AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
+  initialized_ = false;
   if (!assign_state_interfaces_()) {
     release_state_interfaces_();
     return controller_interface::CallbackReturn::ERROR;
@@ -256,12 +257,12 @@ void AdmittanceController::configure_joint_names_() {
   if (joint_names_.size() != lbr_fri_ros2::N_JNTS) {
     RCLCPP_ERROR(
         this->get_node()->get_logger(),
-        "Number of joint names (%ld) does not match the number of joints in the robot (%d).",
+        "Number of joint names '%ld' does not match the number of joints in the robot '%d'.",
         joint_names_.size(), lbr_fri_ros2::N_JNTS);
     throw std::runtime_error("Failed to configure joint names.");
   }
   std::string robot_name = this->get_node()->get_parameter("robot_name").as_string();
-  for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
+  for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
     joint_names_[i] = robot_name + "_A" + std::to_string(i + 1);
   }
 }
@@ -270,8 +271,8 @@ void AdmittanceController::configure_admittance_impl_() {
   if (this->get_node()->get_parameter("admittance.mass").as_double_array().size() !=
       lbr_fri_ros2::CARTESIAN_DOF) {
     RCLCPP_ERROR(this->get_node()->get_logger(),
-                 "Number of mass values (%ld) does not match the number of cartesian degrees of "
-                 "freedom (%d).",
+                 "Number of mass values '%ld' does not match the number of cartesian degrees of "
+                 "freedom '%d'.",
                  this->get_node()->get_parameter("admittance.mass").as_double_array().size(),
                  lbr_fri_ros2::CARTESIAN_DOF);
     throw std::runtime_error("Failed to configure admittance parameters.");
@@ -280,32 +281,32 @@ void AdmittanceController::configure_admittance_impl_() {
       lbr_fri_ros2::CARTESIAN_DOF) {
     RCLCPP_ERROR(
         this->get_node()->get_logger(),
-        "Number of damping values (%ld) does not match the number of cartesian degrees of freedom "
-        "(%d).",
+        "Number of damping values '%ld' does not match the number of cartesian degrees of freedom "
+        "'%d'.",
         this->get_node()->get_parameter("admittance.damping").as_double_array().size(),
         lbr_fri_ros2::CARTESIAN_DOF);
     throw std::runtime_error("Failed to configure admittance parameters.");
   }
   if (this->get_node()->get_parameter("admittance.stiffness").as_double_array().size() !=
       lbr_fri_ros2::CARTESIAN_DOF) {
     RCLCPP_ERROR(this->get_node()->get_logger(),
-                 "Number of stiffness values (%ld) does not match the number of cartesian degrees "
+                 "Number of stiffness values '%ld' does not match the number of cartesian degrees "
                  "of freedom "
-                 "(%d).",
+                 "'%d'.",
                  this->get_node()->get_parameter("admittance.stiffness").as_double_array().size(),
                  lbr_fri_ros2::CARTESIAN_DOF);
     throw std::runtime_error("Failed to configure admittance parameters.");
   }
   lbr_fri_ros2::cart_array_t mass_array;
-  for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
+  for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
     mass_array[i] = this->get_node()->get_parameter("admittance.mass").as_double_array()[i];
   }
   lbr_fri_ros2::cart_array_t damping_array;
-  for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
+  for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
     damping_array[i] = this->get_node()->get_parameter("admittance.damping").as_double_array()[i];
   }
   lbr_fri_ros2::cart_array_t stiffness_array;
-  for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
+  for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
     stiffness_array[i] =
         this->get_node()->get_parameter("admittance.stiffness").as_double_array()[i];
   }
@@ -318,7 +319,7 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() {
       lbr_fri_ros2::N_JNTS) {
     RCLCPP_ERROR(
         this->get_node()->get_logger(),
-        "Number of joint gains (%ld) does not match the number of joints in the robot (%d).",
+        "Number of joint gains '%ld' does not match the number of joints in the robot '%d'.",
         this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size(),
         lbr_fri_ros2::N_JNTS);
     throw std::runtime_error("Failed to configure joint gains.");
@@ -327,19 +328,19 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() {
       lbr_fri_ros2::CARTESIAN_DOF) {
     RCLCPP_ERROR(
         this->get_node()->get_logger(),
-        "Number of cartesian gains (%ld) does not match the number of cartesian degrees of freedom "
-        "(%d).",
+        "Number of cartesian gains '%ld' does not match the number of cartesian degrees of freedom "
+        "'%d'.",
         this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size(),
         lbr_fri_ros2::CARTESIAN_DOF);
     throw std::runtime_error("Failed to configure cartesian gains.");
   }
   lbr_fri_ros2::jnt_array_t joint_gains_array;
-  for (unsigned int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
+  for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
     joint_gains_array[i] =
         this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array()[i];
   }
   lbr_fri_ros2::cart_array_t cartesian_gains_array;
-  for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
+  for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
     cartesian_gains_array[i] =
         this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i];
   }

lbr_ros2_control/src/controllers/admittance_controller.cpp

@@ -86,7 +86,7 @@ void LBRJointPositionCommandController::configure_joint_names_() {
     throw std::runtime_error("Failed to configure joint names.");
   }
   std::string robot_name = this->get_node()->get_parameter("robot_name").as_string();
-  for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
+  for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
     joint_names_[i] = robot_name + "_A" + std::to_string(i + 1);
   }
 }