Add an example with a passive joint (backport #172) (#306)

* Add an example with a passive joint (#172) Co-authored-by: Alejandro Hernández Cordero <[email protected]> (cherry picked from commit 7d5ec5d) # Conflicts: # doc/index.rst * Fixed docs Signed-off-by: Alejandro Hernández Cordero <[email protected]> --------- Signed-off-by: Alejandro Hernández Cordero <[email protected]> Co-authored-by: Christoph Fröhlich <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]>
ros-controls · Apr 30, 2024 · 70d9984 · 70d9984
1 parent fe829ce
commit 70d9984
Show file tree

Hide file tree

Showing 5 changed files with 440 additions and 5 deletions.
diff --git a/doc/index.rst b/doc/index.rst
@@ -220,7 +220,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
@@ -232,9 +237,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.
 
@@ -243,11 +245,35 @@ 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 a parallel gripper with a mimic joint:
+
+.. image:: img/gripper.gif
+  :alt: Cart
+
+.. code-block:: shell
 
-The following example shows parallel gripper with mimic joint:
+  ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example_position.launch.py
 
 .. image:: img/gripper.gif
   :alt: Cart
@@ -264,3 +290,23 @@ 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 command interface is used instead, the motion of the pendulum is not calculated correctly and does not move at all, see this `PR <https://github.com/ros-controls/gazebo_ros2_control/issues/240>`__. This also holds true if a mimicked joint with position interface is used. To demonstrate this, run
+
+  .. code-block:: shell
+
+    ros2 launch gazebo_ros2_control_demos pendulum_example_position.launch.py
+    ros2 run gazebo_ros2_control_demos example_position
diff --git a/gazebo_ros2_control_demos/launch/pendulum_example_effort.launch.py b/gazebo_ros2_control_demos/launch/pendulum_example_effort.launch.py
@@ -0,0 +1,86 @@
+# Copyright 2024 ros2_control Development Team
+#
+# 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,
+    ])
diff --git a/gazebo_ros2_control_demos/launch/pendulum_example_position.launch.py b/gazebo_ros2_control_demos/launch/pendulum_example_position.launch.py
@@ -0,0 +1,87 @@
+# Copyright 2024 ros2_control Development Team
+#
+# 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,
+    ])
diff --git a/gazebo_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf b/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/cart_controller_effort.yaml</parameters>
+    </plugin>
+  </gazebo>
+</robot>