From 0a0834a6b56ef1150078b993d30f9141b257467d Mon Sep 17 00:00:00 2001
From: Bourumir Wyngs <bourumir.wyngs@gmail.com>
Date: Sun, 26 May 2024 12:40:43 +0200
Subject: [PATCH] One more passing test

---
 src/tests/data/kuka/kr10r1420_macro.xacro     | 178 ++++++++++++++++++
 .../data/kuka/opw_parameters_kr10r1420.yaml   |  21 +++
 src/tests/urdf_extractor.rs                   |  16 ++
 3 files changed, 215 insertions(+)
 create mode 100644 src/tests/data/kuka/kr10r1420_macro.xacro
 create mode 100644 src/tests/data/kuka/opw_parameters_kr10r1420.yaml

diff --git a/src/tests/data/kuka/kr10r1420_macro.xacro b/src/tests/data/kuka/kr10r1420_macro.xacro
new file mode 100644
index 0000000..e8dd683
--- /dev/null
+++ b/src/tests/data/kuka/kr10r1420_macro.xacro
@@ -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>
diff --git a/src/tests/data/kuka/opw_parameters_kr10r1420.yaml b/src/tests/data/kuka/opw_parameters_kr10r1420.yaml
new file mode 100644
index 0000000..2e8fdb3
--- /dev/null
+++ b/src/tests/data/kuka/opw_parameters_kr10r1420.yaml
@@ -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]
+
diff --git a/src/tests/urdf_extractor.rs b/src/tests/urdf_extractor.rs
index e93442c..b9d5e26 100644
--- a/src/tests/urdf_extractor.rs
+++ b/src/tests/urdf_extractor.rs
@@ -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");
+}
+