switch to mujoco simulation
This commit is contained in:
@@ -0,0 +1,9 @@
|
||||
robot_mujoco_controller:
|
||||
ros__parameters:
|
||||
model_path: "models/robot.xml"
|
||||
rotation_time: 10.0
|
||||
effective_wheel_radius: 0.12
|
||||
track_width: 0.7
|
||||
init_stable_time: 0.5
|
||||
acceleration_duration: 10.0
|
||||
kd: 1.0
|
||||
@@ -0,0 +1,28 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='false',
|
||||
description='是否使用模拟时间'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'rotation_time',
|
||||
default_value='10.0',
|
||||
description='旋转一圈的目标时间(秒)'
|
||||
),
|
||||
Node(
|
||||
package='mujoco_simulation',
|
||||
executable='mujoco_sim_node',
|
||||
name='mujoco_sim_node',
|
||||
output='screen',
|
||||
parameters=[
|
||||
{'use_sim_time': LaunchConfiguration('use_sim_time')},
|
||||
{'rotation_time': LaunchConfiguration('rotation_time')}
|
||||
]
|
||||
)
|
||||
])
|
||||
@@ -0,0 +1,2 @@
|
||||
from .robot_controller import main
|
||||
from .utils import quaternion_to_euler
|
||||
@@ -0,0 +1,122 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Float64MultiArray
|
||||
from sensor_msgs.msg import JointState
|
||||
import mujoco
|
||||
import numpy as np
|
||||
import os
|
||||
from mujoco import viewer
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import threading
|
||||
import time
|
||||
|
||||
class MujocoSimNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('mujoco_sim_node')
|
||||
|
||||
# 配置参数
|
||||
self.declare_parameter('model_path', 'models/hive_core_r0.xml')
|
||||
self.declare_parameter('simulation_rate', 100.0) # Hz
|
||||
self.declare_parameter('render', True)
|
||||
|
||||
model_path = self.get_parameter('model_path').get_parameter_value().string_value
|
||||
sim_rate = self.get_parameter('simulation_rate').get_parameter_value().double_value
|
||||
self.render = self.get_parameter('render').get_parameter_value().bool_value
|
||||
|
||||
# 获取完整模型路径
|
||||
package_path = get_package_share_directory('robot_description')
|
||||
self.full_model_path = os.path.join(package_path, model_path)
|
||||
|
||||
# 初始化MuJoCo模型和数据
|
||||
self.model = mujoco.MjModel.from_xml_path(self.full_model_path)
|
||||
self.data = mujoco.MjData(self.model)
|
||||
|
||||
# 控制指令存储
|
||||
self.joint_commands = np.zeros(self.model.nu)
|
||||
self.running = True # 线程运行标志
|
||||
|
||||
# 启动Viewer线程（关键修复）
|
||||
if self.render:
|
||||
self.viewer_thread = threading.Thread(target=self.run_viewer, daemon=True)
|
||||
self.viewer_thread.start()
|
||||
|
||||
# ROS发布器和订阅器
|
||||
self.joint_state_pub = self.create_publisher(JointState, 'joint_states', 10)
|
||||
self.cmd_sub = self.create_subscription(Float64MultiArray, 'joint_commands', self.cmd_callback, 10)
|
||||
|
||||
self.get_logger().info(f"MuJoCo simulation node started with model: {self.full_model_path}")
|
||||
self.get_logger().info(f"Simulation rate: {sim_rate} Hz")
|
||||
|
||||
def run_viewer(self):
|
||||
"""在独立线程中运行Viewer，避免与ROS线程冲突"""
|
||||
# 启动被动Viewer
|
||||
with viewer.launch_passive(self.model, self.data) as viewer_handle:
|
||||
# 配置相机参数
|
||||
viewer_handle.cam.distance = 3.0
|
||||
viewer_handle.cam.elevation = -20
|
||||
viewer_handle.cam.azimuth = 45
|
||||
|
||||
# 保持Viewer运行，直到节点停止
|
||||
while self.running:
|
||||
self.simulate_step() # 执行单步仿真
|
||||
viewer_handle.sync() # 同步仿真数据到Viewer
|
||||
time.sleep(0.01) # 降低CPU占用
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
"""处理接收到的关节控制指令"""
|
||||
if len(msg.data) == self.model.nu:
|
||||
self.joint_commands = np.array(msg.data)
|
||||
else:
|
||||
self.get_logger().warn(f"Command dimension mismatch: expected {self.model.nu}, got {len(msg.data)}")
|
||||
|
||||
def simulate_step(self):
|
||||
"""执行单步仿真并发布状态"""
|
||||
if not self.running:
|
||||
return
|
||||
|
||||
# 设置控制指令
|
||||
self.data.ctrl[:] = self.joint_commands
|
||||
|
||||
# 运行仿真步
|
||||
mujoco.mj_step(self.model, self.data)
|
||||
|
||||
# 发布关节状态
|
||||
self.publish_joint_states()
|
||||
|
||||
def publish_joint_states(self):
|
||||
"""发布关节状态到ROS话题"""
|
||||
msg = JointState()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
|
||||
# 获取关节名称
|
||||
msg.name = [self.model.joint(i).name for i in range(self.model.njnt)]
|
||||
|
||||
# 转换为Python原生float列表
|
||||
msg.position = [float(x) for x in self.data.qpos.copy().tolist()]
|
||||
msg.velocity = [float(x) for x in self.data.qvel.copy().tolist()]
|
||||
msg.effort = [float(x) for x in self.data.qfrc_actuator.copy().tolist()]
|
||||
|
||||
self.joint_state_pub.publish(msg)
|
||||
|
||||
def destroy_node(self):
|
||||
"""清理资源，确保Viewer线程正确退出"""
|
||||
self.running = False # 停止Viewer线程
|
||||
if hasattr(self, 'viewer_thread'):
|
||||
self.viewer_thread.join() # 等待线程结束
|
||||
super().destroy_node()
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = MujocoSimNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
@@ -0,0 +1,167 @@
|
||||
import rclpy
|
||||
import mujoco
|
||||
import numpy as np
|
||||
import time
|
||||
import os
|
||||
|
||||
from mujoco import viewer
|
||||
from .utils import quaternion_to_euler
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from .s_trajectory import s_curve_jerk_trajectory
|
||||
from rclpy.node import Node
|
||||
|
||||
class RobotMujocoController(Node):
|
||||
def __init__(self):
|
||||
super().__init__('robot_mujoco_controller')
|
||||
|
||||
# 获取模型路径
|
||||
package_path = get_package_share_directory('robot_mujoco_control')
|
||||
model_path = os.path.join(package_path,"models","robot.xml")
|
||||
|
||||
# 读取XML内容并替换package路径
|
||||
try:
|
||||
with open(model_path, 'r') as f:
|
||||
xml_content = f.read()
|
||||
|
||||
# 替换package://路径为绝对路径
|
||||
xml_content = xml_content.replace(
|
||||
'package://robot_mujoco_control',
|
||||
package_path
|
||||
)
|
||||
|
||||
# 从处理后的字符串加载模型
|
||||
self.model = mujoco.MjModel.from_xml_string(xml_content)
|
||||
self.data = mujoco.MjData(self.model)
|
||||
self.get_logger().info(f"成功加载模型: {model_path}")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"模型加载失败: {str(e)}")
|
||||
raise
|
||||
|
||||
# 获取执行器索引
|
||||
self.actuator_names = [
|
||||
"left_leg1_joint_ctrl",
|
||||
"right_leg1_joint_ctrl",
|
||||
"left_leg2_joint_ctrl",
|
||||
"right_leg2_joint_ctrl",
|
||||
"left_wheel1_joint_ctrl",
|
||||
"right_wheel1_joint_ctrl",
|
||||
"left_wheel2_joint_ctrl",
|
||||
"right_wheel2_joint_ctrl",
|
||||
"pitch_body_joint_ctrl",
|
||||
"left_shoulder_joint_ctrl",
|
||||
"right_shoulder_joint_ctrl",
|
||||
"left_arm1_joint_ctrl",
|
||||
"left_arm2_joint_ctrl",
|
||||
"right_arm1_joint_ctrl",
|
||||
"right_arm2_joint_ctrl"
|
||||
]
|
||||
|
||||
self.actuator_ids = []
|
||||
for name in self.actuator_names:
|
||||
aid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
|
||||
if aid == -1:
|
||||
self.get_logger().warn(f"执行器 {name} 未找到")
|
||||
self.actuator_ids.append(aid)
|
||||
|
||||
# 初始化变量
|
||||
self.start_time = None
|
||||
self.rotation_count = 0
|
||||
self.last_yaw = 0
|
||||
self.max_speed_factor = 1.0
|
||||
self.init_end_time = 3.0
|
||||
|
||||
def start_simulation(self):
|
||||
# 使用内置查看器
|
||||
with viewer.launch_passive(self.model, self.data) as viewer_handle:
|
||||
self.start_time = time.time()
|
||||
|
||||
# 设置相机
|
||||
viewer_handle.cam.distance = 3.0
|
||||
viewer_handle.cam.elevation = -20
|
||||
viewer_handle.cam.azimuth = 45
|
||||
|
||||
# 运行模拟
|
||||
while viewer_handle.is_running():
|
||||
current_time = time.time()
|
||||
elapsed_time = current_time - self.start_time
|
||||
|
||||
# 初始稳定阶段
|
||||
if current_time < self.init_end_time + self.start_time:
|
||||
# 保持静止，让机器人稳定
|
||||
# self.data.ctrl[self.actuator_ids] = [-0.349*4.5, -0.349*4.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||
self.data.ctrl[self.actuator_ids] = [-0.349*2, -0.349*2, 0.349*2, 0.349*2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||
else:
|
||||
|
||||
## 位置控制
|
||||
cur_dq = [0.0] * 15
|
||||
des_dq = [0.0] * 15
|
||||
tau = [0.0] * 15
|
||||
|
||||
all_joint_names = [
|
||||
"left_leg1_joint",
|
||||
"right_leg1_joint",
|
||||
"left_leg2_joint",
|
||||
"right_leg2_joint",
|
||||
"left_wheel1_joint",
|
||||
"right_wheel1_joint",
|
||||
"left_wheel2_joint",
|
||||
"right_wheel2_joint",
|
||||
"pitch_body_joint",
|
||||
"left_shoulder_joint",
|
||||
"right_shoulder_joint",
|
||||
"left_arm1_joint",
|
||||
"left_arm2_joint",
|
||||
"right_arm1_joint",
|
||||
"right_arm2_joint"
|
||||
]
|
||||
|
||||
wheel_joint_name = [
|
||||
"left_wheel1_joint",
|
||||
"right_wheel1_joint",
|
||||
"left_wheel2_joint",
|
||||
"right_wheel2_joint"
|
||||
]
|
||||
|
||||
for i, name in enumerate(all_joint_names):
|
||||
jid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name)
|
||||
if jid != -1:
|
||||
cur_dq[i] = self.data.qpos[jid]
|
||||
|
||||
|
||||
|
||||
# 步进模拟
|
||||
mujoco.mj_step(self.model, self.data)
|
||||
|
||||
# 更新查看器
|
||||
viewer_handle.sync()
|
||||
|
||||
# 获取机器人方向
|
||||
base_quat = self.data.qpos[3:7]
|
||||
_, _, yaw = quaternion_to_euler(base_quat)
|
||||
|
||||
# 检测完整旋转
|
||||
if self.last_yaw > 3.0 and yaw < -3.0: # 从正π到负π
|
||||
self.rotation_count += 1
|
||||
elif self.last_yaw < -3.0 and yaw > 3.0: # 从负π到正π
|
||||
self.rotation_count += 1
|
||||
self.last_yaw = yaw
|
||||
|
||||
# 控制模拟速度
|
||||
time.sleep(0.001)
|
||||
|
||||
self.get_logger().info("模拟结束")
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
try:
|
||||
simObj = RobotMujocoController()
|
||||
simObj.start_simulation()
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
except Exception as e:
|
||||
print(f"发生错误: {str(e)}")
|
||||
finally:
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,292 @@
|
||||
import numpy as np
|
||||
|
||||
def s_curve_jerk_trajectory(
|
||||
x0, # 起始位置
|
||||
xg, # 目标位置
|
||||
v_max, # 最大速度限制
|
||||
v0, # 起始速度
|
||||
vg, # 目标速度
|
||||
a_max, # 最大加速度限制
|
||||
j_max, # 最大加加速度（jerk）限制
|
||||
time_step=0.001, # 时间步长（默认0.001s）
|
||||
verbose=False # 是否打印调试信息
|
||||
):
|
||||
"""
|
||||
生成带加加速度（jerk）约束的S型轨迹（7段式：加加速→匀加速→减加速→匀速→加减速→匀减速→减减速）
|
||||
|
||||
参数:
|
||||
x0: 起始位置
|
||||
xg: 目标位置
|
||||
v_max: 最大允许速度
|
||||
v0: 起始速度
|
||||
vg: 目标速度
|
||||
a_max: 最大允许加速度
|
||||
j_max: 最大允许加加速度（jerk）
|
||||
time_step: 轨迹采样时间步长（默认0.001s）
|
||||
verbose: 是否打印轨迹参数（如各阶段时间、总时间等）
|
||||
|
||||
返回:
|
||||
t: 时间序列（数组）
|
||||
pos: 位置序列（数组）
|
||||
vel: 速度序列（数组）
|
||||
acc: 加速度序列（数组）
|
||||
jerk: 加加速度序列（数组）
|
||||
"""
|
||||
# 初始化变量
|
||||
count = 0
|
||||
x_g = xg
|
||||
x_0 = x0
|
||||
v_max = v_max
|
||||
v_0 = v0
|
||||
v_g = vg
|
||||
a_max_input = a_max # 保存原始a_max用于调整
|
||||
|
||||
# --------------------------
|
||||
# 1. 计算各阶段时间参数（加速/减速）
|
||||
# --------------------------
|
||||
# 加速阶段（加加速→匀加速→减加速）
|
||||
if (v_max - v_0) * j_max < a_max ** 2: # 不能达到最大加速度
|
||||
if v_0 > v_max:
|
||||
Tj1 = 0.0 # 加加速时间
|
||||
Ta = 0.0 # 加速总时间
|
||||
alima = 0.0 # 实际最大加速度
|
||||
else:
|
||||
Tj1 = np.sqrt((v_max - v_0) / j_max) # 加加速时间（对称）
|
||||
Ta = 2 * Tj1 # 加速总时间（加加速+减加速，无匀加速）
|
||||
alima = Tj1 * j_max # 实际达到的最大加速度
|
||||
else: # 能达到最大加速度（存在匀加速阶段）
|
||||
Tj1 = a_max / j_max # 加加速时间（达到a_max的时间）
|
||||
Ta = Tj1 + (v_max - v_0) / a_max # 加速总时间（加加速+匀加速+减加速）
|
||||
alima = a_max # 最大加速度为a_max
|
||||
|
||||
# 减速阶段（加减速→匀减速→减减速）
|
||||
if (v_max - v_g) * j_max < a_max ** 2: # 不能达到最大减速度
|
||||
Tj2 = np.sqrt((v_max - v_g) / j_max) # 加减速时间（对称）
|
||||
Td = 2 * Tj2 # 减速总时间（加减速+减减速，无匀减速）
|
||||
alimd = Tj2 * j_max # 实际达到的最大减速度
|
||||
else: # 能达到最大减速度（存在匀减速阶段）
|
||||
Tj2 = a_max / j_max # 加减速时间（达到a_max的时间）
|
||||
Td = Tj2 + (v_max - v_g) / a_max # 减速总时间（加减速+匀减速+减减速）
|
||||
alimd = a_max # 最大减速度为a_max
|
||||
|
||||
# --------------------------
|
||||
# 2. 计算匀速阶段时间及总时间
|
||||
# --------------------------
|
||||
# 初始计算匀速时间（可能为负，需调整）
|
||||
Tv = (x_g - x_0) / v_max - Ta/2 * (1 + v_0/v_max) - Td/2 * (1 + v_g/v_max)
|
||||
T = Tv + Ta + Td # 初始总时间
|
||||
|
||||
# --------------------------
|
||||
# 3. 处理无匀速阶段的情况（Tv ≤ 0）
|
||||
# --------------------------
|
||||
if Tv > 0:
|
||||
# 存在匀速阶段：使用初始计算结果
|
||||
vlim = v_max # 匀速阶段速度
|
||||
T = Tv + Ta + Td
|
||||
else:
|
||||
# 无匀速阶段：重新调整各阶段时间
|
||||
Tv = 0.0 # 强制匀速时间为0
|
||||
a_max = a_max_input # 重置a_max
|
||||
delta = (a_max ** 4) / (j_max ** 2) + 2 * (v_0 **2 + v_g** 2) + a_max * (4 * (x_g - x_0) - 2 * a_max / j_max * (v_0 + v_g))
|
||||
Tj1 = a_max / j_max
|
||||
Ta = (a_max **2 / j_max - 2 * v_0 + np.sqrt(delta)) / (2 * a_max)
|
||||
Tj2 = a_max / j_max
|
||||
Td = (a_max** 2 / j_max - 2 * v_g + np.sqrt(delta)) / (2 * a_max)
|
||||
vlim = v_0 + (Ta - Tj1) * alima # 实际最大速度（未达到v_max）
|
||||
|
||||
# 调整a_max直至Ta、Td合理（避免加速/减速阶段时间过短）
|
||||
while Ta < 2 * Tj1 or Td < 2 * Tj2:
|
||||
count += 1
|
||||
a_max -= a_max_input * 0.1 # 每次减少10%原始a_max
|
||||
if a_max <= 0:
|
||||
a_max = a_max_input * 0.1 # 避免a_max为负
|
||||
break # 防止无限循环
|
||||
|
||||
# 重新计算delta（根据a_max符号调整）
|
||||
if a_max > 0:
|
||||
delta = (a_max **4) / (j_max** 2) + 2 * (v_0 **2 + v_g** 2) + a_max * (4 * (x_g - x_0) - 2 * a_max / j_max * (v_0 + v_g))
|
||||
else:
|
||||
delta = (a_max **4) / (j_max** 2) + 2 * (v_0 **2 + v_g** 2) - a_max * (4 * (x_g - x_0) - 2 * a_max / j_max * (v_0 + v_g))
|
||||
|
||||
# 重新计算各时间参数
|
||||
Tj1 = a_max / j_max if j_max != 0 else 0.0
|
||||
Ta = (a_max **2 / j_max - 2 * v_0 + np.sqrt(delta)) / (2 * a_max) if a_max != 0 else 0.0
|
||||
Tj2 = a_max / j_max if j_max != 0 else 0.0
|
||||
Td = (a_max** 2 / j_max - 2 * v_g + np.sqrt(delta)) / (2 * a_max) if a_max != 0 else 0.0
|
||||
vlim = v_0 + (Ta - Tj1) * alima
|
||||
|
||||
# 处理Ta或Td为负的极端情况（速度方向冲突）
|
||||
if Ta < 0 or Td < 0:
|
||||
if v_0 > v_g:
|
||||
# 起始速度 > 目标速度：仅减速
|
||||
Ta = 0.0
|
||||
Tj1 = 0.0
|
||||
alima = 0.0
|
||||
Td = 2 * (x_g - x_0) / (v_g + v_0) # 减速总时间
|
||||
# 加减速时间计算
|
||||
numerator = j_max * (x_g - x_0) - np.sqrt(j_max * (j_max * (x_g - x_0) **2 + (v_g + v_0)** 2 * (v_g - v_0)))
|
||||
denominator = j_max * (v_g + v_0)
|
||||
Tj2 = numerator / denominator if denominator != 0 else 0.0
|
||||
alimd = -j_max * Tj2 # 实际减速度
|
||||
vlim = v_g - (Td - Tj2) * alimd
|
||||
alimd = -alimd # 取绝对值
|
||||
else:
|
||||
# 起始速度 < 目标速度：仅加速
|
||||
Td = 0.0
|
||||
Tj2 = 0.0
|
||||
alimd = 0.0
|
||||
Ta = 2 * (x_g - x_0) / (v_g + v_0) # 加速总时间
|
||||
# 加加速时间计算
|
||||
numerator = j_max * (x_g - x_0) - np.sqrt(j_max * (j_max * (x_g - x_0) **2 - (v_g + v_0)** 2 * (v_g - v_0)))
|
||||
denominator = j_max * (v_g + v_0)
|
||||
Tj1 = numerator / denominator if denominator != 0 else 0.0
|
||||
alima = j_max * Tj1 # 实际加速度
|
||||
vlim = v_0 + (Ta - Tj1) * alima # 实际最大速度
|
||||
|
||||
# 更新总时间（无匀速阶段）
|
||||
T = Tv + Ta + Td
|
||||
|
||||
# --------------------------
|
||||
# 4. 生成轨迹数据（位置、速度、加速度、加加速度）
|
||||
# --------------------------
|
||||
pos = [] # 位置序列
|
||||
vel = [] # 速度序列
|
||||
acc = [] # 加速度序列
|
||||
jerk = [] # 加加速度序列
|
||||
t_total = T # 总时间
|
||||
time_steps = np.arange(0, t_total, time_step) # 时间序列
|
||||
|
||||
for t in time_steps:
|
||||
# 阶段1：加加速（0 ~ Tj1）
|
||||
if 0 <= t < Tj1:
|
||||
x = x_0 + v_0 * t + j_max * t ** 3 / 6
|
||||
v = v_0 + j_max * t ** 2 / 2
|
||||
a = j_max * t
|
||||
j = j_max
|
||||
# 阶段2：匀加速（Tj1 ~ Ta - Tj1）
|
||||
elif Tj1 <= t < (Ta - Tj1):
|
||||
x = x_0 + v_0 * t + alima / 6 * (3 * t **2 - 3 * Tj1 * t + Tj1** 2)
|
||||
v = v_0 + alima * (t - Tj1 / 2)
|
||||
a = alima
|
||||
j = 0.0
|
||||
# 阶段3：减加速（Ta - Tj1 ~ Ta）
|
||||
elif (Ta - Tj1) <= t < Ta:
|
||||
x = x_0 + (vlim + v_0) * Ta / 2 - vlim * (Ta - t) + j_max * (Ta - t) ** 3 / 6
|
||||
v = vlim - j_max * (Ta - t) ** 2 / 2
|
||||
a = j_max * (Ta - t)
|
||||
j = -j_max
|
||||
# 阶段4：匀速（Ta ~ Ta + Tv）
|
||||
elif Ta <= t < (Ta + Tv):
|
||||
x = x_0 + (vlim + v_0) * Ta / 2 + vlim * (t - Ta)
|
||||
v = vlim
|
||||
a = 0.0
|
||||
j = 0.0
|
||||
# 阶段5：加减速（Ta + Tv ~ T - Td + Tj2）即（T - Td ~ T - Td + Tj2）
|
||||
elif (T - Td) <= t < (T - Td + Tj2):
|
||||
x = x_g - (vlim + v_g) * Td / 2 + vlim * (t - T + Td) - j_max * (t - T + Td) ** 3 / 6
|
||||
v = vlim - j_max * (t - T + Td) ** 2 / 2
|
||||
a = -j_max * (t - T + Td)
|
||||
j = -j_max
|
||||
# 阶段6：匀减速（T - Td + Tj2 ~ T - Tj2）
|
||||
elif (T - Td + Tj2) <= t < (T - Tj2):
|
||||
x = x_g - (vlim + v_g) * Td / 2 + vlim * (t - T + Td) - alimd / 6 * (3 * (t - T + Td) **2 - 3 * Tj2 * (t - T + Td) + Tj2** 2)
|
||||
v = vlim - alimd * (t - T + Td - Tj2 / 2)
|
||||
a = -alimd
|
||||
j = 0.0
|
||||
# 阶段7：减减速（T - Tj2 ~ T）
|
||||
elif (T - Tj2) <= t < T:
|
||||
x = x_g - v_g * (T - t) - j_max * (T - t) ** 3 / 6
|
||||
v = v_g + j_max * (T - t) ** 2 / 2
|
||||
a = -j_max * (T - t)
|
||||
j = j_max
|
||||
else:
|
||||
# 超出总时间范围（取目标值）
|
||||
x = x_g
|
||||
v = v_g
|
||||
a = 0.0
|
||||
j = 0.0
|
||||
|
||||
# 存储数据
|
||||
pos.append(x)
|
||||
vel.append(v)
|
||||
acc.append(a)
|
||||
jerk.append(j)
|
||||
|
||||
# --------------------------
|
||||
# 5. 打印调试信息（可选）
|
||||
# --------------------------
|
||||
if verbose:
|
||||
print(f"轨迹总时间: {T:.4f} s")
|
||||
print(f"加速阶段时间 (Ta): {Ta:.4f} s")
|
||||
print(f"减速阶段时间 (Td): {Td:.4f} s")
|
||||
print(f"匀速阶段时间 (Tv): {Tv:.4f} s")
|
||||
print(f"加加速/加减速时间 (Tj1/Tj2): {Tj1:.4f} s / {Tj2:.4f} s")
|
||||
print(f"实际最大速度 (vlim): {vlim:.4f}")
|
||||
print(f"实际最大加速度 (alima): {alima:.4f}")
|
||||
print(f"实际最大减速度 (alimd): {alimd:.4f}")
|
||||
|
||||
return time_steps, np.array(pos), np.array(vel), np.array(acc), np.array(jerk)
|
||||
|
||||
|
||||
# # --------------------------
|
||||
# # 示例使用
|
||||
# # --------------------------
|
||||
# if __name__ == "__main__":
|
||||
# import matplotlib.pyplot as plt
|
||||
|
||||
# # 轨迹参数（可根据需求调整）
|
||||
# x0 = 0.0 # 起始位置
|
||||
# xg = 60.0 # 目标位置
|
||||
# v_max = 20.0 # 最大速度
|
||||
# v0 = 10.0 # 起始速度
|
||||
# vg = 5.0 # 目标速度
|
||||
# a_max = 15.0 # 最大加速度
|
||||
# j_max = 20.0 # 最大加加速度
|
||||
# time_step = 0.001 # 时间步长
|
||||
|
||||
# # 生成轨迹
|
||||
# t, pos, vel, acc, jerk = s_curve_jerk_trajectory(
|
||||
# x0=x0, xg=xg,
|
||||
# v_max=v_max, v0=v0, vg=vg,
|
||||
# a_max=a_max, j_max=j_max,
|
||||
# time_step=time_step,
|
||||
# verbose=True # 打印调试信息
|
||||
# )
|
||||
|
||||
# # 绘图验证
|
||||
# fig, axes = plt.subplots(4, 1, figsize=(10, 16))
|
||||
# fig.suptitle("S型加减速轨迹（带加加速度约束）", fontsize=14)
|
||||
|
||||
# # 位置曲线
|
||||
# axes[0].plot(t, pos, label="位置")
|
||||
# axes[0].axhline(y=xg, color='r', linestyle='--', label="目标位置")
|
||||
# axes[0].set_ylabel("位置")
|
||||
# axes[0].legend()
|
||||
# axes[0].grid(True)
|
||||
|
||||
# # 速度曲线
|
||||
# axes[1].plot(t, vel, label="速度")
|
||||
# axes[1].axhline(y=v_max, color='r', linestyle='--', label="最大速度")
|
||||
# axes[1].set_ylabel("速度")
|
||||
# axes[1].legend()
|
||||
# axes[1].grid(True)
|
||||
|
||||
# # 加速度曲线
|
||||
# axes[2].plot(t, acc, label="加速度")
|
||||
# axes[2].axhline(y=a_max, color='r', linestyle='--', label="最大加速度")
|
||||
# axes[2].axhline(y=-a_max, color='r', linestyle='--', label="最大减速度")
|
||||
# axes[2].set_ylabel("加速度")
|
||||
# axes[2].legend()
|
||||
# axes[2].grid(True)
|
||||
|
||||
# # 加加速度曲线
|
||||
# axes[3].plot(t, jerk, label="加加速度")
|
||||
# axes[3].axhline(y=j_max, color='r', linestyle='--', label="最大加加速度")
|
||||
# axes[3].axhline(y=-j_max, color='r', linestyle='--', label="最大减加速度")
|
||||
# axes[3].set_xlabel("时间 (s)")
|
||||
# axes[3].set_ylabel("加加速度")
|
||||
# axes[3].legend()
|
||||
# axes[3].grid(True)
|
||||
|
||||
# plt.tight_layout()
|
||||
# plt.show()
|
||||
23
HiveCoreR0/src/mujoco_simulation/mujoco_simulation/utils.py
Normal file
23
HiveCoreR0/src/mujoco_simulation/mujoco_simulation/utils.py
Normal file
@@ -0,0 +1,23 @@
|
||||
import numpy as np
|
||||
|
||||
def quaternion_to_euler(q):
|
||||
"""将四元数转换为欧拉角 (roll, pitch, yaw)"""
|
||||
w, x, y, z = q
|
||||
# 绕x轴旋转 (roll)
|
||||
sinr_cosp = 2 * (w * x + y * z)
|
||||
cosr_cosp = 1 - 2 * (x * x + y * y)
|
||||
roll = np.arctan2(sinr_cosp, cosr_cosp)
|
||||
|
||||
# 绕y轴旋转 (pitch)
|
||||
sinp = 2 * (w * y - z * x)
|
||||
if np.abs(sinp) >= 1:
|
||||
pitch = np.copysign(np.pi / 2, sinp)
|
||||
else:
|
||||
pitch = np.arcsin(sinp)
|
||||
|
||||
# 绕z轴旋转 (yaw)
|
||||
siny_cosp = 2 * (w * z + x * y)
|
||||
cosy_cosp = 1 - 2 * (y * y + z * z)
|
||||
yaw = np.arctan2(siny_cosp, cosy_cosp)
|
||||
|
||||
return roll, pitch, yaw
|
||||
26
HiveCoreR0/src/mujoco_simulation/package.xml
Normal file
26
HiveCoreR0/src/mujoco_simulation/package.xml
Normal file
@@ -0,0 +1,26 @@
|
||||
<?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>mujoco_simulation</name>
|
||||
<version>0.0.0</version>
|
||||
<description>ROS 2 package for robot in MuJoCo simulator</description>
|
||||
<maintainer email="Ray@email.com">Ray</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<build_depend>ament_cmake</build_depend>
|
||||
<build_depend>ament_cmake_python</build_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>mujoco</depend>
|
||||
<depend>numpy</depend>
|
||||
|
||||
<test_depend>ament_cmake_pytest</test_depend>
|
||||
<test_depend>pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
4
HiveCoreR0/src/mujoco_simulation/setup.cfg
Normal file
4
HiveCoreR0/src/mujoco_simulation/setup.cfg
Normal file
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/mujoco_simulation
|
||||
[install]
|
||||
install_scripts=$base/lib/mujoco_simulation
|
||||
32
HiveCoreR0/src/mujoco_simulation/setup.py
Normal file
32
HiveCoreR0/src/mujoco_simulation/setup.py
Normal file
@@ -0,0 +1,32 @@
|
||||
from setuptools import setup
|
||||
import os
|
||||
from glob import glob
|
||||
|
||||
package_name = 'mujoco_simulation'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
|
||||
(os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
|
||||
(os.path.join('share', package_name, 'meshes'), glob('meshes/*')),
|
||||
(os.path.join('share', package_name, 'models'), glob('models/*.xml'))
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='Ray',
|
||||
maintainer_email='Ray@email.com',
|
||||
description='ROS 2 package for controlling robot in MuJoCo',
|
||||
license='Apache-2.0',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'mujoco_sim_node = mujoco_simulation.mujoco_sim_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -6,9 +6,9 @@ from launch.substitutions import PathJoinSubstitution
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
sim_pkg_share = FindPackageShare(package="robot_simulation")
|
||||
sim_launch_path = PathJoinSubstitution(
|
||||
[sim_pkg_share, "launch", "gazebo_simulation.launch.py"]
|
||||
mujoco_sim_pkg_share = FindPackageShare(package="mujoco_simulation")
|
||||
mujoco_sim_launch_path = PathJoinSubstitution(
|
||||
[mujoco_sim_pkg_share, "launch", "mujoco_simulation.launch.py"]
|
||||
)
|
||||
|
||||
control_pkg_share = FindPackageShare(package="robot_control")
|
||||
@@ -18,7 +18,7 @@ def generate_launch_description():
|
||||
|
||||
|
||||
include_simulation = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(sim_launch_path),
|
||||
PythonLaunchDescriptionSource(mujoco_sim_launch_path),
|
||||
)
|
||||
|
||||
include_control = IncludeLaunchDescription(
|
||||
|
||||
@@ -10,13 +10,14 @@ find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
add_executable(robot_control_node src/robot_control.cpp)
|
||||
ament_target_dependencies(robot_control_node rclcpp geometry_msgs)
|
||||
ament_target_dependencies(robot_control_node rclcpp geometry_msgs sensor_msgs)
|
||||
|
||||
install(TARGETS
|
||||
robot_control_node
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "std_msgs/msg/float64_multi_array.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
@@ -19,6 +20,7 @@ namespace robot_control
|
||||
private:
|
||||
void print_help();
|
||||
|
||||
void joint_states_callback(const sensor_msgs::msg::JointState::SharedPtr msg);
|
||||
void twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||
void timer_callback();
|
||||
void handle_phase_motion(std_msgs::msg::Float64MultiArray& cmd_msg, double current_time);
|
||||
@@ -27,6 +29,7 @@ namespace robot_control
|
||||
// 成员变量
|
||||
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmd_pub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_states_sub_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
rclcpp::Time start_time_;
|
||||
rclcpp::Time last_time_;
|
||||
@@ -42,6 +45,7 @@ namespace robot_control
|
||||
std::vector<int> leg_joint_indices_; // 腿部关节索引
|
||||
std::vector<int> arm_joint_indices_; // 手臂关节索引
|
||||
std::vector<int> wheel_joint_indices_; // 轮子关节索引
|
||||
std::vector<int> pitch_body_indices_; // 腿部轮子关节索引
|
||||
std::vector<int> joint_directions_; // 关节方向
|
||||
std::vector<int> wheel_joint_directions_; // 轮子关节方向
|
||||
|
||||
@@ -53,6 +57,14 @@ namespace robot_control
|
||||
double angular_z_ = 0.0; // 角速度（rad/s）
|
||||
double linear_speed_; // 最大线速度（m/s）
|
||||
double angular_speed_; // 最大角速度（rad/s）
|
||||
|
||||
// 运动状态
|
||||
std::vector<double> joint_positions_; // 关节位置（弧度）
|
||||
std::vector<double> joint_velocities_; // 关节速度（弧度/秒）
|
||||
std::vector<double> joint_accelerations_; // 关节加速度（弧度/秒^2）
|
||||
std::vector<double> joint_efforts_; // 关节力矩（牛顿米）
|
||||
|
||||
sensor_msgs::msg::JointState joint_states_;
|
||||
};
|
||||
|
||||
} // namespace robot_control
|
||||
|
||||
120
HiveCoreR0/src/robot_control/include/robot_fsm.hpp
Normal file
120
HiveCoreR0/src/robot_control/include/robot_fsm.hpp
Normal file
@@ -0,0 +1,120 @@
|
||||
#ifndef ROBOT_FSM_H
|
||||
#define ROBOT_FSM_H
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
// 前向声明
|
||||
class InputDevice;
|
||||
|
||||
// 机器人运动状态枚举
|
||||
enum class RobotState {
|
||||
STOPPED, // 停止
|
||||
MOVING_FORWARD,// 前进
|
||||
MOVING_BACKWARD,// 后退
|
||||
TURNING_LEFT, // 左转
|
||||
TURNING_RIGHT, // 右转
|
||||
MOVING_UP, // 上升
|
||||
MOVING_DOWN // 下降
|
||||
};
|
||||
|
||||
// 控制命令枚举
|
||||
enum class ControlCommand {
|
||||
NONE,
|
||||
STOP,
|
||||
FORWARD,
|
||||
BACKWARD,
|
||||
LEFT,
|
||||
RIGHT,
|
||||
UP,
|
||||
DOWN
|
||||
};
|
||||
|
||||
// 输入设备类型
|
||||
enum class InputType {
|
||||
KEYBOARD,
|
||||
GAMEPAD,
|
||||
VOICE
|
||||
};
|
||||
|
||||
|
||||
class RobotFsm {
|
||||
private:
|
||||
RobotState currentState;
|
||||
std::map<InputType, std::unique_ptr<InputDevice>> inputDevices;
|
||||
|
||||
// 状态转换表
|
||||
std::map<std::pair<RobotState, ControlCommand>, RobotState> transitionTable;
|
||||
|
||||
// 初始化状态转换规则
|
||||
void initTransitionTable();
|
||||
|
||||
// 执行当前状态对应的动作
|
||||
void executeStateAction();
|
||||
|
||||
public:
|
||||
RobotFsm();
|
||||
|
||||
// 添加输入设备
|
||||
void addInputDevice(InputType type, std::unique_ptr<InputDevice> device);
|
||||
|
||||
// 状态机主循环
|
||||
void run();
|
||||
|
||||
// 处理命令并转换状态
|
||||
void processCommand(ControlCommand cmd);
|
||||
|
||||
// 获取当前状态
|
||||
RobotState getCurrentState() const { return currentState; }
|
||||
|
||||
// 状态转换为字符串
|
||||
std::string stateToString(RobotState state) const;
|
||||
};
|
||||
|
||||
// 输入设备接口
|
||||
class InputDevice {
|
||||
protected:
|
||||
InputType type;
|
||||
|
||||
public:
|
||||
InputDevice(InputType t) : type(t) {}
|
||||
virtual ~InputDevice() = default;
|
||||
|
||||
InputType getType() const { return type; }
|
||||
virtual ControlCommand getCommand() = 0;
|
||||
virtual std::string getDeviceName() const = 0;
|
||||
};
|
||||
|
||||
// 键盘输入设备
|
||||
class KeyboardInput : public InputDevice {
|
||||
public:
|
||||
KeyboardInput() : InputDevice(InputType::KEYBOARD) {}
|
||||
|
||||
ControlCommand getCommand() override;
|
||||
std::string getDeviceName() const override { return "Keyboard"; }
|
||||
};
|
||||
|
||||
// 手柄输入设备
|
||||
class GamepadInput : public InputDevice {
|
||||
public:
|
||||
GamepadInput() : InputDevice(InputType::GAMEPAD) {}
|
||||
|
||||
ControlCommand getCommand() override;
|
||||
std::string getDeviceName() const override { return "Gamepad"; }
|
||||
};
|
||||
|
||||
// 语音输入设备
|
||||
class VoiceInput : public InputDevice {
|
||||
public:
|
||||
VoiceInput() : InputDevice(InputType::VOICE) {}
|
||||
|
||||
ControlCommand getCommand() override;
|
||||
std::string getDeviceName() const override { return "Voice Control"; }
|
||||
};
|
||||
|
||||
#endif // ROBOT_FSM_H
|
||||
|
||||
@@ -9,7 +9,6 @@ namespace robot_control
|
||||
{
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
|
||||
RobotControlNode::RobotControlNode(): Node("robot_phase_motion_node")
|
||||
{
|
||||
// 声明并获取参数
|
||||
@@ -40,17 +39,28 @@ namespace robot_control
|
||||
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}; // 轮子方向
|
||||
leg_joint_indices_ = {0, 1, 2, 3}; // 腿部关节索引
|
||||
wheel_joint_indices_ = {4, 5, 6, 7};
|
||||
pitch_body_indices_ = {8}; // 身体关节索引
|
||||
arm_joint_indices_ = {9, 10, 11, 12, 13, 14}; // 手臂关节索引
|
||||
|
||||
wheel_joint_directions_ = {1, -1, 1, -1}; // 轮子方向
|
||||
|
||||
// 初始化轮子目标角度
|
||||
wheel_targets_.resize(2, 0.0);
|
||||
wheel_targets_.resize(4, 0.0);
|
||||
|
||||
// 初始化关节状态
|
||||
joint_positions_.resize(total_joints_, 0.0);
|
||||
joint_velocities_.resize(total_joints_, 0.0);
|
||||
|
||||
// 创建发布者
|
||||
cmd_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
|
||||
"/robot_position_controller/commands", 10
|
||||
"joint_commands", 10
|
||||
);
|
||||
|
||||
joint_states_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"joint_states", 10,
|
||||
std::bind(&RobotControlNode::joint_states_callback, this, std::placeholders::_1)
|
||||
);
|
||||
|
||||
// 创建订阅者（接收来自速度指令）
|
||||
@@ -68,8 +78,6 @@ namespace robot_control
|
||||
// 记录起始时间
|
||||
start_time_ = this->now();
|
||||
last_time_ = this->now();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "机器人分阶段运动节点启动（含轮子控制）");
|
||||
}
|
||||
|
||||
RobotControlNode::~RobotControlNode()
|
||||
@@ -86,8 +94,25 @@ namespace robot_control
|
||||
cmd_pub_->publish(reset_msg);
|
||||
}
|
||||
|
||||
|
||||
void RobotControlNode::joint_states_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
|
||||
{
|
||||
// 保存当前关节角度
|
||||
joint_states_ = *msg;
|
||||
for (size_t i = 0; i < msg->position.size(); i++)
|
||||
{
|
||||
// joint_positions_[i] = msg->position[i];
|
||||
// joint_velocities_[i] = msg->velocity[i];
|
||||
}
|
||||
}
|
||||
|
||||
void RobotControlNode::twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
if (!msg) { // 检查消息是否有效
|
||||
RCLCPP_WARN(this->get_logger(), "收到空的Twist消息，忽略");
|
||||
return;
|
||||
}
|
||||
|
||||
// 接收外部速度指令
|
||||
linear_x_ = msg->linear.x;
|
||||
angular_z_ = msg->angular.z;
|
||||
@@ -114,6 +139,9 @@ namespace robot_control
|
||||
|
||||
// 发送指令
|
||||
cmd_pub_->publish(cmd_msg);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Joint 1 command: %f", cmd_msg.data[1]);
|
||||
|
||||
}
|
||||
|
||||
void RobotControlNode::handle_phase_motion(std_msgs::msg::Float64MultiArray& cmd_msg, double current_time)
|
||||
@@ -181,7 +209,7 @@ namespace robot_control
|
||||
cmd_msg.data[arm_joint_indices_[i]] = arm_angle;
|
||||
}
|
||||
|
||||
// cmd_msg.data[arm_joint_indices_[i]] = 0;
|
||||
cmd_msg.data[arm_joint_indices_[i]] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -200,6 +228,9 @@ namespace robot_control
|
||||
wheel_targets_[0] += left_omega * dt_sec; // left_wheel1
|
||||
wheel_targets_[1] += right_omega * dt_sec; // right_wheel1
|
||||
|
||||
wheel_targets_[2] += left_omega * dt_sec; // left_wheel2
|
||||
wheel_targets_[3] += right_omega * dt_sec; // right_wheel2
|
||||
|
||||
// 赋值轮子目标角度到指令数组
|
||||
for (size_t i = 0; i < wheel_joint_indices_.size(); ++i)
|
||||
{
|
||||
|
||||
200
HiveCoreR0/src/robot_control/src/robot_fsm.cpp
Normal file
200
HiveCoreR0/src/robot_control/src/robot_fsm.cpp
Normal file
@@ -0,0 +1,200 @@
|
||||
#include "robot_fsm.hpp"
|
||||
#include <conio.h> // 用于键盘输入 (Windows)
|
||||
#include <random> // 用于模拟一些输入
|
||||
|
||||
// 机器人控制器构造函数
|
||||
RobotFsm::RobotFsm() : currentState(RobotState::STOPPED) {
|
||||
initTransitionTable();
|
||||
}
|
||||
|
||||
// 初始化状态转换表
|
||||
void RobotFsm::initTransitionTable() {
|
||||
// 从任意状态都可以转换到停止状态
|
||||
transitionTable[{RobotState::STOPPED, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable[{RobotState::TURNING_LEFT, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable[{RobotState::TURNING_RIGHT, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable[{RobotState::MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable[{RobotState::MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
|
||||
// 从停止状态转换到其他状态
|
||||
transitionTable[{RobotState::STOPPED, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
|
||||
transitionTable[{RobotState::STOPPED, ControlCommand::BACKWARD}] = RobotState::MOVING_BACKWARD;
|
||||
transitionTable[{RobotState::STOPPED, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
|
||||
transitionTable[{RobotState::STOPPED, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
|
||||
transitionTable[{RobotState::STOPPED, ControlCommand::UP}] = RobotState::MOVING_UP;
|
||||
transitionTable[{RobotState::STOPPED, ControlCommand::DOWN}] = RobotState::MOVING_DOWN;
|
||||
|
||||
// 从运动状态转换到其他运动状态 TODO: 不支持直接切换
|
||||
// transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::BACKWARD}] = RobotState::MOVING_BACKWARD;
|
||||
// transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
|
||||
// transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
|
||||
// transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
|
||||
// transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
|
||||
// transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
|
||||
// 其他可能的状态转换...
|
||||
// transitionTable[{RobotState::TURNING_LEFT, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
|
||||
// transitionTable[{RobotState::TURNING_RIGHT, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
|
||||
}
|
||||
|
||||
// 添加输入设备
|
||||
void RobotFsm::addInputDevice(InputType type, std::unique_ptr<InputDevice> device) {
|
||||
inputDevices[type] = std::move(device);
|
||||
std::cout << "Added input device: " << inputDevices[type]->getDeviceName() << std::endl;
|
||||
}
|
||||
|
||||
// 状态机主循环
|
||||
void RobotFsm::run() {
|
||||
std::cout << "Robot controller started. Press 'q' to quit." << std::endl;
|
||||
std::cout << "Keyboard controls: WASD to move, Q to quit, U to up, D to down" << std::endl;
|
||||
|
||||
while (true) {
|
||||
// 检查所有输入设备
|
||||
ControlCommand cmd = ControlCommand::NONE;
|
||||
for (const auto& [type, device] : inputDevices) {
|
||||
ControlCommand deviceCmd = device->getCommand();
|
||||
if (deviceCmd != ControlCommand::NONE) {
|
||||
cmd = deviceCmd;
|
||||
break; // 优先级：先获取到的命令先执行
|
||||
}
|
||||
}
|
||||
|
||||
// 处理退出命令
|
||||
if (cmd == ControlCommand::NONE) {
|
||||
// 检查键盘的退出命令（特殊处理）
|
||||
if (_kbhit()) {
|
||||
char key = _getch();
|
||||
if (key == 'q' || key == 'Q') {
|
||||
std::cout << "Exiting robot controller..." << std::endl;
|
||||
processCommand(ControlCommand::STOP);
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
processCommand(cmd);
|
||||
}
|
||||
|
||||
// 执行当前状态的动作
|
||||
executeStateAction();
|
||||
|
||||
// 控制循环频率
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
}
|
||||
}
|
||||
|
||||
// 处理命令并转换状态
|
||||
void RobotController::processCommand(ControlCommand cmd) {
|
||||
if (cmd == ControlCommand::NONE) return;
|
||||
|
||||
auto it = transitionTable.find({currentState, cmd});
|
||||
if (it != transitionTable.end()) {
|
||||
RobotState newState = it->second;
|
||||
if (newState != currentState) {
|
||||
std::cout << "State changed: " << stateToString(currentState)
|
||||
<< " -> " << stateToString(newState) << std::endl;
|
||||
currentState = newState;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 执行当前状态对应的动作
|
||||
void RobotController::executeStateAction() {
|
||||
// 在实际应用中，这里会发送相应的控制信号给机器人
|
||||
switch (currentState) {
|
||||
case RobotState::STOPPED:
|
||||
// 发送停止命令
|
||||
break;
|
||||
case RobotState::MOVING_FORWARD:
|
||||
// 发送前进命令
|
||||
break;
|
||||
case RobotState::MOVING_BACKWARD:
|
||||
// 发送后退命令
|
||||
break;
|
||||
case RobotState::TURNING_LEFT:
|
||||
// 发送左转命令
|
||||
break;
|
||||
case RobotState::TURNING_RIGHT:
|
||||
// 发送右转命令
|
||||
break;
|
||||
case RobotState::MOVING_UP:
|
||||
// 发送上升命令
|
||||
break;
|
||||
case RobotState::MOVING_DOWN:
|
||||
// 发送下降命令
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 状态转换为字符串
|
||||
std::string RobotController::stateToString(RobotState state) const {
|
||||
switch (state) {
|
||||
case RobotState::STOPPED: return "STOPPED";
|
||||
case RobotState::MOVING_FORWARD: return "MOVING_FORWARD";
|
||||
case RobotState::MOVING_BACKWARD: return "MOVING_BACKWARD";
|
||||
case RobotState::TURNING_LEFT: return "TURNING_LEFT";
|
||||
case RobotState::TURNING_RIGHT: return "TURNING_RIGHT";
|
||||
case RobotState::MOVING_UP: return "MOVING_UP";
|
||||
case RobotState::MOVING_DOWN: return "MOVING_DOWN";
|
||||
default: return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
// 键盘输入处理
|
||||
ControlCommand KeyboardInput::getCommand() {
|
||||
if (_kbhit()) {
|
||||
char key = _getch();
|
||||
switch (key) {
|
||||
case 'w': case 'W': return ControlCommand::FORWARD;
|
||||
case 's': case 'S': return ControlCommand::BACKWARD;
|
||||
case 'a': case 'A': return ControlCommand::LEFT;
|
||||
case 'd': case 'D': return ControlCommand::RIGHT;
|
||||
case 'u': case 'U': return ControlCommand::UP;
|
||||
case 'j': case 'J': return ControlCommand::DOWN; // 使用J代替D避免与后退冲突
|
||||
case ' ': return ControlCommand::STOP; // 空格键停止
|
||||
default: return ControlCommand::NONE;
|
||||
}
|
||||
}
|
||||
return ControlCommand::NONE;
|
||||
}
|
||||
|
||||
// 手柄输入处理（模拟实现）
|
||||
ControlCommand GamepadInput::getCommand() {
|
||||
// 实际应用中这里会读取游戏手柄的输入
|
||||
// 这里使用随机值模拟手柄输入，仅作示例
|
||||
static std::random_device rd;
|
||||
static std::mt19937 gen(rd());
|
||||
static std::uniform_int_distribution<> dist(0, 100);
|
||||
|
||||
int val = dist(gen);
|
||||
|
||||
if (val < 10) return ControlCommand::FORWARD;
|
||||
else if (val < 20) return ControlCommand::BACKWARD;
|
||||
else if (val < 30) return ControlCommand::LEFT;
|
||||
else if (val < 40) return ControlCommand::RIGHT;
|
||||
else if (val < 45) return ControlCommand::UP;
|
||||
else if (val < 50) return ControlCommand::DOWN;
|
||||
else if (val < 55) return ControlCommand::STOP;
|
||||
else return ControlCommand::NONE;
|
||||
}
|
||||
|
||||
// 语音输入处理（模拟实现）
|
||||
ControlCommand VoiceInput::getCommand() {
|
||||
// 实际应用中这里会处理语音识别结果
|
||||
// 这里使用随机值模拟语音输入，仅作示例
|
||||
static std::random_device rd;
|
||||
static std::mt19937 gen(rd());
|
||||
static std::uniform_int_distribution<> dist(0, 100);
|
||||
|
||||
int val = dist(gen);
|
||||
|
||||
if (val < 5) return ControlCommand::FORWARD;
|
||||
else if (val < 10) return ControlCommand::BACKWARD;
|
||||
else if (val < 15) return ControlCommand::LEFT;
|
||||
else if (val < 20) return ControlCommand::RIGHT;
|
||||
else if (val < 23) return ControlCommand::UP;
|
||||
else if (val < 26) return ControlCommand::DOWN;
|
||||
else if (val < 29) return ControlCommand::STOP;
|
||||
else return ControlCommand::NONE;
|
||||
}
|
||||
|
||||
@@ -5,14 +5,14 @@ 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(urdf REQUIRED)
|
||||
find_package(xacro REQUIRED)
|
||||
find_package(urdf REQUIRED)
|
||||
|
||||
# 安装模型文件
|
||||
install(DIRECTORY urdf meshes config
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
# Install directories
|
||||
install(DIRECTORY launch models
|
||||
DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
ament_package()
|
||||
|
||||
|
||||
@@ -1,57 +0,0 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 100 # Hz
|
||||
use_sim_time: true
|
||||
|
||||
# 关节状态广播器 - 必须配置，用于发布关节状态
|
||||
robot_joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
use_sim_time: true
|
||||
|
||||
# 关节位置控制器
|
||||
robot_position_controller:
|
||||
type: position_controllers/JointGroupPositionController
|
||||
|
||||
# 腿部和手臂关节位置控制器配置
|
||||
robot_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 # 弧度
|
||||
@@ -0,0 +1,51 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
import os
|
||||
|
||||
def generate_launch_description():
|
||||
# 找到包的路径
|
||||
package_share_dir = FindPackageShare(package='robot_description').find('robot_description')
|
||||
|
||||
# 声明启动参数
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
urdf_path = os.path.join(package_share_dir, 'models', 'robot.urdf')
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'
|
||||
),
|
||||
|
||||
# 启动joint_state_publisher_gui
|
||||
Node(
|
||||
package='joint_state_publisher_gui',
|
||||
executable='joint_state_publisher_gui',
|
||||
name='joint_state_publisher_gui',
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# 启动robot_state_publisher
|
||||
Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'robot_description': open(urdf_path).read()
|
||||
}]
|
||||
),
|
||||
|
||||
# 启动RViz2
|
||||
Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': use_sim_time}]
|
||||
)
|
||||
])
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
236
HiveCoreR0/src/robot_description/models/hive_core_r0.xml
Normal file
236
HiveCoreR0/src/robot_description/models/hive_core_r0.xml
Normal file
@@ -0,0 +1,236 @@
|
||||
<mujoco model="hive_core_r0">
|
||||
<option iterations="50" timestep="0.001" solver="PGS" gravity="0 0 -9.81" />
|
||||
|
||||
<compiler angle="radian" meshdir="meshes" eulerseq="zyx" autolimits="true" />
|
||||
|
||||
<!-- <default>
|
||||
<joint limited="true" damping="0.01" armature="0.01" frictionloss="0.01" />
|
||||
<geom condim="4" contype="1" conaffinity="15" friction="0.9 0.2 0.2" solref="0.001 2" />
|
||||
<motor ctrllimited="false" />
|
||||
<equality solref="0.001 2" />
|
||||
<default class="visualgeom">
|
||||
<geom material="visualgeom" condim="1" contype="0" conaffinity="0" />
|
||||
</default>
|
||||
</default> -->
|
||||
|
||||
<default>
|
||||
<!-- 关节参数：减小关节摩擦 -->
|
||||
<joint limited="true" damping="0.005" armature="0.01" frictionloss="0.001" /> <!-- 降低frictionloss和damping -->
|
||||
|
||||
<!-- 几何体参数：减小摩擦和碰撞响应 -->
|
||||
<geom condim="4" contype="1" conaffinity="15"
|
||||
friction="0.3 0.01 0.05"
|
||||
solref="0.001 3"
|
||||
solimp="0.005 0.9 0.1" /> <!-- 增加穿透容忍度（第一个值），减少碰撞力 -->
|
||||
|
||||
<motor ctrllimited="false" />
|
||||
<equality solref="0.01 3" /> <!-- 同步调整equality的碰撞参数 -->
|
||||
|
||||
<default class="visualgeom">
|
||||
<geom material="visualgeom" condim="1" contype="0" conaffinity="0" />
|
||||
</default>
|
||||
</default>
|
||||
|
||||
|
||||
<asset>
|
||||
<mesh name="base_link" content_type="model/stl" file="base_link.STL" />
|
||||
<mesh name="left_leg1_link" content_type="model/stl" file="left_leg1_link.STL" />
|
||||
<mesh name="left_wheel1_link" content_type="model/stl" file="left_wheel1_link.STL" />
|
||||
<mesh name="right_leg1_link" content_type="model/stl" file="right_leg1_link.STL" />
|
||||
<mesh name="right_wheel1_link" content_type="model/stl" file="right_wheel1_link.STL" />
|
||||
<mesh name="pitch_body_link" content_type="model/stl" file="pitch_body_link.STL" />
|
||||
<mesh name="left_shoulder_link" content_type="model/stl" file="left_shoulder_link.STL" />
|
||||
<mesh name="left_arm1_link" content_type="model/stl" file="left_arm1_link.STL" />
|
||||
<mesh name="left_arm2_link" content_type="model/stl" file="left_arm2_link.STL" />
|
||||
<mesh name="right_shoulder_link" content_type="model/stl" file="right_shoulder_link.STL" />
|
||||
<mesh name="right_arm1_link" content_type="model/stl" file="right_arm1_link.STL" />
|
||||
<mesh name="right_arm2_link" content_type="model/stl" file="right_arm2_link.STL" />
|
||||
<mesh name="left_leg2_link" content_type="model/stl" file="left_leg2_link.STL" />
|
||||
<mesh name="left_wheel2_link" content_type="model/stl" file="left_wheel2_link.STL" />
|
||||
<mesh name="right_leg2_link" content_type="model/stl" file="right_leg2_link.STL" />
|
||||
<mesh name="right_wheel2_link" content_type="model/stl" file="right_wheel2_link.STL" />
|
||||
<texture name="texplane" type="2d" builtin="checker" rgb1=".0 .0 .0" rgb2=".8 .8 .8" width="100" height="100" />
|
||||
<material name="matplane" reflectance="0." texture="texplane" texrepeat="1 1" texuniform="true" />
|
||||
<material name="visualgeom" rgba="0.5 0.9 0.2 1" />
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<light directional="true" diffuse="0.4 0.4 0.4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false" />
|
||||
<light directional="true" diffuse="0.6 0.6 0.6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1" />
|
||||
<geom name="ground" type="plane" pos="0 0 0" size="100 100 0.001" quat="1 0 0 0" material="matplane" condim="3" conaffinity="15" />
|
||||
<camera name="fixed" pos="0 -3.0 0.9062448722851582" xyaxes="1 0 0 0 0 1" />
|
||||
<camera name="track" mode="trackcom" pos="0 -3.0 0.9062448722851582" xyaxes="1 0 0 0 0 1" />
|
||||
<body name="root" pos="0 0 0.4062448722851581" quat="1 0 0 0">
|
||||
<freejoint name="root" />
|
||||
<site name="imu" size="0.01" pos="0 0 0" />
|
||||
<geom type="mesh" rgba="0.592157 0.619608 0.65098 1" mesh="base_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="0.592157 0.619608 0.65098 1" mesh="base_link" />
|
||||
<body name="left_leg1_link" pos="-0.025573 0.143 0.031674" quat="0.685177 0.728377 0 0">
|
||||
<inertial pos="0.0365132 -0.100285 -0.000439159" quat="0.701159 -0.118757 -0.126604 0.691552" mass="0.687222" diaginertia="0.0112809 0.011113 0.000216194" />
|
||||
<joint name="left_leg1_joint" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57" actuatorfrcrange="-60 60" />
|
||||
<geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="left_leg1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="left_leg1_link" />
|
||||
<body name="left_wheel1_link" pos="0.14246 -0.39141 0.03757">
|
||||
<inertial pos="3.63169e-07 2.62194e-06 -0.00076961" quat="0.706246 0.0348808 -0.0348808 0.706246" mass="0.75" diaginertia="0.00042099 0.00032 0.00031901" />
|
||||
<joint name="left_wheel1_joint" pos="0 0 0" axis="0 0 -1" range="-1000 1000" actuatorfrcrange="-30 30" />
|
||||
<geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="left_wheel1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="left_wheel1_link" />
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_leg1_link" pos="-0.025573 -0.143 0.031674" quat="0.728374 0.685179 0 0">
|
||||
<inertial pos="0.036491 -0.100293 0.000439159" quat="0.691664 -0.126482 -0.118674 0.701084" mass="0.687222" diaginertia="0.0112809 0.0111118 0.000207362" />
|
||||
<joint name="right_leg1_joint" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57" actuatorfrcrange="-60 60" />
|
||||
<geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="right_leg1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="right_leg1_link" />
|
||||
<body name="right_wheel1_link" pos="0.142461 -0.391408 -0.0375696" quat="0 1 0 0">
|
||||
<inertial pos="-2.01863e-06 -1.71218e-06 -0.00076961" quat="0.706246 -0.0348808 0.0348808 0.706246" mass="0.75" diaginertia="0.00042099 0.00032 0.00031901" />
|
||||
<joint name="right_wheel1_joint" pos="0 0 0" axis="0 0 -1" range="-1000 1000" actuatorfrcrange="-30 30" />
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="right_wheel1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="right_wheel1_link" />
|
||||
</body>
|
||||
</body>
|
||||
<body name="pitch_body_link" pos="-0.022999 0.039 0.048" quat="0.707105 0.707108 0 0">
|
||||
<inertial pos="0.000405026 0.164222 0.038213" quat="0.999026 0.0383753 0.0211692 -0.00503889" mass="4.31439" diaginertia="0.0256579 0.0186955 0.0159865" />
|
||||
<joint name="pitch_body_joint" pos="0 0 0" axis="0 0 -1" range="-0.5236 0.5236" actuatorfrcrange="-20 20" />
|
||||
<geom type="mesh" rgba="0.592157 0.619608 0.65098 1" mesh="pitch_body_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="0.592157 0.619608 0.65098 1" mesh="pitch_body_link" />
|
||||
<body name="left_shoulder_link" pos="0.0039637 0.22096 -0.0095" quat="0.707105 -0.707108 0 0">
|
||||
<inertial pos="0.0139818 -0.000283388 0.0469667" quat="0.706719 0.706719 0.0234277 -0.0234277" mass="1.12175" diaginertia="0.000990664 0.000839336 0.00082" />
|
||||
<joint name="left_shoulder_joint" pos="0 0 0" axis="-0.017935 0 -0.999839" range="-1.57 1.57" actuatorfrcrange="-60 60" />
|
||||
<geom type="mesh" rgba="0.501961 0.501961 0.501961 1" mesh="left_shoulder_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="0.501961 0.501961 0.501961 1" mesh="left_shoulder_link" />
|
||||
<body name="left_arm1_link" pos="0.040338 -0.00076903 0.046784" quat="0.915825 -0.401579 0 0">
|
||||
<inertial pos="-0.0222765 0.116446 0.130396" quat="0.713034 0.69112 0.104336 0.0552205" mass="0.97181" diaginertia="0.00209185 0.0020505 0.000677649" />
|
||||
<joint name="left_arm1_joint" pos="0 0 0" axis="0.99965 0 -0.026469" range="-1.57 1.57" actuatorfrcrange="-60 60" />
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="left_arm1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="left_arm1_link" />
|
||||
<body name="left_arm2_link" pos="0.0085407 0.12037 0.13366" quat="-3.67321e-06 -1 0 0">
|
||||
<inertial pos="-0.00344285 -0.0871901 -0.09473" quat="0.668762 0.691991 -0.148059 0.228003" mass="0.153345" diaginertia="0.000866593 0.000844058 8.93494e-05" />
|
||||
<joint name="left_arm2_joint" pos="0 0 0" axis="-0.99965 0 -0.026469" range="-1.57 1.57" actuatorfrcrange="-20 20" />
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="left_arm2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="left_arm2_link" />
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_shoulder_link" pos="0.0039637 0.22096 0.0865" quat="0.707105 -0.707108 0 0">
|
||||
<inertial pos="0.0139829 0.000183964 0.0469281" quat="0.706719 0.706719 0.0234277 -0.0234277" mass="1.12175" diaginertia="0.000990664 0.000839336 0.00082" />
|
||||
<joint name="right_shoulder_joint" pos="0 0 0" axis="-0.017935 0 -0.999839" range="-1.57 1.57" actuatorfrcrange="-60 60" />
|
||||
<geom type="mesh" rgba="0.898039 0.921569 0.929412 1" mesh="right_shoulder_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="0.898039 0.921569 0.929412 1" mesh="right_shoulder_link" />
|
||||
<body name="right_arm1_link" pos="0.040343 0.000445 0.046784" quat="0.960932 0.276783 0 0">
|
||||
<inertial pos="-0.0237185 -0.146707 0.0947584" quat="0.700546 0.70096 -0.0393265 -0.127846" mass="0.97181" diaginertia="0.00208774 0.00204969 0.000682566" />
|
||||
<joint name="right_arm1_joint" pos="0 0 0" axis="0.999776 0 -0.0211789" range="-1.57 1.57" actuatorfrcrange="-60 60" />
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="right_arm1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="right_arm1_link" />
|
||||
<body name="right_arm2_link" pos="0.00696183 -0.151368 0.0972794" quat="0 -1 0 0">
|
||||
<inertial pos="-0.00446701 0.108002 -0.0700232" quat="0.707099 0.702409 0.00330707 0.0813729" mass="0.153345" diaginertia="0.00086092 0.00085 9.90801e-05" />
|
||||
<joint name="right_arm2_joint" pos="0 0 0" axis="-0.999776 0 -0.0211789" range="-1.57 1.57" actuatorfrcrange="-20 20" />
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="right_arm2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="right_arm2_link" />
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="left_leg2_link" pos="-0.090467 0.14336 0.031579" quat="0.685177 0.728377 0 0">
|
||||
<inertial pos="-0.0333228 -0.0915849 0.000701841" quat="0.693033 0.128657 0.116355 0.699724" mass="0.758445" diaginertia="0.0118728 0.0117401 0.000237059" />
|
||||
<joint name="left_leg2_joint" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57" actuatorfrcrange="-60 60" />
|
||||
<geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="left_leg2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="left_leg2_link" />
|
||||
<body name="left_wheel2_link" pos="-0.14242 -0.39129 0.037923">
|
||||
<inertial pos="1.94616e-06 1.79413e-06 -0.00076961" quat="0.706246 0.0348808 -0.0348808 0.706246" mass="0.75" diaginertia="0.00042099 0.00032 0.00031901" />
|
||||
<joint name="left_wheel2_joint" pos="0 0 0" axis="0 0 -1" range="-1000 1000" actuatorfrcrange="-30 30" />
|
||||
<geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="left_wheel2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="left_wheel2_link" />
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_leg2_link" pos="-0.090467 -0.14336 0.031579" quat="0.707105 0.707108 0 0">
|
||||
<inertial pos="-0.0333429 -0.0914497 0.00489015" quat="0.702629 0.116043 0.129569 0.68997" mass="0.758445" diaginertia="0.0118729 0.0117377 0.00022938" />
|
||||
<joint name="right_leg2_joint" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57" actuatorfrcrange="-60 60" />
|
||||
<geom type="mesh" rgba="0.501961 0.501961 0.501961 1" mesh="right_leg2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="0.501961 0.501961 0.501961 1" mesh="right_leg2_link" />
|
||||
<body name="right_wheel2_link" pos="-0.14242 -0.39288 -0.013965" quat="0.0305416 0.999533 0 0">
|
||||
<inertial pos="-2.61861e-06 -3.86428e-07 -0.00076961" quat="0.706246 -0.0348808 0.0348808 0.706246" mass="0.75" diaginertia="0.00042099 0.00032 0.00031901" />
|
||||
<joint name="right_wheel2_joint" pos="0 0 0" axis="0 0 -1" range="-1000 1000" actuatorfrcrange="-30 30" />
|
||||
<geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="right_wheel2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
|
||||
<geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="right_wheel2_link" />
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
|
||||
<actuator>
|
||||
<!-- 腿部关节 -->
|
||||
<position name="left_leg1_joint_ctrl" joint="left_leg1_joint" kp="500" kv="0.6" />
|
||||
<position name="right_leg1_joint_ctrl" joint="right_leg1_joint" kp="500" kv="0.6" />
|
||||
<position name="left_leg2_joint_ctrl" joint="left_leg2_joint" kp="500" kv="0.6" />
|
||||
<position name="right_leg2_joint_ctrl" joint="right_leg2_joint" kp="500" kv="0.6" />
|
||||
|
||||
<!-- 车轮关节 -->
|
||||
<position name="left_wheel1_joint_ctrl" joint="left_wheel1_joint" kp="500" kv="0.6" />
|
||||
<position name="right_wheel1_joint_ctrl" joint="right_wheel1_joint" kp="500" kv="0.6" />
|
||||
<position name="left_wheel2_joint_ctrl" joint="left_wheel2_joint" kp="500" kv="0.6" />
|
||||
<position name="right_wheel2_joint_ctrl" joint="right_wheel2_joint" kp="500" kv="0.6" />
|
||||
|
||||
<!-- 身体关节 -->
|
||||
<position name="pitch_body_joint_ctrl" joint="pitch_body_joint" kp="500" kv="0.6" />
|
||||
|
||||
<!-- 手臂关节 -->
|
||||
<position name="left_shoulder_joint_ctrl" joint="left_shoulder_joint" kp="500" kv="0.6" />
|
||||
<position name="right_shoulder_joint_ctrl" joint="right_shoulder_joint" kp="500" kv="0.6" />
|
||||
<position name="left_arm1_joint_ctrl" joint="left_arm1_joint" kp="500" kv="0.6" />
|
||||
<position name="left_arm2_joint_ctrl" joint="left_arm2_joint" kp="500" kv="0.6" />
|
||||
<position name="right_arm1_joint_ctrl" joint="right_arm1_joint" kp="500" kv="0.6" />
|
||||
<position name="right_arm2_joint_ctrl" joint="right_arm2_joint" kp="500" kv="0.6" />
|
||||
</actuator>
|
||||
|
||||
<sensor>
|
||||
<actuatorpos name="left_leg1_joint_p" actuator="left_leg1_joint_ctrl" />
|
||||
<actuatorvel name="left_leg1_joint_v" actuator="left_leg1_joint_ctrl" />
|
||||
<actuatorfrc name="left_leg1_joint_f" actuator="left_leg1_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="left_wheel1_joint_p" actuator="left_wheel1_joint_ctrl" />
|
||||
<actuatorvel name="left_wheel1_joint_v" actuator="left_wheel1_joint_ctrl" />
|
||||
<actuatorfrc name="left_wheel1_joint_f" actuator="left_wheel1_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="right_leg1_joint_p" actuator="right_leg1_joint_ctrl" />
|
||||
<actuatorvel name="right_leg1_joint_v" actuator="right_leg1_joint_ctrl" />
|
||||
<actuatorfrc name="right_leg1_joint_f" actuator="right_leg1_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="right_wheel1_joint_p" actuator="right_wheel1_joint_ctrl" />
|
||||
<actuatorvel name="right_wheel1_joint_v" actuator="right_wheel1_joint_ctrl" />
|
||||
<actuatorfrc name="right_wheel1_joint_f" actuator="right_wheel1_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="pitch_body_joint_p" actuator="pitch_body_joint_ctrl" />
|
||||
<actuatorvel name="pitch_body_joint_v" actuator="pitch_body_joint_ctrl" />
|
||||
<actuatorfrc name="pitch_body_joint_f" actuator="pitch_body_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="left_shoulder_joint_p" actuator="left_shoulder_joint_ctrl" />
|
||||
<actuatorvel name="left_shoulder_joint_v" actuator="left_shoulder_joint_ctrl" />
|
||||
<actuatorfrc name="left_shoulder_joint_f" actuator="left_shoulder_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="left_arm1_joint_p" actuator="left_arm1_joint_ctrl" />
|
||||
<actuatorvel name="left_arm1_joint_v" actuator="left_arm1_joint_ctrl" />
|
||||
<actuatorfrc name="left_arm1_joint_f" actuator="left_arm1_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="left_arm2_joint_p" actuator="left_arm2_joint_ctrl" />
|
||||
<actuatorvel name="left_arm2_joint_v" actuator="left_arm2_joint_ctrl" />
|
||||
<actuatorfrc name="left_arm2_joint_f" actuator="left_arm2_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="right_shoulder_joint_p" actuator="right_shoulder_joint_ctrl" />
|
||||
<actuatorvel name="right_shoulder_joint_v" actuator="right_shoulder_joint_ctrl" />
|
||||
<actuatorfrc name="right_shoulder_joint_f" actuator="right_shoulder_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="right_arm1_joint_p" actuator="right_arm1_joint_ctrl" />
|
||||
<actuatorvel name="right_arm1_joint_v" actuator="right_arm1_joint_ctrl" />
|
||||
<actuatorfrc name="right_arm1_joint_f" actuator="right_arm1_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="right_arm2_joint_p" actuator="right_arm2_joint_ctrl" />
|
||||
<actuatorvel name="right_arm2_joint_v" actuator="right_arm2_joint_ctrl" />
|
||||
<actuatorfrc name="right_arm2_joint_f" actuator="right_arm2_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="left_leg2_joint_p" actuator="left_leg2_joint_ctrl" />
|
||||
<actuatorvel name="left_leg2_joint_v" actuator="left_leg2_joint_ctrl" />
|
||||
<actuatorfrc name="left_leg2_joint_f" actuator="left_leg2_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="left_wheel2_joint_p" actuator="left_wheel2_joint_ctrl" />
|
||||
<actuatorvel name="left_wheel2_joint_v" actuator="left_wheel2_joint_ctrl" />
|
||||
<actuatorfrc name="left_wheel2_joint_f" actuator="left_wheel2_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="right_leg2_joint_p" actuator="right_leg2_joint_ctrl" />
|
||||
<actuatorvel name="right_leg2_joint_v" actuator="right_leg2_joint_ctrl" />
|
||||
<actuatorfrc name="right_leg2_joint_f" actuator="right_leg2_joint_ctrl" noise="0.001" />
|
||||
<actuatorpos name="right_wheel2_joint_p" actuator="right_wheel2_joint_ctrl" />
|
||||
<actuatorvel name="right_wheel2_joint_v" actuator="right_wheel2_joint_ctrl" />
|
||||
<actuatorfrc name="right_wheel2_joint_f" actuator="right_wheel2_joint_ctrl" noise="0.001" />
|
||||
<framequat name="orientation" objtype="site" noise="0.001" objname="imu" />
|
||||
<gyro name="angular-velocity" site="imu" noise="0.005" cutoff="34.9" />
|
||||
</sensor>
|
||||
</mujoco>
|
||||
216
HiveCoreR0/src/robot_description/models/hive_core_r0_bak.xml
Normal file
216
HiveCoreR0/src/robot_description/models/hive_core_r0_bak.xml
Normal file
@@ -0,0 +1,216 @@
|
||||
<mujoco model="hive_core_r0">
|
||||
<default>
|
||||
<default class="robot">
|
||||
<default class="motor">
|
||||
<joint />
|
||||
<motor />
|
||||
</default>
|
||||
<default class="visual">
|
||||
<geom material="visualgeom" contype="0" conaffinity="0" group="2" />
|
||||
</default>
|
||||
<default class="collision">
|
||||
<geom material="collision_material" condim="3" contype="0" conaffinity="1" group="1" solref="0.005 1" friction="1 0.01 0.01" />
|
||||
<equality solimp="0.99 0.999 1e-05" solref="0.005 1" />
|
||||
</default>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
<compiler angle="radian" />
|
||||
<option timestep="0.001" gravity="0 0 -9.81" wind="0 0 0" integrator="RK4"
|
||||
density="1.625"
|
||||
viscosity="1.8e-5" />
|
||||
|
||||
<visual>
|
||||
<global realtime="1" />
|
||||
<quality shadowsize="2048" numslices="28" offsamples="4" />
|
||||
<headlight diffuse="1 1 1" specular="0.5 0.5 0.5" active="1" />
|
||||
<rgba fog="1 0 0 1" haze="1 1 1 1" />
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<texture name="plane" type="2d" builtin="checker" rgb1=".1 .1 .1" rgb2=".9 .9 .9"
|
||||
width="512" height="512" mark="cross" markrgb=".8 .8 .8" />
|
||||
<material name="plane" reflectance="0.3" texture="plane" texrepeat="1 1" texuniform="true" />
|
||||
<material name="metal" rgba="1 1 1 1" />
|
||||
<material name="rubber" rgba="0.5 0.5 0.5 1" />
|
||||
<material name="default_material" rgba="0.7 0.7 0.7 1" />
|
||||
<material name="collision_material" rgba="1.0 0.28 0.1 0.9" />
|
||||
<mesh name="base_link.STL" file="../meshes/base_link.STL" />
|
||||
<mesh name="left_leg1_link.STL" file="../meshes/left_leg1_link.STL" />
|
||||
<mesh name="left_wheel1_link.STL" file="../meshes/left_wheel1_link.STL" />
|
||||
<mesh name="right_leg1_link.STL" file="../meshes/right_leg1_link.STL" />
|
||||
<mesh name="right_wheel1_link.STL" file="../meshes/right_wheel1_link.STL" />
|
||||
<mesh name="pitch_body_link.STL" file="../meshes/pitch_body_link.STL" />
|
||||
<mesh name="left_shoulder_link.STL" file="../meshes/left_shoulder_link.STL" />
|
||||
<mesh name="left_arm1_link.STL" file="../meshes/left_arm1_link.STL" />
|
||||
<mesh name="left_arm2_link.STL" file="../meshes/left_arm2_link.STL" />
|
||||
<mesh name="right_shoulder_link.STL" file="../meshes/right_shoulder_link.STL" />
|
||||
<mesh name="right_arm1_link.STL" file="../meshes/right_arm1_link.STL" />
|
||||
<mesh name="right_arm2_link.STL" file="../meshes/right_arm2_link.STL" />
|
||||
<mesh name="left_leg2_link.STL" file="../meshes/left_leg2_link.STL" />
|
||||
<mesh name="left_wheel2_link.STL" file="../meshes/left_wheel2_link.STL" />
|
||||
<mesh name="right_leg2_link.STL" file="../meshes/right_leg2_link.STL" />
|
||||
<mesh name="right_wheel2_link.STL" file="../meshes/right_wheel2_link.STL" />
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<geom name="floor" pos="0 0 0" size="0 0 .25" type="plane" material="plane"
|
||||
condim="3" friction="1.5 0.3 0.05"
|
||||
solimp="0.9 0.95 0.001" solref="0.02 1"/>
|
||||
<light directional="true" ambient=".3 .3 .3" pos="30 30 30" dir="0 -2 -1"
|
||||
diffuse=".5 .5 .5" specular=".5 .5 .5" />
|
||||
<body name="base_link" pos="0.00000000 0.00000000 0.48591989" quat="1 0 0 0" childclass="robot">
|
||||
<freejoint name="floating_base" />
|
||||
<geom name="base_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="base_link.STL" class="collision" />
|
||||
<geom name="base_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="base_link.STL" class="visual" />
|
||||
<body name="left_leg1_link" pos="-0.023189 -0.14297 0.032124" quat="0.7283743282895457 0.6851794202168897 0.0 0.0">
|
||||
<joint name="left_leg1_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="0 0 -1" />
|
||||
<inertial pos="0.0026919722577641 -0.106691141065344 0.000439159272074496" quat="1.0 0.0 0.0 0.0" mass="0.687221704005751" diaginertia="0.00309344360201386 0.000199758758340417 0.00327480626360329" />
|
||||
<geom name="left_leg1_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_leg1_link.STL" class="collision" />
|
||||
<geom name="left_leg1_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="left_leg1_link.STL" class="visual" />
|
||||
<body name="left_wheel1_link" pos="0.010552 -0.41639 -0.03757" quat="-3.673205103346574e-06 0.9999999999932537 0.0 -0.0">
|
||||
<joint name="left_wheel1_joint" type="hinge" ref="0.0" class="motor" range="-1000 1000" axis="0 0 -1" />
|
||||
<inertial pos="-2.01863499148247E-06 -1.71217600630769E-06 -0.000769610134319532" quat="1.0 0.0 0.0 0.0" mass="0.243841354713671" diaginertia="0.000147760027881631 0.000147759325738658 0.000229476928281019" />
|
||||
<geom name="left_wheel1_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_wheel1_link.STL" class="collision" />
|
||||
<geom name="left_wheel1_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="rubber" type="mesh" mesh="left_wheel1_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_leg1_link" pos="-0.022996 0.14297 0.032127" quat="0.6851767447439675 0.728376845089175 0.0 0.0">
|
||||
<joint name="right_leg1_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="0 0 -1" />
|
||||
<inertial pos="-3.01562743165443E-05 -0.106725092599043 -0.000439159317208282" quat="1.0 0.0 0.0 0.0" mass="0.687221703818328" diaginertia="0.00309524606532856 0.000197956293734699 0.00327480626231139" />
|
||||
<geom name="right_leg1_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_leg1_link.STL" class="collision" />
|
||||
<geom name="right_leg1_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="right_leg1_link.STL" class="visual" />
|
||||
<body name="right_wheel1_link" pos="-0.00016377 -0.41653 0.03757" quat="1.0 0.0 0.0 0.0">
|
||||
<joint name="right_wheel1_joint" type="hinge" ref="0.0" class="motor" range="-1000 1000" axis="0 0 -1" />
|
||||
<inertial pos="1.92448237847231E-06 1.8173611248673E-06 -0.000769610176954372" quat="1.0 0.0 0.0 0.0" mass="0.243841354713671" diaginertia="0.000147759799888368 0.000147759553731921 0.000229476928281019" />
|
||||
<geom name="right_wheel1_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_wheel1_link.STL" class="collision" />
|
||||
<geom name="right_wheel1_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="rubber" type="mesh" mesh="right_wheel1_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
<body name="pitch_body_link" pos="-0.022999 0.039 0.048" quat="0.7071054825112363 0.7071080798594735 0.0 0.0">
|
||||
<joint name="pitch_body_joint" type="hinge" ref="0.0" class="motor" range="-0.5236 0.5236" axis="0 0 -1" />
|
||||
<inertial pos="0.000405025729528165 0.164222263553734 0.038212974552542" quat="1.0 0.0 0.0 0.0" mass="6.27735184123949" diaginertia="0.012112631055994 0.00923713569100348 0.00865286317711146" />
|
||||
<geom name="pitch_body_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="pitch_body_link.STL" class="collision" />
|
||||
<geom name="pitch_body_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="pitch_body_link.STL" class="visual" />
|
||||
<body name="left_shoulder_link" pos="0.00396365185327967 0.220964452984708 0.0865" quat="0.7071067811865462 -0.7071067811865488 0.0 0.0">
|
||||
<joint name="left_shoulder_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="-0.0179350762557234 0 -0.999839153584066" />
|
||||
<inertial pos="0.0139828606345992 0.000183964382035662 0.0469280544333041" quat="1.0 0.0 0.0 0.0" mass="2.24001373537068" diaginertia="0.00187573272868104 0.00158854268624343 0.00161229514279373" />
|
||||
<geom name="left_shoulder_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_shoulder_link.STL" class="collision" />
|
||||
<geom name="left_shoulder_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="left_shoulder_link.STL" class="visual" />
|
||||
<body name="left_arm1_link" pos="0.0403430563984948 0.00044500097133069 0.0467839692408788" quat="0.9609327754350184 0.27678186554532885 0.0 0.0">
|
||||
<joint name="left_arm1_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="0.99977570211955 0 -0.0211788916461878" />
|
||||
<inertial pos="-0.0237185143086008 -0.146707321130601 0.0947584340472464" quat="1.0 0.0 0.0 0.0" mass="1.97210272038181" diaginertia="0.0020453207276327 0.00151626513015615 0.00183373717106277" />
|
||||
<geom name="left_arm1_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_arm1_link.STL" class="collision" />
|
||||
<geom name="left_arm1_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="left_arm1_link.STL" class="visual" />
|
||||
<body name="left_arm2_link" pos="0.00696183199978448 -0.151367984040579 0.0972793725016652" quat="1.6714577709334716e-14 -1.0 0.0 0.0">
|
||||
<joint name="left_arm2_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="-0.99977570211955 0 -0.0211788916461861" />
|
||||
<inertial pos="-0.0044670114805947 0.108002437145143 -0.0700232476387601" quat="1.0 0.0 0.0 0.0" mass="0.153345483355902" diaginertia="0.000846185788600758 0.000325958142834647 0.000633417429555725" />
|
||||
<geom name="left_arm2_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_arm2_link.STL" class="collision" />
|
||||
<geom name="left_arm2_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="left_arm2_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_shoulder_link" pos="0.00396365185327941 0.220964452984708 -0.00950000000000008" quat="0.7071067811865462 -0.7071067811865488 0.0 0.0">
|
||||
<joint name="right_shoulder_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="-0.0179350762557234 0 -0.999839153584066" />
|
||||
<inertial pos="0.0139817634960264 -0.000283387663890286 0.0469666821426944" quat="1.0 0.0 0.0 0.0" mass="2.24001360218308" diaginertia="0.00187572320791288 0.00158896107508856 0.00161188619803665" />
|
||||
<geom name="right_shoulder_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_shoulder_link.STL" class="collision" />
|
||||
<geom name="right_shoulder_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="right_shoulder_link.STL" class="visual" />
|
||||
<body name="right_arm1_link" pos="0.040338077059205 -0.000769030026202884 0.046784058560075" quat="0.9158240905998835 -0.4015796746311951 0.0 0.0">
|
||||
<joint name="right_arm1_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="0.999649642897155 0 -0.0264686882106157" />
|
||||
<inertial pos="-0.0222764790210273 0.116445846258557 0.130395530944397" quat="1.0 0.0 0.0 0.0" mass="1.97210271472831" diaginertia="0.00204747347595548 0.00173434064125734 0.00161350890019016" />
|
||||
<geom name="right_arm1_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_arm1_link.STL" class="collision" />
|
||||
<geom name="right_arm1_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="right_arm1_link.STL" class="visual" />
|
||||
<body name="right_arm2_link" pos="0.00854070115695219 0.120366284301785 0.133656328091823" quat="1.1607551796058996e-14 -1.0 0.0 0.0">
|
||||
<joint name="right_arm2_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="-0.999649642897155 0 -0.0264686882106164" />
|
||||
<inertial pos="-0.00344285141443855 -0.0871900700243705 -0.0947300074211656" quat="1.0 0.0 0.0 0.0" mass="0.153345483533995" diaginertia="0.000848016249761213 0.000513290254168688 0.00044425485583754" />
|
||||
<geom name="right_arm2_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_arm2_link.STL" class="collision" />
|
||||
<geom name="right_arm2_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="right_arm2_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="left_leg2_link" pos="-0.092811 -0.14333 0.032022" quat="0.7071054825112363 0.7071080798594735 0.0 0.0">
|
||||
<joint name="left_leg2_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="0 0 -1" />
|
||||
<inertial pos="-0.00247971563497956 -0.0972883314998296 0.00524725682640842" quat="1.0 0.0 0.0 0.0" mass="0.758444942474565" diaginertia="0.00308402598971299 0.00021886670519613 0.00323764016118335" />
|
||||
<geom name="left_leg2_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_leg2_link.STL" class="collision" />
|
||||
<geom name="left_leg2_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="left_leg2_link.STL" class="visual" />
|
||||
<body name="left_wheel2_link" pos="-0.010549 -0.41781 -0.01244" quat="0.03054157666518473 0.9995334972349874 0.0 0.0">
|
||||
<joint name="left_wheel2_joint" type="hinge" ref="0.0" class="motor" range="-1000 1000" axis="0 0 -1" />
|
||||
<inertial pos="-2.63258476833339E-06 -2.75556383766418E-07 -0.000769610176954399" quat="1.0 0.0 0.0 0.0" mass="0.243841354713671" diaginertia="0.000147761781558832 0.000147757572061457 0.000229476928281019" />
|
||||
<geom name="left_wheel2_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_wheel2_link.STL" class="collision" />
|
||||
<geom name="left_wheel2_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="rubber" type="mesh" mesh="left_wheel2_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_leg2_link" pos="-0.092614 0.14333 0.032015" quat="0.6851767447439675 0.728376845089175 0.0 0.0">
|
||||
<joint name="right_leg2_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="0 0 -1" />
|
||||
<inertial pos="-0.0050516474502517 -0.0973277461307459 0.000701841039130763" quat="1.0 0.0 0.0 0.0" mass="0.758444958401021" diaginertia="0.00307823597256189 0.000212654851830719 0.00324964203847903" />
|
||||
<geom name="right_leg2_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_leg2_link.STL" class="collision" />
|
||||
<geom name="right_leg2_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="right_leg2_link.STL" class="visual" />
|
||||
<body name="right_wheel2_link" pos="-0.021629 -0.41584 0.037923" quat="1.0 0.0 0.0 0.0">
|
||||
<joint name="right_wheel2_joint" type="hinge" ref="0.0" class="motor" range="-1000 1000" axis="0 0 -1" />
|
||||
<inertial pos="1.93300484906123E-06 1.80829370854951E-06 -0.000769610176954413" quat="1.0 0.0 0.0 0.0" mass="0.243841354713671" diaginertia="0.000147759820077608 0.000147759533542682 0.000229476928281019" />
|
||||
<geom name="right_wheel2_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_wheel2_link.STL" class="collision" />
|
||||
<geom name="right_wheel2_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="rubber" type="mesh" mesh="right_wheel2_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
<site name="base_link_site" pos="0 0 0" quat="1 0 0 0" />
|
||||
<camera name="front_camera" mode="track" fovy="90.0" quat="4.329780281177467e-17 4.329780281177466e-17 0.7071067811865475 0.7071067811865476" pos="0.0 2.0 0.5" />
|
||||
<camera name="side_camera" mode="track" fovy="90.0" quat="-0.5 -0.4999999999999999 0.5 0.5000000000000001" pos="-2.0 0.0 0.5" />
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<!-- 腿部关节 -->
|
||||
<position name="left_leg1_joint_ctrl" joint="left_leg1_joint" kp="500" kv="0.6" />
|
||||
<position name="right_leg1_joint_ctrl" joint="right_leg1_joint" kp="500" kv="0.6" />
|
||||
<position name="left_leg2_joint_ctrl" joint="left_leg2_joint" kp="500" kv="0.6" />
|
||||
<position name="right_leg2_joint_ctrl" joint="right_leg2_joint" kp="500" kv="0.6" />
|
||||
|
||||
<!-- 车轮关节 -->
|
||||
<position name="left_wheel1_joint_ctrl" joint="left_wheel1_joint" kp="500" kv="0.6" />
|
||||
<position name="right_wheel1_joint_ctrl" joint="right_wheel1_joint" kp="500" kv="0.6" />
|
||||
<position name="left_wheel2_joint_ctrl" joint="left_wheel2_joint" kp="500" kv="0.6" />
|
||||
<position name="right_wheel2_joint_ctrl" joint="right_wheel2_joint" kp="500" kv="0.6" />
|
||||
|
||||
<!-- 身体关节 -->
|
||||
<position name="pitch_body_joint_ctrl" joint="pitch_body_joint" kp="500" kv="0.6" />
|
||||
|
||||
<!-- 肩部关节 -->
|
||||
<position name="left_shoulder_joint_ctrl" joint="left_shoulder_joint" kp="500" kv="0.6" />
|
||||
<position name="right_shoulder_joint_ctrl" joint="right_shoulder_joint" kp="500" kv="0.6" />
|
||||
|
||||
<!-- 手臂关节 -->
|
||||
<position name="left_arm1_joint_ctrl" joint="left_arm1_joint" kp="500" kv="0.6" />
|
||||
<position name="left_arm2_joint_ctrl" joint="left_arm2_joint" kp="500" kv="0.6" />
|
||||
<position name="right_arm1_joint_ctrl" joint="right_arm1_joint" kp="500" kv="0.6" />
|
||||
<position name="right_arm2_joint_ctrl" joint="right_arm2_joint" kp="500" kv="0.6" />
|
||||
</actuator>
|
||||
|
||||
|
||||
|
||||
<!-- <contact>
|
||||
<exclude body1="base_link" body2="left_leg1_link" />
|
||||
<exclude body1="left_leg1_link" body2="left_wheel1_link" />
|
||||
<exclude body1="base_link" body2="right_leg1_link" />
|
||||
<exclude body1="right_leg1_link" body2="right_wheel1_link" />
|
||||
<exclude body1="base_link" body2="pitch_body_link" />
|
||||
<exclude body1="pitch_body_link" body2="left_shoulder_link" />
|
||||
<exclude body1="left_shoulder_link" body2="left_arm1_link" />
|
||||
<exclude body1="left_arm1_link" body2="left_arm2_link" />
|
||||
<exclude body1="pitch_body_link" body2="right_shoulder_link" />
|
||||
<exclude body1="right_shoulder_link" body2="right_arm1_link" />
|
||||
<exclude body1="right_arm1_link" body2="right_arm2_link" />
|
||||
<exclude body1="base_link" body2="left_leg2_link" />
|
||||
<exclude body1="left_leg2_link" body2="left_wheel2_link" />
|
||||
<exclude body1="base_link" body2="right_leg2_link" />
|
||||
<exclude body1="right_leg2_link" body2="right_wheel2_link" />
|
||||
</contact> -->
|
||||
|
||||
<sensor>
|
||||
<framepos name="base_link_site_pos" objtype="site" objname="base_link_site" />
|
||||
<framequat name="base_link_site_quat" objtype="site" objname="base_link_site" />
|
||||
<framelinvel name="base_link_site_linvel" objtype="site" objname="base_link_site" />
|
||||
<frameangvel name="base_link_site_angvel" objtype="site" objname="base_link_site" />
|
||||
<velocimeter name="base_link_site_vel" site="base_link_site" />
|
||||
</sensor>
|
||||
</mujoco>
|
||||
BIN
HiveCoreR0/src/robot_description/models/meshes/base_link.STL
Normal file
BIN
HiveCoreR0/src/robot_description/models/meshes/base_link.STL
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
917
HiveCoreR0/src/robot_description/models/robot.urdf
Normal file
917
HiveCoreR0/src/robot_description/models/robot.urdf
Normal file
@@ -0,0 +1,917 @@
|
||||
<?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="robot6">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0223121090242412 0.00180410615499794 0.5294578734419628"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="4.6626" />
|
||||
<inertia
|
||||
ixx="0.0345"
|
||||
ixy="0.00001"
|
||||
ixz="-0.00027"
|
||||
iyy="0.00625"
|
||||
iyz="-0.00013"
|
||||
izz="0.03414" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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="package://robot_description/models/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="left_leg1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.036513226420101 -0.100284747490821 -0.000439159326447586"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.687221700874968" />
|
||||
<inertia
|
||||
ixx="0.00984"
|
||||
ixy="-0.00003"
|
||||
ixz="0.0035"
|
||||
iyy="0.01128"
|
||||
iyz="0.00009"
|
||||
izz="0.00149" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/meshes/left_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="package://robot_description/models/meshes/left_leg1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_leg1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.025573 0.143 0.031674"
|
||||
rpy="1.6319 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="left_leg1_link" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_wheel1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="3.63168532302449E-07 2.62193503952624E-06 -0.000769610176954177"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.75" />
|
||||
<inertia
|
||||
ixx="0.00032"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.00042"
|
||||
iyz="0.00001"
|
||||
izz="0.00032" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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://robot_description/models/meshes/left_wheel1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_wheel1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.14246 -0.39141 0.03757"
|
||||
rpy="0 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="0.0364910389102163 -0.100292823100675 0.00043915928386723"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.687221701602252" />
|
||||
<inertia
|
||||
ixx="0.00984"
|
||||
ixy="0.00003"
|
||||
ixz="0.0035"
|
||||
iyy="0.01128"
|
||||
iyz="-0.00009"
|
||||
izz="0.00148" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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="package://robot_description/models/meshes/right_leg1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_leg1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.025573 -0.143 0.031674"
|
||||
rpy="1.5097 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="right_leg1_link" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_wheel1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-2.01863498816568E-06 -1.71217600375417E-06 -0.000769610176954427"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.75" />
|
||||
<inertia
|
||||
ixx="0.00032"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.00042"
|
||||
iyz="-0.00001"
|
||||
izz="0.00032" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/meshes/right_wheel1_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://robot_description/models/meshes/right_wheel1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_wheel1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.142460785057373 -0.391407790102909 -0.0375696405522724"
|
||||
rpy="3.14159265358979 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.000405025729528163 0.164222263553734 0.038212974552542"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="4.31439" />
|
||||
<inertia
|
||||
ixx="0.02564"
|
||||
ixy="-0.00005"
|
||||
ixz="-0.00041"
|
||||
iyy="0.01868"
|
||||
iyz="0.00021"
|
||||
izz="0.01602" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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="package://robot_description/models/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.0139817634960264 -0.000283387663890286 0.0469666821426944"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.12175" />
|
||||
<inertia
|
||||
ixx="0.00099"
|
||||
ixy="0"
|
||||
ixz="-0.00001"
|
||||
iyy="0.00082"
|
||||
iyz="0"
|
||||
izz="0.00084" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/meshes/left_shoulder_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="package://robot_description/models/meshes/left_shoulder_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_shoulder_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0039637 0.22096 -0.0095"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="pitch_body_link" />
|
||||
<child
|
||||
link="left_shoulder_link" />
|
||||
<axis
|
||||
xyz="-0.017935 0 -0.99984" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_arm1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0222764790210273 0.116445846258557 0.130395530944397"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.97181" />
|
||||
<inertia
|
||||
ixx="0.00202"
|
||||
ixy="0.00031"
|
||||
ixz="-0.00001"
|
||||
iyy="0.00075"
|
||||
iyz="0.00003"
|
||||
izz="0.00205" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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://robot_description/models/meshes/left_arm1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_arm1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.040338 -0.00076903 0.046784"
|
||||
rpy="-0.82648 0 0" />
|
||||
<parent
|
||||
link="left_shoulder_link" />
|
||||
<child
|
||||
link="left_arm1_link" />
|
||||
<axis
|
||||
xyz="0.99965 0 -0.026469" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_arm2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00344285141443852 -0.0871900700243705 -0.0947300074211656"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.153345483533995" />
|
||||
<inertia
|
||||
ixx="0.00085"
|
||||
ixy="0.00009"
|
||||
ixz="0.00001"
|
||||
iyy="0.00010"
|
||||
iyz="0"
|
||||
izz="0.00085" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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://robot_description/models/meshes/left_arm2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_arm2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0085407 0.12037 0.13366"
|
||||
rpy="-3.1416 0 0" />
|
||||
<parent
|
||||
link="left_arm1_link" />
|
||||
<child
|
||||
link="left_arm2_link" />
|
||||
<axis
|
||||
xyz="-0.99965 0 -0.026469" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="20"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_shoulder_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0139828606345992 0.000183964382035669 0.0469280544333042"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.12175" />
|
||||
<inertia
|
||||
ixx="0.00099"
|
||||
ixy="0"
|
||||
ixz="-0.00001"
|
||||
iyy="0.00082"
|
||||
iyz="0"
|
||||
izz="0.00084" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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="package://robot_description/models/meshes/right_shoulder_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_shoulder_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0039637 0.22096 0.0865"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="pitch_body_link" />
|
||||
<child
|
||||
link="right_shoulder_link" />
|
||||
<axis
|
||||
xyz="-0.017935 0 -0.99984" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_arm1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0237185143086008 -0.146707321130596 0.0947584340472521"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.97181" />
|
||||
<inertia
|
||||
ixx="0.00201"
|
||||
ixy="-0.00032"
|
||||
ixz="0"
|
||||
iyy="0.00076"
|
||||
iyz="0.00002"
|
||||
izz="0.00205" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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://robot_description/models/meshes/right_arm1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_arm1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.040343 0.000445 0.046784"
|
||||
rpy="0.56089 0 0" />
|
||||
<parent
|
||||
link="right_shoulder_link" />
|
||||
<child
|
||||
link="right_arm1_link" />
|
||||
<axis
|
||||
xyz="0.99978 0 -0.021179" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_arm2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0044670114805945 0.108002437145137 -0.0700232476387662"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.153345483355902" />
|
||||
<inertia
|
||||
ixx="0.00085"
|
||||
ixy="0.00009"
|
||||
ixz="0"
|
||||
iyy="0.00011"
|
||||
iyz="0.00001"
|
||||
izz="0.00085" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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://robot_description/models/meshes/right_arm2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_arm2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.00696183199978448 -0.151367984040579 0.0972793725016652"
|
||||
rpy="-3.14159265358976 0 0" />
|
||||
<parent
|
||||
link="right_arm1_link" />
|
||||
<child
|
||||
link="right_arm2_link" />
|
||||
<axis
|
||||
xyz="-0.99977570211955 0 -0.0211788916461861" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="20"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_leg2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0333228055348214 -0.0915849336519395 0.000701841034553452"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.758444949404803" />
|
||||
<inertia
|
||||
ixx="0.01040"
|
||||
ixy="0.00006"
|
||||
ixz="-0.00369"
|
||||
iyy="0.01187"
|
||||
iyz="0.00017"
|
||||
izz="0.00158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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="package://robot_description/models/meshes/left_leg2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_leg2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.090467 0.14336 0.031579"
|
||||
rpy="1.6319 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="left_leg2_link" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_wheel2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.94616062534902E-06 1.79412737411067E-06 -0.000769610176954219"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.75" />
|
||||
<inertia
|
||||
ixx="0.00032"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.00042"
|
||||
iyz="0.00001"
|
||||
izz="0.00032" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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://robot_description/models/meshes/left_wheel2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_wheel2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.14242 -0.39129 0.037923"
|
||||
rpy="0 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.0333429112408204 -0.0914496518628595 0.00489014788422593"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.758444940033566" />
|
||||
<inertia
|
||||
ixx="0.01039"
|
||||
ixy="-0.00006"
|
||||
ixz="-0.0037"
|
||||
iyy="0.01187"
|
||||
iyz="-0.000173"
|
||||
izz="0.00158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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="package://robot_description/models/meshes/right_leg2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_leg2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.090467 -0.14336 0.031579"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="right_leg2_link" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_wheel2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-2.61860790495838E-06 -3.86427901677422E-07 -0.000769610176954372"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.75" />
|
||||
<inertia
|
||||
ixx="0.00032"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.00042"
|
||||
iyz="-0.00001"
|
||||
izz="0.00032" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://robot_description/models/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://robot_description/models/meshes/right_wheel2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_wheel2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.14242 -0.39288 -0.013965"
|
||||
rpy="3.0805 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>
|
||||
@@ -3,30 +3,22 @@
|
||||
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>
|
||||
|
||||
name="robot6">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0223121090242412 0.00180410615499793 0.0294578734419628"
|
||||
xyz="-0.0223121090242412 0.00180410615499794 0.5294578734419628"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="8.01740117497269" />
|
||||
value="4.6626" />
|
||||
<inertia
|
||||
ixx="0.0100767591002942"
|
||||
ixy="-8.33871415118017E-06"
|
||||
ixz="-3.04794782095214E-06"
|
||||
iyy="0.00687778146550088"
|
||||
iyz="2.70300324348596E-06"
|
||||
izz="0.0095089226755309" />
|
||||
ixx="0.0345"
|
||||
ixy="0.00001"
|
||||
ixz="-0.00027"
|
||||
iyy="0.00625"
|
||||
iyz="-0.00013"
|
||||
izz="0.03414" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -34,7 +26,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/base_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -48,7 +40,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/base_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -56,17 +48,17 @@
|
||||
name="left_leg1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0026919722577641 -0.106691141065344 0.000439159272074496"
|
||||
xyz="0.036513226420101 -0.100284747490821 -0.000439159326447586"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.687221704005751" />
|
||||
value="0.687221700874968" />
|
||||
<inertia
|
||||
ixx="0.00309344360201386"
|
||||
ixy="7.22427359994752E-05"
|
||||
ixz="-1.7837563113331E-07"
|
||||
iyy="0.000199758758340417"
|
||||
iyz="5.25184127616714E-06"
|
||||
izz="0.00327480626360329" />
|
||||
ixx="0.00984"
|
||||
ixy="-0.00003"
|
||||
ixz="0.0035"
|
||||
iyy="0.01128"
|
||||
iyz="0.00009"
|
||||
izz="0.00149" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -74,12 +66,12 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_leg1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_leg1_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
|
||||
rgba="0.686274509803922 0.686274509803922 0.686274509803922 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
@@ -88,7 +80,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_leg1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_leg1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -96,8 +88,8 @@
|
||||
name="left_leg1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.023189 -0.14297 0.032124"
|
||||
rpy="1.5097 0 0" />
|
||||
xyz="-0.025573 0.143 0.031674"
|
||||
rpy="1.6319 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
@@ -105,7 +97,7 @@
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="0"
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
@@ -114,17 +106,17 @@
|
||||
name="left_wheel1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-2.01863499148247E-06 -1.71217600630769E-06 -0.000769610134319532"
|
||||
xyz="3.63168532302449E-07 2.62193503952624E-06 -0.000769610176954177"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.243841354713671" />
|
||||
value="0.75" />
|
||||
<inertia
|
||||
ixx="0.000147760027881631"
|
||||
ixy="2.12254130748857E-09"
|
||||
ixz="-1.63113803254759E-08"
|
||||
iyy="0.000147759325738658"
|
||||
iyz="-1.38350688180478E-08"
|
||||
izz="0.000229476928281019" />
|
||||
ixx="0.00032"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.00042"
|
||||
iyz="0.00001"
|
||||
izz="0.00032" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -132,7 +124,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_wheel1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_wheel1_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -146,7 +138,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_wheel1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_wheel1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -154,8 +146,8 @@
|
||||
name="left_wheel1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.010552 -0.41639 -0.03757"
|
||||
rpy="3.1416 0 0" />
|
||||
xyz="0.14246 -0.39141 0.03757"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="left_leg1_link" />
|
||||
<child
|
||||
@@ -172,17 +164,17 @@
|
||||
name="right_leg1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-3.01562743165443E-05 -0.106725092599043 -0.000439159317208282"
|
||||
xyz="0.0364910389102163 -0.100292823100675 0.00043915928386723"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.687221703818328" />
|
||||
value="0.687221701602252" />
|
||||
<inertia
|
||||
ixx="0.00309524606532856"
|
||||
ixy="-4.05589498874716E-09"
|
||||
ixz="-4.73342140491208E-08"
|
||||
iyy="0.000197956293734699"
|
||||
iyz="-5.25465636656606E-06"
|
||||
izz="0.00327480626231139" />
|
||||
ixx="0.00984"
|
||||
ixy="0.00003"
|
||||
ixz="0.0035"
|
||||
iyy="0.01128"
|
||||
iyz="-0.00009"
|
||||
izz="0.00148" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -190,7 +182,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_leg1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_leg1_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -204,7 +196,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_leg1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_leg1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -212,8 +204,8 @@
|
||||
name="right_leg1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.022996 0.14297 0.032127"
|
||||
rpy="1.6319 0 0" />
|
||||
xyz="-0.025573 -0.143 0.031674"
|
||||
rpy="1.5097 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
@@ -221,7 +213,7 @@
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="0"
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
@@ -230,17 +222,17 @@
|
||||
name="right_wheel1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.92448237847231E-06 1.8173611248673E-06 -0.000769610176954372"
|
||||
xyz="-2.01863498816568E-06 -1.71217600375417E-06 -0.000769610176954427"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.243841354713671" />
|
||||
value="0.75" />
|
||||
<inertia
|
||||
ixx="0.000147759799888368"
|
||||
ixy="2.14785580095997E-09"
|
||||
ixz="1.55505894769142E-08"
|
||||
iyy="0.000147759553731921"
|
||||
iyz="1.46850067839037E-08"
|
||||
izz="0.000229476928281019" />
|
||||
ixx="0.00032"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.00042"
|
||||
iyz="-0.00001"
|
||||
izz="0.00032" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -248,12 +240,12 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_wheel1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_wheel1_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
@@ -262,7 +254,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_wheel1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_wheel1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -270,8 +262,8 @@
|
||||
name="right_wheel1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.00016377 -0.41653 0.03757"
|
||||
rpy="0 0 0" />
|
||||
xyz="0.142460785057373 -0.391407790102909 -0.0375696405522724"
|
||||
rpy="3.14159265358979 0 0" />
|
||||
<parent
|
||||
link="right_leg1_link" />
|
||||
<child
|
||||
@@ -288,17 +280,17 @@
|
||||
name="pitch_body_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000405025729528165 0.164222263553734 0.038212974552542"
|
||||
xyz="0.000405025729528163 0.164222263553734 0.038212974552542"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="6.27735184123949" />
|
||||
value="4.31439" />
|
||||
<inertia
|
||||
ixx="0.012112631055994"
|
||||
ixy="0.000262225328016326"
|
||||
ixz="-4.41212676410044E-06"
|
||||
iyy="0.00923713569100348"
|
||||
iyz="-6.27634886947933E-05"
|
||||
izz="0.00865286317711146" />
|
||||
ixx="0.02564"
|
||||
ixy="-0.00005"
|
||||
ixz="-0.00041"
|
||||
iyy="0.01868"
|
||||
iyz="0.00021"
|
||||
izz="0.01602" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -306,7 +298,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/pitch_body_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/pitch_body_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -320,7 +312,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/pitch_body_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/pitch_body_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -346,17 +338,17 @@
|
||||
name="left_shoulder_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0139828606345992 0.000183964382035662 0.0469280544333041"
|
||||
xyz="0.0139817634960264 -0.000283387663890286 0.0469666821426944"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="2.24001373537068" />
|
||||
value="1.12175" />
|
||||
<inertia
|
||||
ixx="0.00187573272868104"
|
||||
ixy="5.61569065704759E-06"
|
||||
ixz="-1.58654763334498E-05"
|
||||
iyy="0.00158854268624343"
|
||||
iyz="8.8447275683334E-07"
|
||||
izz="0.00161229514279373" />
|
||||
ixx="0.00099"
|
||||
ixy="0"
|
||||
ixz="-0.00001"
|
||||
iyy="0.00082"
|
||||
iyz="0"
|
||||
izz="0.00084" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -364,12 +356,12 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_shoulder_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_shoulder_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.898039215686275 0.92156862745098 0.929411764705882 1" />
|
||||
rgba="0.501960784313725 0.501960784313725 0.501960784313725 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
@@ -378,7 +370,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_shoulder_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_shoulder_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -386,14 +378,14 @@
|
||||
name="left_shoulder_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.00396365185327967 0.220964452984708 0.0865"
|
||||
rpy="-1.5707963267949 0 0" />
|
||||
xyz="0.0039637 0.22096 -0.0095"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="pitch_body_link" />
|
||||
<child
|
||||
link="left_shoulder_link" />
|
||||
<axis
|
||||
xyz="-0.0179350762557234 0 -0.999839153584066" />
|
||||
xyz="-0.017935 0 -0.99984" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
@@ -404,17 +396,17 @@
|
||||
name="left_arm1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0237185143086008 -0.146707321130601 0.0947584340472464"
|
||||
xyz="-0.0222764790210273 0.116445846258557 0.130395530944397"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.97210272038181" />
|
||||
value="0.97181" />
|
||||
<inertia
|
||||
ixx="0.0020453207276327"
|
||||
ixy="-7.93710872896332E-05"
|
||||
ixz="3.82437020118978E-05"
|
||||
iyy="0.00151626513015615"
|
||||
iyz="0.000308320536526838"
|
||||
izz="0.00183373717106277" />
|
||||
ixx="0.00202"
|
||||
ixy="0.00031"
|
||||
ixz="-0.00001"
|
||||
iyy="0.00075"
|
||||
iyz="0.00003"
|
||||
izz="0.00205" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -422,7 +414,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_arm1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_arm1_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -436,7 +428,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_arm1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_arm1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -444,14 +436,14 @@
|
||||
name="left_arm1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0403430563984948 0.00044500097133069 0.0467839692408788"
|
||||
rpy="0.560887033481642 0 0" />
|
||||
xyz="0.040338 -0.00076903 0.046784"
|
||||
rpy="-0.82648 0 0" />
|
||||
<parent
|
||||
link="left_shoulder_link" />
|
||||
<child
|
||||
link="left_arm1_link" />
|
||||
<axis
|
||||
xyz="0.99977570211955 0 -0.0211788916461878" />
|
||||
xyz="0.99965 0 -0.026469" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
@@ -462,17 +454,17 @@
|
||||
name="left_arm2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0044670114805947 0.108002437145143 -0.0700232476387601"
|
||||
xyz="-0.00344285141443852 -0.0871900700243705 -0.0947300074211656"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.153345483355902" />
|
||||
value="0.153345483533995" />
|
||||
<inertia
|
||||
ixx="0.000846185788600758"
|
||||
ixy="8.22351279275958E-05"
|
||||
ixz="-4.72026465483579E-05"
|
||||
iyy="0.000325958142834647"
|
||||
iyz="0.000341442751456307"
|
||||
izz="0.000633417429555725" />
|
||||
ixx="0.00085"
|
||||
ixy="0.00009"
|
||||
ixz="0.00001"
|
||||
iyy="0.00010"
|
||||
iyz="0"
|
||||
izz="0.00085" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -480,7 +472,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_arm2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_arm2_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -494,7 +486,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_arm2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_arm2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -502,14 +494,14 @@
|
||||
name="left_arm2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.00696183199978448 -0.151367984040579 0.0972793725016652"
|
||||
rpy="-3.14159265358976 0 0" />
|
||||
xyz="0.0085407 0.12037 0.13366"
|
||||
rpy="-3.1416 0 0" />
|
||||
<parent
|
||||
link="left_arm1_link" />
|
||||
<child
|
||||
link="left_arm2_link" />
|
||||
<axis
|
||||
xyz="-0.99977570211955 0 -0.0211788916461861" />
|
||||
xyz="-0.99965 0 -0.026469" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
@@ -520,17 +512,17 @@
|
||||
name="right_shoulder_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0139817634960264 -0.000283387663890286 0.0469666821426944"
|
||||
xyz="0.0139828606345992 0.000183964382035669 0.0469280544333042"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="2.24001360218308" />
|
||||
value="1.12175" />
|
||||
<inertia
|
||||
ixx="0.00187572320791288"
|
||||
ixy="-7.22979277002269E-06"
|
||||
ixz="-1.38305404469916E-05"
|
||||
iyy="0.00158896107508856"
|
||||
iyz="-4.80454269915803E-07"
|
||||
izz="0.00161188619803665" />
|
||||
ixx="0.00099"
|
||||
ixy="0"
|
||||
ixz="-0.00001"
|
||||
iyy="0.00082"
|
||||
iyz="0"
|
||||
izz="0.00084" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -538,7 +530,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_shoulder_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_shoulder_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -552,7 +544,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_shoulder_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_shoulder_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -560,14 +552,14 @@
|
||||
name="right_shoulder_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.00396365185327941 0.220964452984708 -0.00950000000000008"
|
||||
rpy="-1.5707963267949 0 0" />
|
||||
xyz="0.0039637 0.22096 0.0865"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="pitch_body_link" />
|
||||
<child
|
||||
link="right_shoulder_link" />
|
||||
<axis
|
||||
xyz="-0.0179350762557234 0 -0.999839153584066" />
|
||||
xyz="-0.017935 0 -0.99984" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
@@ -578,17 +570,17 @@
|
||||
name="right_arm1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0222764790210273 0.116445846258557 0.130395530944397"
|
||||
xyz="-0.0237185143086008 -0.146707321130596 0.0947584340472521"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.97210271472831" />
|
||||
value="0.97181" />
|
||||
<inertia
|
||||
ixx="0.00204747347595548"
|
||||
ixy="4.42933377299481E-05"
|
||||
ixz="6.73328370107109E-05"
|
||||
iyy="0.00173434064125734"
|
||||
iyz="-0.000339327591519728"
|
||||
izz="0.00161350890019016" />
|
||||
ixx="0.00201"
|
||||
ixy="-0.00032"
|
||||
ixz="0"
|
||||
iyy="0.00076"
|
||||
iyz="0.00002"
|
||||
izz="0.00205" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -596,7 +588,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_arm1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_arm1_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -610,7 +602,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_arm1_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_arm1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -618,14 +610,14 @@
|
||||
name="right_arm1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.040338077059205 -0.000769030026202884 0.046784058560075"
|
||||
rpy="-0.826482123984997 0 0" />
|
||||
xyz="0.040343 0.000445 0.046784"
|
||||
rpy="0.56089 0 0" />
|
||||
<parent
|
||||
link="right_shoulder_link" />
|
||||
<child
|
||||
link="right_arm1_link" />
|
||||
<axis
|
||||
xyz="0.999649642897155 0 -0.0264686882106157" />
|
||||
xyz="0.99978 0 -0.021179" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
@@ -636,17 +628,17 @@
|
||||
name="right_arm2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00344285141443855 -0.0871900700243705 -0.0947300074211656"
|
||||
xyz="-0.0044670114805945 0.108002437145137 -0.0700232476387662"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.153345483533995" />
|
||||
value="0.153345483355902" />
|
||||
<inertia
|
||||
ixx="0.000848016249761213"
|
||||
ixy="-5.62001253862557E-05"
|
||||
ixz="-6.88219353067134E-05"
|
||||
iyy="0.000513290254168688"
|
||||
iyz="-0.000373425902214173"
|
||||
izz="0.00044425485583754" />
|
||||
ixx="0.00085"
|
||||
ixy="0.00009"
|
||||
ixz="0"
|
||||
iyy="0.00011"
|
||||
iyz="0.00001"
|
||||
izz="0.00085" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -654,7 +646,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_arm2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_arm2_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -668,7 +660,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_arm2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_arm2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -676,14 +668,14 @@
|
||||
name="right_arm2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.00854070115695219 0.120366284301785 0.133656328091823"
|
||||
rpy="-3.14159265358977 0 0" />
|
||||
xyz="0.00696183199978448 -0.151367984040579 0.0972793725016652"
|
||||
rpy="-3.14159265358976 0 0" />
|
||||
<parent
|
||||
link="right_arm1_link" />
|
||||
<child
|
||||
link="right_arm2_link" />
|
||||
<axis
|
||||
xyz="-0.999649642897155 0 -0.0264686882106164" />
|
||||
xyz="-0.99977570211955 0 -0.0211788916461861" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
@@ -694,17 +686,17 @@
|
||||
name="left_leg2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00247971563497956 -0.0972883314998296 0.00524725682640842"
|
||||
xyz="-0.0333228055348214 -0.0915849336519395 0.000701841034553452"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.758444942474565" />
|
||||
value="0.758444949404803" />
|
||||
<inertia
|
||||
ixx="0.00308402598971299"
|
||||
ixy="-7.39501679752198E-05"
|
||||
ixz="4.61546712808476E-06"
|
||||
iyy="0.00021886670519613"
|
||||
iyz="0.000190799531555152"
|
||||
izz="0.00323764016118335" />
|
||||
ixx="0.01040"
|
||||
ixy="0.00006"
|
||||
ixz="-0.00369"
|
||||
iyy="0.01187"
|
||||
iyz="0.00017"
|
||||
izz="0.00158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -712,7 +704,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_leg2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_leg2_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -726,7 +718,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_leg2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_leg2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -734,8 +726,8 @@
|
||||
name="left_leg2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.092811 -0.14333 0.032022"
|
||||
rpy="1.5708 0 0" />
|
||||
xyz="-0.090467 0.14336 0.031579"
|
||||
rpy="1.6319 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
@@ -743,7 +735,7 @@
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="0"
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
@@ -752,17 +744,17 @@
|
||||
name="left_wheel2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-2.63258476833339E-06 -2.75556383766418E-07 -0.000769610176954399"
|
||||
xyz="1.94616062534902E-06 1.79412737411067E-06 -0.000769610176954219"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.243841354713671" />
|
||||
value="0.75" />
|
||||
<inertia
|
||||
ixx="0.000147761781558832"
|
||||
ixy="4.45494954114067E-10"
|
||||
ixz="-2.127234078802E-08"
|
||||
iyy="0.000147757572061457"
|
||||
iyz="-2.226606099262E-09"
|
||||
izz="0.000229476928281019" />
|
||||
ixx="0.00032"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.00042"
|
||||
iyz="0.00001"
|
||||
izz="0.00032" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -770,7 +762,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_wheel2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_wheel2_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -784,7 +776,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/left_wheel2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/left_wheel2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -792,8 +784,8 @@
|
||||
name="left_wheel2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.010549 -0.41781 -0.01244"
|
||||
rpy="3.0805 0 0" />
|
||||
xyz="-0.14242 -0.39129 0.037923"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="left_leg2_link" />
|
||||
<child
|
||||
@@ -810,17 +802,17 @@
|
||||
name="right_leg2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0050516474502517 -0.0973277461307459 0.000701841039130763"
|
||||
xyz="-0.0333429112408204 -0.0914496518628595 0.00489014788422593"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.758444958401021" />
|
||||
value="0.758444940033566" />
|
||||
<inertia
|
||||
ixx="0.00307823597256189"
|
||||
ixy="-0.000148312070292207"
|
||||
ixz="-3.2725667530637E-07"
|
||||
iyy="0.000212654851830719"
|
||||
iyz="-5.42028503051418E-06"
|
||||
izz="0.00324964203847903" />
|
||||
ixx="0.01039"
|
||||
ixy="-0.00006"
|
||||
ixz="-0.0037"
|
||||
iyy="0.01187"
|
||||
iyz="-0.000173"
|
||||
izz="0.00158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -828,7 +820,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_leg2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_leg2_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -842,7 +834,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_leg2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_leg2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -850,8 +842,8 @@
|
||||
name="right_leg2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.092614 0.14333 0.032015"
|
||||
rpy="1.6319 0 0" />
|
||||
xyz="-0.090467 -0.14336 0.031579"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
@@ -859,7 +851,7 @@
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="0"
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="60"
|
||||
velocity="50" />
|
||||
@@ -868,17 +860,17 @@
|
||||
name="right_wheel2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.93300484906123E-06 1.80829370854951E-06 -0.000769610176954413"
|
||||
xyz="-2.61860790495838E-06 -3.86427901677422E-07 -0.000769610176954372"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.243841354713671" />
|
||||
value="0.75" />
|
||||
<inertia
|
||||
ixx="0.000147759820077608"
|
||||
ixy="2.14660364874833E-09"
|
||||
ixz="1.56194544574533E-08"
|
||||
iyy="0.000147759533542682"
|
||||
iyz="1.46117384240508E-08"
|
||||
izz="0.000229476928281019" />
|
||||
ixx="0.00032"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.00042"
|
||||
iyz="-0.00001"
|
||||
izz="0.00032" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
@@ -886,7 +878,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_wheel2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_wheel2_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
@@ -900,7 +892,7 @@
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot5/urdf/meshes/right_wheel2_link.STL" />
|
||||
filename="/home/hivecore/ros2_ws/softwaresystem/HiveCoreR0/src/robot_description/urdf/meshes/right_wheel2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@@ -908,8 +900,8 @@
|
||||
name="right_wheel2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.021629 -0.41584 0.037923"
|
||||
rpy="0 0 0" />
|
||||
xyz="-0.14242 -0.39288 -0.013965"
|
||||
rpy="3.0805 0 0" />
|
||||
<parent
|
||||
link="right_leg2_link" />
|
||||
<child
|
||||
@@ -8,8 +8,11 @@
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>xacro</buildtool_depend>
|
||||
|
||||
<depend>urdf</depend>
|
||||
<depend>xacro</depend>
|
||||
<depend>joint_state_publisher_gui</depend>
|
||||
<depend>rviz2</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
|
||||
@@ -1,16 +0,0 @@
|
||||
<?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>
|
||||
@@ -1,71 +0,0 @@
|
||||
<?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>
|
||||
@@ -1,523 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot name="hive_core_bot0" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<!-- 引入XACRO命名空间 -->
|
||||
<xacro:property name="meshes_path" value="file://$(find robot_description)/meshes" />
|
||||
|
||||
<xacro:include filename="$(find robot_description)/urdf/generic_link.xacro" />
|
||||
<xacro:include filename="$(find robot_description)/urdf/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 robot_description)/urdf/robot_ros2_control.xacro" />
|
||||
<xacro:robot_ros2_control />
|
||||
|
||||
</robot>
|
||||
@@ -1,118 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<!-- 宏定义：ros2_control硬件接口配置（适配位置控制器和轮子速度控制器） -->
|
||||
<xacro:macro name="robot_ros2_control">
|
||||
<!-- 1. ros2_control系统配置：定义所有关节的控制接口 -->
|
||||
<ros2_control name="RobotGazeboSystem" 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 robot_description)/config/robot_ros2_controller.yaml</parameters>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
|
||||
@@ -1,16 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
project(robot_simulation)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# 依赖
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
# 安装启动文件和世界文件
|
||||
install(DIRECTORY launch world
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -1,202 +0,0 @@
|
||||
|
||||
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.
|
||||
@@ -1,111 +0,0 @@
|
||||
import os
|
||||
import launch
|
||||
import launch_ros
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import ExecuteProcess
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch.actions import TimerAction
|
||||
|
||||
def generate_launch_description():
|
||||
# 声明参数
|
||||
robot_name_in_model = "hive_core_bot0"
|
||||
robot_description_package = 'robot_description'
|
||||
robot_description_path = get_package_share_directory(robot_description_package)
|
||||
robot_model_path = os.path.join(robot_description_path,'urdf','robot.urdf.xacro')
|
||||
|
||||
robot_simulation_package = 'robot_simulation'
|
||||
robot_simulation_path = get_package_share_directory(robot_simulation_package)
|
||||
default_world_path = os.path.join(robot_simulation_path, 'world', 'empty.world')
|
||||
|
||||
# 声明模型路径参数
|
||||
action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
|
||||
name='model', default_value=str(robot_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(robot_description_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',
|
||||
'robot_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',
|
||||
'robot_position_controller'],
|
||||
output='screen'
|
||||
)]
|
||||
)
|
||||
|
||||
# 在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,
|
||||
])
|
||||
@@ -1,15 +0,0 @@
|
||||
<?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>robot_simulation</name>
|
||||
<version>1.0.0</version>
|
||||
<description>Robot simulation package</description>
|
||||
<maintainer email="Ray@email.com">Your Name</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,116 +0,0 @@
|
||||
<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>
|
||||
@@ -1,11 +0,0 @@
|
||||
<?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>
|
||||
@@ -1,297 +0,0 @@
|
||||
<?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>
|
||||
Reference in New Issue
Block a user