From b75aee3373b6c0280fa208304b7398c6031263cd Mon Sep 17 00:00:00 2001 From: chameau5050 <54971185+chameau5050@users.noreply.github.com> Date: Mon, 6 May 2024 17:58:06 -0400 Subject: [PATCH] Change initial pose of pendulum --- doc/index.rst | 70 +++++++ gazebo_ros2_control/CMakeLists.txt | 3 + .../gazebo_ros2_control/gazebo_system.hpp | 8 + .../gazebo_system_interface.hpp | 2 + gazebo_ros2_control/package.xml | 3 +- gazebo_ros2_control/src/gazebo_system.cpp | 196 +++++++++++++++--- gazebo_ros2_control_demos/CMakeLists.txt | 7 + .../config/cart_controller_position.yaml | 14 ++ .../examples/example_position_pid.cpp | 58 ++++++ ...rtical_cart_example_position_pid.launch.py | 85 ++++++++ ...rtical_cart_example_velocity_pid.launch.py | 85 ++++++++ ...test_vertical_cart_position_pid.xacro.urdf | 77 +++++++ ...test_vertical_cart_velocity_pid.xacro.urdf | 77 +++++++ 13 files changed, 660 insertions(+), 25 deletions(-) create mode 100644 gazebo_ros2_control_demos/config/cart_controller_position.yaml create mode 100644 gazebo_ros2_control_demos/examples/example_position_pid.cpp create mode 100644 gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py create mode 100644 gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py create mode 100644 gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf create mode 100644 gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf diff --git a/doc/index.rst b/doc/index.rst index 21d9e9cc..297ce3e3 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -127,6 +127,58 @@ The mimic joint must not have command interfaces configured in the ```` tag, the mimic joint will use the position interface of the gazebo classic physic engine to follow the position of the mimicked joint. +Using PID control joints +----------------------------------------------------------- + +To use PID control joints in gazebo_ros2_control, you should define their parameters inside the ```` tag +within the ```` tag. These PID joints can be controlled either in position or velocity. + +- To control a joint with velocity PID, simply set its ``command_interface`` to ``velocity_PID``. +- To control a joint with position PID, set its ``command_interface`` to ``position_PID``. + +.. note:: + You cannot have both command interfaces set to position and position_PID for the same joint. The same restriction applies to velocity (and velocity_PID). + +To create a system with one joint that can be controlled using both position_PID and velocity_PID, follow this example: + +.. code-block:: xml + + + + gazebo_ros2_control/GazeboSystem + + + + 10 + 1 + 2 + 10000 + + 10 + 5 + 2 + 10000 + + + + + + 1.0 + + + + + + +Where the parameters are as follows: + +- ``pos_kp``: Proportional gain +- ``pos_ki``: Integral gain +- ``pos_kd``: Derivative gain +- ``pos_max_integral_error``: Maximum summation of the error + +The same definitions apply to the ``vel_*`` parameters. + Add the gazebo_ros2_control plugin ========================================== @@ -242,6 +294,7 @@ When the Gazebo world is launched you can run some of the following commands to .. code-block:: shell ros2 run gazebo_ros2_control_demos example_position + ros2 run gazebo_ros2_control_demos example_position_pid ros2 run gazebo_ros2_control_demos example_velocity ros2 run gazebo_ros2_control_demos example_effort @@ -311,3 +364,20 @@ degree of freedom on the rail, and the physics of the passive joint of the pendu ros2 launch gazebo_ros2_control_demos pendulum_example_position.launch.py ros2 run gazebo_ros2_control_demos example_position + + + +PID control joints +----------------------------------------------------------- + +The following examples shows a vertical cart control by a PID joint using position and velocity cmd. + +.. code-block:: shell + + ros2 launch gazebo_ros2_control_demos vertical_cart_example_position_pid.launch.py + ros2 launch gazebo_ros2_control_demos vertical_cart_example_velocity_pid.launch.py + +.. code-block:: shell + + ros2 run gazebo_ros2_control_demos example_position_pid + ros2 run gazebo_ros2_control_demos example_velocity diff --git a/gazebo_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt index cfba1298..01a40ee2 100644 --- a/gazebo_ros2_control/CMakeLists.txt +++ b/gazebo_ros2_control/CMakeLists.txt @@ -4,6 +4,7 @@ project(gazebo_ros2_control) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) find_package(controller_manager REQUIRED) +find_package(control_toolbox REQUIRED) find_package(gazebo_dev REQUIRED) find_package(gazebo_ros REQUIRED) find_package(hardware_interface REQUIRED) @@ -33,6 +34,7 @@ add_library(${PROJECT_NAME} SHARED ament_target_dependencies(${PROJECT_NAME} angles controller_manager + control_toolbox gazebo_dev gazebo_ros hardware_interface @@ -46,6 +48,7 @@ add_library(gazebo_hardware_plugins SHARED ) ament_target_dependencies(gazebo_hardware_plugins angles + control_toolbox gazebo_dev hardware_interface rclcpp diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp index 8230a4ec..8473b9b6 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp @@ -16,12 +16,16 @@ #ifndef GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_ #define GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_ +#define VELOCITY_PID_PARAMS_PREFIX "vel_" +#define POSITION_PID_PARAMS_PREFIX "pos_" + #include #include #include #include "angles/angles.h" +#include "control_toolbox/pid.hpp" #include "gazebo_ros2_control/gazebo_system_interface.hpp" #include "std_msgs/msg/bool.hpp" @@ -86,6 +90,10 @@ class GazeboSystem : public GazeboSystemInterface const hardware_interface::HardwareInfo & hardware_info, gazebo::physics::ModelPtr parent_model); + control_toolbox::Pid extractPID( + std::string prefix, + hardware_interface::ComponentInfo joint_info); + /// \brief Private data class std::unique_ptr dataPtr; }; diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp index 8a4cf1fb..8bded30c 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp @@ -79,6 +79,8 @@ class GazeboSystemInterface POSITION = (1 << 0), VELOCITY = (1 << 1), EFFORT = (1 << 2), + VELOCITY_PID = (1 << 3), + POSITION_PID = (1 << 4), }; typedef SafeEnum ControlMethod; diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml index 256a7654..57572581 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -19,9 +19,10 @@ ament_cmake angles + controller_manager + control_toolbox gazebo_dev gazebo_ros - controller_manager hardware_interface pluginlib rclcpp diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 1a573cab..a96a395b 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -19,6 +19,7 @@ #include #include +#include "control_toolbox/pid.hpp" #include "gazebo_ros2_control/gazebo_system.hpp" #include "gazebo/sensors/ImuSensor.hh" #include "gazebo/sensors/ForceTorqueSensor.hh" @@ -77,6 +78,18 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief vector with the current cmd joint effort std::vector joint_effort_cmd_; + /// \brief Joint velocity PID utils + std::vector vel_pid; + + /// \brief Joint position PID utils + std::vector pos_pid; + + // \brief control flag + std::vector is_pos_pid; + + // \brief control flag + std::vector is_vel_pid; + /// \brief handles to the imus from within Gazebo std::vector sim_imu_sensors_; @@ -157,6 +170,67 @@ bool GazeboSystem::initSim( return true; } +control_toolbox::Pid GazeboSystem::extractPID( + std::string prefix, + hardware_interface::ComponentInfo joint_info) +{ + double kp; + double ki; + double kd; + double max_integral_error; + double min_integral_error; + bool antiwindup; + + if (joint_info.parameters.find(prefix + "kp") != joint_info.parameters.end()) { + kp = std::stod(joint_info.parameters.at(prefix + "kp")); + } else { + kp = 0.0; + } + + if (joint_info.parameters.find(prefix + "ki") != joint_info.parameters.end()) { + ki = std::stod(joint_info.parameters.at(prefix + "ki")); + } else { + ki = 0.0; + } + + if (joint_info.parameters.find(prefix + "kd") != joint_info.parameters.end()) { + kd = std::stod(joint_info.parameters.at(prefix + "kd")); + } else { + kd = 0.0; + } + + if (joint_info.parameters.find(prefix + "max_integral_error") != joint_info.parameters.end()) { + max_integral_error = std::stod(joint_info.parameters.at(prefix + "max_integral_error")); + } else { + max_integral_error = std::numeric_limits::max(); + } + + if (joint_info.parameters.find(prefix + "min_integral_error") != joint_info.parameters.end()) { + min_integral_error = std::stod(joint_info.parameters.at(prefix + "min_integral_error")); + } else { + min_integral_error = std::numeric_limits::min(); + } + + if (joint_info.parameters.find(prefix + "antiwindup") != joint_info.parameters.end()) { + if (joint_info.parameters.at(prefix + "antiwindup") == "true" || + joint_info.parameters.at(prefix + "antiwindup") == "True") + { + antiwindup = true; + } + } else { + antiwindup = false; + } + + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), + "Setting kp = " << kp << "\t" + << " ki = " << ki << "\t" + << " kd = " << kd << "\t" + << " max_integral_error = " << max_integral_error); + + return control_toolbox::Pid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup); +} + void GazeboSystem::registerJoints( const hardware_interface::HardwareInfo & hardware_info, gazebo::physics::ModelPtr parent_model) @@ -172,6 +246,10 @@ void GazeboSystem::registerJoints( this->dataPtr->joint_position_cmd_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_velocity_cmd_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_effort_cmd_.resize(this->dataPtr->n_dof_); + this->dataPtr->vel_pid.resize(this->dataPtr->n_dof_); + this->dataPtr->pos_pid.resize(this->dataPtr->n_dof_); + this->dataPtr->is_pos_pid.resize(this->dataPtr->n_dof_); + this->dataPtr->is_vel_pid.resize(this->dataPtr->n_dof_); for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) { auto & joint_info = hardware_info.joints[j]; @@ -266,9 +344,32 @@ void GazeboSystem::registerJoints( RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:"); // register the command handles + bool has_already_registered_vel = false; + bool has_already_registered_pos = false; + for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) { - if (joint_info.command_interfaces[i].name == "position") { - RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); + if (joint_info.command_interfaces[i].name == "position" || + joint_info.command_interfaces[i].name == "position_pid") + { + if (has_already_registered_pos) { + RCLCPP_ERROR_STREAM( + this->nh_->get_logger(), + "can't have position and position" + << "pid command_interface at same time !!!"); + continue; + } + has_already_registered_pos = true; + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "\t\t " + << joint_info.command_interfaces[i].name); + + if (joint_info.command_interfaces[i].name == "position_pid") { + this->dataPtr->is_pos_pid[j] = true; + this->dataPtr->pos_pid[j] = this->extractPID(POSITION_PID_PARAMS_PREFIX, joint_info); + } else { + this->dataPtr->is_pos_pid[j] = false; + } + this->dataPtr->command_interfaces_.emplace_back( joint_name, hardware_interface::HW_IF_POSITION, @@ -281,8 +382,25 @@ void GazeboSystem::registerJoints( if (!std::isnan(initial_position)) { this->dataPtr->sim_joints_[j]->SetPosition(0, initial_position, true); } - if (joint_info.command_interfaces[i].name == "velocity") { - RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); + if (joint_info.command_interfaces[i].name == "velocity" || + joint_info.command_interfaces[i].name == "velocity_pid") + { + if (has_already_registered_vel) { + RCLCPP_ERROR_STREAM( + this->nh_->get_logger(), "can't have velocity and velocity_pid " + << "command_interface at same time !!!"); + continue; + } + has_already_registered_vel = true; + + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "\t\t " + << joint_info.command_interfaces[i].name); + + if (joint_info.command_interfaces[i].name == "velocity_pid") { + this->dataPtr->is_vel_pid[j] = true; + this->dataPtr->vel_pid[j] = this->extractPID(VELOCITY_PID_PARAMS_PREFIX, joint_info); + } this->dataPtr->command_interfaces_.emplace_back( joint_name, hardware_interface::HW_IF_VELOCITY, @@ -500,34 +618,46 @@ GazeboSystem::perform_command_mode_switch( for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { for (const std::string & interface_name : stop_interfaces) { // Clear joint control method bits corresponding to stop interfaces - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + - hardware_interface::HW_IF_POSITION)) + if (interface_name == // NOLINT + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) { - this->dataPtr->joint_control_methods_[j] &= static_cast(VELOCITY & EFFORT); - } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT - hardware_interface::HW_IF_VELOCITY)) + this->dataPtr->joint_control_methods_[j] &= + static_cast(VELOCITY & VELOCITY_PID & EFFORT); + + } else if (interface_name == // NOLINT + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) { - this->dataPtr->joint_control_methods_[j] &= static_cast(POSITION & EFFORT); - } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT - hardware_interface::HW_IF_EFFORT)) + this->dataPtr->joint_control_methods_[j] &= + static_cast(POSITION & POSITION_PID & EFFORT); + + } else if (interface_name == // NOLINT + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) { this->dataPtr->joint_control_methods_[j] &= - static_cast(POSITION & VELOCITY); + static_cast(POSITION & POSITION_PID & VELOCITY_PID & VELOCITY); } } // Set joint control method bits corresponding to start interfaces for (const std::string & interface_name : start_interfaces) { - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + - hardware_interface::HW_IF_POSITION)) + if (interface_name == + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) { - this->dataPtr->joint_control_methods_[j] |= POSITION; - } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT - hardware_interface::HW_IF_VELOCITY)) + if (!this->dataPtr->is_pos_pid[j]) { + this->dataPtr->joint_control_methods_[j] |= POSITION; + } else { + this->dataPtr->joint_control_methods_[j] |= POSITION_PID; + } + } else if (interface_name == // NOLINT + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) { - this->dataPtr->joint_control_methods_[j] |= VELOCITY; - } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT - hardware_interface::HW_IF_EFFORT)) + if (!this->dataPtr->is_vel_pid[j]) { + this->dataPtr->joint_control_methods_[j] |= VELOCITY; + } else { + this->dataPtr->joint_control_methods_[j] |= VELOCITY_PID; + } + } else if (interface_name == // NOLINT + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) { this->dataPtr->joint_control_methods_[j] |= EFFORT; } @@ -599,15 +729,33 @@ hardware_interface::return_type GazeboSystem::write( this->dataPtr->joint_position_[mimic_joint.mimicked_joint_index]; } + uint64_t dt = sim_period.nanoseconds(); + for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { if (this->dataPtr->sim_joints_[j]) { if (this->dataPtr->joint_control_methods_[j] & POSITION) { - this->dataPtr->sim_joints_[j]->SetPosition(0, this->dataPtr->joint_position_cmd_[j], true); + double cmd = this->dataPtr->joint_position_cmd_[j]; + this->dataPtr->sim_joints_[j]->SetPosition(0, cmd, true); this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); - } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { // NOLINT + + } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]); - } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT + + } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]); + + } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY_PID) { + double vel_goal = this->dataPtr->joint_velocity_cmd_[j]; + double vel = this->dataPtr->sim_joints_[j]->GetVelocity(0); + double cmd = this->dataPtr->vel_pid[j].computeCommand(vel_goal - vel, dt); + this->dataPtr->sim_joints_[j]->SetForce(0, cmd); + + } else if (this->dataPtr->joint_control_methods_[j] & POSITION_PID) { + double pos_goal = this->dataPtr->joint_position_cmd_[j]; + double pos = this->dataPtr->sim_joints_[j]->Position(0); + double cmd = this->dataPtr->pos_pid[j].computeCommand(pos_goal - pos, dt); + this->dataPtr->sim_joints_[j]->SetForce(0, cmd); + } else if (this->dataPtr->is_joint_actuated_[j] && this->dataPtr->hold_joints_) { // Fallback case is a velocity command of zero (only for actuated joints) this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); diff --git a/gazebo_ros2_control_demos/CMakeLists.txt b/gazebo_ros2_control_demos/CMakeLists.txt index 6823378a..50488e59 100644 --- a/gazebo_ros2_control_demos/CMakeLists.txt +++ b/gazebo_ros2_control_demos/CMakeLists.txt @@ -54,6 +54,12 @@ ament_target_dependencies(example_gripper std_msgs ) +add_executable(example_position_pid examples/example_position_pid.cpp) +ament_target_dependencies(example_position_pid + rclcpp + std_msgs +) + add_executable(example_diff_drive examples/example_diff_drive.cpp) ament_target_dependencies(example_diff_drive rclcpp @@ -77,6 +83,7 @@ endif() install( TARGETS example_position + example_position_pid example_velocity example_effort example_diff_drive diff --git a/gazebo_ros2_control_demos/config/cart_controller_position.yaml b/gazebo_ros2_control_demos/config/cart_controller_position.yaml new file mode 100644 index 00000000..d12a2295 --- /dev/null +++ b/gazebo_ros2_control_demos/config/cart_controller_position.yaml @@ -0,0 +1,14 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + position_controller: + type: position_controllers/JointGroupPositionController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +position_controller: + ros__parameters: + joints: + - slider_to_cart diff --git a/gazebo_ros2_control_demos/examples/example_position_pid.cpp b/gazebo_ros2_control_demos/examples/example_position_pid.cpp new file mode 100644 index 00000000..07ea680d --- /dev/null +++ b/gazebo_ros2_control_demos/examples/example_position_pid.cpp @@ -0,0 +1,58 @@ +// Copyright 2024 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. + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/float64_multi_array.hpp" + +std::shared_ptr node; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + node = std::make_shared("velocity_test_node"); + + auto publisher = node->create_publisher( + "/position_controller/commands", 10); + + RCLCPP_INFO(node->get_logger(), "node created"); + + std_msgs::msg::Float64MultiArray commands; + + using namespace std::chrono_literals; + + commands.data.push_back(0); + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 1; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = -1; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + rclcpp::shutdown(); + + return 0; +} diff --git a/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py b/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py new file mode 100644 index 00000000..d708dbfe --- /dev/null +++ b/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py @@ -0,0 +1,85 @@ +# Copyright 2024 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_vertical_cart_position_pid.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', 'cart'], + output='screen') + + 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', 'position_controller'], + output='screen' + ) + + return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_joint_trajectory_controller], + ) + ), + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) diff --git a/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py b/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py new file mode 100644 index 00000000..ae16fd1c --- /dev/null +++ b/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py @@ -0,0 +1,85 @@ +# Copyright 2024 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_vertical_cart_velocity_pid.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', 'cart'], + output='screen') + + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + load_velocity_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'velocity_controller'], + output='screen' + ) + + return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_velocity_controller], + ) + ), + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) diff --git a/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf new file mode 100644 index 00000000..cc3ce1cc --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + 100 + 1 + 10 + 10000 + + + + + + + + + + + + + $(find gazebo_ros2_control_demos)/config/cart_controller_position.yaml + + + diff --git a/gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf new file mode 100644 index 00000000..58661be1 --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + 10 + 10 + 0.1 + 10000 + + + + + + + + + + + + + $(find gazebo_ros2_control_demos)/config/cart_controller_velocity.yaml + + +