HiveCoreRD/hivecore_robot_os
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

gazebo sim added

Browse Source
This commit is contained in:
Ray
2025-08-02 12:07:02 +08:00
parent 7060c424c8
commit 80a06ef898
306 changed files with 22706 additions and 2008 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
HiveCoreR0/hive_core_bot0.gv Normal file
View File

@@ -0,0 +1,47 @@
digraph G {
node [shape=box];
"base_link" [label="base_link"];
"left_hip_pitch_link" [label="left_hip_pitch_link"];
"left_leg_pitch_link" [label="left_leg_pitch_link"];
"left_wheel2_link" [label="left_wheel2_link"];
"left_wheel1_link" [label="left_wheel1_link"];
"left_shoulder_link" [label="left_shoulder_link"];
"left_arm1_link" [label="left_arm1_link"];
"left_arm2_link" [label="left_arm2_link"];
"right_hip_pitch_link" [label="right_hip_pitch_link"];
"right_leg_pitch_link" [label="right_leg_pitch_link"];
"right_wheel2_link" [label="right_wheel2_link"];
"right_wheel1_link" [label="right_wheel1_link"];
"right_shoulder_link" [label="right_shoulder_link"];
"right_arm1_link" [label="right_arm1_link"];
"right_arm2_link" [label="right_arm2_link"];
node [shape=ellipse, color=blue, fontcolor=blue];
"base_link" -> "left_hip_pitch_joint" [label="xyz: -0.06 -0.1358 -0.037269 \nrpy: -1.5708 -0.0095732 0"]
"left_hip_pitch_joint" -> "left_hip_pitch_link"
"left_hip_pitch_link" -> "left_leg_pitch_joint" [label="xyz: 0 0 -0.0295 \nrpy: 0 -0 0.01205"]
"left_leg_pitch_joint" -> "left_leg_pitch_link"
"left_leg_pitch_link" -> "left_wheel2_joint" [label="xyz: 0 0.4805 0.004 \nrpy: 3.14159 -3.30872e-24 -0.0024764"]
"left_wheel2_joint" -> "left_wheel2_link"
"left_hip_pitch_link" -> "left_wheel1_joint" [label="xyz: 0 0.48 0.01 \nrpy: 0 -0 0.0095732"]
"left_wheel1_joint" -> "left_wheel1_link"
"base_link" -> "left_shoulder_joint" [label="xyz: -0.06 -0.0878 0.36288 \nrpy: 0 -0 0"]
"left_shoulder_joint" -> "left_shoulder_link"
"left_shoulder_link" -> "left_arm1_joint" [label="xyz: 0 0 0.0385 \nrpy: 1.5708 4.33681e-19 -0.0074682"]
"left_arm1_joint" -> "left_arm1_link"
"left_arm1_link" -> "left_arm2_joint" [label="xyz: 0.020127 0.0040341 0.17817 \nrpy: -1.6062 0.0074682 4.33693e-19"]
"left_arm2_joint" -> "left_arm2_link"
"base_link" -> "right_hip_pitch_joint" [label="xyz: -0.06 0.1358 -0.037269 \nrpy: 1.5708 -0.0029091 0"]
"right_hip_pitch_joint" -> "right_hip_pitch_link"
"right_hip_pitch_link" -> "right_leg_pitch_joint" [label="xyz: 0 0 -0.0295 \nrpy: 0 0 -0.0029091"]
"right_leg_pitch_joint" -> "right_leg_pitch_link"
"right_leg_pitch_link" -> "right_wheel2_joint" [label="xyz: 1.9386e-05 -0.4805 0.004 \nrpy: -3.14159 -0 0"]
"right_wheel2_joint" -> "right_wheel2_link"
"right_hip_pitch_link" -> "right_wheel1_joint" [label="xyz: 0 -0.48 0.01 \nrpy: 0 0 -0.0029091"]
"right_wheel1_joint" -> "right_wheel1_link"
"base_link" -> "right_shoulder_joint" [label="xyz: -0.06 0.0878 0.36288 \nrpy: 0 -0 0"]
"right_shoulder_joint" -> "right_shoulder_link"
"right_shoulder_link" -> "right_arm1_joint" [label="xyz: 0 0 0.0385 \nrpy: 1.5708 -0 0"]
"right_arm1_joint" -> "right_arm1_link"
"right_arm1_link" -> "right_arm2_joint" [label="xyz: 0.017969 0.0026972 -0.17843 \nrpy: -1.5793 0 0"]
"right_arm2_joint" -> "right_arm2_link"
}

BIN
HiveCoreR0/hive_core_bot0.pdf Normal file
View File

Binary file not shown.

49
HiveCoreR0/src/hive_core_r0/CMakeLists.txt Normal file
View File

@@ -0,0 +1,49 @@
cmake_minimum_required(VERSION 3.10)
project(hive_core_r0)
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(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_executable(arm_control_node src/arm_control_node.cpp)
ament_target_dependencies(arm_control_node rclcpp geometry_msgs)
add_executable(hiveBot1_control_node src/hiveBot1_control_node.cpp)
ament_target_dependencies(hiveBot1_control_node rclcpp geometry_msgs)
add_executable(keyboard_control_node src/keyboard_control_node.cpp)
ament_target_dependencies(keyboard_control_node rclcpp geometry_msgs)
install(TARGETS
arm_control_node
hiveBot1_control_node
keyboard_control_node
DESTINATION lib/${PROJECT_NAME})
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 launch urdf config world src
DESTINATION share/${PROJECT_NAME}
)
ament_package()

202
HiveCoreR0/src/hive_core_r0/LICENSE Normal file
View File

@@ -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.

73
HiveCoreR0/src/hive_core_r0/config/bot0_ros2_controller.yaml Normal file
View File

@@ -0,0 +1,73 @@
controller_manager:
ros__parameters:
update_rate: 100 # Hz
use_sim_time: true
# 关节状态广播器 - 必须配置，用于发布关节状态
bot0_joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
use_sim_time: true
# 关节位置控制器
bot0_position_controller:
type: position_controllers/JointGroupPositionController
# 轮子速度控制器（已从差速控制器改为速度控制器）
bot0_wheel_velocity_controller:
type: velocity_controllers/JointGroupVelocityController
# 腿部和手臂关节位置控制器配置
bot0_position_controller:
ros__parameters:
joints:
- left_hip_pitch_joint
- left_leg_pitch_joint
- right_hip_pitch_joint
- right_leg_pitch_joint
- left_shoulder_joint
- left_arm1_joint
- left_arm2_joint
- right_shoulder_joint
- right_arm1_joint
- right_arm2_joint
command_interfaces:
- position
state_interfaces:
- position
- velocity
# 位置控制器PID参数
gains:
left_hip_pitch_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
left_leg_pitch_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_hip_pitch_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_leg_pitch_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
left_shoulder_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
left_arm1_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
left_arm2_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_shoulder_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_arm1_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_arm2_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
# 位置控制允许的误差范围
allowable_position_error: 0.1 # 弧度
# 轮子速度控制器配置（替代原来的差速控制器）
bot0_wheel_velocity_controller:
ros__parameters:
joints:
- left_wheel1_joint
- left_wheel2_joint
- right_wheel1_joint
- right_wheel2_joint
command_interfaces:
- velocity
state_interfaces:
- position
- velocity
# 速度控制器PID参数
gains:
left_wheel1_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
left_wheel2_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_wheel1_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_wheel2_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
# 速度控制允许的误差范围
allowable_velocity_error: 0.2 # rad/s

57
HiveCoreR0/src/hive_core_r0/config/bot1_ros2_controller.yaml Normal file
View File

@@ -0,0 +1,57 @@
controller_manager:
ros__parameters:
update_rate: 100 # Hz
use_sim_time: true
# 关节状态广播器 - 必须配置，用于发布关节状态
bot1_joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
use_sim_time: true
# 关节位置控制器
bot1_position_controller:
type: position_controllers/JointGroupPositionController
# 腿部和手臂关节位置控制器配置
bot1_position_controller:
ros__parameters:
joints:
- left_leg1_joint
- left_wheel1_joint
- left_leg2_joint
- left_wheel2_joint
- right_leg1_joint
- right_wheel1_joint
- right_leg2_joint
- right_wheel2_joint
- pitch_body_joint
- left_shoulder_joint
- left_arm1_joint
- left_arm2_joint
- right_shoulder_joint
- right_arm1_joint
- right_arm2_joint
command_interfaces:
- position
state_interfaces:
- position
- velocity
# 位置控制器PID参数
gains:
left_leg1_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
left_wheel1_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
left_leg2_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
left_wheel2_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
right_leg1_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
right_wheel1_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
right_leg2_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
right_wheel2_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
pitch_body_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
left_shoulder_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
left_arm1_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
left_arm2_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
right_shoulder_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
right_arm1_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
right_arm2_joint: {p: 10000, i: 0.0, d: 1000, i_clamp: 0.01}
# 位置控制允许的误差范围
allowable_position_error: 0.1 # 弧度

119
HiveCoreR0/src/hive_core_r0/launch/gazebo_sim_bot0.launch.py Normal file
View File

@@ -0,0 +1,119 @@
import os
import launch
import launch_ros
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler
from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution, FindExecutable
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.event_handlers import OnProcessStart
from launch.actions import TimerAction
def generate_launch_description():
# 声明参数
robot_name_in_model = "HiveBot0"
urdf_package = 'hive_core_r0'
urdf_tutorial_path = get_package_share_directory(urdf_package)
default_world_path = os.path.join(urdf_tutorial_path, 'world', 'table.world')
default_model_path = urdf_tutorial_path + '/urdf/hiveBot0/hive_core_bot0.urdf.xacro'
# 声明模型路径参数
action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
name='model', default_value=str(default_model_path),
description='URDF 的绝对路径')
# 获取文件内容生成新的参数
robot_description = launch_ros.parameter_descriptions.ParameterValue(
launch.substitutions.Command(
['xacro ', launch.substitutions.LaunchConfiguration('model')]),
value_type=str)
# 声明机器人状态发布节点
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': robot_description}],
)
# 启动Gazebo - 添加 paused=true 参数使仿真暂停
launch_gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch',
'gazebo.launch.py')
]),
launch_arguments={
# 'world': default_world_path,
'verbose': 'true',
}.items()
)
# 在Gazebo中生成机器人
spawn_entity_node = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=[
'-entity', robot_name_in_model, # 模型名称
'-topic', '/robot_description', # 从ROS话题获取URDF
'-x', '0.0', # 初始位置x
'-y', '0.0', # 初始位置y
'-z', '0.55', # 初始位置z (高于地面)
'-Y', '0.0' # 初始朝向
]
)
# 加载并激活 load_joint_state_controller 控制器
load_joint_state_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'bot0_joint_state_broadcaster'],
output='screen'
)
# 加载并激活 load_bot0_position_controller 控制器
load_bot0_position_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','bot0_position_controller'],
output='screen')
load_bot0_wheel_velocity_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','bot0_wheel_velocity_controller'],
output='screen')
action_node_arm_control = launch_ros.actions.Node(
package='hive_core_r0',
executable='arm_control_node',
name='arm_control_node',
output='screen'
)
return LaunchDescription([
action_declare_arg_mode_path,
# 先启动Gazebo
launch_gazebo,
# 然后启动状态发布器
robot_state_publisher_node,
# 延迟后生成机器人（确保Gazebo完全启动）
TimerAction(
period= 5.0,
actions=[spawn_entity_node]
),
# 事件动作，当加载机器人结束后执行
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=spawn_entity_node,
on_exit=[load_joint_state_controller, load_bot0_position_controller, load_bot0_wheel_velocity_controller],)
),
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=load_bot0_position_controller,
on_exit=[action_node_arm_control],)
),
])

140
HiveCoreR0/src/hive_core_r0/launch/gazebo_sim_bot1.launch.py Normal file
View File

@@ -0,0 +1,140 @@
import os
import launch
import launch_ros
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler
from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution, FindExecutable
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.event_handlers import OnProcessStart
from launch.actions import TimerAction
def generate_launch_description():
# 声明参数
robot_name_in_model = "HiveBot0"
urdf_package = 'hive_core_r0'
urdf_tutorial_path = get_package_share_directory(urdf_package)
default_model_path = urdf_tutorial_path + '/urdf/hiveBot1/hive_core_bot1.urdf.xacro'
default_world_path = os.path.join(urdf_tutorial_path, 'world', 'empty.world')
# 声明模型路径参数
action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
name='model', default_value=str(default_model_path),
description='URDF 的绝对路径')
# 获取文件内容生成新的参数
robot_description = launch_ros.parameter_descriptions.ParameterValue(
launch.substitutions.Command(
['xacro ', launch.substitutions.LaunchConfiguration('model')]),
value_type=str)
# 声明机器人状态发布节点
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': robot_description}],
)
# 启动Gazebo - 添加 paused=true 参数使仿真暂停
launch_gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch',
'gazebo.launch.py')
]),
launch_arguments={
'world': default_world_path,
'verbose': 'true',
}.items()
)
# 在Gazebo中生成机器人
spawn_entity_node = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=[
'-entity', robot_name_in_model, # 模型名称
'-topic', '/robot_description', # 从ROS话题获取URDF
'-x', '0.0', # 初始位置x
'-y', '0.0', # 初始位置y
'-z', '0.45', # 初始位置z (高于地面)
'-Y', '0.0' # 初始朝向
]
)
# 1. 启动controller_manager（通常由gazebo_ros2_control自动启动，若未启动则显式添加）
controller_manager_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[
{'robot_description': robot_description},
os.path.join(urdf_tutorial_path, 'config', 'bot1_ros2_controller.yaml') # 显式指定配置文件
],
output="screen"
)
# 加载并激活 load_joint_state_controller 控制器
load_joint_state_controller = TimerAction(
period=3.0, # 延迟3秒，等待初始化
actions=[ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'bot1_joint_state_broadcaster'],
output='screen'
)]
)
# 加载并激活 load_bot0_position_controller 控制器
load_bot1_position_controller = TimerAction(
period=2.0, # 延迟2秒
actions=[ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'bot1_position_controller'],
output='screen'
)]
)
action_node_hiveBot1_control = launch_ros.actions.Node(
package='hive_core_r0',
executable='hiveBot1_control_node',
name='hiveBot1_control_node',
output='screen'
)
# 4. 延迟启动控制节点（在位置控制器加载后）
start_control_node = TimerAction(
period=2.0,
actions=[action_node_hiveBot1_control]
)
keyboard_control_node = launch_ros.actions.Node(
package='hive_core_r0',
executable='keyboard_control_node',
name='keyboard_control_node',
output='screen'
)
# 5. 延迟启动键盘控制节点（在控制节点启动后）
start_keyboard_control_node = TimerAction(
period=2.0,
actions=[keyboard_control_node]
)
# 在LaunchDescription中按顺序添加
return LaunchDescription([
action_declare_arg_mode_path,
launch_gazebo,
robot_state_publisher_node,
# 延迟后生成机器人（确保Gazebo完全启动）
TimerAction(
period= 5.0,
actions=[spawn_entity_node]
),
controller_manager_node, # 显式启动控制器管理器
load_joint_state_controller,
load_bot1_position_controller,
start_control_node,
# start_keyboard_control_node,
])

