Merge pull request #176 from lbr-stack/dev-humble-launch

Humble v2.0.0 Release

lbr_demos/lbr_demos_advanced_py/package.xml

@@ -5,6 +5,7 @@
   <version>2.0.0</version>
   <description>Advanced Python demos for the lbr_ros2_control.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
+  <maintainer email="[email protected]">cmower</maintainer>
   <license>Apache License 2.0</license>
 
   <exec_depend>lbr_description</exec_depend>

lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst

@@ -12,6 +12,21 @@ MoveIt via RViZ
 
 To run MoveIt via RViZ, simply follow:
 
+Simulation
+~~~~~~~~~~
+#. Run the robot driver:
+
+    .. code-block:: bash
+
+        ros2 launch lbr_bringup bringup.launch.py \
+            moveit:=true \
+            sim:=true \
+            model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
+
+#. You can now move the robot via MoveIt in RViZ!
+
+Hardware
+~~~~~~~~
 #. Client side configurations:
 
     #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_system_parameters.yaml>`_:octicon:`link-external`
@@ -30,13 +45,4 @@ To run MoveIt via RViZ, simply follow:
         - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL``
         - ``FRI client command mode``: ``POSITION``
 
-#. Run the robot driver:
-
-    .. code-block:: bash
-
-        ros2 launch lbr_bringup bringup.launch.py \
-            moveit:=true \
-            sim:=false \
-            model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
-
-#. You can now move the robot via MoveIt in RViZ!
+#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``.

lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp

@@ -126,20 +126,18 @@ class JointPIDArray {
   using pid_array_t = std::array<control_toolbox::Pid, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
 
 public:
-  JointPIDArray() = default;
+  JointPIDArray() = delete;
+  JointPIDArray(const PIDParameters &pid_parameters);
 
   void compute(const value_array_t &command_target, const value_array_t &state,
                const std::chrono::nanoseconds &dt, value_array_t &command);
   void compute(const value_array_t &command_target, const double *state,
                const std::chrono::nanoseconds &dt, value_array_t &command);
-  void initialize(const PIDParameters &pid_parameters, const double &dt);
-  inline const bool &is_initialized() const { return initialized_; };
-
   void log_info() const;
 
 protected:
-  bool initialized_{false};     /**< True if initialized.*/
-  pid_array_t pid_controllers_; /**< PID controllers for each joint.*/
+  PIDParameters pid_parameters_; /**< PID parameters for all joints.*/
+  pid_array_t pid_controllers_;  /**< PID controllers for each joint.*/
 };
 } // end of namespace lbr_fri_ros2
 #endif // LBR_FRI_ROS2__FILTERS_HPP_

lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp

@@ -49,7 +49,6 @@ class BaseCommandInterface {
 
 protected:
   std::unique_ptr<CommandGuard> command_guard_;
-  PIDParameters pid_parameters_;
   JointPIDArray joint_position_pid_;
   idl_command_t command_, command_target_;
 };

lbr_fri_ros2/src/filters.cpp

@@ -43,6 +43,17 @@ void JointExponentialFilterArray::initialize(const double &cutoff_frequency,
   initialized_ = true;
 }
 
+JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters)
+    : pid_parameters_(pid_parameters) // keep local copy of parameters since
+                                      // controller_toolbox::Pid::getGains is not const correct
+                                      // (i.e. can't be called in this->log_info)
+{
+  std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) {
+    pid.initPid(pid_parameters_.p, pid_parameters_.i, pid_parameters_.d, pid_parameters_.i_max,
+                pid_parameters_.i_min, pid_parameters_.antiwindup);
+  });
+}
+
 void JointPIDArray::compute(const value_array_t &command_target, const value_array_t &state,
                             const std::chrono::nanoseconds &dt, value_array_t &command) {
   std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable {
@@ -59,11 +70,14 @@ void JointPIDArray::compute(const value_array_t &command_target, const double *s
   });
 }
 
-void JointPIDArray::initialize(const PIDParameters &pid_parameters, const double &dt) {
-  std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) {
-    pid.initPid(pid_parameters.p * dt, pid_parameters.i * dt, pid_parameters.d * dt,
-                pid_parameters.i_max * dt, pid_parameters.i_min * dt, pid_parameters.antiwindup);
-  });
-  initialized_ = true;
-}
+void JointPIDArray::log_info() const {
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:");
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   p: %.1f", pid_parameters_.p);
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   i: %.1f", pid_parameters_.i);
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   d: %.1f", pid_parameters_.d);
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   i_max: %.1f", pid_parameters_.i_max);
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   i_min: %.1f", pid_parameters_.i_min);
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   antiwindup: %s",
+              pid_parameters_.antiwindup ? "true" : "false");
+};
 } // end of namespace lbr_fri_ros2

