ar4/ar4_description/urdf/include/common_macros.urdf.xacro at 82bf720b1a73db3d18e9c1c6acd09dbd5604e003 · Ekumen-OS/ar4 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- ===================== Arm link xacro ================================== -->

<xacro:include filename="$(find ${package_name})/urdf/include/materials.xacro" />
<xacro:macro name="link" params="link_name *origin inertial visual collision meshes:=^ materials:=^">
  <link name="${link_name}">
    <xacro:insert_block name="origin"/>

    <xacro:box_inertia m="${inertial['mass']}"
                       x="${inertial['size']['dx']}"
                       y="${inertial['size']['dy']}"
                       z="${inertial['size']['dz']}"
                       o_xyz="${inertial['origin']['dx']} ${inertial['origin']['dy']} ${inertial['origin']['dz']}"
                       o_rpy="${inertial['origin']['dr']} ${inertial['origin']['dp']} ${inertial['origin']['dY']}">
    </xacro:box_inertia>

    <xacro:mesh_loop meshes="${meshes}" materials="${materials}" visual="${visual}" collision="${collision}" />
  </link>
</xacro:macro>

<!-- Recursive macro to iterate over meshes -->
<xacro:macro name="mesh_loop" params="meshes:=^ materials:=^ visual collision">
  <xacro:if value="${meshes}">
    <xacro:property name="mesh" value="${meshes.pop(0)}"/>
    <xacro:property name="material" value="${materials.pop(0)}"/>

    <visual>
      <origin rpy="${visual['origin']['dr']} ${visual['origin']['dp']} ${visual['origin']['dY']}"
              xyz="${visual['origin']['dx']} ${visual['origin']['dy']} ${visual['origin']['dz']}"/>
      <geometry>
         <mesh filename="file://$(find ar4_description)/meshes/${mesh}.stl"/>
      </geometry>
      <material name="${material}"/>
    </visual>

    <collision>
      <origin rpy="${collision['origin']['dr']} ${collision['origin']['dp']} ${collision['origin']['dY']}"
              xyz="${collision['origin']['dx']} ${collision['origin']['dy']} ${collision['origin']['dz']}"/>
      <geometry>
        <mesh filename="file://$(find ar4_description)/meshes/${mesh}.stl"/>
      </geometry>
    </collision>

    <!-- Recursive call to process the remaining list -->
    <xacro:mesh_loop meshes="${meshes}" materials="${materials}" visual="${visual}" collision="${collision}" />
  </xacro:if>
</xacro:macro>


<!-- ===================== Box inertia xacro ================================== -->
<!-- params -->
<!-- - m [float] link mass; -->
<!-- - x [float] link dimension on the X-axis; -->
<!-- - y [float] link dimension on the Y-axis; -->
<!-- - z [float] link dimension on the Z-axis; -->
<!-- - o_xyz [string] position offset -->
<!-- - o_rpy [string] rotation offset -->
<!-- ========================================================================== -->

 <xacro:macro name="box_inertia" params="m x y z o_xyz:='0.0 0.0 0.0' o_rpy:='0.0 0.0 0.0'">
    <inertial>
      <mass value="${m}"/>
      <inertia ixx="${m / 12.0 * (y*y + z*z)}" ixy="0.0" ixz="0.0"
               iyy="${m / 12.0 * (x*x + z*z)}" iyz="0.0"
               izz="${m / 12.0 * (x*x + y*y)}"/>
      <origin xyz="${o_xyz}" rpy="${o_rpy}" />
    </inertial>
  </xacro:macro>

</robot>