138 lines
7.5 KiB
XML
138 lines
7.5 KiB
XML
<?xml version="1.0"?>
|
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
|
|
|
<xacro:macro name="openarm-inertials" params="name inertials:=^ reflect:=1">
|
|
<xacro:unless value="${name in inertials}">
|
|
${xacro.warning('No inertia properties defined for: ' + name)}
|
|
</xacro:unless>
|
|
<xacro:if value="${name in inertials}">
|
|
<xacro:property name="I" value="${inertials[name]}" lazy_eval="false" />
|
|
<inertial>
|
|
<origin
|
|
xyz="${I.origin.x} ${reflect * I.origin.y} ${I.origin.z}"
|
|
rpy="${I.origin.roll} ${I.origin.pitch} ${I.origin.yaw}" />
|
|
<mass value="${I.mass}" />
|
|
<xacro:if value="${'inertia' in I}">
|
|
<xacro:property name="inert" value="${I.inertia}" />
|
|
<inertia ixx="${inert.xx}" ixy="${inert.xy}" ixz="${inert.xz}"
|
|
iyy="${inert.yy}" iyz="${inert.yz}" izz="${inert.zz}" />
|
|
</xacro:if>
|
|
</inertial>
|
|
</xacro:if>
|
|
</xacro:macro>
|
|
|
|
<xacro:macro name="link_with_sc" params="name no_prefix:='false' rpy:='0 0 0' kinematics_link:='' reflect:=1 inertials:=^">
|
|
<!-- <xacro:property name="prefix" value="${'' if no_prefix else arm_prefix + arm_type + '_'}" /> -->
|
|
<xacro:property name="prefix" value="${'' if no_prefix else 'openarm' + '_' + arm_prefix}" />
|
|
<link name="${prefix}${name}">
|
|
<visual name="${prefix}${name}_visual">
|
|
<xacro:openarm-kinematics-link config="${kinematics_link}" name="${name}" />
|
|
<geometry>
|
|
<mesh filename="package://${description_pkg}/meshes/arm/${arm_type}/visual/${name}.dae" scale="0.001 ${0.001*reflect} 0.001" />
|
|
</geometry>
|
|
</visual>
|
|
<collision name="${prefix}${name}_collision">
|
|
<xacro:openarm-kinematics-link config="${kinematics_link}" name="${name}" />
|
|
<geometry>
|
|
<mesh filename="package://openarm_description/meshes/arm/${arm_type}/collision/${name}_symp.stl" scale="0.001 ${0.001*reflect} 0.001" />
|
|
<!-- <mesh filename="package://openarm_description/meshes/arm/${arm_type}/collision/${name}.stl" scale="0.001 0.001 0.001" /> -->
|
|
</geometry>
|
|
</collision>
|
|
<xacro:openarm-inertials name="${name}" inertials="${inertials}" reflect="${reflect}" />
|
|
</link>
|
|
</xacro:macro>
|
|
|
|
<xacro:macro name="transmission-openarm-state" params="arm_type:=fer">
|
|
<transmission name="${arm_prefix}${arm_type}_openarm_state">
|
|
<type>openarm_hw/openarmStateInterface</type>
|
|
<joint name="${arm_prefix}${arm_type}_joint1"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></joint>
|
|
<joint name="${arm_prefix}${arm_type}_joint2"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></joint>
|
|
<joint name="${arm_prefix}${arm_type}_joint3"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></joint>
|
|
<joint name="${arm_prefix}${arm_type}_joint4"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></joint>
|
|
<joint name="${arm_prefix}${arm_type}_joint5"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></joint>
|
|
<joint name="${arm_prefix}${arm_type}_joint6"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></joint>
|
|
<joint name="${arm_prefix}${arm_type}_joint7"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></joint>
|
|
|
|
<actuator name="${arm_prefix}${arm_type}_motor1"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></actuator>
|
|
<actuator name="${arm_prefix}${arm_type}_motor2"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></actuator>
|
|
<actuator name="${arm_prefix}${arm_type}_motor3"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></actuator>
|
|
<actuator name="${arm_prefix}${arm_type}_motor4"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></actuator>
|
|
<actuator name="${arm_prefix}${arm_type}_motor5"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></actuator>
|
|
<actuator name="${arm_prefix}${arm_type}_motor6"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></actuator>
|
|
<actuator name="${arm_prefix}${arm_type}_motor7"><hardwareInterface>openarm_hw/openarmStateInterface</hardwareInterface></actuator>
|
|
</transmission>
|
|
</xacro:macro>
|
|
|
|
<xacro:macro name="transmission-openarm-model" params="arm_type:=v10 root:=v10_joint1 tip:=v10_joint7">
|
|
<transmission name="${arm_prefix}${arm_type}_openarm_model">
|
|
<type>openarm_hw/openarmModelInterface</type>
|
|
<joint name="${root}">
|
|
<role>root</role>
|
|
<hardwareInterface>openarm_hw/openarmModelInterface</hardwareInterface>
|
|
</joint>
|
|
<joint name="${tip}">
|
|
<role>tip</role>
|
|
<hardwareInterface>openarm_hw/openarmModelInterface</hardwareInterface>
|
|
</joint>
|
|
<actuator name="${root}_motor_root"><hardwareInterface>openarm_hw/openarmModelInterface</hardwareInterface></actuator>
|
|
<actuator name="${tip}_motor_tip" ><hardwareInterface>openarm_hw/openarmModelInterface</hardwareInterface></actuator>
|
|
</transmission>
|
|
</xacro:macro>
|
|
<xacro:macro name="inertia-cylinder" params="mass radius h">
|
|
<inertial>
|
|
<mass value="${mass}" />
|
|
<inertia ixx="${1./12 * mass * (3 * radius**2 + h**2)}" ixy = "0" ixz = "0"
|
|
iyy="${1./12 * mass * (3 * radius**2 + h**2)}" iyz = "0"
|
|
izz="${1./2 * mass * radius**2}" />
|
|
</inertial>
|
|
</xacro:macro>
|
|
|
|
|
|
<xacro:macro name="openarm-limits" params="name config reflect:=1 offset:=0">
|
|
<xacro:property name="limits" value="${config[name]['limit']}"/>
|
|
<xacro:property name="raw_lower" value="${limits.lower * reflect + offset}" />
|
|
<xacro:property name="raw_upper" value="${limits.upper * reflect + offset}" />
|
|
|
|
<xacro:property name="lower" value="0.0" />
|
|
<xacro:property name="upper" value="0.0" />
|
|
|
|
<xacro:if value="${raw_lower < raw_upper}">
|
|
<xacro:property name="lower" value="${raw_lower}" />
|
|
<xacro:property name="upper" value="${raw_upper}" />
|
|
</xacro:if>
|
|
|
|
<xacro:unless value="${raw_lower < raw_upper}">
|
|
<xacro:property name="lower" value="${raw_upper}" />
|
|
<xacro:property name="upper" value="${raw_lower}" />
|
|
</xacro:unless>
|
|
|
|
<limit
|
|
lower="${lower}"
|
|
upper="${upper}"
|
|
effort="${limits.effort}"
|
|
velocity="${limits.velocity}" />
|
|
</xacro:macro>
|
|
|
|
<xacro:macro name="openarm-kinematics" params="name config offset:=none reflect:=1">
|
|
<xacro:property name="kinematics" value="${config[name]['kinematic']}" lazy_eval="false" />
|
|
<xacro:if value="${offset != 'none'}">
|
|
<xacro:property name="offset" value="${offset[name]['kinematic_offset']}" lazy_eval="false" />
|
|
<origin
|
|
xyz="${kinematics.x + offset.x} ${kinematics.y + offset.y} ${kinematics.z + offset.z}"
|
|
rpy="${reflect*(kinematics.roll + offset.roll)} ${reflect*(kinematics.pitch + offset.pitch)} ${reflect*(kinematics.yaw + offset.yaw)}" />
|
|
</xacro:if>
|
|
<xacro:unless value="${offset != 'none'}">
|
|
<origin
|
|
xyz="${kinematics.x} ${kinematics.y} ${kinematics.z}"
|
|
rpy="${reflect*kinematics.roll} ${reflect*kinematics.pitch} ${reflect*kinematics.yaw}" />
|
|
</xacro:unless>
|
|
</xacro:macro>
|
|
|
|
<xacro:macro name="openarm-kinematics-link" params="config name">
|
|
<xacro:property name="kinematics_link" value="${config[name]['kinematic']}" lazy_eval="false" />
|
|
<origin rpy="${kinematics_link.roll} ${kinematics_link.pitch} ${kinematics_link.yaw}"
|
|
xyz="${kinematics_link.x} ${kinematics_link.y} ${kinematics_link.z}" />
|
|
</xacro:macro>
|
|
|
|
</robot>
|