18
HiveCoreR0/src/hive_core_r0/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>hive_core_r0</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ray@hivecore.cn">hivecore</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

116
HiveCoreR0/src/hive_core_r0/src/arm_control_node.cpp Normal file
View File

@@ -0,0 +1,116 @@
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include <cmath>
#include <chrono>
using namespace std::chrono_literals;
class ArmSineMotionNode : public rclcpp::Node
{
public:
ArmSineMotionNode() : Node("arm_sine_motion_node")
{
// 1. 声明并获取参数（振幅默认50度，频率默认0.5Hz）
this->declare_parameter("amplitude_deg", 50.0); // 振幅（度）
this->declare_parameter("frequency", 0.5); // 频率（Hz）
this->declare_parameter("control_period_ms", 1); // 控制周期（毫秒，默认20ms=50Hz）
// 2. 读取参数并转换单位（度→弧度）
double amplitude_deg = this->get_parameter("amplitude_deg").as_double();
amplitude_rad_ = amplitude_deg * M_PI / 180.0; // 50度→弧度
frequency_ = this->get_parameter("frequency").as_double();
int period_ms = this->get_parameter("control_period_ms").as_int();
// 3. 手臂关节在控制器指令数组中的索引（对应joints配置顺序）
// 控制器joints顺序：[0-3腿部, 4-9手臂]
arm_joint_indices_ = {4, 5, 6, 7, 8, 9}; // 6个手臂关节索引
total_joints_ = 10; // 控制器总关节数（4腿部+6手臂）
// 4. 创建发布者（发送位置指令到位置控制器）
cmd_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
"/bot0_position_controller/commands",
10
);
// 5. 创建定时器（周期性发送指令）
timer_ = this->create_wall_timer(
std::chrono::milliseconds(period_ms),
std::bind(&ArmSineMotionNode::timer_callback, this)
);
// 6. 记录起始时间（用于计算正弦运动相位）
start_time_ = this->now();
RCLCPP_INFO(this->get_logger(), "手臂正弦运动节点启动（振幅50度，频率%.2fHz）", frequency_);
}
void RestJoint()
{
auto reset_msg = std_msgs::msg::Float64MultiArray();
reset_msg.data.resize(10, 0.0);
cmd_pub_->publish(reset_msg);
}
private:
void timer_callback()
{
// 1. 计算当前运动时间（秒）
rclcpp::Duration elapsed = this->now() - start_time_;
double current_time = elapsed.seconds();
// 2. 计算正弦位置：y = A*sin(2πft)
// A=振幅（弧度），f=频率（Hz），t=时间（秒）
double sine_pos = amplitude_rad_ * std::sin(2 * M_PI * frequency_ * current_time);
// 3. 构建指令数组（10个关节：腿部保持0，手臂按正弦运动）
auto cmd_msg = std_msgs::msg::Float64MultiArray();
cmd_msg.data.resize(total_joints_, 0.0); // 初始化所有关节为0
// 4. 为手臂关节赋值正弦位置
// for (size_t i = 0; i < arm_joint_indices_.size(); ++i) {
// int joint_idx = arm_joint_indices_[i];
// cmd_msg.data[joint_idx] = 0;//sine_pos; // 所有手臂关节同步运动
// }
for (int i = 0; i < total_joints_; ++i) {
// int joint_idx = arm_joint_indices_[i];
if (i == 1 || i == 3)
{
cmd_msg.data[i] = sine_pos * 2;
}
else
{
cmd_msg.data[i] = sine_pos; // 所有手臂关节同步运动
}
}
cmd_msg.data.resize(total_joints_, 0.0); // 初始化所有关节为0
// 5. 发送指令
cmd_pub_->publish(cmd_msg);
}
// 成员变量
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmd_pub_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time start_time_; // 起始时间
double amplitude_rad_; // 振幅（弧度）
double frequency_; // 频率（Hz）
std::vector<int> arm_joint_indices_; // 手臂关节索引
int total_joints_; // 总关节数
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<ArmSineMotionNode>();
// 运行节点（按Ctrl+C退出）
rclcpp::spin(node);
// 退出时发送复位指令（所有关节回0）
node->RestJoint();
RCLCPP_INFO(node->get_logger(), "节点退出，发送复位指令");
rclcpp::shutdown();
return 0;
}

336
HiveCoreR0/src/hive_core_r0/src/hiveBot1_control_node.cpp Normal file
View File

@@ -0,0 +1,336 @@
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <cmath>
#include <chrono>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
using namespace std::chrono_literals;
// 键盘输入处理函数（非阻塞模式）
int kbhit()
{
struct termios oldt, newt;
int ch;
int oldf;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
fcntl(STDIN_FILENO, F_SETFL, oldf);
if(ch != EOF)
{
ungetc(ch, stdin);
return 1;
}
return 0;
}
class RobotPhaseMotionNode : public rclcpp::Node
{
public:
RobotPhaseMotionNode() : Node("robot_phase_motion_node")
{
// 声明并获取参数
this->declare_parameter("leg_target_deg", 0.0); // 腿部目标角度（度）
this->declare_parameter("arm_amplitude_deg", 30.0); // 手臂摆动振幅（度）
this->declare_parameter("arm_frequency", 0.5); // 手臂摆动频率（Hz）
this->declare_parameter("control_period_ms", 10); // 控制周期（毫秒）
this->declare_parameter("wheel_radius", 0.05); // 轮子半径（米）
this->declare_parameter("wheel_separation", 0.3); // 轮距（米）
this->declare_parameter("linear_speed", 0.2); // 线速度（m/s）
this->declare_parameter("angular_speed", 0.5); // 角速度（rad/s）
// 读取参数并转换单位
double leg_target_deg = this->get_parameter("leg_target_deg").as_double();
leg_target_rad_ = leg_target_deg * M_PI / 180.0;
double arm_amplitude_deg = this->get_parameter("arm_amplitude_deg").as_double();
arm_amplitude_rad_ = arm_amplitude_deg * M_PI / 180.0;
arm_frequency_ = this->get_parameter("arm_frequency").as_double();
int period_ms = this->get_parameter("control_period_ms").as_int();
wheel_radius_ = this->get_parameter("wheel_radius").as_double();
wheel_separation_ = this->get_parameter("wheel_separation").as_double();
linear_speed_ = this->get_parameter("linear_speed").as_double();
angular_speed_ = this->get_parameter("angular_speed").as_double();
joint_directions_ = {1, -1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; // 关节方向
total_joints_ = 15; // 总关节数
// 关节索引定义
leg_joint_indices_ = {0, 2, 4, 6}; // 腿部关节索引
arm_joint_indices_ = {9, 10, 11, 12, 13}; // 手臂关节索引
wheel_joint_indices_ = {1, 5}; // 轮子关节索引（left_wheel1, right_wheel1 ）两个主动轮
wheel_joint_directions_ = {-1, 1}; // 轮子方向
// 初始化轮子目标角度
wheel_targets_.resize(2, 0.0);
// 创建发布者
cmd_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
"/bot1_position_controller/commands", 10
);
// 创建订阅者（接收来自速度指令）
twist_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", 10,
std::bind(&RobotPhaseMotionNode::twist_callback, this, std::placeholders::_1)
);
// 创建定时器
timer_ = this->create_wall_timer(
std::chrono::milliseconds(period_ms),
std::bind(&RobotPhaseMotionNode::timer_callback, this)
);
// 记录起始时间
start_time_ = this->now();
last_time_ = this->now();
// 打印控制说明
print_help();
RCLCPP_INFO(this->get_logger(), "机器人分阶段运动节点启动（含轮子控制）");
}
// 关节复位
void resetJoints()
{
auto reset_msg = std_msgs::msg::Float64MultiArray();
reset_msg.data.resize(total_joints_, 0.0);
cmd_pub_->publish(reset_msg);
}
private:
void print_help()
{
RCLCPP_INFO(this->get_logger(), "键盘控制说明:");
RCLCPP_INFO(this->get_logger(), " w : 前进");
RCLCPP_INFO(this->get_logger(), " s : 后退");
RCLCPP_INFO(this->get_logger(), " a : 左转");
RCLCPP_INFO(this->get_logger(), " d : 右转");
RCLCPP_INFO(this->get_logger(), " x : 停止");
RCLCPP_INFO(this->get_logger(), " q : 退出");
}
void twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
// 接收外部速度指令
linear_x_ = msg->linear.x;
angular_z_ = msg->angular.z;
}
void timer_callback()
{
// 处理键盘输入
handle_keyboard_input();
// 计算时间差
rclcpp::Time current_time = this->now();
rclcpp::Duration dt = current_time - last_time_;
double dt_sec = dt.seconds();
last_time_ = current_time;
// 构建指令数组
auto cmd_msg = std_msgs::msg::Float64MultiArray();
cmd_msg.data.resize(total_joints_, 0.0);
// 分阶段控制腿部和手臂
double phase_time = (current_time - start_time_).seconds();
handle_phase_motion(cmd_msg, phase_time);
// 计算并设置轮子角度
handle_wheel_control(cmd_msg, dt_sec);
// 发送指令
cmd_pub_->publish(cmd_msg);
}
void handle_phase_motion(std_msgs::msg::Float64MultiArray& cmd_msg, double current_time)
{
// 第一阶段：前5秒，腿部关节缓慢张开
if (current_time <= 5.0)
{
double progress = current_time / 5.0;
double current_leg_angle = leg_target_rad_ * progress;
// 设置腿部关节角度
for (int idx : leg_joint_indices_)
{
if (idx == 0 || idx == 4) // 左腿1、左腿2关节
{
cmd_msg.data[idx] = -current_leg_angle;
}
else
{
cmd_msg.data[idx] = current_leg_angle;
}
}
// 手臂保持初始位置
for (int idx : arm_joint_indices_)
{
cmd_msg.data[idx] = 0.0;
}
if (current_time >= 4.9 && !phase1_notified_)
{
RCLCPP_INFO(this->get_logger(), "第一阶段即将结束，准备进入手臂摆动阶段");
phase1_notified_ = true;
}
}
// 第二阶段：5秒后，双臂做摆手动作
else
{
// 腿部保持在目标角度
for (int idx : leg_joint_indices_)
{
if (idx == 0 || idx == 4) // 左腿1、左腿2关节
{
cmd_msg.data[idx] = -leg_target_rad_;
}
else
{
cmd_msg.data[idx] = leg_target_rad_;
}
}
// 计算手臂摆动角度
double arm_phase_time = current_time - 5.0;
double arm_angle = arm_amplitude_rad_ * std::sin(2 * M_PI * arm_frequency_ * arm_phase_time);
// 设置手臂关节角度（左右臂对称）
for (size_t i = 0; i < arm_joint_indices_.size(); ++i)
{
if (i >= arm_joint_indices_.size() / 2)
{
cmd_msg.data[arm_joint_indices_[i]] = -arm_angle;
}
else
{
cmd_msg.data[arm_joint_indices_[i]] = arm_angle;
}
cmd_msg.data[arm_joint_indices_[i]] = 0;
}
}
}
void handle_wheel_control(std_msgs::msg::Float64MultiArray& cmd_msg, double dt_sec)
{
// 计算左右轮目标线速度
double left_speed = linear_x_ + (angular_z_ * wheel_separation_) / 2.0;
double right_speed = linear_x_ - (angular_z_ * wheel_separation_) / 2.0;
// 计算轮子角速度（rad/s）
double left_omega = left_speed / wheel_radius_;
double right_omega = right_speed / wheel_radius_;
// 积分计算目标角度（弧度）
wheel_targets_[0] += left_omega * dt_sec; // left_wheel1
wheel_targets_[1] += right_omega * dt_sec; // right_wheel1
// 赋值轮子目标角度到指令数组
for (size_t i = 0; i < wheel_joint_indices_.size(); ++i)
{
cmd_msg.data[wheel_joint_indices_[i]] = wheel_joint_directions_[i] * wheel_targets_[i];
}
}
void handle_keyboard_input()
{
if(kbhit())
{
char key = getchar();
switch(key)
{
case 'w': // 前进
linear_x_ = linear_speed_;
angular_z_ = 0.0;
RCLCPP_INFO(this->get_logger(), "前进");
break;
case 's': // 后退
linear_x_ = -linear_speed_;
angular_z_ = 0.0;
RCLCPP_INFO(this->get_logger(), "后退");
break;
case 'a': // 左转
linear_x_ = 0.0;
angular_z_ = angular_speed_;
RCLCPP_INFO(this->get_logger(), "左转");
break;
case 'd': // 右转
linear_x_ = 0.0;
angular_z_ = -angular_speed_;
RCLCPP_INFO(this->get_logger(), "右转");
break;
case 'x': // 停止
linear_x_ = 0.0;
angular_z_ = 0.0;
RCLCPP_INFO(this->get_logger(), "停止");
break;
case 'q': // 退出
RCLCPP_INFO(this->get_logger(), "退出控制");
resetJoints();
rclcpp::shutdown();
exit(0);
break;
default:
print_help();
break;
}
}
}
// 成员变量
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmd_pub_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time start_time_;
rclcpp::Time last_time_;
// 运动参数
double leg_target_rad_; // 腿部目标角度（弧度）
double arm_amplitude_rad_; // 手臂摆动振幅（弧度）
double arm_frequency_; // 手臂摆动频率（Hz）
int total_joints_; // 总关节数
bool phase1_notified_ = false; // 第一阶段结束提示标记
// 关节索引
std::vector<int> leg_joint_indices_; // 腿部关节索引
std::vector<int> arm_joint_indices_; // 手臂关节索引
std::vector<int> wheel_joint_indices_; // 轮子关节索引
std::vector<int> joint_directions_; // 关节方向
std::vector<int> wheel_joint_directions_; // 轮子关节方向
// 轮子控制相关
double wheel_radius_; // 轮子半径（米）
double wheel_separation_; // 轮距（米）
std::vector<double> wheel_targets_; // 轮子目标角度（弧度）
double linear_x_ = 0.0; // 线速度（m/s）
double angular_z_ = 0.0; // 角速度（rad/s）
double linear_speed_; // 最大线速度（m/s）
double angular_speed_; // 最大角速度（rad/s）
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<RobotPhaseMotionNode>();
rclcpp::spin(node);
node->resetJoints();
RCLCPP_INFO(node->get_logger(), "节点退出，发送复位指令");
rclcpp::shutdown();
return 0;
}

