diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 69f07037..7a1b40f9 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -42,11 +42,11 @@ jobs: cd /home/ros2_ws/ . /opt/ros/foxy/local_setup.sh export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH - colcon build --packages-up-to gazebo_ros2_control + colcon build --packages-up-to gazebo_ros2_control_demos - name: Run tests id: test run: | cd /home/ros2_ws/ . /opt/ros/foxy/local_setup.sh - colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control + colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control gazebo_ros2_control_demos colcon test-result diff --git a/gazebo_ros2_control_demos/CMakeLists.txt b/gazebo_ros2_control_demos/CMakeLists.txt new file mode 100644 index 00000000..34bfa83f --- /dev/null +++ b/gazebo_ros2_control_demos/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.5.0) +project(gazebo_ros2_control_demos) + +# Default to C11 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 11) +endif() +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(NOT WIN32) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) + +install(DIRECTORY + launch + config + urdf + DESTINATION share/${PROJECT_NAME}/ +) + +add_executable(example_position examples/example_position.cpp) +ament_target_dependencies(example_position + rclcpp + rclcpp_action + control_msgs +) + +add_executable(example_velocity examples/example_velocity.cpp) +ament_target_dependencies(example_velocity + rclcpp + rclcpp_action + control_msgs +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + + ament_lint_auto_find_test_dependencies() +endif() + +## Install +install(TARGETS example_position example_velocity + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/gazebo_ros2_control_demos/config/cartpole_controller.yaml b/gazebo_ros2_control_demos/config/cartpole_controller.yaml new file mode 100644 index 00000000..1f39ba1e --- /dev/null +++ b/gazebo_ros2_control_demos/config/cartpole_controller.yaml @@ -0,0 +1,17 @@ +cart_pole_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - slider_to_cart + write_op_modes: + - slider_to_cart + state_publish_rate: 25 + action_monitor_rate: 20 + constraints: + stopped_velocity_tolerance: 0.05 + goal_time: 5 + +joint_state_controller: + ros__parameters: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/gazebo_ros2_control_demos/examples/example_position.cpp b/gazebo_ros2_control_demos/examples/example_position.cpp new file mode 100644 index 00000000..2cafb297 --- /dev/null +++ b/gazebo_ros2_control_demos/examples/example_position.cpp @@ -0,0 +1,179 @@ +// Copyright 2020 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 "rclcpp_action/rclcpp_action.hpp" + +#include "control_msgs/action/follow_joint_trajectory.hpp" + +std::shared_ptr node; +bool common_goal_accepted = false; +rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN; +int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; + +void common_goal_response( + std::shared_future::SharedPtr> future) +{ + RCLCPP_DEBUG( + node->get_logger(), "common_goal_response time: %f", + rclcpp::Clock().now().seconds()); + auto goal_handle = future.get(); + if (!goal_handle) { + common_goal_accepted = false; + printf("Goal rejected\n"); + } else { + common_goal_accepted = true; + printf("Goal accepted\n"); + } +} + +void common_result_response( + const rclcpp_action::ClientGoalHandle + ::WrappedResult & result) +{ + printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); + common_resultcode = result.code; + common_action_result_code = result.result->error_code; + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + printf("SUCCEEDED result code\n"); + break; + case rclcpp_action::ResultCode::ABORTED: + printf("Goal was aborted\n"); + return; + case rclcpp_action::ResultCode::CANCELED: + printf("Goal was canceled\n"); + return; + default: + printf("Unknown result code\n"); + return; + } +} + +void common_feedback( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) +{ + std::cout << "feedback->desired.positions :"; + for (auto & x : feedback->desired.positions) { + std::cout << x << "\t"; + } + std::cout << std::endl; + std::cout << "feedback->desired.velocities :"; + for (auto & x : feedback->desired.velocities) { + std::cout << x << "\t"; + } + std::cout << std::endl; +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + node = std::make_shared("trajectory_test_node"); + + std::cout << "node created" << std::endl; + + rclcpp_action::Client::SharedPtr action_client; + action_client = rclcpp_action::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + "/cart_pole_controller/follow_joint_trajectory"); + + bool response = + action_client->wait_for_action_server(std::chrono::seconds(1)); + if (!response) { + throw std::runtime_error("could not get action server"); + } + std::cout << "Created action server" << std::endl; + + std::vector joint_names = {"slider_to_cart"}; + + std::vector points; + trajectory_msgs::msg::JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap + point.positions.resize(joint_names.size()); + + point.positions[0] = 0.0; + + trajectory_msgs::msg::JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(1.0); + point2.positions.resize(joint_names.size()); + point2.positions[0] = -1.0; + + trajectory_msgs::msg::JointTrajectoryPoint point3; + point3.time_from_start = rclcpp::Duration::from_seconds(2.0); + point3.positions.resize(joint_names.size()); + point3.positions[0] = 1.0; + + trajectory_msgs::msg::JointTrajectoryPoint point4; + point4.time_from_start = rclcpp::Duration::from_seconds(3.0); + point4.positions.resize(joint_names.size()); + point4.positions[0] = 0.0; + + points.push_back(point); + points.push_back(point2); + points.push_back(point3); + points.push_back(point4); + + rclcpp_action::Client::SendGoalOptions opt; + opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1); + opt.result_callback = std::bind(common_result_response, std::placeholders::_1); + opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2); + + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); + goal_msg.trajectory.joint_names = joint_names; + goal_msg.trajectory.points = points; + + auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); + + if (rclcpp::spin_until_future_complete(node, goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); + return 1; + } + RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); + + rclcpp_action::ClientGoalHandle::SharedPtr + goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); + return 1; + } + RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server"); + + // Wait for the server to be done with the goal + auto result_future = action_client->async_get_result(goal_handle); + RCLCPP_INFO(node->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "get result call failed :("); + return 1; + } + + std::cout << "async_send_goal" << std::endl; + rclcpp::shutdown(); + + return 0; +} diff --git a/gazebo_ros2_control_demos/examples/example_velocity.cpp b/gazebo_ros2_control_demos/examples/example_velocity.cpp new file mode 100644 index 00000000..528af503 --- /dev/null +++ b/gazebo_ros2_control_demos/examples/example_velocity.cpp @@ -0,0 +1,188 @@ +// Copyright 2020 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 "rclcpp_action/rclcpp_action.hpp" + +#include "control_msgs/action/follow_joint_trajectory.hpp" + +std::shared_ptr node; +bool common_goal_accepted = false; +rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN; +int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; + +void common_goal_response( + std::shared_future::SharedPtr> future) +{ + RCLCPP_DEBUG( + node->get_logger(), "common_goal_response time: %f", + rclcpp::Clock().now().seconds()); + auto goal_handle = future.get(); + if (!goal_handle) { + common_goal_accepted = false; + printf("Goal rejected\n"); + } else { + common_goal_accepted = true; + printf("Goal accepted\n"); + } +} + +void common_result_response( + const rclcpp_action::ClientGoalHandle + ::WrappedResult & result) +{ + printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); + common_resultcode = result.code; + common_action_result_code = result.result->error_code; + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + printf("SUCCEEDED result code\n"); + break; + case rclcpp_action::ResultCode::ABORTED: + printf("Goal was aborted\n"); + return; + case rclcpp_action::ResultCode::CANCELED: + printf("Goal was canceled\n"); + return; + default: + printf("Unknown result code\n"); + return; + } +} + +void common_feedback( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) +{ + std::cout << "feedback->desired.positions :"; + for (auto & x : feedback->desired.positions) { + std::cout << x << "\t"; + } + std::cout << std::endl; + std::cout << "feedback->desired.velocities :"; + for (auto & x : feedback->desired.velocities) { + std::cout << x << "\t"; + } + std::cout << std::endl; + std::cout << std::endl; +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + node = std::make_shared("trajectory_test_node"); + + std::cout << "node created" << std::endl; + + rclcpp_action::Client::SharedPtr action_client; + action_client = rclcpp_action::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + "/cart_pole_controller/follow_joint_trajectory"); + + bool response = + action_client->wait_for_action_server(std::chrono::seconds(1)); + if (!response) { + throw std::runtime_error("could not get action server"); + } + std::cout << "Created action server" << std::endl; + + std::vector joint_names = {"slider_to_cart"}; + + std::vector points; + trajectory_msgs::msg::JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap + point.positions.resize(joint_names.size()); + point.velocities.resize(joint_names.size()); + + point.positions[0] = 0.0; + point.velocities[0] = 0.0; + + trajectory_msgs::msg::JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(1.0); + point2.positions.resize(joint_names.size()); + point2.velocities.resize(joint_names.size()); + point2.positions[0] = -1.0; + point2.velocities[0] = -1.0; + + trajectory_msgs::msg::JointTrajectoryPoint point3; + point3.time_from_start = rclcpp::Duration::from_seconds(2.0); + point3.positions.resize(joint_names.size()); + point3.velocities.resize(joint_names.size()); + point3.positions[0] = 1.0; + point3.velocities[0] = 1.0; + + trajectory_msgs::msg::JointTrajectoryPoint point4; + point4.time_from_start = rclcpp::Duration::from_seconds(3.0); + point4.positions.resize(joint_names.size()); + point4.velocities.resize(joint_names.size()); + point4.positions[0] = 0.0; + point4.velocities[0] = 0.0; + + points.push_back(point); + points.push_back(point2); + points.push_back(point3); + points.push_back(point4); + + rclcpp_action::Client::SendGoalOptions opt; + opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1); + opt.result_callback = std::bind(common_result_response, std::placeholders::_1); + opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2); + + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); + goal_msg.trajectory.joint_names = joint_names; + goal_msg.trajectory.points = points; + + auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); + + if (rclcpp::spin_until_future_complete(node, goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); + return 1; + } + RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); + + rclcpp_action::ClientGoalHandle::SharedPtr + goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); + return 1; + } + RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server"); + + // Wait for the server to be done with the goal + auto result_future = action_client->async_get_result(goal_handle); + RCLCPP_INFO(node->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "get result call failed :("); + return 1; + } + + std::cout << "async_send_goal" << std::endl; + rclcpp::shutdown(); + + return 0; +} diff --git a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py new file mode 100644 index 00000000..82ccd352 --- /dev/null +++ b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py @@ -0,0 +1,61 @@ +# Copyright 2020 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 IncludeLaunchDescription +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_cart_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=['-file', '/tmp/test_cart_position.urdf', '-entity', 'cartpole'], + output='screen') + + return LaunchDescription([ + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) diff --git a/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py new file mode 100644 index 00000000..921dc632 --- /dev/null +++ b/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py @@ -0,0 +1,62 @@ +# Copyright 2020 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 IncludeLaunchDescription +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_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=['-file', '/tmp/test_cart_position_pid.urdf', + '-entity', 'cartpole'], + output='screen') + + return LaunchDescription([ + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) diff --git a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py new file mode 100644 index 00000000..95a90c6b --- /dev/null +++ b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -0,0 +1,60 @@ +# Copyright 2020 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 IncludeLaunchDescription +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_cart_velocity.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=['-file', '/tmp/test_cart_velocity.urdf', '-entity', 'cartpole'], + output='screen') + + return LaunchDescription([ + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml new file mode 100644 index 00000000..b74c7ed1 --- /dev/null +++ b/gazebo_ros2_control_demos/package.xml @@ -0,0 +1,44 @@ + + + gazebo_ros2_control_demos + 0.0.1 + gazebo_ros2_control_demos + + Alejandro Hernandez + + Apache License 2.0 + + http://ros.org/wiki/gazebo_ros_control + https://github.com/ros-simulation/gazebo_ros2_control/issues + https://github.com/ros-simulation/gazebo_ros2_control + + Alejandro Hernández + + ament_cmake + + control_msgs + rclcpp + rclcpp_action + + ament_index_python + control_msgs + gazebo_ros2_control + gazebo_ros + hardware_interface + joint_trajectory_controller + joint_state_controller + launch + launch_ros + robot_state_publisher + rclcpp + transmission_interface + xacro + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf new file mode 100644 index 00000000..2f0d8f82 --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + gazebo_ros2_control/DefaultRobotHWSim + $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + + + diff --git a/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf new file mode 100644 index 00000000..2fed7bf0 --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + / + 50.0 + 10.0 + 15.0 + 3.0 + -3.0 + false + + gazebo_ros2_control/DefaultRobotHWSim + $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + + + diff --git a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf new file mode 100644 index 00000000..490a1f7c --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 1 + + + + + gazebo_ros2_control/DefaultRobotHWSim + $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + + +