111 lines
5.4 KiB
XML
111 lines
5.4 KiB
XML
<?xml version="1.0"?>
|
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
|
|
|
<!-- ============================================================== -->
|
|
<!-- Macro to add an <inertial> tag based on yaml-load properties -->
|
|
<!-- -->
|
|
<!-- name: Name of the end_effector (without prefix) -->
|
|
<!-- ee_inertials: Dictionary of inertia properties (see inertial.yaml) -->
|
|
<!-- ============================================================== -->
|
|
<xacro:macro name="ee-inertials" params="name ee_inertials:=^">
|
|
<xacro:unless value="${name in ee_inertials}">${xacro.warning('No inertia properties defined for: ' + name)}</xacro:unless>
|
|
<xacro:if value="${name in ee_inertials}">
|
|
<!-- Access inertia properties of link 'name' -->
|
|
<xacro:property name="link_inertials" value="${ee_inertials[name]}" lazy_eval="false" />
|
|
<inertial>
|
|
<origin rpy="${link_inertials.origin.rpy}" xyz="${link_inertials.origin.xyz}" />
|
|
<mass value="${link_inertials.mass}" />
|
|
<xacro:if value="${'inertia' in link_inertials}">
|
|
<xacro:property name="I" value="${link_inertials.inertia}" />
|
|
<inertia ixx="${I.xx}" ixy="${I.xy}" ixz="${I.xz}" iyy="${I.yy}" iyz="${I.yz}" izz="${I.zz}" />
|
|
</xacro:if>
|
|
</inertial>
|
|
</xacro:if>
|
|
</xacro:macro>
|
|
|
|
<!-- ========================================================== -->
|
|
<!-- Macro to add a single link, both with -->
|
|
<!-- * detailed meshes for environmental collision checking -->
|
|
<!-- * and coarse geometry models for self-collision checking -->
|
|
<!-- (only if 'gazebo' param is set) -->
|
|
<!-- -->
|
|
<!-- name: Name of the end-effector (without prefix) -->
|
|
<!-- rpy: Rotation of the *_sc link relative to parent [rad] -->
|
|
<!-- self_collision_geometries: self <collision> models -->
|
|
<!-- ========================================================== -->
|
|
<xacro:macro name="ee_link_with_sc" params="name prefix=${ee_prefix} rpy:='0 0 0' **self_collision_geometries gazebo:=false with_sc:=false">
|
|
<!-- sub-link defining detailed meshes for collision with environment -->
|
|
<link name="${prefix}${name}">
|
|
<visual name="${prefix}${name}_visual">
|
|
<geometry>
|
|
<mesh filename="package://${description_pkg}/meshes/robot_ee/${ee_id}/visual/${name}.dae" />
|
|
</geometry>
|
|
</visual>
|
|
<collision name="${prefix}${name}_collision">
|
|
<geometry>
|
|
<mesh filename="package://franka_description/meshes/robot_ee/${ee_id}/collision/${name}.stl" />
|
|
</geometry>
|
|
</collision>
|
|
<xacro:ee-inertials name="${name}" />
|
|
</link>
|
|
|
|
<!-- sub-link defining coarse geometries of real end-effector's internal self-collision -->
|
|
<xacro:if value="${with_sc}">
|
|
<link name="${prefix}${name}_sc">
|
|
<xacro:unless value="${gazebo}">
|
|
<xacro:insert_block name="self_collision_geometries" />
|
|
</xacro:unless>
|
|
</link>
|
|
<!-- fixed joint between both sub-links -->
|
|
<joint name="${prefix}${name}_sc_joint" type="fixed">
|
|
<origin rpy="${rpy}" />
|
|
<parent link="${prefix}${name}" />
|
|
<child link="${prefix}${name}_sc" />
|
|
</joint>
|
|
</xacro:if>
|
|
</xacro:macro>
|
|
|
|
<!-- =========================================================== -->
|
|
<!-- Add a <collision> tag with a capsule, made from a cylinder -->
|
|
<!-- with two spheres at its caps. The capsule will always be -->
|
|
<!-- aligned with the axis of 'direction' you pass along. -->
|
|
<!-- -->
|
|
<!-- radius: Radii of both the cylinder and both spheres [m] -->
|
|
<!-- length: Length of the cylinder/distance between the centers -->
|
|
<!-- of the spheres. NOT overall length of capsule! -->
|
|
<!-- xyz: Position of the center of the capsule/cylinder -->
|
|
<!-- direction: One of { x, y, z, -x, -y, -z } -->
|
|
<!-- =========================================================== -->
|
|
<xacro:macro name="collision_capsule" params="radius length xyz:='0 0 0' direction:='z'">
|
|
<xacro:property name="r" value="${pi/2.0 if 'y' in direction else 0}" />
|
|
<xacro:property name="p" value="${pi/2.0 if 'x' in direction else 0}" />
|
|
<xacro:property name="y" value="0" />
|
|
<xacro:property name="x" value="${xyz.split(' ')[0]}" />
|
|
<xacro:property name="y" value="${xyz.split(' ')[1]}" />
|
|
<xacro:property name="z" value="${xyz.split(' ')[2]}" />
|
|
<!-- Sphere center offsets from center of cylinder -->
|
|
<xacro:property name="sx" value="${length / 2.0 if 'x' in direction else 0}" />
|
|
<xacro:property name="sy" value="${length / 2.0 if 'y' in direction else 0}" />
|
|
<xacro:property name="sz" value="${length / 2.0 if 'z' in direction else 0}" />
|
|
|
|
<collision>
|
|
<origin xyz="${x} ${y} ${z}" rpy="${r} ${p} ${y}"/>
|
|
<geometry>
|
|
<cylinder radius="${radius}" length="${length}" />
|
|
</geometry>
|
|
</collision>
|
|
<collision>
|
|
<origin xyz="${x+sx} ${y+sy} ${z+sz}" />
|
|
<geometry>
|
|
<sphere radius="${radius}" />
|
|
</geometry>
|
|
</collision>
|
|
<collision>
|
|
<origin xyz="${x-sx} ${y-sy} ${z-sz}" />
|
|
<geometry>
|
|
<sphere radius="${radius}" />
|
|
</geometry>
|
|
</collision>
|
|
</xacro:macro>
|
|
|
|
</robot> |