diff --git a/doc/index.rst b/doc/index.rst index e24a2308..26f85f9a 100644 --- 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,40 @@ 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 +<<<<<<< HEAD The following example shows parallel gripper with mimic joint: +======= +Gripper +----------------------------------------------------------- +The following example shows a parallel gripper with a mimic joint: + +.. image:: img/gripper.gif + :alt: Cart + +.. code-block:: shell + + ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example_position.launch.py +>>>>>>> 7d5ec5d (Add an example with a passive joint (#172)) .. image:: img/gripper.gif :alt: Cart @@ -264,3 +295,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 `__. 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 new file mode 100644 index 00000000..059028ee --- /dev/null +++ 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 new file mode 100644 index 00000000..4932b4f3 --- /dev/null +++ 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 new file mode 100644 index 00000000..bd3c18ff --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + -1000 + 1000 + + + 1.0 + + + + + + + + + + + + + + + + + $(find gazebo_ros2_control_demos)/config/cart_controller_effort.yaml + + + diff --git a/gazebo_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf new file mode 100644 index 00000000..185cd26b --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + -15 + 15 + + + 1.0 + + + + + + + + + + + + + + + + + $(find gazebo_ros2_control_demos)/config/cart_controller.yaml + + +