lbr_fri_ros2/src/interfaces/base_command.cpp

@@ -5,7 +5,7 @@ namespace lbr_fri_ros2 {
 BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters,
                                            const CommandGuardParameters &command_guard_parameters,
                                            const std::string &command_guard_variant)
-    : pid_parameters_(pid_parameters) {
+    : joint_position_pid_(pid_parameters) {
   command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant);
 };
 
@@ -18,13 +18,6 @@ void BaseCommandInterface::init_command(const_idl_state_t_ref state) {
 
 void BaseCommandInterface::log_info() const {
   command_guard_->log_info();
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*** Parameters:");
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.p: %.1f", pid_parameters_.p);
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.i: %.1f", pid_parameters_.i);
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.d: %.1f", pid_parameters_.d);
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.i_max: %.1f", pid_parameters_.i_max);
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.i_min: %.1f", pid_parameters_.i_min);
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*   pid.antiwindup: %s",
-              pid_parameters_.antiwindup ? "true" : "false");
+  joint_position_pid_.log_info();
 }
 } // namespace lbr_fri_ros2

lbr_fri_ros2/src/interfaces/position_command.cpp

@@ -41,9 +41,6 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command
   }
 
   // PID
-  if (!joint_position_pid_.is_initialized()) {
-    joint_position_pid_.initialize(pid_parameters_, state.sample_time);
-  }
   joint_position_pid_.compute(
       command_target_.joint_position, state.measured_joint_position,
       std::chrono::nanoseconds(static_cast<int64_t>(state.sample_time * 1.e9)),

lbr_fri_ros2/src/interfaces/torque_command.cpp

@@ -31,9 +31,6 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
   }
 
   // PID
-  if (!joint_position_pid_.is_initialized()) {
-    joint_position_pid_.initialize(pid_parameters_, state.sample_time);
-  }
   joint_position_pid_.compute(
       command_target_.joint_position, state.measured_joint_position,
       std::chrono::nanoseconds(static_cast<int64_t>(state.sample_time * 1.e9)),

lbr_fri_ros2/src/interfaces/wrench_command.cpp

@@ -29,9 +29,6 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
   }
 
   // PID
-  if (!joint_position_pid_.is_initialized()) {
-    joint_position_pid_.initialize(pid_parameters_, state.sample_time);
-  }
   joint_position_pid_.compute(
       command_target_.joint_position, state.measured_joint_position,
       std::chrono::nanoseconds(static_cast<int64_t>(state.sample_time * 1.e9)),

lbr_moveit_config/doc/lbr_moveit_config.rst

@@ -12,15 +12,15 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14
 
     sudo apt install ros-$ROS_DISTRO-moveit*
 
-#. Make sure the ``lbr_fri_ros2_stack`` is installed and **sourced**, see :ref:`Installation`.
+#. Make sure the ``lbr-stack`` is installed and **sourced**, see :ref:`Installation`.
 
 #. Launch the setup assistant
 
 .. code-block:: bash
 
     ros2 launch moveit_setup_assistant setup_assistant.launch.py
 
-#. .. dropdown:: ``Load Files``: E.g. ``lbr_fri_ros2_stack_ws/install/lbr_description/share/lbr_description/urdf/iiwa7/iiwa7.xacro``
+#. .. dropdown:: ``Load Files``: E.g. ``lbr-stack/install/lbr_description/share/lbr_description/urdf/iiwa7/iiwa7.xacro``
 
     .. thumbnail:: img/00_start_screen.png
 
@@ -94,7 +94,7 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14
 
 Update MoveIt Configuration 
 ---------------------------
-#. Make sure the ``lbr_fri_ros2_stack`` is installed and sourced, see :ref:`Installation`.
+#. Make sure the ``lbr-stack`` is installed and sourced, see :ref:`Installation`.
 
 #. Run the setup assistant for the existing configuration.
 

lbr_ros2_control/config/lbr_system_parameters.yaml

@@ -7,7 +7,7 @@ hardware:
   port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups
   remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection
   rt_prio: 80 # real-time priority for the control loop
-  pid_p: 10.0 # P gain for the joint position  (useful for asynchronous control)
+  pid_p: 0.1 # P gain for the joint position  (useful for asynchronous control)
   pid_i: 0.0 # I gain for the joint position command
   pid_d: 0.0 # D gain for the joint position command
   pid_i_max: 0.0 # max integral value for the joint position command