140
HiveCoreR0/src/hive_core_r0/src/keyboard_control_node.cpp Normal file
View File

@@ -0,0 +1,140 @@
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <chrono>
using namespace std::chrono_literals;
// 键盘输入处理函数（非阻塞模式）
int kbhit()
{
struct termios oldt, newt;
int ch;
int oldf;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
fcntl(STDIN_FILENO, F_SETFL, oldf);
if(ch != EOF)
{
ungetc(ch, stdin);
return 1;
}
return 0;
}
class KeyboardControlNode : public rclcpp::Node
{
public:
KeyboardControlNode() : Node("keyboard_control_node")
{
// 声明参数：移动速度、旋转速度
this->declare_parameter("linear_speed", 0.2); // 线速度（m/s）
this->declare_parameter("angular_speed", 0.5); // 角速度（rad/s）
// 读取参数
linear_speed_ = this->get_parameter("linear_speed").as_double();
angular_speed_ = this->get_parameter("angular_speed").as_double();
// 创建发布者（发送速度指令）
twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
"/cmd_vel", 10
);
// 打印控制说明
print_help();
// 创建定时器（周期性检查键盘输入）
timer_ = this->create_wall_timer(
50ms, // 20Hz检查频率
std::bind(&KeyboardControlNode::timer_callback, this)
);
}
private:
void print_help()
{
RCLCPP_INFO(this->get_logger(), "键盘控制说明:");
RCLCPP_INFO(this->get_logger(), " w : 前进");
RCLCPP_INFO(this->get_logger(), " s : 后退");
RCLCPP_INFO(this->get_logger(), " a : 左转");
RCLCPP_INFO(this->get_logger(), " d : 右转");
RCLCPP_INFO(this->get_logger(), " x : 停止");
RCLCPP_INFO(this->get_logger(), " q : 退出");
}
void timer_callback()
{
geometry_msgs::msg::Twist twist_msg;
if(kbhit())
{
char key = getchar();
switch(key)
{
case 'w': // 前进
twist_msg.linear.x = linear_speed_;
twist_msg.angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "前进");
break;
case 's': // 后退
twist_msg.linear.x = -linear_speed_;
twist_msg.angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "后退");
break;
case 'a': // 左转
twist_msg.linear.x = 0.0;
twist_msg.angular.z = angular_speed_;
RCLCPP_INFO(this->get_logger(), "左转");
break;
case 'd': // 右转
twist_msg.linear.x = 0.0;
twist_msg.angular.z = -angular_speed_;
RCLCPP_INFO(this->get_logger(), "右转");
break;
case 'x': // 停止
twist_msg.linear.x = 0.0;
twist_msg.angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "停止");
break;
case 'q': // 退出
RCLCPP_INFO(this->get_logger(), "退出控制");
rclcpp::shutdown();
return;
default:
twist_msg.linear.x = 0.0;
twist_msg.angular.z = 0.0;
print_help();
break;
}
twist_pub_->publish(twist_msg);
}
}
// 成员变量
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
rclcpp::TimerBase::SharedPtr timer_;
double linear_speed_; // 线速度（m/s）
double angular_speed_; // 角速度（rad/s）
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<KeyboardControlNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

165
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/bot0.ros2_control.xacro Normal file
View File

@@ -0,0 +1,165 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 宏定义：ros2_control硬件接口配置（适配位置控制器和轮子速度控制器） -->
<xacro:macro name="bot0_ros2_control">
<!-- 1. ros2_control系统配置：定义所有关节的控制接口 -->
<ros2_control name="Bot0GazeboSystem" type="system">
<!-- 硬件插件：关联Gazebo仿真 -->
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<!-- 2. 腿部+手臂关节（位置控制接口，对应bot0_position_controller） -->
<!-- 2.1 腿部关节 -->
<joint name="left_hip_pitch_joint">
<command_interface name="position"> <!-- 位置控制器的命令接口 -->
<param name="min">-1.0</param> <!-- 对应关节限位（弧度） -->
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" /> <!-- 反馈接口：位置 -->
<state_interface name="velocity" /> <!-- 反馈接口：速度 -->
<state_interface name="effort" /> <!-- 反馈接口：力（用于调试） -->
</joint>
<joint name="left_leg_pitch_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_hip_pitch_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_leg_pitch_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<!-- 2.2 手臂关节 -->
<joint name="left_shoulder_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="left_arm1_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="left_arm2_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_shoulder_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_arm1_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_arm2_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<!-- 3. 车轮关节（速度控制接口，对应bot0_wheel_velocity_controller） -->
<joint name="left_wheel1_joint">
<command_interface name="velocity"> <!-- 速度控制器的命令接口 -->
<param name="min">-50</param> <!-- 对应关节速度限位（rad/s） -->
<param name="max">50</param>
</command_interface>
<state_interface name="position" /> <!-- 反馈接口：位置 -->
<state_interface name="velocity" /> <!-- 反馈接口：速度 -->
<state_interface name="effort" /> <!-- 反馈接口：力 -->
</joint>
<joint name="left_wheel2_joint">
<command_interface name="velocity">
<param name="min">-50</param>
<param name="max">50</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_wheel1_joint">
<command_interface name="velocity">
<param name="min">-50</param>
<param name="max">50</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_wheel2_joint">
<command_interface name="velocity">
<param name="min">-50</param>
<param name="max">50</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
</ros2_control>
<!-- 4. Gazebo插件配置：关联控制器参数文件 -->
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<!-- 加载控制器配置文件（路径需替换为你的实际路径） -->
<parameters>$(find hive_core_r0)/config/bot0_ros2_controller.yaml</parameters>
</plugin>
</gazebo>
</xacro:macro>
</robot>

15
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/generic_joint.xacro Normal file
View File

@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="generic_joint" params="joint_name type parent child
origin_xyz origin_rpy axis
limit_lower limit_upper effort velocity">
<joint name="${joint_name}" type="${type}">
<origin xyz="${origin_xyz}" rpy="${origin_rpy}" />
<parent link="${parent}" />
<child link="${child}" />
<axis xyz="${axis}" />
<limit lower="${limit_lower}" upper="${limit_upper}"
effort="${effort}" velocity="${velocity}" />
</joint>
</xacro:macro>
</robot>

65
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/generic_link.xacro Normal file
View File

@@ -0,0 +1,65 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="generic_link" params="link_name mesh_file mass
inertia_ixx inertia_ixy inertia_ixz
inertia_iyy inertia_iyz inertia_izz
origin_xyz origin_rpy color_rgba enable_collision">
<link name="${link_name}">
<!-- 惯性参数 -->
<inertial>
<origin xyz="${origin_xyz}" rpy="${origin_rpy}" />
<mass value="${mass}" />
<inertia ixx="${inertia_ixx}" ixy="${inertia_ixy}" ixz="${inertia_ixz}"
iyy="${inertia_iyy}" iyz="${inertia_iyz}" izz="${inertia_izz}" />
</inertial>
<!-- 可视化模型 -->
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="${mesh_file}" />
</geometry>
<material name="${link_name}_material">
<color rgba="${color_rgba}" />
</material>
</visual>
<!-- 碰撞模型（与可视化模型一致） -->
<!-- 条件生成碰撞体：仅当enable_collision=true时添加 -->
<xacro:if value="${enable_collision}">
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.05" length="0.03"/>
</geometry>
<material name="blue">
<color rgba="0.1 0.1 1.0 0.5" />
</material>
</collision>
</xacro:if>
</link>
<!-- 仅当启用碰撞时，添加碰撞物理参数 -->
<xacro:if value="${enable_collision}">
<gazebo reference="${link_name}">
<collision>
<surface>
<contact>
<ode>
<kp>1e3</kp> <!-- 刚度：1e6~1e8（根据质量调整） -->
<kd>1e2</kd> <!-- 阻尼：与kp比例1:100左右 -->
<mu1>0.8</mu1> <!-- 静摩擦系数：0.5~1.0 -->
<mu2>0.8</mu2> <!-- 动摩擦系数：与mu1接近 -->
<min_depth>0.001</min_depth> <!-- 允许微小穿透，减少抖动 -->
</ode>
</contact>
<bounce>
<restitution_coefficient>0.0</restitution_coefficient> <!-- 反弹系数0 -->
<threshold>10000.0</threshold> <!-- 阈值设大，确保所有碰撞都不反弹 -->
</bounce>
</surface>
</collision>
</gazebo>
</xacro:if>
</xacro:macro>
</robot>

860
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf Executable file
View File

@@ -0,0 +1,860 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="hive_core_bot0">
<link
name="base_link">
<inertial>
<origin
xyz="-0.0598891820748674 5.31626531352636E-07 0.164869219254393"
rpy="0 0 0" />
<mass
value="5.4247767" />
<inertia
ixx="0.1131876"
ixy="0.0000008"
ixz="0.0001546"
iyy="0.0858545"
iyz="0.0000006"
izz="0.0325040" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.666666666666667 0.666666666666667 0.666666666666667 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link
name="left_hip_pitch_link">
<inertial>
<origin
xyz="3.93767161736252E-06 0.161490126813208 0.0158567790568734"
rpy="0 0 0" />
<mass
value="0.6475263" />
<inertia
ixx="0.0182516"
ixy="0.0000058"
ixz="0.0001729"
iyy="0.0181748"
iyz="-0.0006055"
izz="0.0003913" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_hip_pitch_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_hip_pitch_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_hip_pitch_joint"
type="revolute">
<origin
xyz="-0.06 -0.1358 -0.037269"
rpy="-1.5708 -0.0095732 0" />
<parent
link="base_link" />
<child
link="left_hip_pitch_link" />
<axis
xyz="0 0 -1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="left_leg_pitch_link">
<inertial>
<origin
xyz="-2.63464180747426E-05 0.197159145100908 0.000498203326348359"
rpy="0 0 0" />
<mass
value="0.5126336" />
<inertia
ixx="0.0135141"
ixy="0.0000005"
ixz="-0.0000325"
iyy="0.0135986"
iyz="0.0001480"
izz="0.0002025" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_leg_pitch_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 0.372549019607843 0.294117647058824 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_leg_pitch_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_leg_pitch_joint"
type="revolute">
<origin
xyz="0 0 -0.0295"
rpy="0 0 0.01205" />
<parent
link="left_hip_pitch_link" />
<child
link="left_leg_pitch_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="left_wheel2_link">
<inertial>
<origin
xyz="4.1585564079967E-08 -5.47447156262759E-06 0.0187838263233595"
rpy="0 0 0" />
<mass
value="0.745" />
<inertia
ixx="0.0004716"
ixy="0"
ixz="0"
iyy="0.0007945"
iyz="0.0000001"
izz="0.0004716" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_wheel2_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_wheel2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_wheel2_joint"
type="revolute">
<origin
xyz="0 0.4805 0.004"
rpy="-3.1416 0 -0.0024764" />
<parent
link="left_leg_pitch_link" />
<child
link="left_wheel2_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1000"
upper="1000"
effort="100"
velocity="50" />
</joint>
<link
name="left_wheel1_link">
<inertial>
<origin
xyz="1.97042367054093E-06 5.10773910927798E-06 0.0187838263233595"
rpy="0 0 0" />
<mass
value="0.745" />
<inertia
ixx="0.0004716"
ixy="0"
ixz="0"
iyy="0.0007945"
iyz="0.0000001"
izz="0.0004716" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_wheel1_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_wheel1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_wheel1_joint"
type="revolute">
<origin
xyz="0 0.48 0.01"
rpy="0 0 0.0095732" />
<parent
link="left_hip_pitch_link" />
<child
link="left_wheel1_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1000"
upper="1000"
effort="100"
velocity="50" />
</joint>
<link
name="right_hip_pitch_link">
<inertial>
<origin
xyz="-3.97209815898069E-06 -0.161490067811833 0.0158567790568734"
rpy="0 0 0" />
<mass
value="0.6475263" />
<inertia
ixx="0.0182531"
ixy="-0.0000017"
ixz="0.0000501"
iyy="0.0181748"
iyz="0.0006055"
izz="0.0003897" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_hip_pitch_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_hip_pitch_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_hip_pitch_joint"
type="revolute">
<origin
xyz="-0.06 0.1358 -0.037269"
rpy="1.5708 -0.0029091 0" />
<parent
link="base_link" />
<child
link="right_hip_pitch_link" />
<axis
xyz="0 0 -1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="right_leg_pitch_link">
<inertial>
<origin
xyz="3.43008600186093E-05 -0.197159143801203 0.000498203308628542"
rpy="0 0 0" />
<mass
value="0.5126336" />
<inertia
ixx="0.0135142"
ixy="0.0000001"
ixz="0.0000001"
iyy="0.0135986"
iyz="-0.0001480"
izz="0.0002024" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_leg_pitch_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_leg_pitch_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_leg_pitch_joint"
type="revolute">
<origin
xyz="0 0 -0.0295"
rpy="0 0 -0.0029091" />
<parent
link="right_hip_pitch_link" />
<child
link="right_leg_pitch_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="right_wheel2_link">
<inertial>
<origin
xyz="-5.49216228909084E-08 5.47435401360152E-06 0.0187838263233596"
rpy="0 0 0" />
<mass
value="0.745" />
<inertia
ixx="0.0004716"
ixy="0"
ixz="0"
iyy="0.0007945"
iyz="0.0000001"
izz="0.0004716" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_wheel2_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_wheel2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_wheel2_joint"
type="revolute">
<origin
xyz="1.9386E-05 -0.4805 0.004"
rpy="3.1416 0 0" />
<parent
link="right_leg_pitch_link" />
<child
link="right_wheel2_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1000"
upper="1000"
effort="100"
velocity="50" />
</joint>
<link
name="right_wheel1_link">
<inertial>
<origin
xyz="-1.90651569380871E-06 -5.13193590762073E-06 0.0187838263233596"
rpy="0 0 0" />
<mass
value="0.745" />
<inertia
ixx="0.0004716"
ixy="0"
ixz="0"
iyy="0.0007945"
iyz="0.0000001"
izz="0.0004716" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_wheel1_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_wheel1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_wheel1_joint"
type="revolute">
<origin
xyz="0 -0.48 0.01"
rpy="0 0 -0.0029091" />
<parent
link="right_hip_pitch_link" />
<child
link="right_wheel1_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1000"
upper="1000"
effort="100"
velocity="50" />
</joint>
<link
name="left_shoulder_link">
<inertial>
<origin
xyz="-0.0144837915216556 0.000108169744531657 0.0209530786562939"
rpy="0 0 0" />
<mass
value="0.0131461" />
<inertia
ixx="0.0000070"
ixy="0"
ixz="0.0000017"
iyy="0.0000071"
iyz="0"
izz="0.0000045" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_shoulder_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_shoulder_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_shoulder_joint"
type="revolute">
<origin
xyz="-0.06 -0.0878 0.36288"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="left_shoulder_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="left_arm1_link">
<inertial>
<origin
xyz="-0.00228631297282665 -0.000694770956028379 0.0248681244469774"
rpy="0 0 0" />
<mass
value="0.5854363" />
<inertia
ixx="0.0008783"
ixy="-0.0000561"
ixz="0.0000086"
iyy="0.0002821"
iyz="-0.0000017"
izz="0.0009299" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_arm1_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_arm1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_arm1_joint"
type="revolute">
<origin
xyz="0 0 0.0385"
rpy="1.5708 0 -0.0074682" />
<parent
link="left_shoulder_link" />
<child
link="left_arm1_link" />
<axis
xyz="-1 0 0" />
<limit
lower="-1.5"
upper="1.5"
effort="42.8"
velocity="50" />
</joint>
<link
name="left_arm2_link">
<inertial>
<origin
xyz="-0.0237485342372672 -0.0546317049801525 1.0877545797372E-05"
rpy="0 0 0" />
<mass
value="0.7955302" />
<inertia
ixx="0.0035648"
ixy="-0.0000258"
ixz="0.0000009"
iyy="0.0002355"
iyz="-0.0001213"
izz="0.0036543" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_arm2_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_arm2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_arm2_joint"
type="revolute">
<origin
xyz="0.020127 0.0040341 0.17817"
rpy="-1.6062 0.0074682 0" />
<parent
link="left_arm1_link" />
<child
link="left_arm2_link" />
<axis
xyz="-0.99997 0.0074634 0.0002647" />
<limit
lower="-1.5"
upper="1.5"
effort="18"
velocity="50" />
</joint>
<link
name="right_shoulder_link">
<inertial>
<origin
xyz="-0.0144831349305154 -0.000175271563297497 0.0209530786562937"
rpy="0 0 0" />
<mass
value="0.0131461" />
<inertia
ixx="0.0000070"
ixy="0"
ixz="0.0000017"
iyy="0.0000071"
iyz="0"
izz="0.0000045" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_shoulder_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_shoulder_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_shoulder_joint"
type="revolute">
<origin
xyz="-0.06 0.0878 0.36288"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="right_shoulder_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="right_arm1_link">
<inertial>
<origin
xyz="-0.00258654183724675 0.00163071193731268 -0.0247948407356193"
rpy="0 0 0" />
<mass
value="0.5854363" />
<inertia
ixx="0.0008778"
ixy="0.0000592"
ixz="-0.0000064"
iyy="0.0002836"
iyz="-0.0000262"
izz="0.0009288" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_arm1_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_arm1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_arm1_joint"
type="revolute">
<origin
xyz="0 0 0.0385"
rpy="1.5708 0 0" />
<parent
link="right_shoulder_link" />
<child
link="right_arm1_link" />
<axis
xyz="1 0 0" />
<limit
lower="-1.5"
upper="1.5"
effort="42.8"
velocity="50" />
</joint>
<link
name="right_arm2_link">
<inertial>
<origin
xyz="-0.0240175529876588 0.0558375631647781 2.11385309717427E-06"
rpy="0 0 0" />
<mass
value="0.7955302" />
<inertia
ixx="0.0035728"
ixy="0.0000412"
ixz="-0.0000004"
iyy="0.0002317"
iyz="0.0000292"
izz="0.0036666" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_arm2_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_arm2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_arm2_joint"
type="revolute">
<origin
xyz="0.017969 0.0026972 -0.17843"
rpy="-1.5793 0 0" />
<parent
link="right_arm1_link" />
<child
link="right_arm2_link" />
<axis
xyz="-0.99993 -0.0121 -0.00010316" />
<limit
lower="-1.5"
upper="1.5"
effort="18"
velocity="50" />
</joint>
</robot>

