106 lines
3.6 KiB
XML
106 lines
3.6 KiB
XML
<?xml version="1.0"?>
|
||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="block">
|
||
<xacro:include filename="$(find gazebo_world_manager)/scene/grasp_box/models/apriltag/urdf/apriltag.xacro"/>
|
||
|
||
<!-- ===== 参数定义 ===== -->
|
||
<xacro:arg name="box_length" default="0.5"/> <!-- x -->
|
||
<xacro:arg name="box_width" default="0.3"/> <!-- y -->
|
||
<xacro:arg name="box_height" default="0.2"/> <!-- z -->
|
||
<xacro:arg name="box_mass" default="1.0"/>
|
||
<xacro:arg name="tag_id" default="0"/>
|
||
|
||
<!-- ===== 派生参数计算 ===== -->
|
||
<!-- 从参数创建属性 -->
|
||
<xacro:property name="box_length" value="$(arg box_length)"/>
|
||
<xacro:property name="box_width" value="$(arg box_width)"/>
|
||
<xacro:property name="box_height" value="$(arg box_height)"/>
|
||
<xacro:property name="box_mass" value="$(arg box_mass)"/>
|
||
|
||
<!-- 耳朵长度：箱子宽度 - 0.05 -->
|
||
<xacro:property name="ear_length" value="${box_width - 0.05}"/>
|
||
<!-- 耳朵偏移：箱子长度的一半 + 0.025 -->
|
||
<xacro:property name="ear_offset" value="${box_length/2 + 0.025}"/>
|
||
<xacro:property name="ear_width" value="0.1"/>
|
||
<xacro:property name="ear_thickness" value="0.025"/>
|
||
|
||
<!-- 惯性矩阵计算 -->
|
||
<xacro:property name="ixx" value="${(1.0/12.0) * box_mass * (box_height*box_height + box_width*box_width)}"/>
|
||
<xacro:property name="iyy" value="${(1.0/12.0) * box_mass * (box_length*box_length + box_height*box_height)}"/>
|
||
<xacro:property name="izz" value="${(1.0/12.0) * box_mass * (box_length*box_length + box_width*box_width)}"/>
|
||
|
||
<!-- ===== 耳朵宏定义 ===== -->
|
||
<!-- 参数：
|
||
side_offset：左右位置（正值右耳，负值左耳）
|
||
-->
|
||
<xacro:macro name="ear" params="side_offset">
|
||
<visual>
|
||
<geometry>
|
||
<box size="${ear_width} ${ear_length} ${ear_thickness}"/>
|
||
</geometry>
|
||
<origin xyz="${side_offset} 0.0 ${box_height/2 - ear_thickness}" rpy="0 0 0"/>
|
||
<material name="red"/>
|
||
</visual>
|
||
<collision>
|
||
<geometry>
|
||
<box size="${ear_width} ${ear_length} ${ear_thickness}"/>
|
||
</geometry>
|
||
<origin xyz="${side_offset} 0.0 ${box_height/2 - ear_thickness}" rpy="0 0 0"/>
|
||
<surface>
|
||
<contact>
|
||
<max_contacts>10</max_contacts>
|
||
<ode>
|
||
<kp>1e6</kp>
|
||
<kd>100</kd>
|
||
</ode>
|
||
</contact>
|
||
</surface>
|
||
</collision>
|
||
</xacro:macro>
|
||
|
||
<!-- 主体 -->
|
||
<link name="base_link">
|
||
<!-- 主体视觉 -->
|
||
<visual>
|
||
<geometry>
|
||
<box size="${box_length} ${box_width} ${box_height}"/>
|
||
</geometry>
|
||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||
<material name="red">
|
||
<color rgba="1 0 0 1"/>
|
||
</material>
|
||
</visual>
|
||
<!-- 主体碰撞 -->
|
||
<collision>
|
||
<geometry>
|
||
<box size="${box_length} ${box_width} ${box_height}"/>
|
||
</geometry>
|
||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||
<surface>
|
||
<contact>
|
||
<max_contacts>10</max_contacts>
|
||
<ode>
|
||
<kp>1e6</kp>
|
||
<kd>100</kd>
|
||
</ode>
|
||
</contact>
|
||
</surface>
|
||
</collision>
|
||
|
||
<!-- 左耳（调用宏） -->
|
||
<xacro:ear side_offset="-${ear_offset}"/>
|
||
<!-- 右耳（调用宏） -->
|
||
<xacro:ear side_offset="${ear_offset}"/>
|
||
|
||
<!-- 质量惯性（自动计算） -->
|
||
<inertial>
|
||
<mass value="${box_mass}"/>
|
||
<inertia ixx="${ixx}" iyy="${iyy}" izz="${izz}" ixy="0" iyz="0" ixz="0"/>
|
||
</inertial>
|
||
</link>
|
||
|
||
<xacro:apriltag_marker parent_link="base_link" id="$(arg tag_id)">
|
||
<origin xyz="0 -${box_width/2 + 0.001} 0" rpy="-${pi/2} 0 0"/>
|
||
</xacro:apriltag_marker>
|
||
|
||
</robot>
|