Add an example with a passive joint

doc/index.rst

@@ -234,7 +234,12 @@ gazebo_ros2_control_demos
 
 This package contains the contents for testing gazebo_ros2_control. It is running Gazebo Classic and some other ROS 2 nodes.
 
-There are some examples in the *Gazebo_ros2_control_demos* package. These examples allow to launch a cart in a 30 meter rail.
+There are some examples in the *Gazebo_ros2_control_demos* package.
+
+Cart on rail
+-----------------------------------------------------------
+
+These examples allow to launch a cart in a 30 meter rail.
 
 .. image:: img/cart.gif
   :alt: Cart
@@ -246,9 +251,6 @@ You can run some of the configuration running the following commands:
   ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py
   ros2 launch gazebo_ros2_control_demos cart_example_velocity.launch.py
   ros2 launch gazebo_ros2_control_demos cart_example_effort.launch.py
-  ros2 launch gazebo_ros2_control_demos diff_drive.launch.py
-  ros2 launch gazebo_ros2_control_demos tricycle_drive.launch.py
-
 
 When the Gazebo world is launched you can run some of the following commands to move the cart.
 
@@ -257,24 +259,58 @@ When the Gazebo world is launched you can run some of the following commands to
   ros2 run gazebo_ros2_control_demos example_position
   ros2 run gazebo_ros2_control_demos example_velocity
   ros2 run gazebo_ros2_control_demos example_effort
+
+Mobile robots
+-----------------------------------------------------------
+
+You can run some of the mobile robots running the following commands:
+
+.. code-block:: shell
+
+  ros2 launch gazebo_ros2_control_demos diff_drive.launch.py
+  ros2 launch gazebo_ros2_control_demos tricycle_drive.launch.py
+
+
+When the Gazebo world is launched you can run some of the following commands to move the robots.
+
+.. code-block:: shell
+
   ros2 run gazebo_ros2_control_demos example_diff_drive
   ros2 run gazebo_ros2_control_demos example_tricycle_drive
 
-
+Gripper
+-----------------------------------------------------------
 The following example shows parallel gripper with mimic joint:
 
 .. image:: img/gripper.gif
   :alt: Cart
 
-
 .. code-block:: shell
 
   ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example.launch.py
 
-
 Send example commands:
 
-
 .. code-block:: shell
 
   ros2 run gazebo_ros2_control_demos example_gripper
+
+Pendulum with passive joints
+-----------------------------------------------------------
+
+The following example shows a cart with a pendulum arm. This uses the effort command interface for the cart's
+degree of freedom on the rail, and the physics of the passive joint of the pendulum is solved correctly.
+
+.. code-block:: shell
+
+  ros2 launch gazebo_ros2_control_demos pendulum_example_effort.launch.py
+  ros2 run gazebo_ros2_control_demos example_effort
+
+.. note::
+
+  If the position or velocity command interface is used instead, the motion of the pendulum is not calculated correctly and does not move at all, see the following example. This also holds true if a mimicked joint with position or velocity interface is used.
+
+.. code-block:: shell
+
+  ros2 launch gazebo_ros2_control_demos pendulum_example_position.launch.py
+  ros2 run gazebo_ros2_control_demos example_position

gazebo_ros2_control_demos/launch/pendulum_example_effort.launch.py

@@ -0,0 +1,86 @@
+# Copyright 2023 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+    gazebo = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([os.path.join(
+                    get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
+             )
+
+    gazebo_ros2_control_demos_path = os.path.join(
+        get_package_share_directory('gazebo_ros2_control_demos'))
+
+    xacro_file = os.path.join(gazebo_ros2_control_demos_path,
+                              'urdf',
+                              'test_pendulum_effort.xacro.urdf')
+
+    doc = xacro.parse(open(xacro_file))
+    xacro.process_doc(doc)
+    params = {'robot_description': doc.toxml()}
+
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[params]
+    )
+
+    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+                        arguments=['-topic', 'robot_description',
+                                   '-entity', 'cartpole'],
+                        output='screen')
+
+    load_joint_state_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    load_joint_trajectory_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'effort_controller'],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=spawn_entity,
+                on_exit=[load_joint_state_controller],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_joint_state_controller,
+                on_exit=[load_joint_trajectory_controller],
+            )
+        ),
+        gazebo,
+        node_robot_state_publisher,
+        spawn_entity,
+    ])