440
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf copy.xacro Normal file
View File

@@ -0,0 +1,440 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- 转换自 URDF 的 XACRO 文件，保留原模型参数并简化重复结构 -->
<robot name="hive_core_bot0" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find hive_core_r0)/urdf/generic_link.xacro" />
<xacro:include filename="$(find hive_core_r0)/urdf/generic_joint.xacro" />
<!-- 1. 基础链接（base_link） -->
<xacro:generic_link
link_name="base_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/base_link.STL"
mass="0.4247767"
inertia_ixx="0.1131876" inertia_ixy="0.0000008" inertia_ixz="0.0001546"
inertia_iyy="0.0858545" inertia_iyz="0.0000006" inertia_izz="0.0325040"
origin_xyz="-0.0598891820748674 5.31626531352636E-07 0.164869219254393"
origin_rpy="0 0 0"
color_rgba="0.666666666666667 0.666666666666667 0.666666666666667 1"
enable_collision="false"
/>
<!-- <link name="world">
<inertial>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0"
iyy="0.0" iyz="0.0"
izz="0.0"/>
</inertial>
</link>
<joint name="base_link_to_world_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="world" />
<child link="base_link" />
</joint> -->
<!-- 2. 左髋关节及腿部（left_hip/leg/wheel） -->
<!-- 2.1 左髋链接 -->
<xacro:generic_link
link_name="left_hip_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_hip_pitch_link.STL"
mass="0.6475263"
inertia_ixx="0.0182516" inertia_ixy="0.0000058" inertia_ixz="0.0001729"
inertia_iyy="0.0181748" inertia_iyz="-0.0006055" inertia_izz="0.0003913"
origin_xyz="3.93767161736252E-06 0.161490126813208 0.0158567790568734"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 2.2 左髋关节 -->
<xacro:generic_joint
joint_name="left_hip_pitch_joint"
type="revolute"
parent="base_link"
child="left_hip_pitch_link"
origin_xyz="-0.06 -0.1358 -0.037269"
origin_rpy="-1.5708 0.523 0.0"
axis="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 2.3 左腿链接 -->
<xacro:generic_link
link_name="left_leg_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_leg_pitch_link.STL"
mass="0.5126336"
inertia_ixx="0.0135141" inertia_ixy="0.0000005" inertia_ixz="-0.0000325"
inertia_iyy="0.0135986" inertia_iyz="0.0001480" inertia_izz="0.0002025"
origin_xyz="-2.63464180747426E-05 0.197159145100908 0.000498203326348359"
origin_rpy="0 0 0"
color_rgba="1 0.372549019607843 0.294117647058824 1"
enable_collision="false"
/>
<!-- 2.4 左腿关节 -->
<xacro:generic_joint
joint_name="left_leg_pitch_joint"
type="revolute"
parent="left_hip_pitch_link"
child="left_leg_pitch_link"
origin_xyz="0 0 -0.0295"
origin_rpy="0 0 -1.046"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 2.5 左车轮2链接 -->
<xacro:generic_link
link_name="left_wheel2_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_wheel2_link.STL"
mass="0.745"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="4.1585564079967E-08 -5.47447156262759E-06 0.0187838263233595"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 2.6 左车轮2关节 -->
<xacro:generic_joint
joint_name="left_wheel2_joint"
type="revolute"
parent="left_leg_pitch_link"
child="left_wheel2_link"
origin_xyz="0 0.4805 0.004"
origin_rpy="-3.1416 0 -0.0024764"
axis="0 0 1"
limit_lower="-1000"
limit_upper="1000"
effort="2"
velocity="3"
/>
<!-- 2.7 左车轮1链接 -->
<xacro:generic_link
link_name="left_wheel1_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_wheel1_link.STL"
mass="0.745"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="1.97042367054093E-06 5.10773910927798E-06 0.0187838263233595"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 2.8 左车轮1关节 -->
<xacro:generic_joint
joint_name="left_wheel1_joint"
type="revolute"
parent="left_hip_pitch_link"
child="left_wheel1_link"
origin_xyz="0 0.48 0.01"
origin_rpy="0 0 0.0095732"
axis="0 0 1"
limit_lower="-1000"
limit_upper="1000"
effort="2"
velocity="3"
/>
<!-- 3. 右髋关节及腿部（right_hip/leg/wheel） -->
<!-- 3.1 右髋链接 -->
<xacro:generic_link
link_name="right_hip_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_hip_pitch_link.STL"
mass="0.6475263"
inertia_ixx="0.0182531" inertia_ixy="-0.0000017" inertia_ixz="0.0000501"
inertia_iyy="0.0181748" inertia_iyz="0.0006055" inertia_izz="0.0003897"
origin_xyz="-3.97209815898069E-06 -0.161490067811833 0.0158567790568734"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 3.2 右髋关节 -->
<xacro:generic_joint
joint_name="right_hip_pitch_joint"
type="revolute"
parent="base_link"
child="right_hip_pitch_link"
origin_xyz="-0.06 0.1358 -0.037269"
origin_rpy="1.5708 0.523 0"
axis="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 3.3 右腿链接 -->
<xacro:generic_link
link_name="right_leg_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_leg_pitch_link.STL"
mass="0.5126336"
inertia_ixx="0.0135142" inertia_ixy="0.0000001" inertia_ixz="0.0000001"
inertia_iyy="0.0135986" inertia_iyz="-0.0001480" inertia_izz="0.0002024"
origin_xyz="3.43008600186093E-05 -0.197159143801203 0.000498203308628542"
origin_rpy="0 0 0"
color_rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"
enable_collision="false"
/>
<!-- 3.4 右腿关节 -->
<xacro:generic_joint
joint_name="right_leg_pitch_joint"
type="revolute"
parent="right_hip_pitch_link"
child="right_leg_pitch_link"
origin_xyz="0 0 -0.0295"
origin_rpy="0 0 1.046"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 3.5 右车轮2链接 -->
<xacro:generic_link
link_name="right_wheel2_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_wheel2_link.STL"
mass="0.745"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="-5.49216228909084E-08 5.47435401360152E-06 0.0187838263233596"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 3.6 右车轮2关节 -->
<xacro:generic_joint
joint_name="right_wheel2_joint"
type="revolute"
parent="right_leg_pitch_link"
child="right_wheel2_link"
origin_xyz="1.9386E-05 -0.4805 0.004"
origin_rpy="3.1416 0 0"
axis="0 0 1"
limit_lower="-1000"
limit_upper="1000"
effort="2"
velocity="3"
/>
<!-- 3.7 右车轮1链接 -->
<xacro:generic_link
link_name="right_wheel1_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_wheel1_link.STL"
mass="0.745"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="-1.90651569380871E-06 -5.13193590762073E-06 0.0187838263233596"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 3.8 右车轮1关节 -->
<xacro:generic_joint
joint_name="right_wheel1_joint"
type="revolute"
parent="right_hip_pitch_link"
child="right_wheel1_link"
origin_xyz="0 -0.48 0.01"
origin_rpy="0 0 -0.0029091"
axis="0 0 1"
limit_lower="-1000"
limit_upper="1000"
effort="2"
velocity="3"
/>
<!-- 4. 左手臂（left_shoulder/arm） -->
<!-- 4.1 左肩链接 -->
<xacro:generic_link
link_name="left_shoulder_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_shoulder_link.STL"
mass="0.0131461"
inertia_ixx="0.0000070" inertia_ixy="0" inertia_ixz="0.0000017"
inertia_iyy="0.0000071" inertia_iyz="0" inertia_izz="0.0000045"
origin_xyz="-0.0144837915216556 0.000108169744531657 0.0209530786562939"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.2 左肩关节 -->
<xacro:generic_joint
joint_name="left_shoulder_joint"
type="revolute"
parent="base_link"
child="left_shoulder_link"
origin_xyz="-0.06 -0.0878 0.36288"
origin_rpy="0 0 0"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 4.3 左手臂1链接 -->
<xacro:generic_link
link_name="left_arm1_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_arm1_link.STL"
mass="0.5854363"
inertia_ixx="0.0008783" inertia_ixy="-0.0000561" inertia_ixz="0.0000086"
inertia_iyy="0.0002821" inertia_iyz="-0.0000017" inertia_izz="0.0009299"
origin_xyz="-0.00228631297282665 -0.000694770956028379 0.0248681244469774"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.4 左手臂1关节 -->
<xacro:generic_joint
joint_name="left_arm1_joint"
type="revolute"
parent="left_shoulder_link"
child="left_arm1_link"
origin_xyz="0 0 0.0385"
origin_rpy="1.5708 0 -0.0074682"
axis="-1 0 0"
limit_lower="-1.5"
limit_upper="1.5"
effort="2"
velocity="3"
/>
<!-- 4.5 左手臂2链接 -->
<xacro:generic_link
link_name="left_arm2_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_arm2_link.STL"
mass="0.7955302"
inertia_ixx="0.0035648" inertia_ixy="-0.0000258" inertia_ixz="0.0000009"
inertia_iyy="0.0002355" inertia_iyz="-0.0001213" inertia_izz="0.0036543"
origin_xyz="-0.0237485342372672 -0.0546317049801525 1.0877545797372E-05"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.6 左手臂2关节 -->
<xacro:generic_joint
joint_name="left_arm2_joint"
type="revolute"
parent="left_arm1_link"
child="left_arm2_link"
origin_xyz="0.020127 0.0040341 0.17817"
origin_rpy="-1.6062 0.0074682 0"
axis="-0.99997 0.0074634 0.0002647"
limit_lower="-1.5"
limit_upper="1.5"
effort="2"
velocity="3"
/>
<!-- 5. 右手臂（right_shoulder/arm） -->
<!-- 5.1 右肩链接 -->
<xacro:generic_link
link_name="right_shoulder_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_shoulder_link.STL"
mass="0.0131461"
inertia_ixx="0.0000070" inertia_ixy="0" inertia_ixz="0.0000017"
inertia_iyy="0.0000071" inertia_iyz="0" inertia_izz="0.0000045"
origin_xyz="-0.0144831349305154 -0.000175271563297497 0.0209530786562937"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.2 右肩关节 -->
<xacro:generic_joint
joint_name="right_shoulder_joint"
type="revolute"
parent="base_link"
child="right_shoulder_link"
origin_xyz="-0.06 0.0878 0.36288"
origin_rpy="0 0 0"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 5.3 右手臂1链接 -->
<xacro:generic_link
link_name="right_arm1_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_arm1_link.STL"
mass="0.5854363"
inertia_ixx="0.0008778" inertia_ixy="0.0000592" inertia_ixz="-0.0000064"
inertia_iyy="0.0002836" inertia_iyz="-0.0000262" inertia_izz="0.0009288"
origin_xyz="-0.00258654183724675 0.00163071193731268 -0.0247948407356193"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.4 右手臂1关节 -->
<xacro:generic_joint
joint_name="right_arm1_joint"
type="revolute"
parent="right_shoulder_link"
child="right_arm1_link"
origin_xyz="0 0 0.0385"
origin_rpy="1.5708 0 0"
axis="1 0 0"
limit_lower="-1.5"
limit_upper="1.5"
effort="2"
velocity="3"
/>
<!-- 5.5 右手臂2链接 -->
<xacro:generic_link
link_name="right_arm2_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_arm2_link.STL"
mass="0.7955302"
inertia_ixx="0.0035728" inertia_ixy="0.0000412" inertia_ixz="-0.0000004"
inertia_iyy="0.0002317" inertia_iyz="0.0000292" inertia_izz="0.0036666"
origin_xyz="-0.0240175529876588 0.0558375631647781 2.11385309717427E-06"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.6 右手臂2关节 -->
<xacro:generic_joint
joint_name="right_arm2_joint"
type="revolute"
parent="right_arm1_link"
child="right_arm2_link"
origin_xyz="0.017969 0.0026972 -0.17843"
origin_rpy="-1.5793 0 0"
axis="-0.99993 -0.0121 -0.00010316"
limit_lower="-1.5"
limit_upper="1.5"
effort="2"
velocity="3"
/>
<xacro:include filename="$(find hive_core_r0)/urdf/bot0.ros2_control.xacro" />
<xacro:bot0_ros2_control />
</robot>

