From 6b6f4b2707ad7c0c8d1c10a0ac970a7e96d73232 Mon Sep 17 00:00:00 2001
From: Aditya Bhat <125924171+AdityaBhat-TRI@users.noreply.github.com>
Date: Mon, 24 Jul 2023 14:55:27 -0400
Subject: [PATCH] Updated inertias for link 2 and link 4 for the iiwas that
were rotated by 90 degrees (#19830)
---
.../sdf/iiwa14_no_collision.sdf | 8 +-
.../sdf/iiwa14_polytope_collision.sdf | 8 +-
.../test/iiwa14_variants_parsing_test.cc | 124 ++++++++++++++----
.../urdf/dual_iiwa14_polytope_collision.urdf | 8 +-
.../models/iiwa_description/urdf/iiwa14.xacro | 4 +-
.../urdf/iiwa14_no_collision.urdf | 4 +-
.../urdf/iiwa14_polytope_collision.urdf | 4 +-
.../urdf/iiwa14_primitive_collision.urdf | 4 +-
.../urdf/iiwa14_spheres_collision.urdf | 4 +-
.../urdf/iiwa14_spheres_dense_collision.urdf | 4 +-
.../iiwa14_spheres_dense_elbow_collision.urdf | 4 +-
..._iiwa14_spheres_dense_elbow_collision.urdf | 4 +-
12 files changed, 130 insertions(+), 50 deletions(-)
diff --git a/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf b/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf
index 491cc17676e7..5a1cd40431ad 100644
--- a/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf
+++ b/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf
@@ -93,9 +93,9 @@ Later edited by hand to:
0.0305
0
0
- 0.0304
+ 0.011
0
- 0.011
+ 0.0304
@@ -226,9 +226,9 @@ Later edited by hand to:
0.017
0
0
- 0.0164
+ 0.006
0
- 0.006
+ 0.0164
diff --git a/manipulation/models/iiwa_description/sdf/iiwa14_polytope_collision.sdf b/manipulation/models/iiwa_description/sdf/iiwa14_polytope_collision.sdf
index c94ab787e398..d67301144681 100644
--- a/manipulation/models/iiwa_description/sdf/iiwa14_polytope_collision.sdf
+++ b/manipulation/models/iiwa_description/sdf/iiwa14_polytope_collision.sdf
@@ -88,9 +88,9 @@ Later edited by hand to rename the base link and remove damping from the joints.
0.0305
0
0
- 0.0304
+ 0.011
0
- 0.011
+ 0.0304
@@ -217,9 +217,9 @@ Later edited by hand to rename the base link and remove damping from the joints.
0.017
0
0
- 0.0164
+ 0.006
0
- 0.006
+ 0.0164
diff --git a/manipulation/models/iiwa_description/test/iiwa14_variants_parsing_test.cc b/manipulation/models/iiwa_description/test/iiwa14_variants_parsing_test.cc
index 0420ab665ae8..3d51dda18392 100644
--- a/manipulation/models/iiwa_description/test/iiwa14_variants_parsing_test.cc
+++ b/manipulation/models/iiwa_description/test/iiwa14_variants_parsing_test.cc
@@ -21,6 +21,38 @@ multibody::ModelInstanceIndex LoadIiwa14CanonicalModel(
return parser.AddModels(canonical_model_file).at(0);
}
+// Read the common robot model files
+const std::vector GetCommonIiwaModelFiles() {
+ const std::vector model_files = {
+ "drake/manipulation/models/iiwa_description/sdf/"
+ "iiwa14_polytope_collision.sdf",
+ "drake/manipulation/models/iiwa_description/urdf/"
+ "iiwa14_no_collision.urdf",
+ "drake/manipulation/models/iiwa_description/urdf/"
+ "iiwa14_polytope_collision.urdf",
+ "drake/manipulation/models/iiwa_description/urdf/"
+ "iiwa14_primitive_collision.urdf",
+ "drake/manipulation/models/iiwa_description/urdf/"
+ "iiwa14_spheres_collision.urdf",
+ "drake/manipulation/models/iiwa_description/urdf/"
+ "iiwa14_spheres_dense_collision.urdf",
+ "drake/manipulation/models/iiwa_description/urdf/"
+ "iiwa14_spheres_dense_elbow_collision.urdf"};
+ return model_files;
+}
+
+// Read the dual IIWA model file
+const std::string GetDualIiwaModelFile() {
+ return "drake/manipulation/models/iiwa_description/urdf/"
+ "dual_iiwa14_polytope_collision.urdf";
+}
+
+// Read the planar IIWA model file
+const std::string GetPlanarIiwaModelFile() {
+ return "drake/manipulation/models/iiwa_description/urdf/"
+ "planar_iiwa14_spheres_dense_elbow_collision.urdf";
+}
+
// Compares velocity, acceleration, effort and position limits of two given
// actuators.
void CompareActuatorLimits(const multibody::JointActuator& joint_a,
@@ -43,6 +75,17 @@ void CompareActuatorLimits(const multibody::JointActuator& joint_a,
EXPECT_EQ(joint_a.default_rotor_inertia(), joint_b.default_rotor_inertia());
}
+// Compare rotational inertias i.e moments and products of inertia
+void CompareRotationalInertias(const multibody::Body& canonical_body,
+ const multibody::Body& robot_body) {
+ EXPECT_TRUE(
+ CompareMatrices(canonical_body.default_rotational_inertia().get_moments(),
+ robot_body.default_rotational_inertia().get_moments()));
+ EXPECT_TRUE(CompareMatrices(
+ canonical_body.default_rotational_inertia().get_products(),
+ robot_body.default_rotational_inertia().get_products()));
+}
+
// Tests that KUKA LBR iiwa14 models have consistent joint limits.
// It takes iiwa14_no_collisions.sdf as the canonical model.
// It assumes all joints are declared in the same order.
@@ -59,23 +102,8 @@ GTEST_TEST(JointLimitsIiwa14, TestEffortVelocityPositionValues) {
const std::vector joint_canonical_indices =
canonical_plant.GetJointIndices(canonical_model_instance);
- const std::vector model_files = {
- "drake/manipulation/models/iiwa_description/sdf/"
- "iiwa14_polytope_collision.sdf",
- "drake/manipulation/models/iiwa_description/urdf/"
- "iiwa14_no_collision.urdf",
- "drake/manipulation/models/iiwa_description/urdf/"
- "iiwa14_polytope_collision.urdf",
- "drake/manipulation/models/iiwa_description/urdf/"
- "iiwa14_primitive_collision.urdf",
- "drake/manipulation/models/iiwa_description/urdf/"
- "iiwa14_spheres_collision.urdf",
- "drake/manipulation/models/iiwa_description/urdf/"
- "iiwa14_spheres_dense_collision.urdf",
- "drake/manipulation/models/iiwa_description/urdf/"
- "iiwa14_spheres_dense_elbow_collision.urdf",
- "drake/manipulation/models/iiwa_description/urdf/"
- "dual_iiwa14_polytope_collision.urdf"};
+ std::vector model_files = GetCommonIiwaModelFiles();
+ model_files.push_back(GetDualIiwaModelFile());
for (auto& model_file : model_files) {
multibody::MultibodyPlant plant(0.0);
@@ -94,8 +122,8 @@ GTEST_TEST(JointLimitsIiwa14, TestEffortVelocityPositionValues) {
// Test the joints from the second instance of the dual iiwa14 polytope
// collision model. They correspond to joints 7 to 13 of the model.
- if (model_file.substr(model_file.find_last_of('/') + 1) ==
- "dual_iiwa14_polytope_collision.urdf") {
+ std::filesystem::path model_path(model_file);
+ if (model_path.filename() == "dual_iiwa14_polytope_collision.urdf") {
const multibody::JointActuator& second_instance_joint_actuator =
plant.get_joint_actuator(
drake::multibody::JointActuatorIndex(i + 7));
@@ -119,9 +147,7 @@ GTEST_TEST(JointLimitsIiwa14, TestEffortVelocityPositionValuesPlanarModel) {
multibody::MultibodyPlant plant(0.0);
multibody::Parser parser(&plant);
- parser.AddModels(
- FindResourceOrThrow("drake/manipulation/models/iiwa_description/urdf/"
- "planar_iiwa14_spheres_dense_elbow_collision.urdf"));
+ parser.AddModels(FindResourceOrThrow(GetPlanarIiwaModelFile()));
plant.Finalize();
// This model only has three actuators. They correspond to actuators 1, 3 and
@@ -139,6 +165,60 @@ GTEST_TEST(JointLimitsIiwa14, TestEffortVelocityPositionValuesPlanarModel) {
drake::multibody::JointActuatorIndex(5)),
plant.get_joint_actuator(drake::multibody::JointActuatorIndex(2)));
}
+
+// Tests that KUKA LBR iiwa14 models have consistent inertias.
+// It takes iiwa14_no_collisions.sdf as the canonical model.
+// It assumes all links are declared in the same order.
+// It checks values directly on urdf files, generated from xacro.
+GTEST_TEST(InertiasIiwa14, TestInertiaValues) {
+ multibody::MultibodyPlant canonical_plant(0.0);
+ multibody::ModelInstanceIndex canonical_model_instance =
+ LoadIiwa14CanonicalModel(&canonical_plant);
+ canonical_plant.Finalize();
+
+ const std::vector body_canonical_indices =
+ canonical_plant.GetBodyIndices(canonical_model_instance);
+
+ std::vector model_files = GetCommonIiwaModelFiles();
+ model_files.push_back(GetDualIiwaModelFile());
+ model_files.push_back(GetPlanarIiwaModelFile());
+
+ std::vector link_names = {
+ "iiwa_link_0", "iiwa_link_1", "iiwa_link_2", "iiwa_link_3",
+ "iiwa_link_4", "iiwa_link_5", "iiwa_link_6", "iiwa_link_7"};
+
+ for (auto& model_file : model_files) {
+ SCOPED_TRACE(fmt::format("model file: {}", model_file));
+ multibody::MultibodyPlant plant(0.0);
+ multibody::Parser parser(&plant);
+ parser.AddModels(FindResourceOrThrow(model_file));
+ plant.Finalize();
+ std::filesystem::path model_path(model_file);
+ if (model_path.filename() == "dual_iiwa14_polytope_collision.urdf") {
+ for (size_t i = 0; i < link_names.size(); ++i) {
+ SCOPED_TRACE(fmt::format("Link: {}", link_names[i]));
+ const multibody::Body& canonical_body =
+ canonical_plant.GetBodyByName(link_names[i]);
+ const multibody::Body& left_robot_body =
+ plant.GetBodyByName("left_" + link_names[i]);
+ const multibody::Body& right_robot_body =
+ plant.GetBodyByName("right_" + link_names[i]);
+ CompareRotationalInertias(canonical_body, left_robot_body);
+ CompareRotationalInertias(canonical_body, right_robot_body);
+ }
+ } else {
+ for (size_t i = 0; i < link_names.size(); ++i) {
+ SCOPED_TRACE(fmt::format("Link: {}", link_names[i]));
+ const multibody::Body& canonical_body =
+ canonical_plant.GetBodyByName(link_names[i]);
+ const multibody::Body& robot_body =
+ plant.GetBodyByName(link_names[i]);
+ CompareRotationalInertias(canonical_body, robot_body);
+ }
+ }
+ }
+}
+
} // namespace
} // namespace manipulation
} // namespace drake
diff --git a/manipulation/models/iiwa_description/urdf/dual_iiwa14_polytope_collision.urdf b/manipulation/models/iiwa_description/urdf/dual_iiwa14_polytope_collision.urdf
index 1ff1f24cf17d..d1a53e7bc1a3 100644
--- a/manipulation/models/iiwa_description/urdf/dual_iiwa14_polytope_collision.urdf
+++ b/manipulation/models/iiwa_description/urdf/dual_iiwa14_polytope_collision.urdf
@@ -109,7 +109,7 @@
-
+
@@ -180,7 +180,7 @@
-
+
@@ -533,7 +533,7 @@
-
+
@@ -604,7 +604,7 @@
-
+
diff --git a/manipulation/models/iiwa_description/urdf/iiwa14.xacro b/manipulation/models/iiwa_description/urdf/iiwa14.xacro
index 51ef9296f6d8..387590cb805f 100644
--- a/manipulation/models/iiwa_description/urdf/iiwa14.xacro
+++ b/manipulation/models/iiwa_description/urdf/iiwa14.xacro
@@ -123,7 +123,7 @@
-
+
@@ -238,7 +238,7 @@
-
+
diff --git a/manipulation/models/iiwa_description/urdf/iiwa14_no_collision.urdf b/manipulation/models/iiwa_description/urdf/iiwa14_no_collision.urdf
index b6550e695c16..f9c9653fa99c 100644
--- a/manipulation/models/iiwa_description/urdf/iiwa14_no_collision.urdf
+++ b/manipulation/models/iiwa_description/urdf/iiwa14_no_collision.urdf
@@ -98,7 +98,7 @@
-
+
@@ -169,7 +169,7 @@
-
+
diff --git a/manipulation/models/iiwa_description/urdf/iiwa14_polytope_collision.urdf b/manipulation/models/iiwa_description/urdf/iiwa14_polytope_collision.urdf
index df42a4842a7c..ce4b66b89f84 100644
--- a/manipulation/models/iiwa_description/urdf/iiwa14_polytope_collision.urdf
+++ b/manipulation/models/iiwa_description/urdf/iiwa14_polytope_collision.urdf
@@ -104,7 +104,7 @@
-
+
@@ -175,7 +175,7 @@
-
+
diff --git a/manipulation/models/iiwa_description/urdf/iiwa14_primitive_collision.urdf b/manipulation/models/iiwa_description/urdf/iiwa14_primitive_collision.urdf
index 0d98ce55c17c..2094b128a2a8 100644
--- a/manipulation/models/iiwa_description/urdf/iiwa14_primitive_collision.urdf
+++ b/manipulation/models/iiwa_description/urdf/iiwa14_primitive_collision.urdf
@@ -120,7 +120,7 @@
-
+
@@ -205,7 +205,7 @@
-
+
diff --git a/manipulation/models/iiwa_description/urdf/iiwa14_spheres_collision.urdf b/manipulation/models/iiwa_description/urdf/iiwa14_spheres_collision.urdf
index ea39fa71f34e..e59f54be281a 100644
--- a/manipulation/models/iiwa_description/urdf/iiwa14_spheres_collision.urdf
+++ b/manipulation/models/iiwa_description/urdf/iiwa14_spheres_collision.urdf
@@ -120,7 +120,7 @@
-
+
@@ -226,7 +226,7 @@
-
+
diff --git a/manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_collision.urdf b/manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_collision.urdf
index 8008099bf715..6b50de0477b3 100644
--- a/manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_collision.urdf
+++ b/manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_collision.urdf
@@ -169,7 +169,7 @@
-
+
@@ -366,7 +366,7 @@
-
+
diff --git a/manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_elbow_collision.urdf b/manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_elbow_collision.urdf
index d2e44401637d..766f371fa8b7 100644
--- a/manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_elbow_collision.urdf
+++ b/manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_elbow_collision.urdf
@@ -120,7 +120,7 @@
-
+
@@ -268,7 +268,7 @@
-
+
diff --git a/manipulation/models/iiwa_description/urdf/planar_iiwa14_spheres_dense_elbow_collision.urdf b/manipulation/models/iiwa_description/urdf/planar_iiwa14_spheres_dense_elbow_collision.urdf
index b945c29fe326..b0406ef551b0 100644
--- a/manipulation/models/iiwa_description/urdf/planar_iiwa14_spheres_dense_elbow_collision.urdf
+++ b/manipulation/models/iiwa_description/urdf/planar_iiwa14_spheres_dense_elbow_collision.urdf
@@ -92,7 +92,7 @@
-
+
@@ -237,7 +237,7 @@
-
+