diff --git a/src/meldog_simple_description/CMakeLists.txt b/src/meldog_simple_description/CMakeLists.txt new file mode 100644 index 00000000..4ed9caf2 --- /dev/null +++ b/src/meldog_simple_description/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.8) +project(meldog_simple_description) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(xacro REQUIRED) +find_package(urdf REQUIRED) +find_package(rviz2 REQUIRED) +find_package(robot_state_publisher REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + + +install(DIRECTORY + test_controllers + description + launch + worlds + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/src/meldog_simple_description/LICENSE b/src/meldog_simple_description/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/src/meldog_simple_description/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/src/meldog_simple_description/description/gazebo/gazebo_macros.urdf.xacro b/src/meldog_simple_description/description/gazebo/gazebo_macros.urdf.xacro new file mode 100644 index 00000000..02a7170c --- /dev/null +++ b/src/meldog_simple_description/description/gazebo/gazebo_macros.urdf.xacro @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + Gazebo/${color} + + + + + + + + + + + + + + + + + + + + + ${implicitSpringDamper_value} + ${springStiffness_value} + ${springReference_value} + ${stopErp_value} + ${stopCfm_value} + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/joints/joint_macros.urdf.xacro b/src/meldog_simple_description/description/joints/joint_macros.urdf.xacro new file mode 100644 index 00000000..604c8032 --- /dev/null +++ b/src/meldog_simple_description/description/joints/joint_macros.urdf.xacro @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/joints/joint_properties.urdf.xacro b/src/meldog_simple_description/description/joints/joint_properties.urdf.xacro new file mode 100644 index 00000000..122e1de5 --- /dev/null +++ b/src/meldog_simple_description/description/joints/joint_properties.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/links/feet/feet.urdf.xacro b/src/meldog_simple_description/description/links/feet/feet.urdf.xacro new file mode 100644 index 00000000..29b4263a --- /dev/null +++ b/src/meldog_simple_description/description/links/feet/feet.urdf.xacro @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/links/hip/hip.urdf.xacro b/src/meldog_simple_description/description/links/hip/hip.urdf.xacro new file mode 100644 index 00000000..02b47890 --- /dev/null +++ b/src/meldog_simple_description/description/links/hip/hip.urdf.xacro @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/links/shank/shank.urdf.xacro b/src/meldog_simple_description/description/links/shank/shank.urdf.xacro new file mode 100644 index 00000000..a430226c --- /dev/null +++ b/src/meldog_simple_description/description/links/shank/shank.urdf.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/links/thigh/thigh.urdf.xacro b/src/meldog_simple_description/description/links/thigh/thigh.urdf.xacro new file mode 100644 index 00000000..f4620406 --- /dev/null +++ b/src/meldog_simple_description/description/links/thigh/thigh.urdf.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/meldog_simple_description/description/links/trunk/trunk.urdf.xacro b/src/meldog_simple_description/description/links/trunk/trunk.urdf.xacro new file mode 100644 index 00000000..3b92d811 --- /dev/null +++ b/src/meldog_simple_description/description/links/trunk/trunk.urdf.xacro @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/meldog_simple_description/description/meldog_core.urdf.xacro b/src/meldog_simple_description/description/meldog_core.urdf.xacro new file mode 100644 index 00000000..383f566e --- /dev/null +++ b/src/meldog_simple_description/description/meldog_core.urdf.xacro @@ -0,0 +1,198 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro b/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro new file mode 100644 index 00000000..9c8b37b7 --- /dev/null +++ b/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_macros.urdf.xacro new file mode 100644 index 00000000..ac32c422 --- /dev/null +++ b/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_macros.urdf.xacro @@ -0,0 +1,25 @@ + + + + + + + + ${lower} + ${upper} + + + -${motor_max_velocity} + ${motor_max_velocity} + + + -${motor_max_torque} + ${motor_max_torque} + + + + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_properties.urdf.xacro b/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_properties.urdf.xacro new file mode 100644 index 00000000..f2c9b8f6 --- /dev/null +++ b/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_properties.urdf.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/ros2_control/ros2_control_core.urdf.xacro b/src/meldog_simple_description/description/ros2_control/ros2_control_core.urdf.xacro new file mode 100644 index 00000000..0862edaf --- /dev/null +++ b/src/meldog_simple_description/description/ros2_control/ros2_control_core.urdf.xacro @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro b/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro new file mode 100644 index 00000000..5c9ec2ea --- /dev/null +++ b/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + + $(find meldog_simple_description)/test_controllers/joint_trajectory_controller_test.yaml + + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/ros2_control/transmission/transmission_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/transmission/transmission_macros.urdf.xacro new file mode 100644 index 00000000..71cab3f9 --- /dev/null +++ b/src/meldog_simple_description/description/ros2_control/transmission/transmission_macros.urdf.xacro @@ -0,0 +1,51 @@ + + + + + + + transmission_interface/SimpleTransmission + + + ${reduction} + ${offset} + + + + + + + + transmission_interface/FourBarLinkageTransmission + + ${reduction} + + + ${reduction} + + + 1.0 + ${first_offset} + + + 1.0 + ${second_offset} + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py b/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py new file mode 100644 index 00000000..1fde7048 --- /dev/null +++ b/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py @@ -0,0 +1,162 @@ +# 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, ExecuteProcess +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch_ros.parameter_descriptions import ParameterValue + +from launch_ros.actions import Node + + +def generate_launch_description(): + + # Declare arguments + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "package", + default_value="meldog_simple_description", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "urdf_file", + default_value="meldog_gazebo.urdf.xacro", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "controller_type_file", + default_value="joint_trajectory_controller_test.yaml", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "controller_type", + default_value="joint_trajectory_controller", + ) + ) + + # Initialize Arguments + package_name = LaunchConfiguration("package") + urdf_file = LaunchConfiguration("urdf_file") + controller_file = LaunchConfiguration("controller_type_file") + controller_type = LaunchConfiguration("controller_type") + + + # Get URDF via xacro + robot_description_content = ParameterValue( + Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare(package_name), + "description", + urdf_file, + ] + ), + ] + ), value_type=str) + + # Set up dictionary parameters: + robot_description = {"robot_description": robot_description_content} + use_sim_time = {"use_sim_time": True} + + # Load world for gazebo sim + world = PathJoinSubstitution( + [ + FindPackageShare(package_name), + "worlds", + 'empty.world' + ] + ) + + # robot_state_publisher node: + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description, use_sim_time], + ) + + # Spawn Meldog: + spawn_entity = Node(package='ros_gz_sim', executable='create', + arguments=['-topic', 'robot_description', + '-name', 'Meldog'], + output='screen') + + # Load joint_state_broadcaster + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + # Load joint_trajectory_controller + load_joint_effort_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', controller_type], + output='screen' + ) + + # Bridge between ros2 and Ignition Gazebo + # ros2_gazebo_sim_bridge = IncludeLaunchDescription( + # PythonLaunchDescriptionSource( + # [os.path.join(get_package_share_directory('ros_ign_gazebo'), + # 'launch', 'ign_gazebo.launch.py')]), + # launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]) + gazebo_sim_bridge = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']), + launch_arguments={'gz_args': ['-r -v -v4 ', world], 'on_exit_shutdown': 'true'}.items() + ) + + nodes = [ + gazebo_sim_bridge, + + 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_effort_controller], + ) + ), + + robot_state_pub_node, + spawn_entity, + ] + return LaunchDescription(declared_arguments + nodes) \ No newline at end of file diff --git a/src/meldog_simple_description/launch/meldog_rviz.launch.py b/src/meldog_simple_description/launch/meldog_rviz.launch.py new file mode 100644 index 00000000..e32d3ec9 --- /dev/null +++ b/src/meldog_simple_description/launch/meldog_rviz.launch.py @@ -0,0 +1,96 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "package", + default_value="meldog_description", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "urdf_file", + default_value="meldog_core.urdf.xacro", + ) + ) + + + # Initialize Arguments + package_name = LaunchConfiguration("package") + urdf_file = LaunchConfiguration("urdf_file") + + # Get URDF via xacro + robot_description_content = ParameterValue( + Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare(package_name), + "description", + urdf_file, + ] + ), + ] + ), value_type=str) + robot_description = {"robot_description": robot_description_content} + + # rviz_config_file = PathJoinSubstitution( + # [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"] + # ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_publisher_gui = Node( + package = "joint_state_publisher_gui", + executable = "joint_state_publisher_gui", + ) + + # Initialize Arguments + gui = LaunchConfiguration("gui") + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + # arguments=["-d", rviz_config_file], + condition=IfCondition(gui), + ) + + + + nodes = [ + robot_state_pub_node, + joint_state_publisher_gui, + rviz_node + ] + + return LaunchDescription(declared_arguments + nodes) \ No newline at end of file diff --git a/src/meldog_simple_description/package.xml b/src/meldog_simple_description/package.xml new file mode 100644 index 00000000..18acb8bb --- /dev/null +++ b/src/meldog_simple_description/package.xml @@ -0,0 +1,28 @@ + + + + meldog_simple_description + 0.0.0 + TODO: Package description + bartek + Apache-2.0 + + ament_cmake + + ros2launch + rviz2 + urdf + xacro + joint_state_publisher_gui + robot_state_publisher + ros_gz_bridge + ros_gz_image + ros_gz_sim + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/meldog_simple_description/simple_commands/joint_trajectory_example.txt b/src/meldog_simple_description/simple_commands/joint_trajectory_example.txt new file mode 100644 index 00000000..1a3431d6 --- /dev/null +++ b/src/meldog_simple_description/simple_commands/joint_trajectory_example.txt @@ -0,0 +1,35 @@ + POZYCJA STARTOWA: + + ros2 action send_goal /joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory -f "{ + trajectory: { + joint_names: ['FLH_joint', 'FRH_joint', 'BLH_joint', 'BRH_joint', 'FLT_joint', 'FRT_joint', 'BLT_joint', 'BRT_joint', 'FLS_joint', 'FRS_joint', 'BLS_joint', 'BRS_joint'], + points: [ + { positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start: { sec: 1 } }, + { positions: [0.0, 0.0, 0.0, 0.0, -0.78539, -0.78539, -0.78539, -0.78539, 1.5707, 1.5707, 1.5707, 1.5707], time_from_start: { sec: 6 } } ] + } + }" + + + +PRZYSIAD: + + ros2 action send_goal /joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory -f "{ + trajectory: { + joint_names: ['FLH_joint', 'FRH_joint', 'BLH_joint', 'BRH_joint', 'FLT_joint', 'FRT_joint', 'BLT_joint', 'BRT_joint', 'FLS_joint', 'FRS_joint', 'BLS_joint', 'BRS_joint'], + points: [ + { positions: [0.0, 0.0, 0.0, 0.0, -0.78539, -0.78539, -0.78539, -0.78539, 1.5707, 1.5707, 1.5707, 1.5707], time_from_start: { sec: 3 } }, + { positions: [0.0, 0.0, 0.0, 0.0, -1.04719, -1.04719, -1.04719, -1.04719, 2.094, 2.094, 2.094, 2.094], time_from_start: { sec: 6 } }, + { positions: [0.0, 0.0, 0.0, 0.0, -0.78539, -0.78539, -0.78539, -0.78539, 1.5707, 1.5707, 1.5707, 1.5707], time_from_start: { sec: 9 } } ] + } + }" + + WYPROSTOWANIE: + + ros2 action send_goal /joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory -f "{ + trajectory: { + joint_names: ['FLH_joint', 'FRH_joint', 'BLH_joint', 'BRH_joint', 'FLT_joint', 'FRT_joint', 'BLT_joint', 'BRT_joint', 'FLS_joint', 'FRS_joint', 'BLS_joint', 'BRS_joint'], + points: [ + { positions: [0.0, 0.0, 0.0, 0.0, -0.78539, -0.78539, -0.78539, -0.78539, 1.5707, 1.5707, 1.5707, 1.5707], time_from_start: { sec: 1 } }, + { positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start: { sec: 6 } } ] + } + }" diff --git a/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml b/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml new file mode 100644 index 00000000..c8e730c1 --- /dev/null +++ b/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml @@ -0,0 +1,119 @@ +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + use_sim_time: true + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +# imu_sensor_broadcaster: +# type: imu_sensor_broadcaster/IMUSensorBroadcaster # TODO: DODAJ IMU + +# imu_sensor_broadcaster: +# ros__parameters: +# sensor_name: MPU6050sensor +# frame_id: imu + +joint_trajectory_controller: + ros__parameters: + joints: + - FLH_joint + - FRH_joint + - BLH_joint + - BRH_joint + - FLT_joint + - FRT_joint + - BLT_joint + - BRT_joint + - FLS_joint + - FRS_joint + - BLS_joint + - BRS_joint + + command_joints: + - FLH_joint + - FRH_joint + - BLH_joint + - BRH_joint + - FLT_joint + - FRT_joint + - BLT_joint + - BRT_joint + - FLS_joint + - FRS_joint + - BLS_joint + - BRS_joint + + command_interfaces: + - effort + state_interfaces: + - position + - velocity + action_monitor_rate: 1000.0 + state_publish_rate: 1000.0 # Hz + allow_nonzero_velocity_at_trajectory_end: false + + gains: + FLH_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 + FRH_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 + BLH_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 + BRH_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 + FLT_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 + FRT_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 + BLT_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 + BRT_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 + FLS_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 + FRS_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 + BLS_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 + BRS_joint: + d: 100.0 + i: 0.0 + i_clamp: 0.0 + p: 1500.0 \ No newline at end of file diff --git a/src/meldog_simple_description/worlds/empty.world b/src/meldog_simple_description/worlds/empty.world new file mode 100644 index 00000000..4abcd714 --- /dev/null +++ b/src/meldog_simple_description/worlds/empty.world @@ -0,0 +1,73 @@ + + + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun + + + + + false + + + + + + 0.319654 -0.235002 9.29441 0 1.5138 0.009599 + orbit + perspective + + + + + 2500.0 + 0.0005 + 1 + + + quick + 200 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + + \ No newline at end of file