438
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf.xacro Normal file
View File

@@ -0,0 +1,438 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- 转换自 URDF 的 XACRO 文件，保留原模型参数并简化重复结构 -->
<robot name="hive_core_bot0" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find hive_core_r0)/urdf/generic_link.xacro" />
<xacro:include filename="$(find hive_core_r0)/urdf/generic_joint.xacro" />
<!-- 1. 基础链接（base_link） -->
<xacro:generic_link
link_name="base_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/base_link.STL"
mass="0.2"
inertia_ixx="0.00076" inertia_ixy="0.0000008" inertia_ixz="0.00001546"
inertia_iyy="0.000858545" inertia_iyz="0.0000006" inertia_izz="0.000325040"
origin_xyz="-0.0598891820748674 5.31626531352636E-07 0.164869219254393"
origin_rpy="0 0 0"
color_rgba="0.666666666666667 0.666666666666667 0.666666666666667 1"
enable_collision="false"
/>
<link name="world">
<inertial>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0"
iyy="0.0" iyz="0.0"
izz="0.0"/>
</inertial>
</link>
<joint name="base_link_to_world_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="world" />
<child link="base_link" />
</joint>
<!-- 2. 左髋关节及腿部（left_hip/leg/wheel） -->
<!-- 2.1 左髋链接 -->
<xacro:generic_link
link_name="left_hip_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_hip_pitch_link.STL"
mass="0.1475263"
inertia_ixx="0.000182516" inertia_ixy="0.0000058" inertia_ixz="0.0001729"
inertia_iyy="0.000181748" inertia_iyz="-0.0006055" inertia_izz="0.0003913"
origin_xyz="3.93767161736252E-06 0.161490126813208 0.0158567790568734"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 2.2 左髋关节 -->
<xacro:generic_joint
joint_name="left_hip_pitch_joint"
type="revolute"
parent="base_link"
child="left_hip_pitch_link"
origin_xyz="-0.06 -0.1358 -0.037269"
origin_rpy="-1.5708 0.523 0.0"
axis="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 2.3 左腿链接 -->
<xacro:generic_link
link_name="left_leg_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_leg_pitch_link.STL"
mass="0.1126336"
inertia_ixx="0.000135141" inertia_ixy="0.0000005" inertia_ixz="-0.0000325"
inertia_iyy="0.000135986" inertia_iyz="0.0001480" inertia_izz="0.0002025"
origin_xyz="-2.63464180747426E-05 0.197159145100908 0.000498203326348359"
origin_rpy="0 0 0"
color_rgba="1 0.372549019607843 0.294117647058824 1"
enable_collision="false"
/>
<!-- 2.4 左腿关节 -->
<xacro:generic_joint
joint_name="left_leg_pitch_joint"
type="revolute"
parent="left_hip_pitch_link"
child="left_leg_pitch_link"
origin_xyz="0 0 -0.0295"
origin_rpy="0 0 -1.046"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 2.5 左车轮2链接 -->
<xacro:generic_link
link_name="left_wheel2_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_wheel2_link.STL"
mass="0.145"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="4.1585564079967E-08 -5.47447156262759E-06 0.0187838263233595"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 2.6 左车轮2关节 -->
<xacro:generic_joint
joint_name="left_wheel2_joint"
type="revolute"
parent="left_leg_pitch_link"
child="left_wheel2_link"
origin_xyz="0 0.4805 0.004"
origin_rpy="-3.1416 0 -0.0024764"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 2.7 左车轮1链接 -->
<xacro:generic_link
link_name="left_wheel1_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_wheel1_link.STL"
mass="0.145"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="1.97042367054093E-06 5.10773910927798E-06 0.0187838263233595"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 2.8 左车轮1关节 -->
<xacro:generic_joint
joint_name="left_wheel1_joint"
type="revolute"
parent="left_hip_pitch_link"
child="left_wheel1_link"
origin_xyz="0 0.48 0.01"
origin_rpy="0 0 0.0095732"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 3. 右髋关节及腿部（right_hip/leg/wheel） -->
<!-- 3.1 右髋链接 -->
<xacro:generic_link
link_name="right_hip_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_hip_pitch_link.STL"
mass="0.1475263"
inertia_ixx="0.000182531" inertia_ixy="-0.0000017" inertia_ixz="0.0000501"
inertia_iyy="0.000181748" inertia_iyz="0.0006055" inertia_izz="0.0003897"
origin_xyz="-3.97209815898069E-06 -0.161490067811833 0.0158567790568734"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 3.2 右髋关节 -->
<xacro:generic_joint
joint_name="right_hip_pitch_joint"
type="revolute"
parent="base_link"
child="right_hip_pitch_link"
origin_xyz="-0.06 0.1358 -0.037269"
origin_rpy="1.5708 0.523 0"
axis="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 3.3 右腿链接 -->
<xacro:generic_link
link_name="right_leg_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_leg_pitch_link.STL"
mass="0.1126336"
inertia_ixx="0.000135142" inertia_ixy="0.0000001" inertia_ixz="0.0000001"
inertia_iyy="0.000135986" inertia_iyz="-0.0001480" inertia_izz="0.0002024"
origin_xyz="3.43008600186093E-05 -0.197159143801203 0.000498203308628542"
origin_rpy="0 0 0"
color_rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"
enable_collision="false"
/>
<!-- 3.4 右腿关节 -->
<xacro:generic_joint
joint_name="right_leg_pitch_joint"
type="revolute"
parent="right_hip_pitch_link"
child="right_leg_pitch_link"
origin_xyz="0 0 -0.0295"
origin_rpy="0 0 1.046"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 3.5 右车轮2链接 -->
<xacro:generic_link
link_name="right_wheel2_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_wheel2_link.STL"
mass="0.145"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="-5.49216228909084E-08 5.47435401360152E-06 0.0187838263233596"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 3.6 右车轮2关节 -->
<xacro:generic_joint
joint_name="right_wheel2_joint"
type="revolute"
parent="right_leg_pitch_link"
child="right_wheel2_link"
origin_xyz="1.9386E-05 -0.4805 0.004"
origin_rpy="3.1416 0 0"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 3.7 右车轮1链接 -->
<xacro:generic_link
link_name="right_wheel1_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_wheel1_link.STL"
mass="0.145"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="-1.90651569380871E-06 -5.13193590762073E-06 0.0187838263233596"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 3.8 右车轮1关节 -->
<xacro:generic_joint
joint_name="right_wheel1_joint"
type="revolute"
parent="right_hip_pitch_link"
child="right_wheel1_link"
origin_xyz="0 -0.48 0.01"
origin_rpy="0 0 -0.0029091"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 4. 左手臂（left_shoulder/arm） -->
<!-- 4.1 左肩链接 -->
<xacro:generic_link
link_name="left_shoulder_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_shoulder_link.STL"
mass="0.0131461"
inertia_ixx="0.0000070" inertia_ixy="0" inertia_ixz="0.0000017"
inertia_iyy="0.0000071" inertia_iyz="0" inertia_izz="0.0000045"
origin_xyz="-0.0144837915216556 0.000108169744531657 0.0209530786562939"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.2 左肩关节 -->
<xacro:generic_joint
joint_name="left_shoulder_joint"
type="revolute"
parent="base_link"
child="left_shoulder_link"
origin_xyz="-0.06 -0.0878 0.36288"
origin_rpy="0 0 0"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 4.3 左手臂1链接 -->
<xacro:generic_link
link_name="left_arm1_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_arm1_link.STL"
mass="0.1854363"
inertia_ixx="0.0008783" inertia_ixy="-0.0000561" inertia_ixz="0.0000086"
inertia_iyy="0.0002821" inertia_iyz="-0.0000017" inertia_izz="0.0009299"
origin_xyz="-0.00228631297282665 -0.000694770956028379 0.0248681244469774"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.4 左手臂1关节 -->
<xacro:generic_joint
joint_name="left_arm1_joint"
type="revolute"
parent="left_shoulder_link"
child="left_arm1_link"
origin_xyz="0 0 0.0385"
origin_rpy="1.5708 0 -0.0074682"
axis="-1 0 0"
limit_lower="-1.5"
limit_upper="1.5"
effort="2000"
velocity="30"
/>
<!-- 4.5 左手臂2链接 -->
<xacro:generic_link
link_name="left_arm2_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_arm2_link.STL"
mass="0.1955302"
inertia_ixx="0.000035648" inertia_ixy="-0.0000258" inertia_ixz="0.0000009"
inertia_iyy="0.0002355" inertia_iyz="-0.0001213" inertia_izz="0.0036543"
origin_xyz="-0.0237485342372672 -0.0546317049801525 1.0877545797372E-05"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.6 左手臂2关节 -->
<xacro:generic_joint
joint_name="left_arm2_joint"
type="revolute"
parent="left_arm1_link"
child="left_arm2_link"
origin_xyz="0.020127 0.0040341 0.17817"
origin_rpy="-1.6062 0.0074682 0"
axis="-0.99997 0.0074634 0.0002647"
limit_lower="-1.5"
limit_upper="1.5"
effort="2000"
velocity="30"
/>
<!-- 5. 右手臂（right_shoulder/arm） -->
<!-- 5.1 右肩链接 -->
<xacro:generic_link
link_name="right_shoulder_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_shoulder_link.STL"
mass="0.0131461"
inertia_ixx="0.0000070" inertia_ixy="0" inertia_ixz="0.0000017"
inertia_iyy="0.0000071" inertia_iyz="0" inertia_izz="0.0000045"
origin_xyz="-0.0144831349305154 -0.000175271563297497 0.0209530786562937"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.2 右肩关节 -->
<xacro:generic_joint
joint_name="right_shoulder_joint"
type="revolute"
parent="base_link"
child="right_shoulder_link"
origin_xyz="-0.06 0.0878 0.36288"
origin_rpy="0 0 0"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 5.3 右手臂1链接 -->
<xacro:generic_link
link_name="right_arm1_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_arm1_link.STL"
mass="0.1854363"
inertia_ixx="0.0008778" inertia_ixy="0.0000592" inertia_ixz="-0.0000064"
inertia_iyy="0.0002836" inertia_iyz="-0.0000262" inertia_izz="0.0009288"
origin_xyz="-0.00258654183724675 0.00163071193731268 -0.0247948407356193"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.4 右手臂1关节 -->
<xacro:generic_joint
joint_name="right_arm1_joint"
type="revolute"
parent="right_shoulder_link"
child="right_arm1_link"
origin_xyz="0 0 0.0385"
origin_rpy="1.5708 0 0"
axis="1 0 0"
limit_lower="-1.5"
limit_upper="1.5"
effort="2000"
velocity="30"
/>
<!-- 5.5 右手臂2链接 -->
<xacro:generic_link
link_name="right_arm2_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_arm2_link.STL"
mass="0.1955302"
inertia_ixx="0.0035728" inertia_ixy="0.0000412" inertia_ixz="-0.0000004"
inertia_iyy="0.0002317" inertia_iyz="0.0000292" inertia_izz="0.0036666"
origin_xyz="-0.0240175529876588 0.0558375631647781 2.11385309717427E-06"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.6 右手臂2关节 -->
<xacro:generic_joint
joint_name="right_arm2_joint"
type="revolute"
parent="right_arm1_link"
child="right_arm2_link"
origin_xyz="0.017969 0.0026972 -0.17843"
origin_rpy="-1.5793 0 0"
axis="-0.99993 -0.0121 -0.00010316"
limit_lower="-1.5"
limit_upper="1.5"
effort="2000"
velocity="30"
/>
<xacro:include filename="$(find hive_core_r0)/urdf/hiveBot0/bot0.ros2_control.xacro" />
<xacro:bot0_ros2_control />
</robot>

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/base_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_arm1_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_arm2_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_hip_pitch_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_leg_pitch_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_shoulder_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_wheel1_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_wheel2_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_arm1_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_arm2_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_hip_pitch_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_leg_pitch_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_shoulder_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_wheel1_link.STL Executable file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_wheel2_link.STL Executable file
View File

Binary file not shown.

152
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/robot_roll.py Executable file
View File

