Skip to content

Commit

Permalink
Updated inertias for link 2 and link 4 for the iiwas that were rotate…
Browse files Browse the repository at this point in the history 
…d by 90 degrees (#19830)
  • Loading branch information
@AdityaBhat-TRI
AdityaBhat-TRI authored Jul 24, 2023
1 parent 3fdd74c commit 6b6f4b2
Show file tree
Hide file tree
Showing 12 changed files with 130 additions and 50 deletions.
8 changes: 4 additions & 4 deletions manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,9 +93,9 @@ Later edited by hand to:
<ixx>0.0305</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0304</iyy>
<iyy>0.011</iyy>
<iyz>0</iyz>
<izz>0.011</izz>
<izz>0.0304</izz>
</inertia>
</inertial>
<visual name="iiwa_link_2_visual_grey">
Expand Down Expand Up @@ -226,9 +226,9 @@ Later edited by hand to:
<ixx>0.017</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0164</iyy>
<iyy>0.006</iyy>
<iyz>0</iyz>
<izz>0.006</izz>
<izz>0.0164</izz>
</inertia>
</inertial>
<visual name="iiwa_link_4_visual_grey">
Expand Down
8 changes: 4 additions & 4 deletions manipulation/models/iiwa_description/sdf/iiwa14_polytope_collision.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,9 +88,9 @@ Later edited by hand to rename the base link and remove damping from the joints.
<ixx>0.0305</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0304</iyy>
<iyy>0.011</iyy>
<iyz>0</iyz>
<izz>0.011</izz>
<izz>0.0304</izz>
</inertia>
</inertial>
<visual name="iiwa_link_2_visual_grey">
Expand Down Expand Up @@ -217,9 +217,9 @@ Later edited by hand to rename the base link and remove damping from the joints.
<ixx>0.017</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0164</iyy>
<iyy>0.006</iyy>
<iyz>0</iyz>
<izz>0.006</izz>
<izz>0.0164</izz>
</inertia>
</inertial>
<visual name="iiwa_link_4_visual_grey">
Expand Down
124 changes: 102 additions & 22 deletions manipulation/models/iiwa_description/test/iiwa14_variants_parsing_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,38 @@ multibody::ModelInstanceIndex LoadIiwa14CanonicalModel(
return parser.AddModels(canonical_model_file).at(0);
}

// Read the common robot model files
const std::vector<std::string> GetCommonIiwaModelFiles() {
const std::vector<std::string> 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<double>& joint_a,
Expand All @@ -43,6 +75,17 @@ void CompareActuatorLimits(const multibody::JointActuator<double>& 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<double>& canonical_body,
const multibody::Body<double>& 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.
Expand All @@ -59,23 +102,8 @@ GTEST_TEST(JointLimitsIiwa14, TestEffortVelocityPositionValues) {
const std::vector<multibody::JointIndex> joint_canonical_indices =
canonical_plant.GetJointIndices(canonical_model_instance);

const std::vector<std::string> 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<std::string> model_files = GetCommonIiwaModelFiles();
model_files.push_back(GetDualIiwaModelFile());

for (auto& model_file : model_files) {
multibody::MultibodyPlant<double> plant(0.0);
Expand All @@ -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<double>& second_instance_joint_actuator =
plant.get_joint_actuator(
drake::multibody::JointActuatorIndex(i + 7));
Expand All @@ -119,9 +147,7 @@ GTEST_TEST(JointLimitsIiwa14, TestEffortVelocityPositionValuesPlanarModel) {

multibody::MultibodyPlant<double> 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
Expand All @@ -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<double> canonical_plant(0.0);
multibody::ModelInstanceIndex canonical_model_instance =
LoadIiwa14CanonicalModel(&canonical_plant);
canonical_plant.Finalize();

const std::vector<multibody::BodyIndex> body_canonical_indices =
canonical_plant.GetBodyIndices(canonical_model_instance);

std::vector<std::string> model_files = GetCommonIiwaModelFiles();
model_files.push_back(GetDualIiwaModelFile());
model_files.push_back(GetPlanarIiwaModelFile());

std::vector<std::string> 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<double> 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<double>& canonical_body =
canonical_plant.GetBodyByName(link_names[i]);
const multibody::Body<double>& left_robot_body =
plant.GetBodyByName("left_" + link_names[i]);
const multibody::Body<double>& 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<double>& canonical_body =
canonical_plant.GetBodyByName(link_names[i]);
const multibody::Body<double>& robot_body =
plant.GetBodyByName(link_names[i]);
CompareRotationalInertias(canonical_body, robot_body);
}
}
}
}

} // namespace
} // namespace manipulation
} // namespace drake
8 changes: 4 additions & 4 deletions manipulation/models/iiwa_description/urdf/dual_iiwa14_polytope_collision.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@
<mass value="6.35"/>
<!-- 3.95 kuka CAD value-->
<!--4 Original Drake URDF value-->
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.0304" iyz="0" izz="0.011"/>
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.011" iyz="0" izz="0.0304"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down Expand Up @@ -180,7 +180,7 @@
<mass value="3.5"/>
<!--2.74 kuka CAD value-->
<!--2.7 Original Drake URDF value-->
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.0164" iyz="0" izz="0.006"/>
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.006" iyz="0" izz="0.0164"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down Expand Up @@ -533,7 +533,7 @@
<mass value="6.35"/>
<!-- 3.95 kuka CAD value-->
<!--4 Original Drake URDF value-->
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.0304" iyz="0" izz="0.011"/>
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.011" iyz="0" izz="0.0304"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down Expand Up @@ -604,7 +604,7 @@
<mass value="3.5"/>
<!--2.74 kuka CAD value-->
<!--2.7 Original Drake URDF value-->
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.0164" iyz="0" izz="0.006"/>
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.006" iyz="0" izz="0.0164"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down
4 changes: 2 additions & 2 deletions manipulation/models/iiwa_description/urdf/iiwa14.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@
<mass value="6.35"/>
<!-- 3.95 kuka CAD value-->
<!--4 Original Drake URDF value-->
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.0304" iyz="0" izz="0.011"/>
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.011" iyz="0" izz="0.0304"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down Expand Up @@ -238,7 +238,7 @@
<mass value="3.5"/>
<!--2.74 kuka CAD value-->
<!--2.7 Original Drake URDF value-->
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.0164" iyz="0" izz="0.006"/>
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.006" iyz="0" izz="0.0164"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down
4 changes: 2 additions & 2 deletions manipulation/models/iiwa_description/urdf/iiwa14_no_collision.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@
<mass value="6.35"/>
<!-- 3.95 kuka CAD value-->
<!--4 Original Drake URDF value-->
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.0304" iyz="0" izz="0.011"/>
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.011" iyz="0" izz="0.0304"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down Expand Up @@ -169,7 +169,7 @@
<mass value="3.5"/>
<!--2.74 kuka CAD value-->
<!--2.7 Original Drake URDF value-->
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.0164" iyz="0" izz="0.006"/>
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.006" iyz="0" izz="0.0164"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down
4 changes: 2 additions & 2 deletions manipulation/models/iiwa_description/urdf/iiwa14_polytope_collision.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@
<mass value="6.35"/>
<!-- 3.95 kuka CAD value-->
<!--4 Original Drake URDF value-->
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.0304" iyz="0" izz="0.011"/>
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.011" iyz="0" izz="0.0304"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down Expand Up @@ -175,7 +175,7 @@
<mass value="3.5"/>
<!--2.74 kuka CAD value-->
<!--2.7 Original Drake URDF value-->
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.0164" iyz="0" izz="0.006"/>
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.006" iyz="0" izz="0.0164"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down
4 changes: 2 additions & 2 deletions manipulation/models/iiwa_description/urdf/iiwa14_primitive_collision.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@
<mass value="6.35"/>
<!-- 3.95 kuka CAD value-->
<!--4 Original Drake URDF value-->
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.0304" iyz="0" izz="0.011"/>
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.011" iyz="0" izz="0.0304"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down Expand Up @@ -205,7 +205,7 @@
<mass value="3.5"/>
<!--2.74 kuka CAD value-->
<!--2.7 Original Drake URDF value-->
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.0164" iyz="0" izz="0.006"/>
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.006" iyz="0" izz="0.0164"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down
4 changes: 2 additions & 2 deletions manipulation/models/iiwa_description/urdf/iiwa14_spheres_collision.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@
<mass value="6.35"/>
<!-- 3.95 kuka CAD value-->
<!--4 Original Drake URDF value-->
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.0304" iyz="0" izz="0.011"/>
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.011" iyz="0" izz="0.0304"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down Expand Up @@ -226,7 +226,7 @@
<mass value="3.5"/>
<!--2.74 kuka CAD value-->
<!--2.7 Original Drake URDF value-->
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.0164" iyz="0" izz="0.006"/>
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.006" iyz="0" izz="0.0164"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down
4 changes: 2 additions & 2 deletions manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_collision.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,7 +169,7 @@
<mass value="6.35"/>
<!-- 3.95 kuka CAD value-->
<!--4 Original Drake URDF value-->
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.0304" iyz="0" izz="0.011"/>
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.011" iyz="0" izz="0.0304"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down Expand Up @@ -366,7 +366,7 @@
<mass value="3.5"/>
<!--2.74 kuka CAD value-->
<!--2.7 Original Drake URDF value-->
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.0164" iyz="0" izz="0.006"/>
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.006" iyz="0" izz="0.0164"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down
4 changes: 2 additions & 2 deletions manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_elbow_collision.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@
<mass value="6.35"/>
<!-- 3.95 kuka CAD value-->
<!--4 Original Drake URDF value-->
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.0304" iyz="0" izz="0.011"/>
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.011" iyz="0" izz="0.0304"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down Expand Up @@ -268,7 +268,7 @@
<mass value="3.5"/>
<!--2.74 kuka CAD value-->
<!--2.7 Original Drake URDF value-->
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.0164" iyz="0" izz="0.006"/>
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.006" iyz="0" izz="0.0164"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down
4 changes: 2 additions & 2 deletions manipulation/models/iiwa_description/urdf/planar_iiwa14_spheres_dense_elbow_collision.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@
<mass value="6.35"/>
<!-- 3.95 kuka CAD value-->
<!--4 Original Drake URDF value-->
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.0304" iyz="0" izz="0.011"/>
<inertia ixx="0.0305" ixy="0" ixz="0" iyy="0.011" iyz="0" izz="0.0304"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down Expand Up @@ -237,7 +237,7 @@
<mass value="3.5"/>
<!--2.74 kuka CAD value-->
<!--2.7 Original Drake URDF value-->
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.0164" iyz="0" izz="0.006"/>
<inertia ixx="0.017" ixy="0" ixz="0" iyy="0.006" iyz="0" izz="0.0164"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
Expand Down

0 comments on commit 6b6f4b2

Please sign in to comment.