Skip to content

Commit

Permalink
One more passing test
Browse files Browse the repository at this point in the history
  • Loading branch information
@bourumir-wyngs
bourumir-wyngs committed May 26, 2024
1 parent 90602d2 commit 0a0834a
Show file tree
Hide file tree
Showing 3 changed files with 215 additions and 0 deletions.
178 changes: 178 additions & 0 deletions src/tests/data/kuka/kr10r1420_macro.xacro
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,178 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">

<xacro:macro name="kuka_kr10r1420" params="prefix">
<xacro:include filename="$(find kuka_resources)/urdf/common_materials.xacro"/>
<link name="${prefix}base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/visual/base_link.stl" />
</geometry>
<xacro:material_kuka_black />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/collision/base_link.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}link_1">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/visual/link_1.stl" />
</geometry>
<xacro:material_kuka_orange />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/collision/link_1.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}link_2">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/visual/link_2.stl"/>
</geometry>
<xacro:material_kuka_orange />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/collision/link_2.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}link_3">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/visual/link_3.stl" />
</geometry>
<xacro:material_kuka_orange />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/collision/link_3.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}link_4">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/visual/link_4.stl" />
</geometry>
<xacro:material_kuka_orange />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/collision/link_4.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}link_5">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/visual/link_5.stl" />
</geometry>
<xacro:material_kuka_orange />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/collision/link_5.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}link_6">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/visual/link_6.stl" />
</geometry>
<xacro:material_kuka_pedestal />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://kuka_kr10_support/meshes/kr10r1420/collision/link_6.stl" />
</geometry>
</collision>
</link>
<!-- Following REP199, this frame shall be use to attach EEF or other equipment -->
<link name="${prefix}flange" />

<joint name="${prefix}joint_a1" type="revolute">
<origin xyz="0 0 0.450" rpy="0 0 0"/>
<parent link="${prefix}base_link"/>
<child link="${prefix}link_1"/>
<axis xyz="0 0 -1"/>
<limit effort="0" lower="${radians(-170)}" upper="${radians(170)}" velocity="${radians(220)}"/>
</joint>
<joint name="${prefix}joint_a2" type="revolute">
<origin xyz="0.150 0 0" rpy="0 0 0"/>
<parent link="${prefix}link_1"/>
<child link="${prefix}link_2"/>
<axis xyz="0 1 0"/>
<limit effort="0" lower="${radians(-185)}" upper="${radians(65)}" velocity="${radians(210)}"/>
</joint>
<joint name="${prefix}joint_a3" type="revolute">
<origin xyz="0.610 0 0" rpy="0 0 0"/>
<parent link="${prefix}link_2"/>
<child link="${prefix}link_3"/>
<axis xyz="0 1 0"/>
<limit effort="0" lower="${radians(-137)}" upper="${radians(163)}" velocity="${radians(270)}"/>
</joint>
<joint name="${prefix}joint_a4" type="revolute">
<origin xyz="0 0 0.02" rpy="0 0 0"/>
<parent link="${prefix}link_3"/>
<child link="${prefix}link_4"/>
<axis xyz="-1 0 0"/>
<limit effort="0" lower="${radians(-185)}" upper="${radians(185)}" velocity="${radians(381)}"/>
</joint>
<joint name="${prefix}joint_a5" type="revolute">
<origin xyz="0.660 0 0" rpy="0 0 0"/>
<parent link="${prefix}link_4"/>
<child link="${prefix}link_5"/>
<axis xyz="0 1 0"/>
<limit effort="0" lower="${radians(-120)}" upper="${radians(120)}" velocity="${radians(311)}"/>
</joint>
<joint name="${prefix}joint_a6" type="revolute">
<origin xyz="0.080 0 0" rpy="0 0 0"/>
<parent link="${prefix}link_5"/>
<child link="${prefix}link_6"/>
<axis xyz="-1 0 0"/>
<limit effort="0" lower="${radians(-350)}" upper="${radians(350)}" velocity="${radians(492)}"/>
</joint>
<joint name="${prefix}joint_a6-flange" type="fixed">
<parent link="${prefix}link_6"/>
<child link="${prefix}flange"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

<!-- ROS base_link to KUKA $ROBROOT coordinate system transform -->
<link name="${prefix}base" />
<joint name="${prefix}base_link-base" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="${prefix}base_link"/>
<child link="${prefix}base"/>
</joint>

<!-- This frame corresponds to the $TOOL coordinate system in KUKA KRC controllers -->
<link name="${prefix}tool0" />
<joint name="${prefix}flange-tool0" type="fixed">
<parent link="${prefix}flange"/>
<child link="${prefix}tool0"/>
<origin xyz="0 0 0" rpy="0 ${radians(90)} 0"/>
</joint>
</xacro:macro>
</robot>
21 changes: 21 additions & 0 deletions src/tests/data/kuka/opw_parameters_kr10r1420.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
#
# Parameters for use with IK solvers which support OPW (Ortho-Parallel Wrist)
# kinematic configurations, as described in the paper "An Analytical Solution
# of the Inverse Kinematics Problem of Industrial Serial Manipulators with an
# Ortho-parallel Basis and a Spherical Wrist" by Mathias Brandstötter, Arthur
# Angerer, and Michael Hofbaur (Proceedings of the Austrian Robotics Workshop
# 2014, 22-23 May, 2014, Linz, Austria).
#
# The moveit_opw_kinematics_plugin package provides such a solver.
#
opw_kinematics_geometric_parameters:
a1: 0.150
a2: -0.020
b: 0.0
c1: 0.450
c2: 0.610
c3: 0.660
c4: 0.080
opw_kinematics_joint_offsets: [0.0, deg(-90.0), 0.0, 0.0, 0.0, 0.0]
opw_kinematics_joint_sign_corrections: [-1, 1, 1, -1, 1, -1]

16 changes: 16 additions & 0 deletions src/tests/urdf_extractor.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,4 +147,20 @@ fn test_extraction_kr150() {
assert_eq!(urdf.c4, yaml.c4, "c4 parameter mismatch");
}

#[test]
fn test_extraction_kr10r1420() {
let yaml = Parameters::from_yaml_file("\
src/tests/data/kuka/opw_parameters_kr10r1420.yaml")
.expect("Failed to read or parse URDF");
let urdf = read_urdf("src/tests/data/kuka/kr10r1420_macro.xacro");

assert_eq!(urdf.a1, yaml.a1, "a1 parameter mismatch");
assert_eq!(urdf.a2, yaml.a2, "a2 parameter mismatch");
assert_eq!(urdf.b, yaml.b, "b parameter mismatch");
assert_eq!(urdf.c1, yaml.c1, "c1 parameter mismatch");
assert_eq!(urdf.c2, yaml.c2, "c2 parameter mismatch");
assert_eq!(urdf.c3, yaml.c3, "c3 parameter mismatch");
assert_eq!(urdf.c4, yaml.c4, "c4 parameter mismatch");
}


0 comments on commit 0a0834a

Please sign in to comment.