@@ -0,0 +1,152 @@
import mujoco
import mujoco.viewer
import numpy as np
import time
import math
# 目标Roll倾斜角度（单位：度）
TARGET_ROLL_DEGREE = 7.0 # 目标倾斜7度
ROLL_TRANSITION_DURATION = 1.5 # 倾斜过渡时间（秒）
STABLE_DURATION = 1.0 # 稳定阶段持续时间（秒）
def main():
# 加载模型和创建模拟
model = mujoco.MjModel.from_xml_path("/home/mrji/mujoco_learning/robot3/urdf/robot3.xml")
data = mujoco.MjData(model)
# 完整重置机器人状态（第一次重置）
mujoco.mj_resetData(model, data)
# 设置初始关节角度为0
data.qpos[model.joint("left_hip_pitch_joint").id] = 0
data.qpos[model.joint("right_hip_pitch_joint").id] = 0
data.qpos[model.joint("left_leg_pitch_joint").id] = 0
data.qpos[model.joint("right_leg_pitch_joint").id] = 0
# 重置所有速度为0
data.qvel[:] = 0
# 第二次重置模型数据（规避加载问题）
mujoco.mj_resetData(model, data)
# 再次设置初始关节角度为0
data.qpos[model.joint("left_hip_pitch_joint").id] = 0
data.qpos[model.joint("right_hip_pitch_joint").id] = 0
data.qpos[model.joint("left_leg_pitch_joint").id] = 0
data.qpos[model.joint("right_leg_pitch_joint").id] = 0
# 创建查看器
with mujoco.viewer.launch_passive(model, data) as viewer:
# 设置摄像头位置
viewer.cam.azimuth = 180 # 面向机器人正面
viewer.cam.elevation = -20 # 稍微俯视
viewer.cam.distance = 2.0 # 距离
viewer.cam.lookat[:] = [0.0, 0.0, 0.65] # 看向机器人中心
# 获取执行器ID
hip_left = model.actuator("1_pos").id
leg_left = model.actuator("2_pos").id
hip_right = model.actuator("3_pos").id
leg_right = model.actuator("4_pos").id
# 设置控制信号初始值为0
data.ctrl[hip_left] = 0
data.ctrl[leg_left] = 0
data.ctrl[hip_right] = 0
data.ctrl[leg_right] = 0
# 修改：在最开始的时候额外重置一次
mujoco.mj_resetData(model, data)
# 状态控制变量
start_time = time.time()
current_phase = "STABLE" # STABLE, MOVING, DONE
transition_start_time = 0
# 控制参数
hip_adjustment = 0.1 # 髋关节调整量（根据实验确定）
leg_adjustment = 0.15 # 腿关节调整量（根据实验确定）
initial_roll = 0.0 # 初始Roll角度
current_roll = 0.0 # 当前Roll角度
# 初始稳定步骤 - 让物理引擎稳定
for _ in range(200):
mujoco.mj_step(model, data)
viewer.sync()
time.sleep(0.001)
# 模拟循环
while viewer.is_running():
step_start = time.time()
elapsed_time = time.time() - start_time
# 状态机控制
if current_phase == "STABLE":
# 稳定阶段 - 保持初始姿态
if elapsed_time >= STABLE_DURATION:
# 记录初始姿态（Roll角度）
quat = data.sensor("orientation").data.copy()
initial_roll = get_roll_from_quat(quat)
print(f"稳定阶段结束，初始Roll角度: {math.degrees(initial_roll):.2f}度")
current_phase = "MOVING"
transition_start_time = time.time()
print("开始Roll倾斜过渡...")
elif current_phase == "MOVING":
# 过渡阶段 - 平滑过渡到目标倾斜
transition_elapsed = time.time() - transition_start_time
if transition_elapsed >= ROLL_TRANSITION_DURATION:
# 达到目标倾斜
data.ctrl[hip_left] = 0 - hip_adjustment
data.ctrl[leg_left] = 0 - leg_adjustment
data.ctrl[hip_right] = 0 + hip_adjustment
data.ctrl[leg_right] = 0 + leg_adjustment
current_phase = "DONE"
print("达到目标倾斜状态")
else:
# 线性插值过渡
fraction = transition_elapsed / ROLL_TRANSITION_DURATION
data.ctrl[hip_left] = 0 - hip_adjustment * fraction
data.ctrl[leg_left] = 0 - leg_adjustment * fraction
data.ctrl[hip_right] = 0 + hip_adjustment * fraction
data.ctrl[leg_right] = 0 + leg_adjustment * fraction
elif current_phase == "DONE":
# 倾斜完成阶段 - 保持当前状态
# 计算并输出当前Roll角度
quat = data.sensor("orientation").data.copy()
current_roll = get_roll_from_quat(quat)
roll_angle_deg = math.degrees(current_roll - initial_roll)
print(f"当前Roll倾斜角度: {roll_angle_deg:.2f}度")
# 停留一段时间后退出
if time.time() - transition_start_time > ROLL_TRANSITION_DURATION + 2.0:
print(f"最终Roll倾斜角度: {roll_angle_deg:.2f}度 (目标: {TARGET_ROLL_DEGREE}度)")
break
# 执行物理模拟
mujoco.mj_step(model, data)
# 同步查看器
viewer.sync()
# 控制步长（约1000Hz）
time_until_next_step = 0.001 - (time.time() - step_start)
if time_until_next_step > 0:
time.sleep(time_until_next_step)
def get_roll_from_quat(quat):
"""从四元数中提取Roll角度（弧度）"""
# 四元数转换为旋转矩阵
rot_matrix = np.zeros(9)
mujoco.mju_quat2Mat(rot_matrix, quat)
# 从旋转矩阵中提取Roll角度（绕X轴的旋转）
roll = math.atan2(rot_matrix[7], rot_matrix[8])
return roll
if __name__ == "__main__":
main()

118
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/bot1_ros2_control.xacro Normal file
View File

@@ -0,0 +1,118 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 宏定义：ros2_control硬件接口配置（适配位置控制器和轮子速度控制器） -->
<xacro:macro name="bot1_ros2_control">
<!-- 1. ros2_control系统配置：定义所有关节的控制接口 -->
<ros2_control name="Bot1GazeboSystem" type="system">
<!-- 硬件插件：关联Gazebo仿真 -->
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<!-- 所有关节的接口配置 -->
<!-- 腿部关节 -->
<joint name="left_leg1_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="left_leg2_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="right_leg1_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="right_leg2_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<!-- 轮子关节 -->
<joint name="left_wheel1_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="left_wheel2_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="right_wheel1_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="right_wheel2_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<!-- 身体关节 -->
<joint name="pitch_body_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<!-- 左臂关节 -->
<joint name="left_shoulder_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="left_arm1_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="left_arm2_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<!-- 右臂关节 -->
<joint name="right_shoulder_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="right_arm1_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="right_arm2_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
<!-- 4. Gazebo插件配置：关联控制器参数文件 -->
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<!-- 加载控制器配置文件（路径需替换为你的实际路径） -->
<parameters>$(find hive_core_r0)/config/bot1_ros2_controller.yaml</parameters>
</plugin>
</gazebo>
</xacro:macro>
</robot>

16
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/generic_joint.xacro Normal file
View File

@@ -0,0 +1,16 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 通用关节(joint)宏定义 -->
<xacro:macro name="generic_joint" params="joint_name type parent child
origin_xyz origin_rpy
axis_xyz limit_lower limit_upper effort velocity">
<joint name="${joint_name}" type="${type}">
<origin xyz="${origin_xyz}" rpy="${origin_rpy}" />
<parent link="${parent}" />
<child link="${child}" />
<axis xyz="${axis_xyz}" />
<limit lower="${limit_lower}" upper="${limit_upper}"
effort="${effort}" velocity="${velocity}" />
</joint>
</xacro:macro>
</robot>

71
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/generic_link.xacro Normal file
View File

@@ -0,0 +1,71 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 通用链接(link)宏定义 -->
<xacro:macro name="generic_link" params="link_name mesh_file
inertia_xyz inertia_rpy
mass ixx ixy ixz iyy iyz izz
color_rgba enable_collision">
<link name="${link_name}">
<!-- 惯性参数 -->
<inertial>
<origin xyz="${inertia_xyz}" rpy="${inertia_rpy}" />
<mass value="${mass}" />
<inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}"
iyy="${iyy}" iyz="${iyz}" izz="${izz}" />
</inertial>
<!-- 可视化模型 -->
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="${mesh_file}" />
</geometry>
<material name="${link_name}_material">
<color rgba="${color_rgba}" />
</material>
</visual>
<xacro:if value="${enable_collision}">
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<!-- <mesh filename="${mesh_file}" /> -->
<cylinder radius="0.1" length="0.1" />
</geometry>
<material name="blue">
<color rgba="0.1 0.1 1.0 0.5" />
</material>
</collision>
</xacro:if>
<!-- 仅当启用碰撞时，添加碰撞物理参数 -->
<xacro:if value="${enable_collision}">
<gazebo reference="${link_name}">
<collision>
<surface>
<contact>
<ode>
<!-- 刚度：根据轮子质量调整，建议 1e5~1e6（质量越大，kp适当增大） -->
<kp>5e5</kp>
<!-- 阻尼：kp的1%~5%，确保振动快速衰减（关键参数） -->
<kd>2e4</kd>
<!-- 静/动摩擦系数：0.8~1.0，避免打滑 -->
<mu1>1000</mu1>
<mu2>1000</mu2>
<!-- 允许微小穿透（0.001~0.01m），减少高频抖动 -->
<min_depth>0.005</min_depth>
<!-- 碰撞时的最大校正速度，防止剧烈弹跳 -->
<max_vel>0.1</max_vel>
</ode>
</contact>
<bounce>
<!-- 反弹系数：0 = 完全不反弹（核心参数） -->
<restitution_coefficient>0.0</restitution_coefficient>
<!-- 碰撞速度阈值：设为0，确保所有碰撞都无反弹 -->
<threshold>0.0</threshold>
</bounce>
</surface>
</collision>
</gazebo>
</xacro:if>
</link>
</xacro:macro>
</robot>

925
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/hive_core_bot1.urdf Normal file
View File

