Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
3 changes: 2 additions & 1 deletion kuka_cybertech_support/urdf/kr16_r2010_2.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,8 @@
<xacro:arg name="pitch" default="0"/>
<xacro:arg name="yaw" default="0"/>
<xacro:kuka_cybertech_ros2_control name="$(arg prefix)kr16_r2010_2" driver_version="$(arg driver_version)" client_ip="$(arg client_ip)" client_port="$(arg client_port)" controller_ip="$(arg controller_ip)" prefix="$(arg prefix)" mode="$(arg mode)" use_gpio="$(arg use_gpio)" roundtrip_time="$(arg roundtrip_time)" verify_robot_model="$(arg verify_robot_model)"/>
<xacro:kuka_kr16_r2010_2_robot prefix="$(arg prefix)" package_name="kuka_cybertech_support">
<link name="world" />
<xacro:kuka_kr16_r2010_2_robot prefix="$(arg prefix)" package_name="kuka_cybertech_support" parent="world">
<origin xyz="$(arg x) $(arg y) $(arg z)" rpy="$(arg roll) $(arg pitch) $(arg yaw)"/>
</xacro:kuka_kr16_r2010_2_robot>
</robot>
72 changes: 65 additions & 7 deletions kuka_cybertech_support/urdf/kr16_r2010_2_macro.xacro
Original file line number Diff line number Diff line change
@@ -1,8 +1,45 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:macro name="kuka_kr16_r2010_2_robot" params="prefix package_name *origin">
<xacro:macro name="kuka_kr16_r2010_2_robot" params="prefix package_name *origin parent external_axis:=false">
<xacro:if value="${external_axis}">
<link name="${prefix}rail_base">
<visual>
<origin rpy="0 0 0" xyz="-4.6875 0 0" />
<geometry>
<mesh filename="package://${package_name}/meshes/kr16_r2010_2/visual/rail_base.stl" scale="1 1 1" />
</geometry>
<material name="grey">
<color rgba="0.1 0.1 0.1 1.0" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-4.6875 0 0" />
<geometry>
<mesh filename="package://${package_name}/meshes/kr16_r2010_2/collision/rail_base.stl" scale="1 1 1" />
</geometry>
</collision>
</link>
<link name="${prefix}rail_carriage">
<visual>
<!-- TODO eyeballed this origin, needs to be measured -->
<origin rpy="0 0 0" xyz="-0.4975 0 0" />
<geometry>
<mesh filename="package://${package_name}/meshes/kr16_r2010_2/visual/rail_carriage.stl" scale="1 1 1" />
</geometry>
<material name="light_grey">
<color rgba="0.6 0.6 0.6 1.0" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.4975 0 0" />
<geometry>
<mesh filename="package://${package_name}/meshes/kr16_r2010_2/collision/rail_carriage.stl"
scale="1 1 1" />
</geometry>
</collision>
</link>
</xacro:if>
<!-- links - main serial chain -->
<link name="world"/>
<link name="${prefix}base_link">
<collision>
<geometry>
Expand Down Expand Up @@ -101,11 +138,32 @@
<origin rpy="-0.0 0.0 -0.0" xyz="0.0 0.0 0.0"/>
</visual>
</link>
<joint name="${prefix}world-base_link" type="fixed">
<parent link="world"/>
<child link="${prefix}base_link"/>
<xacro:insert_block name="origin"/>
</joint>
<xacro:if value="${external_axis}">
<joint name="${prefix}parent-rail_base" type="fixed">
<parent link="${parent}" />
<child link="${prefix}rail_base" />
<xacro:insert_block name="origin"/>
</joint>
<joint name="${prefix}linear_rail" type="prismatic">
<axis xyz="1 0 0" />
<parent link="${prefix}rail_base" />
<child link="${prefix}rail_carriage" />
<origin rpy="0 0 0" xyz="0 0 0" />
<limit acceleration="1.0" effort="100000.0" lower="-2.0" upper="2.0" velocity="0.175" />
</joint>
<joint name="${prefix}base_link" type="fixed">
<parent link="${prefix}rail_carriage"/>
<child link="${prefix}base_link"/>
<origin xyz="0 0.159 0" rpy="-1.5708 -1.5708 0" />
</joint>
</xacro:if>
<xacro:unless value="${external_axis}">
<joint name="${prefix}parent-base_link" type="fixed">
<parent link="${parent}"/>
<child link="${prefix}base_link"/>
<xacro:insert_block name="origin"/>
</joint>
</xacro:unless>
<joint name="${prefix}joint_1" type="revolute">
<axis xyz="0.0 0.0 1.0"/>
<child link="${prefix}link_1"/>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="kuka_cybertech_ros2_control" params="name driver_version client_ip client_port controller_ip prefix mode use_gpio roundtrip_time verify_robot_model">
<xacro:macro name="kuka_cybertech_ros2_control" params="name driver_version client_ip client_port controller_ip prefix mode use_gpio roundtrip_time verify_robot_model external_axis:=false">
<!-- gazebo -->
<xacro:if value="${mode == 'gazebo'}">
<xacro:include filename="$(find kuka_gazebo)/gazebo/kuka_gazebo.xacro"/>
Expand Down Expand Up @@ -67,6 +67,14 @@
<param name="initial_value">0.0</param>
</state_interface>
</joint>
<xacro:if value="${external_axis}">
<joint name="${prefix}linear_rail">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
</joint>
</xacro:if>
<xacro:if value="${use_gpio}">
<xacro:include filename="$(find kuka_rsi_driver)/config/gpio_config.xacro"/>
<xacro:gpio_config/>
Expand Down
Loading