gazebo_ros2_control_demos/launch/pendulum_example_position.launch.py

@@ -0,0 +1,87 @@
+# Copyright 2023 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+    gazebo = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([os.path.join(
+                    get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
+             )
+
+    gazebo_ros2_control_demos_path = os.path.join(
+        get_package_share_directory('gazebo_ros2_control_demos'))
+
+    xacro_file = os.path.join(gazebo_ros2_control_demos_path,
+                              'urdf',
+                              'test_pendulum_position.xacro.urdf')
+
+    doc = xacro.parse(open(xacro_file))
+    xacro.process_doc(doc)
+    params = {'robot_description': doc.toxml()}
+
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[params]
+    )
+
+    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+                        arguments=['-topic', 'robot_description',
+                                   '-entity', 'cartpole'],
+                        output='screen')
+
+    load_joint_state_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    load_joint_trajectory_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_trajectory_controller'],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=spawn_entity,
+                on_exit=[load_joint_state_controller],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_joint_state_controller,
+                on_exit=[load_joint_trajectory_controller],
+            )
+        ),
+        gazebo,
+        node_robot_state_publisher,
+        spawn_entity,
+    ])

gazebo_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf

@@ -0,0 +1,108 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="pendulum on cart">
+
+  <xacro:macro name="quadr_block" params="mass width height name">
+    <link name="${name}">
+      <collision>
+        <geometry>
+          <box size="${width} ${width} ${height}"/>
+        </geometry>
+        <origin xyz="0 0 0"/>
+      </collision>
+      <visual>
+        <geometry>
+          <box size="${width} ${width} ${height}"/>
+        </geometry>
+        <origin xyz="0 0 ${height/2}"/>
+        <material name="blue">
+          <color rgba="0 0 .8 1"/>
+        </material>
+      </visual>
+      <inertial>
+        <origin xyz="0 0 ${height/2}" rpy="0 0 0"/>
+        <mass value="${mass}"/>
+        <inertia
+          ixx="${mass / 12.0 * (width*width + height*height)}" ixy="0.0" ixz="0.0"
+          iyy="${mass / 12.0 * (height*height + width*width)}" iyz="0.0"
+          izz="${mass / 12.0 * (width*width + width*width)}"/>
+      </inertial>
+    </link>
+  </xacro:macro>
+
+  <link name="world"/>
+
+  <link name="slideBar">
+    <visual>
+      <geometry>
+        <box size="30 0.05 0.05"/>
+      </geometry>
+      <origin xyz="0 0 0"/>
+      <material name="green">
+        <color rgba="0 0.8 .8 1"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <xacro:quadr_block name="cart" mass="10" width="0.5" height="0.2"/>
+
+  <xacro:quadr_block name="pendulum" mass="1" width="0.2" height="1.0"/>
+
+  <joint name="world_to_base" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 1"/>
+    <parent link="world"/>
+    <child link="slideBar"/>
+  </joint>
+
+  <joint name="slider_to_cart" type="prismatic">
+    <axis xyz="1 0 0"/>
+    <origin xyz="0.0 0.0 0.0"/>
+    <parent link="slideBar"/>
+    <child link="cart"/>
+    <limit effort="1000.0" lower="-15" upper="15" velocity="30"/>
+    <dynamics damping="0.3" friction="0.0"/>
+  </joint>
+
+  <joint name="cart_to_pendulum" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin xyz="0.0 0.35 0.0" rpy="0 0.1 0"/>
+    <parent link="cart"/>
+    <child link="pendulum"/>
+    <limit effort="10000.0" lower="-100000" upper="100000" velocity="100000"/>
+    <dynamics damping="0.1" friction="0.0"/>
+  </joint>
+
+  <ros2_control name="GazeboSystem" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+    </hardware>
+    <joint name="slider_to_cart">
+      <command_interface name="effort">
+        <param name="min">-1000</param>
+        <param name="max">1000</param>
+      </command_interface>
+      <state_interface name="position">
+        <param name="initial_value">1.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="cart_to_pendulum">
+      <state_interface name="position">
+        <!-- this does not work if no command interface is set -->
+        <!-- <param name="initial_value">1.57</param> -->
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+  </ros2_control>
+
+  <gazebo>
+    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+      <parameters>$(find gazebo_ros2_control_demos)/config/cartpole_controller_effort.yaml</parameters>
+    </plugin>
+  </gazebo>
+</robot>