Merge branch 'master' into remove_mimic_legacy

.github/workflows/docker.yml

@@ -42,7 +42,7 @@ jobs:
             type=semver,pattern={{major}}.{{minor}}
 
       - name: Build and push Docker image
-        uses: docker/build-push-action@v5
+        uses: docker/build-push-action@v6
         with:
           context: .
           push: true
@@ -82,7 +82,7 @@ jobs:
             type=semver,pattern={{major}}.{{minor}}
 
       - name: Build and push Docker image
-        uses: docker/build-push-action@v5
+        uses: docker/build-push-action@v6
         with:
           context: .
           push: true

.pre-commit-config.yaml

@@ -62,7 +62,7 @@ repos:
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v18.1.4
+    rev: v18.1.5
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -125,14 +125,14 @@ repos:
   # Spellcheck in comments and docs
   # skipping of *.svg files is not working...
   - repo: https://github.com/codespell-project/codespell
-    rev: v2.2.6
+    rev: v2.3.0
     hooks:
       - id: codespell
         args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel']
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.28.2
+    rev: 0.28.4
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_manager/src/controller_manager.cpp

@@ -2147,6 +2147,17 @@ controller_interface::return_type ControllerManager::update(
         run_controller_at_cm_rate ? period
                                   : rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
 
+      if (
+        *loaded_controller.next_update_cycle_time ==
+        rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
+      {
+        // it is zero after activation
+        RCLCPP_DEBUG(
+          get_logger(), "Setting next_update_cycle_time to %fs for the controller : %s",
+          time.seconds(), loaded_controller.info.name.c_str());
+        *loaded_controller.next_update_cycle_time = time;
+      }
+
       bool controller_go =
         (time ==
          rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
@@ -2182,12 +2193,6 @@ controller_interface::return_type ControllerManager::update(
           controller_ret = controller_interface::return_type::ERROR;
         }
 
-        if (
-          *loaded_controller.next_update_cycle_time ==
-          rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
-        {
-          *loaded_controller.next_update_cycle_time = time;
-        }
         *loaded_controller.next_update_cycle_time += controller_period;
 
         if (controller_ret != controller_interface::return_type::OK)

controller_manager/test/test_controller/test_controller.cpp

@@ -62,7 +62,7 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con
 controller_interface::return_type TestController::update(
   const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
 {
-  update_period_ = period.seconds();
+  update_period_ = period;
   ++internal_counter;
 
   // set value to hardware to produce and test different behaviors there

controller_manager/test/test_controller/test_controller.hpp

@@ -80,7 +80,7 @@ class TestController : public controller_interface::ControllerInterface
   // enables external setting of values to command interfaces - used for simulation of hardware
   // errors
   double set_first_command_interface_value_to;
-  double update_period_ = 0;
+  rclcpp::Duration update_period_ = rclcpp::Duration::from_seconds(0.);
 };
 
 }  // namespace test_controller

controller_manager/test/test_controller_manager.cpp

@@ -374,9 +374,12 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
       controller_interface::return_type::OK,
       cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
     // In case of a non perfect divisor, the update period should respect the rule
-    // [controller_update_rate, 2*controller_update_rate)
-    ASSERT_GE(test_controller->update_period_, 1.0 / cm_update_rate);
-    ASSERT_LT(test_controller->update_period_, 2.0 / cm_update_rate);
+    // [cm_update_rate, 2*cm_update_rate)
+    EXPECT_THAT(
+      test_controller->update_period_,
+      testing::AllOf(
+        testing::Ge(rclcpp::Duration::from_seconds(1.0 / cm_update_rate)),
+        testing::Lt(rclcpp::Duration::from_seconds(2.0 / cm_update_rate))));
     loop_rate.sleep();
   }
   // if we do 2 times of the controller_manager update rate, the internal counter should be
@@ -445,6 +448,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
   EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
 
   // Start controller, will take effect at the end of the update function
+  time_ = test_controller->get_node()->now();  // set to something nonzero
   const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
   std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
   std::vector<std::string> stop_controllers = {};
@@ -472,6 +476,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
   const auto controller_update_rate = test_controller->get_update_rate();
 
   const auto initial_counter = test_controller->internal_counter;
+  // don't start with zero to check if the period is correct if controller is activated anytime
   rclcpp::Time time = time_;
   for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
   {
@@ -480,8 +485,12 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
       cm_->update(time, rclcpp::Duration::from_seconds(0.01)));
     // In case of a non perfect divisor, the update period should respect the rule
     // [controller_update_rate, 2*controller_update_rate)
-    ASSERT_GE(test_controller->update_period_, 1.0 / controller_update_rate);
-    ASSERT_LT(test_controller->update_period_, 2.0 / controller_update_rate);
+    EXPECT_THAT(
+      test_controller->update_period_,
+      testing::AllOf(
+        testing::Ge(rclcpp::Duration::from_seconds(1.0 / controller_update_rate)),
+        testing::Lt(rclcpp::Duration::from_seconds(2.0 / controller_update_rate))))
+      << "update_counter: " << update_counter;
 
     time += rclcpp::Duration::from_seconds(0.01);
     if (update_counter % cm_update_rate == 0)

controller_manager/test/test_spawner_unspawner.cpp

@@ -315,7 +315,8 @@ TEST_F(TestLoadController, unload_on_kill)
   // Launch spawner with unload on kill
   // timeout command will kill it after the specified time with signal SIGINT
   std::stringstream ss;
-  ss << "timeout --signal=INT 5 " << "ros2 run controller_manager spawner "
+  ss << "timeout --signal=INT 5 "
+     << "ros2 run controller_manager spawner "
      << "ctrl_3 -c test_controller_manager -t "
      << std::string(test_controller::TEST_CONTROLLER_CLASS_NAME) << " --unload-on-kill";
 

doc/index.rst

@@ -16,12 +16,6 @@ API Documentation
 
 API documentation is parsed by doxygen and can be found `here <../../api/index.html>`_
 
-=========
-Features
-=========
-
-* :ref:`Command Line Interface (CLI) <ros2controlcli_userdoc>`
-
 ========
 Concepts
 ========
@@ -31,5 +25,6 @@ Concepts
 
    Controller Manager <../controller_manager/doc/userdoc.rst>
    Controller Chaining / Cascade Control <../controller_manager/doc/controller_chaining.rst>
+   Joint Kinematics <../hardware_interface/doc/joints_userdoc.rst>
    Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst>
    Mock Components <../hardware_interface/doc/mock_components_userdoc.rst>

hardware_interface/doc/joints_userdoc.rst

@@ -0,0 +1,132 @@
+:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/joints_userdoc.rst
+
+.. _joints_userdoc:
+
+
+Joint Kinematics for ros2_control
+---------------------------------------------------------
+
+This page should give an overview of the joint kinematics in the context of ros2_control. It is intended to give a brief introduction to the topic and to explain the current implementation in ros2_control.
+
+Nomenclature
+############
+
+Degrees of Freedom (DoF)
+  From `wikipedia <https://en.wikipedia.org/wiki/Degrees_of_freedom_(mechanics)>`__:
+
+    In physics, the degrees of freedom (DoF) of a mechanical system is the number of independent parameters that define its configuration or state.
+
+Joint
+  A joint is a connection between two links. In the ROS ecosystem, three types are more typical: Revolute (A hinge joint with position limits), Continuous (A continuous hinge without any position limits) or Prismatic (A sliding joint that moves along the axis).
+
+  In general, a joint can be actuated or non-actuated, also called passive. Passive joints are joints that do not have their own actuation mechanism but instead allow movement by external forces or by being passively moved by other joints. A passive joint can have a DoF of one, such as a pendulum, or it can be part of a parallel kinematic mechanism with zero DoF.
+
+Serial Kinematics
+  Serial kinematics refers to the arrangement of joints in a robotic manipulator where each joint is independent of the others, and the number of joints is equal to the DoF of the kinematic chain.
+
+  A typical example is an industrial robot with six revolute joints, having 6-DoF. Each joint can be actuated independently, and the end-effector can be moved to any position and orientation in the workspace.
+
+Kinematic Loops
+  On the other hand, kinematic loops, also known as closed-loop mechanisms, involve several joints that are connected in a kinematic chain and being actuated together. This means that the joints are coupled and cannot be moved independently: In general, the number of DoFs is smaller than the number of joints.
+  This structure is typical for parallel kinematic mechanisms, where the end-effector is connected to the base by several kinematic chains.
+
+  An example is the four-bar linkage, which consists of four links and four joints. It can have one or two actuators and consequently one or two DoFs, despite having four joints. Furthermore, we can say that we have one (two) actuated joint and three (two) passive joints, which must satisfy the kinematic constraints of the mechanism.
+
+URDF
+#############
+
+URDF is the default format to describe robot kinematics in ROS. However, only serial kinematic chains are supported, except for the so-called mimic joints. See the `URDF specification <http://wiki.ros.org/urdf/XML/joint>`__ for more details.
+
+Mimic joints can be defined in the following way in the URDF
+
+  .. code-block:: xml
+
+    <joint name="right_finger_joint" type="prismatic">
+      <axis xyz="0 1 0"/>
+      <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
+      <parent link="base"/>
+      <child link="finger_right"/>
+      <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
+    </joint>
+    <joint name="left_finger_joint" type="prismatic">
+      <mimic joint="right_finger_joint" multiplier="1" offset="0"/>
+      <axis xyz="0 1 0"/>
+      <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
+      <parent link="base"/>
+      <child link="finger_left"/>
+      <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
+    </joint>
+
+Mimic joints are an abstraction of the real world. For example, they can be used to describe
+
+* simple closed-loop kinematics with linear dependencies of the joint positions and velocities
+* links connected with belts, like belt and pulley systems or telescope arms
+* a simplified model of passive joints, e.g. a pendulum at the end-effector always pointing downwards
+* abstract complex groups of actuated joints, where several joints are directly controlled by low-level control loops and move synchronously. Without giving a real-world example, this could be several motors with their individual power electronics but commanded with the same setpoint.
+
+Mimic joints defined in the URDF are parsed from the resource manager and stored in a class variable of type ``HardwareInfo``, which can be accessed by the hardware components. The mimic joints must not have command interfaces but can have state interfaces.
+
+.. code-block:: xml
+
+  <ros2_control>
+    <joint name="right_finger_joint">
+      <command_interface name="effort"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="left_finger_joint">
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+  </ros2_control>
+
+From the officially released packages, the following packages are already using this information:
+
+* :ref:`mock_components (generic system) <mock_components_userdoc>`
+* :ref:`gazebo_ros2_control <gazebo_ros2_control>`
+* :ref:`gz_ros2_control <gz_ros2_control>`
+
+As the URDF specifies only the kinematics, the mimic tag has to be independent of the hardware interface type used in ros2_control. This means that we interpret this info in the following way:
+
+* **position = multiplier * other_joint_position + offset**
+* **velocity = multiplier * other_joint_velocity**
+
+If someone wants to deactivate the mimic joint behavior for whatever reason without changing the URDF, it can be done by setting the attribute ``mimic=false`` of the joint tag in the ``<ros2_control>`` section of the XML.
+
+.. code-block:: xml
+
+  <joint name="left_finger_joint" mimic="false">
+    <state_interface name="position"/>
+    <state_interface name="velocity"/>
+    <state_interface name="effort"/>
+  </joint>
+
+Transmission Interface
+#######################
+Mechanical transmissions transform effort/flow variables such that their product (power) remains constant. Effort variables for linear and rotational domains are force and torque; while the flow variables are respectively linear velocity and angular velocity.
+
+In robotics it is customary to place transmissions between actuators and joints. This interface adheres to this naming to identify the input and output spaces of the transformation. The provided interfaces allow bidirectional mappings between actuator and joint spaces for effort, velocity and position. Position is not a power variable, but the mappings can be implemented using the velocity map plus an integration constant representing the offset between actuator and joint zeros.
+
+The ``transmission_interface`` provides a base class and some implementations for plugins, which can be integrated and loaded by custom hardware components. They are not automatically loaded by any hardware component or the gazebo plugins, each hardware component is responsible for loading the appropriate transmission interface to map the actuator readings to joint readings.
+
+Currently the following implementations are available:
+
+* ``SimpleTransmission``: A simple transmission with a constant reduction ratio and no additional dynamics.
+* ``DifferentialTransmission``: A differential transmission with two actuators and two joints.
+* ``FourBarLinkageTransmission``: A four-bar-linkage transmission with two actuators and two joints.
+
+For more information, see :ref:`example_8 <ros2_control_demos_example_8_userdoc>` or the `transmission_interface <http://docs.ros.org/en/{DISTRO}/p/transmission_interface/index.html>`__ documentation.
+
+Simulating Closed-Loop Kinematic Chains
+#######################################
+Depending on the simulation plugin, different approaches can be used to simulate closed-loop kinematic chains. The following list gives an overview of the available simulation plugins and their capabilities:
+
+gazebo_ros2_control:
+  * mimic joints
+  * closed-loop kinematics are supported with ``<gazebo>`` tags in the URDF, see, e.g., `here <http://classic.gazebosim.org/tutorials?tut=kinematic_loop>`__.
+
+gz_ros2_control:
+  * mimic joints
+  * closed-loop kinematics are not directly supported yet, but can be implemented by using a ``DetachableJoint`` via custom plugins. Follow `this issue <https://github.com/gazebosim/gz-physics/issues/25>`__ for updates on this topic.

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -131,7 +131,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    * \note All starting and stopping interface keys are passed to all components, so the function
    * should return return_type::OK by default when given interface keys not relevant for this
    * component. \param[in] start_interfaces vector of string identifiers for the command interfaces
-   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs
+   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
    * stopping. \return return_type::OK if the new command interface combination can be prepared, or
    * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
    */
@@ -150,7 +150,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    * \note All starting and stopping interface keys are passed to all components, so the function
    * should return return_type::OK by default when given interface keys not relevant for this
    * component. \param[in] start_interfaces vector of string identifiers for the command interfaces
-   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs
+   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
    * stopping. \return return_type::OK if the new command interface combination can be switched to,
    * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
    */

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -353,7 +353,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * \note it is assumed that `prepare_command_mode_switch` is called just before this method
    * with the same input arguments.
    * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
-   * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
+   * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping.
    * \return true if switch is performed, false if a component rejects switching.
    */
   bool perform_command_mode_switch(

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -132,7 +132,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    * \note All starting and stopping interface keys are passed to all components, so the function
    * should return return_type::OK by default when given interface keys not relevant for this
    * component. \param[in] start_interfaces vector of string identifiers for the command interfaces
-   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs
+   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
    * stopping. \return return_type::OK if the new command interface combination can be prepared, or
    * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
    */
@@ -151,7 +151,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    * \note All starting and stopping interface keys are passed to all components, so the function
    * should return return_type::OK by default when given interface keys not relevant for this
    * component. \param[in] start_interfaces vector of string identifiers for the command interfaces
-   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs
+   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
    * stopping. \return return_type::OK if the new command interface combination can be switched to,
    * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
    */