@@ -0,0 +1,925 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="robot5">
<mujoco>
<compiler
meshdir="/home/mrji/mujoco_learning/robot5/meshes"
balanceinertia="true"
discardvisual="false" />
</mujoco>
<link
name="base_link">
<inertial>
<origin
xyz="-0.0223121090242412 0.00180410615499793 0.0294578734419628"
rpy="0 0 0" />
<mass
value="8.01740117497269" />
<inertia
ixx="0.0100767591002942"
ixy="-8.33871415118017E-06"
ixz="-3.04794782095214E-06"
iyy="0.00687778146550088"
iyz="2.70300324348596E-06"
izz="0.0095089226755309" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.592156862745098 0.619607843137255 0.650980392156863 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link
name="left_leg1_link">
<inertial>
<origin
xyz="0.0026919722577641 -0.106691141065344 0.000439159272074496"
rpy="0 0 0" />
<mass
value="0.687221704005751" />
<inertia
ixx="0.00309344360201386"
ixy="7.22427359994752E-05"
ixz="-1.7837563113331E-07"
iyy="0.000199758758340417"
iyz="5.25184127616714E-06"
izz="0.00327480626360329" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_leg1_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_leg1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_leg1_joint"
type="revolute">
<origin
xyz="-0.023189 -0.14297 0.032124"
rpy="1.5097 0 0" />
<parent
link="base_link" />
<child
link="left_leg1_link" />
<axis
xyz="0 0 -1" />
<limit
lower="0"
upper="1.57"
effort="60"
velocity="50" />
</joint>
<link
name="left_wheel1_link">
<inertial>
<origin
xyz="-2.01863499148247E-06 -1.71217600630769E-06 -0.000769610134319532"
rpy="0 0 0" />
<mass
value="0.243841354713671" />
<inertia
ixx="0.000147760027881631"
ixy="2.12254130748857E-09"
ixz="-1.63113803254759E-08"
iyy="0.000147759325738658"
iyz="-1.38350688180478E-08"
izz="0.000229476928281019" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_wheel1_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_wheel1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_wheel1_joint"
type="revolute">
<origin
xyz="0.010552 -0.41639 -0.03757"
rpy="3.1416 0 0" />
<parent
link="left_leg1_link" />
<child
link="left_wheel1_link" />
<axis
xyz="0 0 -1" />
<limit
lower="-1000"
upper="1000"
effort="30"
velocity="50" />
</joint>
<link
name="right_leg1_link">
<inertial>
<origin
xyz="-3.01562743165443E-05 -0.106725092599043 -0.000439159317208282"
rpy="0 0 0" />
<mass
value="0.687221703818328" />
<inertia
ixx="0.00309524606532856"
ixy="-4.05589498874716E-09"
ixz="-4.73342140491208E-08"
iyy="0.000197956293734699"
iyz="-5.25465636656606E-06"
izz="0.00327480626231139" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_leg1_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.686274509803922 0.686274509803922 0.686274509803922 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_leg1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_leg1_joint"
type="revolute">
<origin
xyz="-0.022996 0.14297 0.032127"
rpy="1.6319 0 0" />
<parent
link="base_link" />
<child
link="right_leg1_link" />
<axis
xyz="0 0 -1" />
<limit
lower="0"
upper="1.57"
effort="60"
velocity="50" />
</joint>
<link
name="right_wheel1_link">
<inertial>
<origin
xyz="1.92448237847231E-06 1.8173611248673E-06 -0.000769610176954372"
rpy="0 0 0" />
<mass
value="0.243841354713671" />
<inertia
ixx="0.000147759799888368"
ixy="2.14785580095997E-09"
ixz="1.55505894769142E-08"
iyy="0.000147759553731921"
iyz="1.46850067839037E-08"
izz="0.000229476928281019" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_wheel1_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_wheel1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_wheel1_joint"
type="revolute">
<origin
xyz="-0.00016377 -0.41653 0.03757"
rpy="0 0 0" />
<parent
link="right_leg1_link" />
<child
link="right_wheel1_link" />
<axis
xyz="0 0 -1" />
<limit
lower="-1000"
upper="1000"
effort="30"
velocity="50" />
</joint>
<link
name="pitch_body_link">
<inertial>
<origin
xyz="0.000405025729528165 0.164222263553734 0.038212974552542"
rpy="0 0 0" />
<mass
value="6.27735184123949" />
<inertia
ixx="0.012112631055994"
ixy="0.000262225328016326"
ixz="-4.41212676410044E-06"
iyy="0.00923713569100348"
iyz="-6.27634886947933E-05"
izz="0.00865286317711146" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/pitch_body_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.592156862745098 0.619607843137255 0.650980392156863 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/pitch_body_link.STL" />
</geometry>
</collision>
</link>
<joint
name="pitch_body_joint"
type="revolute">
<origin
xyz="-0.022999 0.039 0.048"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="pitch_body_link" />
<axis
xyz="0 0 -1" />
<limit
lower="-0.5236"
upper="0.5236"
effort="20"
velocity="50" />
</joint>
<link
name="left_shoulder_link">
<inertial>
<origin
xyz="0.0139828606345992 0.000183964382035662 0.0469280544333041"
rpy="0 0 0" />
<mass
value="2.24001373537068" />
<inertia
ixx="0.00187573272868104"
ixy="5.61569065704759E-06"
ixz="-1.58654763334498E-05"
iyy="0.00158854268624343"
iyz="8.8447275683334E-07"
izz="0.00161229514279373" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_shoulder_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.898039215686275 0.92156862745098 0.929411764705882 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_shoulder_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_shoulder_joint"
type="revolute">
<origin
xyz="0.00396365185327967 0.220964452984708 0.0865"
rpy="-1.5707963267949 0 0" />
<parent
link="pitch_body_link" />
<child
link="left_shoulder_link" />
<axis
xyz="-0.0179350762557234 0 -0.999839153584066" />
<limit
lower="-1.57"
upper="1.57"
effort="60"
velocity="50" />
</joint>
<link
name="left_arm1_link">
<inertial>
<origin
xyz="-0.0237185143086008 -0.146707321130601 0.0947584340472464"
rpy="0 0 0" />
<mass
value="1.97210272038181" />
<inertia
ixx="0.0020453207276327"
ixy="-7.93710872896332E-05"
ixz="3.82437020118978E-05"
iyy="0.00151626513015615"
iyz="0.000308320536526838"
izz="0.00183373717106277" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_arm1_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_arm1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_arm1_joint"
type="revolute">
<origin
xyz="0.0403430563984948 0.00044500097133069 0.0467839692408788"
rpy="0.560887033481642 0 0" />
<parent
link="left_shoulder_link" />
<child
link="left_arm1_link" />
<axis
xyz="0.99977570211955 0 -0.0211788916461878" />
<limit
lower="-1.57"
upper="1.57"
effort="60"
velocity="50" />
</joint>
<link
name="left_arm2_link">
<inertial>
<origin
xyz="-0.0044670114805947 0.108002437145143 -0.0700232476387601"
rpy="0 0 0" />
<mass
value="0.153345483355902" />
<inertia
ixx="0.000846185788600758"
ixy="8.22351279275958E-05"
ixz="-4.72026465483579E-05"
iyy="0.000325958142834647"
iyz="0.000341442751456307"
izz="0.000633417429555725" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_arm2_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_arm2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_arm2_joint"
type="revolute">
<origin
xyz="0.00696183199978448 -0.151367984040579 0.0972793725016652"
rpy="-3.14159265358976 0 0" />
<parent
link="left_arm1_link" />
<child
link="left_arm2_link" />
<axis
xyz="-0.99977570211955 0 -0.0211788916461861" />
<limit
lower="-1.57"
upper="1.57"
effort="20"
velocity="50" />
</joint>
<link
name="right_shoulder_link">
<inertial>
<origin
xyz="0.0139817634960264 -0.000283387663890286 0.0469666821426944"
rpy="0 0 0" />
<mass
value="2.24001360218308" />
<inertia
ixx="0.00187572320791288"
ixy="-7.22979277002269E-06"
ixz="-1.38305404469916E-05"
iyy="0.00158896107508856"
iyz="-4.80454269915803E-07"
izz="0.00161188619803665" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_shoulder_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.898039215686275 0.92156862745098 0.929411764705882 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_shoulder_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_shoulder_joint"
type="revolute">
<origin
xyz="0.00396365185327941 0.220964452984708 -0.00950000000000008"
rpy="-1.5707963267949 0 0" />
<parent
link="pitch_body_link" />
<child
link="right_shoulder_link" />
<axis
xyz="-0.0179350762557234 0 -0.999839153584066" />
<limit
lower="-1.57"
upper="1.57"
effort="60"
velocity="50" />
</joint>
<link
name="right_arm1_link">
<inertial>
<origin
xyz="-0.0222764790210273 0.116445846258557 0.130395530944397"
rpy="0 0 0" />
<mass
value="1.97210271472831" />
<inertia
ixx="0.00204747347595548"
ixy="4.42933377299481E-05"
ixz="6.73328370107109E-05"
iyy="0.00173434064125734"
iyz="-0.000339327591519728"
izz="0.00161350890019016" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_arm1_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_arm1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_arm1_joint"
type="revolute">
<origin
xyz="0.040338077059205 -0.000769030026202884 0.046784058560075"
rpy="-0.826482123984997 0 0" />
<parent
link="right_shoulder_link" />
<child
link="right_arm1_link" />
<axis
xyz="0.999649642897155 0 -0.0264686882106157" />
<limit
lower="-1.57"
upper="1.57"
effort="60"
velocity="50" />
</joint>
<link
name="right_arm2_link">
<inertial>
<origin
xyz="-0.00344285141443855 -0.0871900700243705 -0.0947300074211656"
rpy="0 0 0" />
<mass
value="0.153345483533995" />
<inertia
ixx="0.000848016249761213"
ixy="-5.62001253862557E-05"
ixz="-6.88219353067134E-05"
iyy="0.000513290254168688"
iyz="-0.000373425902214173"
izz="0.00044425485583754" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_arm2_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_arm2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_arm2_joint"
type="revolute">
<origin
xyz="0.00854070115695219 0.120366284301785 0.133656328091823"
rpy="-3.14159265358977 0 0" />
<parent
link="right_arm1_link" />
<child
link="right_arm2_link" />
<axis
xyz="-0.999649642897155 0 -0.0264686882106164" />
<limit
lower="-1.57"
upper="1.57"
effort="20"
velocity="50" />
</joint>
<link
name="left_leg2_link">
<inertial>
<origin
xyz="-0.00247971563497956 -0.0972883314998296 0.00524725682640842"
rpy="0 0 0" />
<mass
value="0.758444942474565" />
<inertia
ixx="0.00308402598971299"
ixy="-7.39501679752198E-05"
ixz="4.61546712808476E-06"
iyy="0.00021886670519613"
iyz="0.000190799531555152"
izz="0.00323764016118335" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_leg2_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.686274509803922 0.686274509803922 0.686274509803922 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_leg2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_leg2_joint"
type="revolute">
<origin
xyz="-0.092811 -0.14333 0.032022"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="left_leg2_link" />
<axis
xyz="0 0 -1" />
<limit
lower="0"
upper="1.57"
effort="60"
velocity="50" />
</joint>
<link
name="left_wheel2_link">
<inertial>
<origin
xyz="-2.63258476833339E-06 -2.75556383766418E-07 -0.000769610176954399"
rpy="0 0 0" />
<mass
value="0.243841354713671" />
<inertia
ixx="0.000147761781558832"
ixy="4.45494954114067E-10"
ixz="-2.127234078802E-08"
iyy="0.000147757572061457"
iyz="-2.226606099262E-09"
izz="0.000229476928281019" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_wheel2_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_wheel2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_wheel2_joint"
type="revolute">
<origin
xyz="-0.010549 -0.41781 -0.01244"
rpy="3.0805 0 0" />
<parent
link="left_leg2_link" />
<child
link="left_wheel2_link" />
<axis
xyz="0 0 -1" />
<limit
lower="-1000"
upper="1000"
effort="30"
velocity="50" />
</joint>
<link
name="right_leg2_link">
<inertial>
<origin
xyz="-0.0050516474502517 -0.0973277461307459 0.000701841039130763"
rpy="0 0 0" />
<mass
value="0.758444958401021" />
<inertia
ixx="0.00307823597256189"
ixy="-0.000148312070292207"
ixz="-3.2725667530637E-07"
iyy="0.000212654851830719"
iyz="-5.42028503051418E-06"
izz="0.00324964203847903" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_leg2_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.501960784313725 0.501960784313725 0.501960784313725 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_leg2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_leg2_joint"
type="revolute">
<origin
xyz="-0.092614 0.14333 0.032015"
rpy="1.6319 0 0" />
<parent
link="base_link" />
<child
link="right_leg2_link" />
<axis
xyz="0 0 -1" />
<limit
lower="0"
upper="1.57"
effort="60"
velocity="50" />
</joint>
<link
name="right_wheel2_link">
<inertial>
<origin
xyz="1.93300484906123E-06 1.80829370854951E-06 -0.000769610176954413"
rpy="0 0 0" />
<mass
value="0.243841354713671" />
<inertia
ixx="0.000147759820077608"
ixy="2.14660364874833E-09"
ixz="1.56194544574533E-08"
iyy="0.000147759533542682"
iyz="1.46117384240508E-08"
izz="0.000229476928281019" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_wheel2_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_wheel2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_wheel2_joint"
type="revolute">
<origin
xyz="-0.021629 -0.41584 0.037923"
rpy="0 0 0" />
<parent
link="right_leg2_link" />
<child
link="right_wheel2_link" />
<axis
xyz="0 0 -1" />
<limit
lower="-1000"
upper="1000"
effort="30"
velocity="50" />
</joint>
</robot>

523
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/hive_core_bot1.urdf.xacro Normal file
View File

