From 6dd8e49019dd3f1c070f8a4f550953c13ef9b356 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 25 Apr 2024 15:30:34 +0000 Subject: [PATCH] Add cart-pole demo --- doc/index.rst | 60 ++++++++-- .../launch/pendulum_example_effort.launch.py | 97 ++++++++++++++++ .../pendulum_example_position.launch.py | 97 ++++++++++++++++ .../urdf/test_pendulum_effort.xacro.urdf | 109 ++++++++++++++++++ .../urdf/test_pendulum_position.xacro.urdf | 105 +++++++++++++++++ 5 files changed, 457 insertions(+), 11 deletions(-) create mode 100644 gz_ros2_control_demos/launch/pendulum_example_effort.launch.py create mode 100644 gz_ros2_control_demos/launch/pendulum_example_position.launch.py create mode 100644 gz_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf create mode 100644 gz_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf diff --git a/doc/index.rst b/doc/index.rst index 62c7a7de..860bff4d 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -10,12 +10,6 @@ This is a ROS 2 package for integrating the *ros2_control* controller architectu This package provides a Gazebo-Sim system plugin which instantiates a *ros2_control* controller manager and connects it to a Gazebo model. -.. image:: img/gz_ros2_control.gif - :alt: Cart - -.. image:: img/diff_drive.gif - :alt: DiffBot - Usage ====== @@ -212,7 +206,15 @@ The following is a basic configuration of the controllers: gz_ros2_control_demos ========================================== -There are some examples in the *gz_ros2_control_demos* package. These examples allow to launch a cart in a 30 meter rail. +There are some examples in the *gz_ros2_control_demos* package. + +Cart on rail +----------------------------------------------------------- + +These examples allow to launch a cart in a 30 meter rail. + +.. image:: img/gz_ros2_control.gif + :alt: Cart You can run some of the example configurations by running the following commands: @@ -221,8 +223,6 @@ You can run some of the example configurations by running the following commands ros2 launch gz_ros2_control_demos cart_example_position.launch.py ros2 launch gz_ros2_control_demos cart_example_velocity.launch.py ros2 launch gz_ros2_control_demos cart_example_effort.launch.py - ros2 launch gz_ros2_control_demos diff_drive_example.launch.py - ros2 launch gz_ros2_control_demos tricycle_drive_example.launch.py When the Gazebo world is launched, you can run some of the following commands to move the cart. @@ -231,10 +231,31 @@ When the Gazebo world is launched, you can run some of the following commands to ros2 run gz_ros2_control_demos example_position ros2 run gz_ros2_control_demos example_velocity ros2 run gz_ros2_control_demos example_effort + +Mobile robots +----------------------------------------------------------- + +.. image:: img/diff_drive.gif + :alt: DiffBot + +You can run some of the mobile robots running the following commands: + +.. code-block:: shell + + ros2 launch gz_ros2_control_demos diff_drive_example.launch.py + ros2 launch gz_ros2_control_demos tricycle_drive_example.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 gz_ros2_control_demos example_diff_drive ros2 run gz_ros2_control_demos example_tricycle_drive -The following example shows parallel gripper with mimic joint: +Gripper +----------------------------------------------------------- + +The following example shows a parallel gripper with a mimic joint: .. code-block:: shell @@ -254,7 +275,24 @@ instead. Send example commands: - .. code-block:: shell ros2 run gz_ros2_control_demos example_gripper + + +Pendulum with passive joints (cart-pole) +----------------------------------------------------------- + +The following example shows a cart with a pendulum arm: + +.. code-block:: shell + + ros2 launch gz_ros2_control_demos pendulum_example_effort.launch.py + ros2 run gz_ros2_control_demos example_effort + +This uses the effort command interface for the cart's degree of freedom on the rail. To demonstrate that the physics of the passive joint of the pendulum is solved correctly even with the position command interface, run + +.. code-block:: shell + + ros2 launch gz_ros2_control_demos pendulum_example_position.launch.py + ros2 run gz_ros2_control_demos example_position diff --git a/gz_ros2_control_demos/launch/pendulum_example_effort.launch.py b/gz_ros2_control_demos/launch/pendulum_example_effort.launch.py new file mode 100644 index 00000000..a9a54f8e --- /dev/null +++ b/gz_ros2_control_demos/launch/pendulum_example_effort.launch.py @@ -0,0 +1,97 @@ +# 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Launch Arguments + use_sim_time = LaunchConfiguration('use_sim_time', default=True) + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("gz_ros2_control_demos"), + "urdf", "test_pendulum_effort.xacro.urdf"] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + + gz_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=["-topic", "robot_description", + "-name", "cart", "-allow_renaming", "true"], + ) + + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + load_joint_effort_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', + '--set-state', 'active', 'effort_controller'], + output='screen' + ) + + return LaunchDescription([ + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py'])]), + launch_arguments=[('gz_args', [' -r -v 3 empty.sdf'])]), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gz_spawn_entity, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_joint_effort_controller], + ) + ), + node_robot_state_publisher, + gz_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) diff --git a/gz_ros2_control_demos/launch/pendulum_example_position.launch.py b/gz_ros2_control_demos/launch/pendulum_example_position.launch.py new file mode 100644 index 00000000..a95af408 --- /dev/null +++ b/gz_ros2_control_demos/launch/pendulum_example_position.launch.py @@ -0,0 +1,97 @@ +# 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Launch Arguments + use_sim_time = LaunchConfiguration('use_sim_time', default=True) + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("gz_ros2_control_demos"), + "urdf", "test_pendulum_position.xacro.urdf"] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + + gz_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=["-topic", "robot_description", + "-name", "cart", "-allow_renaming", "true"], + ) + + load_joint_state_broadcaster = 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([ + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py'])]), + launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gz_spawn_entity, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_joint_trajectory_controller], + ) + ), + node_robot_state_publisher, + gz_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) diff --git a/gz_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf b/gz_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf new file mode 100644 index 00000000..6e2ea830 --- /dev/null +++ b/gz_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf @@ -0,0 +1,109 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + -1000 + 1000 + + + 1.0 + + + + + + + + + + + + + + + + + + $(find gz_ros2_control_demos)/config/cart_controller_effort.yaml + + + diff --git a/gz_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf b/gz_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf new file mode 100644 index 00000000..44fa9744 --- /dev/null +++ b/gz_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + -15 + 15 + + + 1.0 + + + + + + + + + + + + + + + + + + $(find gz_ros2_control_demos)/config/cart_controller_position.yaml + + +