@@ -0,0 +1,523 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="hive_core_bot1" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 引入XACRO命名空间 -->
<xacro:property name="meshes_path" value="file://$(find hive_core_r0)/urdf/hiveBot1/meshes" />
<xacro:include filename="$(find hive_core_r0)/urdf/hiveBot1/generic_link.xacro" />
<xacro:include filename="$(find hive_core_r0)/urdf/hiveBot1/generic_joint.xacro" />
<!-- 基础链接 -->
<xacro:generic_link
link_name="base_link"
mesh_file="${meshes_path}/base_link.STL"
inertia_xyz="-0.0223121090242412 0.00180410615499793 0.0294578734419628"
inertia_rpy="0 0 0"
mass="8.01740117497269"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.592156862745098 0.619607843137255 0.650980392156863 1"
enable_collision="true"
/>
<!-- 左腿1链接 -->
<xacro:generic_link
link_name="left_leg1_link"
mesh_file="${meshes_path}/left_leg1_link.STL"
inertia_xyz="0.0026919722577641 -0.106691141065344 0.000439159272074496"
inertia_rpy="0 0 0"
mass="0.687221704005751"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.898039215686275 0.917647058823529 0.929411764705882 1"
enable_collision="false"
/>
<!-- 左轮1链接 -->
<xacro:generic_link
link_name="left_wheel1_link"
mesh_file="${meshes_path}/left_wheel1_link.STL"
inertia_xyz="-2.01863499148247E-06 -1.71217600630769E-06 -0.000769610134319532"
inertia_rpy="0 0 0"
mass="0.243841354713671"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 右腿1链接 -->
<xacro:generic_link
link_name="right_leg1_link"
mesh_file="${meshes_path}/right_leg1_link.STL"
inertia_xyz="-3.01562743165443E-05 -0.106725092599043 -0.000439159317208282"
inertia_rpy="0 0 0"
mass="0.687221703818328"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.686274509803922 0.686274509803922 0.686274509803922 1"
enable_collision="false"
/>
<!-- 右轮1链接 -->
<xacro:generic_link
link_name="right_wheel1_link"
mesh_file="${meshes_path}/right_wheel1_link.STL"
inertia_xyz="1.92448237847231E-06 1.8173611248673E-06 -0.000769610176954372"
inertia_rpy="0 0 0"
mass="0.243841354713671"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 俯仰身体链接 -->
<xacro:generic_link
link_name="pitch_body_link"
mesh_file="${meshes_path}/pitch_body_link.STL"
inertia_xyz="0.000405025729528165 0.164222263553734 0.038212974552542"
inertia_rpy="0 0 0"
mass="6.27735184123949"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.592156862745098 0.619607843137255 0.650980392156863 1"
enable_collision="true"
/>
<!-- 左肩链接 -->
<xacro:generic_link
link_name="left_shoulder_link"
mesh_file="${meshes_path}/left_shoulder_link.STL"
inertia_xyz="0.0139828606345992 0.000183964382035662 0.0469280544333041"
inertia_rpy="0 0 0"
mass="2.24001373537068"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.898039215686275 0.92156862745098 0.929411764705882 1"
enable_collision="true"
/>
<!-- 左臂1链接 -->
<xacro:generic_link
link_name="left_arm1_link"
mesh_file="${meshes_path}/left_arm1_link.STL"
inertia_xyz="-0.0237185143086008 -0.146707321130601 0.0947584340472464"
inertia_rpy="0 0 0"
mass="1.97210272038181"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 左臂2链接 -->
<xacro:generic_link
link_name="left_arm2_link"
mesh_file="${meshes_path}/left_arm2_link.STL"
inertia_xyz="-0.0044670114805947 0.108002437145143 -0.0700232476387601"
inertia_rpy="0 0 0"
mass="0.153345483355902"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="1 1 1 1"
enable_collision="true"
/>
<!-- 右肩链接 -->
<xacro:generic_link
link_name="right_shoulder_link"
mesh_file="${meshes_path}/right_shoulder_link.STL"
inertia_xyz="0.0139817634960264 -0.000283387663890286 0.0469666821426944"
inertia_rpy="0 0 0"
mass="2.24001360218308"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.898039215686275 0.92156862745098 0.929411764705882 1"
enable_collision="true"
/>
<!-- 右臂1链接 -->
<xacro:generic_link
link_name="right_arm1_link"
mesh_file="${meshes_path}/right_arm1_link.STL"
inertia_xyz="-0.0222764790210273 0.116445846258557 0.130395530944397"
inertia_rpy="0 0 0"
mass="1.97210271472831"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 右臂2链接 -->
<xacro:generic_link
link_name="right_arm2_link"
mesh_file="${meshes_path}/right_arm2_link.STL"
inertia_xyz="-0.00344285141443855 -0.0871900700243705 -0.0947300074211656"
inertia_rpy="0 0 0"
mass="0.153345483533995"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="1 1 1 1"
enable_collision="true"
/>
<!-- 左腿2链接 -->
<xacro:generic_link
link_name="left_leg2_link"
mesh_file="${meshes_path}/left_leg2_link.STL"
inertia_xyz="-0.00247971563497956 -0.0972883314998296 0.00524725682640842"
inertia_rpy="0 0 0"
mass="0.758444942474565"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.686274509803922 0.686274509803922 0.686274509803922 1"
enable_collision="false"
/>
<!-- 左轮2链接 -->
<xacro:generic_link
link_name="left_wheel2_link"
mesh_file="${meshes_path}/left_wheel2_link.STL"
inertia_xyz="-2.63258476833339E-06 -2.75556383766418E-07 -0.000769610176954399"
inertia_rpy="0 0 0"
mass="0.243841354713671"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 右腿2链接 -->
<xacro:generic_link
link_name="right_leg2_link"
mesh_file="${meshes_path}/right_leg2_link.STL"
inertia_xyz="-0.0050516474502517 -0.0973277461307459 0.000701841039130763"
inertia_rpy="0 0 0"
mass="0.758444958401021"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.501960784313725 0.501960784313725 0.501960784313725 1"
enable_collision="false"
/>
<!-- 右轮2链接 -->
<xacro:generic_link
link_name="right_wheel2_link"
mesh_file="${meshes_path}/right_wheel2_link.STL"
inertia_xyz="1.93300484906123E-06 1.80829370854951E-06 -0.000769610176954413"
inertia_rpy="0 0 0"
mass="0.243841354713671"
ixx="0.1"
ixy="0"
ixz="0"
iyy="0.1"
iyz="0"
izz="0.1"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 左腿1关节 -->
<xacro:generic_joint
joint_name="left_leg1_joint"
type="revolute"
parent="base_link"
child="left_leg1_link"
origin_xyz="-0.023189 -0.14297 0.032124"
origin_rpy="1.5097 -0.523 0"
axis_xyz="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="6000"
velocity="50"
/>
<!-- 左轮1关节 -->
<xacro:generic_joint
joint_name="left_wheel1_joint"
type="revolute"
parent="left_leg1_link"
child="left_wheel1_link"
origin_xyz="0.010552 -0.41639 -0.03757"
origin_rpy="3.1416 0 0"
axis_xyz="0 0 -1"
limit_lower="-1000"
limit_upper="1000"
effort="6000"
velocity="50"
/>
<!-- 右腿1关节 -->
<xacro:generic_joint
joint_name="right_leg1_joint"
type="revolute"
parent="base_link"
child="right_leg1_link"
origin_xyz="-0.022996 0.14297 0.032127"
origin_rpy="1.6319 -0.523 0"
axis_xyz="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="6000"
velocity="50"
/>
<!-- 右轮1关节 -->
<xacro:generic_joint
joint_name="right_wheel1_joint"
type="revolute"
parent="right_leg1_link"
child="right_wheel1_link"
origin_xyz="-0.00016377 -0.41653 0.03757"
origin_rpy="0 0 0"
axis_xyz="0 0 -1"
limit_lower="-1000"
limit_upper="1000"
effort="6000"
velocity="50"
/>
<!-- 俯仰身体关节 -->
<xacro:generic_joint
joint_name="pitch_body_joint"
type="revolute"
parent="base_link"
child="pitch_body_link"
origin_xyz="-0.022999 0.039 0.048"
origin_rpy="1.5708 0 0"
axis_xyz="0 0 -1"
limit_lower="-0.5236"
limit_upper="0.5236"
effort="6000"
velocity="50"
/>
<!-- 左肩关节 -->
<xacro:generic_joint
joint_name="left_shoulder_joint"
type="revolute"
parent="pitch_body_link"
child="left_shoulder_link"
origin_xyz="0.00396365185327967 0.220964452984708 0.0865"
origin_rpy="-1.5707963267949 0 0"
axis_xyz="-0.0179350762557234 0 -0.999839153584066"
limit_lower="-1.57"
limit_upper="1.57"
effort="6000"
velocity="50"
/>
<!-- 左臂1关节 -->
<xacro:generic_joint
joint_name="left_arm1_joint"
type="revolute"
parent="left_shoulder_link"
child="left_arm1_link"
origin_xyz="0.0403430563984948 0.00044500097133069 0.0467839692408788"
origin_rpy="0.560887033481642 0 0"
axis_xyz="0.99977570211955 0 -0.0211788916461878"
limit_lower="-1.57"
limit_upper="1.57"
effort="6000"
velocity="50"
/>
<!-- 左臂2关节 -->
<xacro:generic_joint
joint_name="left_arm2_joint"
type="revolute"
parent="left_arm1_link"
child="left_arm2_link"
origin_xyz="0.00696183199978448 -0.151367984040579 0.0972793725016652"
origin_rpy="-3.14159265358976 0 0"
axis_xyz="-0.99977570211955 0 -0.0211788916461861"
limit_lower="-1.57"
limit_upper="1.57"
effort="6000"
velocity="50"
/>
<!-- 右肩关节 -->
<xacro:generic_joint
joint_name="right_shoulder_joint"
type="revolute"
parent="pitch_body_link"
child="right_shoulder_link"
origin_xyz="0.00396365185327941 0.220964452984708 -0.00950000000000008"
origin_rpy="-1.5707963267949 0 0"
axis_xyz="-0.0179350762557234 0 -0.999839153584066"
limit_lower="-1.57"
limit_upper="1.57"
effort="6000"
velocity="50"
/>
<!-- 右臂1关节 -->
<xacro:generic_joint
joint_name="right_arm1_joint"
type="revolute"
parent="right_shoulder_link"
child="right_arm1_link"
origin_xyz="0.040338077059205 -0.000769030026202884 0.046784058560075"
origin_rpy="-0.826482123984997 0 0"
axis_xyz="0.999649642897155 0 -0.0264686882106157"
limit_lower="-1.57"
limit_upper="1.57"
effort="6000"
velocity="50"
/>
<!-- 右臂2关节 -->
<xacro:generic_joint
joint_name="right_arm2_joint"
type="revolute"
parent="right_arm1_link"
child="right_arm2_link"
origin_xyz="0.00854070115695219 0.120366284301785 0.133656328091823"
origin_rpy="-3.14159265358977 0 0"
axis_xyz="-0.999649642897155 0 -0.0264686882106164"
limit_lower="-1.57"
limit_upper="1.57"
effort="6000"
velocity="50"
/>
<!-- 左腿2关节 -->
<xacro:generic_joint
joint_name="left_leg2_joint"
type="revolute"
parent="base_link"
child="left_leg2_link"
origin_xyz="-0.092811 -0.14333 0.032022"
origin_rpy="1.5708 0.523 0"
axis_xyz="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="6000"
velocity="50"
/>
<!-- 左轮2关节 -->
<xacro:generic_joint
joint_name="left_wheel2_joint"
type="revolute"
parent="left_leg2_link"
child="left_wheel2_link"
origin_xyz="-0.010549 -0.41781 -0.01244"
origin_rpy="3.0805 0 0"
axis_xyz="0 0 -1"
limit_lower="-1000"
limit_upper="1000"
effort="6000"
velocity="50"
/>
<!-- 右腿2关节 -->
<xacro:generic_joint
joint_name="right_leg2_joint"
type="revolute"
parent="base_link"
child="right_leg2_link"
origin_xyz="-0.092614 0.14333 0.032015"
origin_rpy="1.6319 0.523 0"
axis_xyz="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="6000"
velocity="50"
/>
<!-- 右轮2关节 -->
<xacro:generic_joint
joint_name="right_wheel2_joint"
type="revolute"
parent="right_leg2_link"
child="right_wheel2_link"
origin_xyz="-0.021629 -0.41584 0.037923"
origin_rpy="0 0 0"
axis_xyz="0 0 -1"
limit_lower="-1000"
limit_upper="1000"
effort="6000"
velocity="50"
/>
<!-- <link name="world">
<inertial>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0"
iyy="0.0" iyz="0.0"
izz="0.0"/>
</inertial>
</link>
<joint name="base_link_to_world_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="world" />
<child link="base_link" />
</joint> -->
<xacro:include filename="$(find hive_core_r0)/urdf/hiveBot1/bot1_ros2_control.xacro" />
<xacro:bot1_ros2_control />
</robot>

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/base_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_arm1_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_arm2_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_leg1_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_leg2_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_shoulder_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_wheel1_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_wheel2_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/pitch_body_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_arm1_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_arm2_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_leg1_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_leg2_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_shoulder_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_wheel1_link.STL Normal file
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_wheel2_link.STL Normal file
View File

Binary file not shown.

324
HiveCoreR0/src/hive_core_r0/world/bot0.world Normal file
View File

@@ -0,0 +1,324 @@
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='table'>
<static>1</static>
<link name='link'>
<collision name='surface'>
<pose>0 0 1 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.8 0.03</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>0.6</mu>
<mu2>0.6</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual1'>
<pose>0 0 1 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.8 0.03</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Wood</name>
</script>
</material>
</visual>
<collision name='front_left_leg_collision'>
<pose>0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='front_left_leg_visual'>
<pose>0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='front_right_leg_collision'>
<pose>0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='front_right_leg_visual'>
<pose>0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='back_right_leg_collision'>
<pose>-0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='back_right_leg_visual'>
<pose>-0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='back_left_leg_collision'>
<pose>-0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='back_left_leg_visual'>
<pose>-0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-0.772768 -1.3242 0 0 -0 0</pose>
</model>
<state world_name='default'>
<sim_time>83 658000000</sim_time>
<real_time>83 738917407</real_time>
<wall_time>1753783232 542949961</wall_time>
<iterations>83658</iterations>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='table'>
<pose>-0.772768 -1.3242 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-0.772768 -1.3242 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>4.75138 -5.25474 1.98471 0 0.275643 2.35619</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>

2745
HiveCoreR0/src/hive_core_r0/world/custom_room.world Normal file
View File

File diff suppressed because it is too large Load Diff

116
HiveCoreR0/src/hive_core_r0/world/empty.world Normal file
View File

@@ -0,0 +1,116 @@
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<contact>
<ode>
<kp>1e6</kp> <!-- 地面刚度可稍大于轮子 -->
<kd>5e4</kd>
<mu1>1000.0</mu1>
<mu2>1000.0</mu2>
</ode>
</contact>
<bounce>
<restitution_coefficient>0.0</restitution_coefficient>
</bounce>
</surface>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<state world_name='default'>
<sim_time>94 268000000</sim_time>
<real_time>94 323810591</real_time>
<wall_time>1753965160 659575744</wall_time>
<iterations>94268</iterations>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>5 -5 2 0 0.275643 2.35619</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>

11
HiveCoreR0/src/hive_core_r0/world/room/model.config Normal file
View File

@@ -0,0 +1,11 @@
<?xml version="1.0" ?>
<model>
<name>room</name>
<version>1.0</version>
<sdf version="1.7">model.sdf</sdf>
<author>
<name></name>
<email></email>
</author>
<description></description>
</model>

297
HiveCoreR0/src/hive_core_r0/world/room/model.sdf Normal file
View File

@@ -0,0 +1,297 @@
<?xml version='1.0'?>
<sdf version='1.7'>
<model name='room'>
<pose>-0.972666 -0.903877 0 0 -0 0</pose>
<link name='Wall_66'>
<collision name='Wall_66_Collision'>
<geometry>
<box>
<size>19 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_66_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>19 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>0.013921 5.50955 0 0 -0 0</pose>
</link>
<link name='Wall_67'>
<collision name='Wall_67_Collision'>
<geometry>
<box>
<size>11.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_67_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>11.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>9.43892 -0.040455 0 0 -0 -1.5708</pose>
</link>
<link name='Wall_68'>
<collision name='Wall_68_Collision'>
<geometry>
<box>
<size>19 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_68_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>19 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>0.013921 -5.59046 0 0 -0 3.14159</pose>
</link>
<link name='Wall_69'>
<collision name='Wall_69_Collision'>
<geometry>
<box>
<size>11.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_69_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>11.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-9.41108 -0.040455 0 0 -0 1.5708</pose>
</link>
<link name='Wall_71'>
<collision name='Wall_71_Collision'>
<geometry>
<box>
<size>4.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_71_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-1.11769 3.29046 0 0 -0 -1.5708</pose>
</link>
<link name='Wall_72'>
<collision name='Wall_72_Collision'>
<geometry>
<box>
<size>6.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_72_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>6.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-4.41769 0.990456 0 0 -0 3.14159</pose>
</link>
<link name='Wall_79'>
<collision name='Wall_79_Collision'>
<geometry>
<box>
<size>3.47722 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_79_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.47722 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>0.391784 -3.86799 0 0 -0 1.5708</pose>
</link>
<link name='Wall_80'>
<collision name='Wall_80_Collision'>
<geometry>
<box>
<size>7.5 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_80_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>7.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>4.06678 -2.24484 0 0 -0 0</pose>
</link>
<link name='Wall_85'>
<collision name='Wall_85_Collision'>
<geometry>
<box>
<size>6.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_85_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>6.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-6.38892 -1.80969 0 0 -0 0</pose>
</link>
<link name='Wall_86'>
<collision name='Wall_86_Collision'>
<geometry>
<box>
<size>2.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_86_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>2.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-3.33892 -3.10969 0 0 -0 -1.5708</pose>
</link>
<static>1</static>
</model>
</sdf>

321
HiveCoreR0/src/hive_core_r0/world/table.world Normal file
View File

@@ -0,0 +1,321 @@
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link_0'>
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<contact>
<ode>
<kp>1e6</kp> <!-- 地面刚度可稍大于轮子 -->
<kd>5e4</kd>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
</ode>
</contact>
<bounce>
<restitution_coefficient>0.0</restitution_coefficient>
</bounce>
</surface>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='table'>
<static>1</static>
<link name='link_1'>
<collision name='surface'>
<pose>0 0 1 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.8 0.03</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>0.6</mu>
<mu2>0.6</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual1'>
<pose>0 0 1 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.8 0.03</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Wood</name>
</script>
</material>
</visual>
<collision name='front_left_leg_collision'>
<pose>0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='front_left_leg_visual'>
<pose>0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='front_right_leg_collision'>
<pose>0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='front_right_leg_visual'>
<pose>0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='back_right_leg_collision'>
<pose>-0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='back_right_leg_visual'>
<pose>-0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='back_left_leg_collision'>
<pose>-0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='back_left_leg_visual'>
<pose>-0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>0.726316 -1.21623 0 0 -0 0</pose>
</model>
<state world_name='default'>
<sim_time>56 645000000</sim_time>
<real_time>56 704774179</real_time>
<wall_time>1753436842 65734558</wall_time>
<iterations>56645</iterations>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link_0'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='table'>
<pose>0.726316 -1.21623 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link_1'>
<pose>0.726316 -1.21623 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>4.85069 0.30326 2.30729 0 0.443643 -2.92699</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>