diff --git a/sdformat_urdf/src/sdformat_urdf.cpp b/sdformat_urdf/src/sdformat_urdf.cpp index 0bca584..31c5c72 100644 --- a/sdformat_urdf/src/sdformat_urdf.cpp +++ b/sdformat_urdf/src/sdformat_urdf.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include @@ -48,7 +48,7 @@ urdf::JointSharedPtr convert_joint(const sdf::Joint & sdf_joint, sdf::Errors & errors); urdf::Pose -convert_pose(const ignition::math::Pose3d & sdf_pose); +convert_pose(const gz::math::Pose3d & sdf_pose); urdf::GeometrySharedPtr convert_geometry(const sdf::Geometry & sdf_geometry, sdf::Errors & errors); @@ -93,7 +93,7 @@ sdformat_urdf::convert_model(const sdf::Model & sdf_model, sdf::Errors & errors) // A model's pose is the location of the model in a larger context, like a world or parent model // It doesn't make sense in the context of a robot description. - if ("" != sdf_model.PoseRelativeTo() || ignition::math::Pose3d{} != sdf_model.RawPose()) { + if ("" != sdf_model.PoseRelativeTo() || gz::math::Pose3d{} != sdf_model.RawPose()) { errors.emplace_back( sdf::ErrorCode::STRING_READ, " tags with are not currently supported by sdformat_urdf"); @@ -115,7 +115,7 @@ sdformat_urdf::convert_model(const sdf::Model & sdf_model, sdf::Errors & errors) std::string relative_joint_name{""}; for (uint64_t j = 0; j < sdf_model.JointCount(); ++j) { const sdf::Joint * sdf_joint = sdf_model.JointByIndex(j); - if (sdf_joint && sdf_joint->ChildLinkName() == sdf_link->Name()) { + if (sdf_joint && sdf_joint->ChildName() == sdf_link->Name()) { relative_joint_name = sdf_joint->Name(); break; } @@ -212,7 +212,7 @@ sdformat_urdf::convert_model(const sdf::Model & sdf_model, sdf::Errors & errors) // Fix poses and check for tree structure issues while (joint_iter != joints_to_visit.end()) { const sdf::Joint * sdf_joint = *joint_iter; - if (sdf_joint->ParentLinkName() == sdf_parent_link->Name()) { + if (sdf_joint->ParentName() == sdf_parent_link->Name()) { // Visited parent link of this joint - don't look at it again joint_iter = joints_to_visit.erase(joint_iter); @@ -227,7 +227,7 @@ sdformat_urdf::convert_model(const sdf::Model & sdf_model, sdf::Errors & errors) parent_frame_name = urdf_parent_link->parent_joint->name; } - ignition::math::Pose3d joint_pose; + gz::math::Pose3d joint_pose; sdf::Errors pose_errors = sdf_joint->SemanticPose().Resolve(joint_pose, parent_frame_name); if (!pose_errors.empty()) { errors.insert(errors.end(), pose_errors.begin(), pose_errors.end()); @@ -269,7 +269,7 @@ sdformat_urdf::convert_model(const sdf::Model & sdf_model, sdf::Errors & errors) // Explore this child link later link_stack.push_back(sdf_child_link); - } else if (sdf_joint->ChildLinkName() == sdf_parent_link->Name()) { + } else if (sdf_joint->ChildName() == sdf_parent_link->Name()) { // Something is wrong here if (sdf_parent_link == sdf_canonical_link) { // The canonical link can't be a child of a joint @@ -321,7 +321,7 @@ sdformat_urdf::convert_link( // joint to link in joint if this is not the root link, else identity // The pose of the root link does not matter because there is no equivalent in URDF - ignition::math::Pose3d link_pose; + gz::math::Pose3d link_pose; if (!relative_joint_name.empty()) { // urdf link pose is the location of the joint having it as a child sdf::Errors pose_errors = sdf_link.SemanticPose().Resolve(link_pose, relative_joint_name); @@ -334,7 +334,7 @@ sdformat_urdf::convert_link( } } - const ignition::math::Inertiald sdf_inertia = sdf_link.Inertial(); + const gz::math::Inertiald sdf_inertia = sdf_link.Inertial(); urdf_link->inertial = std::make_shared(); urdf_link->inertial->mass = sdf_inertia.MassMatrix().Mass(); // URDF doesn't have link pose concept, so add SDF link pose to inertial @@ -360,7 +360,7 @@ sdformat_urdf::convert_link( urdf_visual->name = sdf_visual->Name(); // URDF visual is relative to link origin - ignition::math::Pose3d visual_pose; + gz::math::Pose3d visual_pose; sdf::Errors pose_errors = sdf_visual->SemanticPose().Resolve(visual_pose, sdf_link.Name()); if (!pose_errors.empty()) { errors.insert(errors.end(), pose_errors.begin(), pose_errors.end()); @@ -431,7 +431,7 @@ sdformat_urdf::convert_link( urdf_collision->name = sdf_collision->Name(); // URDF collision is relative to link origin - ignition::math::Pose3d collision_pose; + gz::math::Pose3d collision_pose; sdf::Errors pose_errors = sdf_collision->SemanticPose().Resolve(collision_pose, sdf_link.Name()); if (!pose_errors.empty()) { @@ -520,7 +520,7 @@ sdformat_urdf::convert_joint(const sdf::Joint & sdf_joint, sdf::Errors & errors) } // URDF expects axis to be expressed in the joint frame - ignition::math::Vector3d axis_xyz; + gz::math::Vector3d axis_xyz; sdf::Errors axis_errors = sdf_axis->ResolveXyz(axis_xyz, sdf_joint.Name()); if (!axis_errors.empty()) { errors.insert(errors.end(), axis_errors.begin(), axis_errors.end()); @@ -573,14 +573,14 @@ sdformat_urdf::convert_joint(const sdf::Joint & sdf_joint, sdf::Errors & errors) } } - urdf_joint->child_link_name = sdf_joint.ChildLinkName(); - urdf_joint->parent_link_name = sdf_joint.ParentLinkName(); + urdf_joint->child_link_name = sdf_joint.ChildName(); + urdf_joint->parent_link_name = sdf_joint.ParentName(); return urdf_joint; } urdf::Pose -sdformat_urdf::convert_pose(const ignition::math::Pose3d & sdf_pose) +sdformat_urdf::convert_pose(const gz::math::Pose3d & sdf_pose) { urdf::Pose pose; pose.position.x = sdf_pose.Pos().X(); diff --git a/sdformat_urdf/test/include/test_tools.hpp b/sdformat_urdf/test/include/test_tools.hpp index f0269a6..0afc933 100644 --- a/sdformat_urdf/test/include/test_tools.hpp +++ b/sdformat_urdf/test/include/test_tools.hpp @@ -34,7 +34,7 @@ get_file(const char * path) #define EXPECT_POSE(expected_ign, actual_urdf) \ do { \ - const auto actual_ign = ignition::math::Pose3d{ \ + const auto actual_ign = gz::math::Pose3d{ \ actual_urdf.position.x, \ actual_urdf.position.y, \ actual_urdf.position.z, \ diff --git a/sdformat_urdf/test/joint_tests.cpp b/sdformat_urdf/test/joint_tests.cpp index 303f65b..8b97dc3 100644 --- a/sdformat_urdf/test/joint_tests.cpp +++ b/sdformat_urdf/test/joint_tests.cpp @@ -17,8 +17,8 @@ #include #include -#include -#include +#include +#include #include #include "sdf_paths.hpp" @@ -203,8 +203,8 @@ TEST(Joint, joint_revolute_axis) urdf::JointConstSharedPtr joint = model->getJoint("joint_revolute"); ASSERT_NE(nullptr, joint); - const ignition::math::Vector3d expected_axis{0.1, 1.23, 4.567}; - const ignition::math::Vector3d actual_axis{joint->axis.x, joint->axis.y, joint->axis.z}; + const gz::math::Vector3d expected_axis{0.1, 1.23, 4.567}; + const gz::math::Vector3d actual_axis{joint->axis.x, joint->axis.y, joint->axis.z}; EXPECT_EQ("joint_revolute", joint->name); EXPECT_EQ(urdf::Joint::REVOLUTE, joint->type); @@ -223,19 +223,19 @@ TEST(Joint, joint_revolute_axis_in_frame) urdf::JointConstSharedPtr joint = model->getJoint("joint_revolute"); ASSERT_NE(nullptr, joint); - const ignition::math::Pose3d model_to_frame_in_model{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; - const ignition::math::Pose3d model_to_child_in_model{0.1, 0, 0.1, 0, 0, 0}; - const ignition::math::Pose3d frame_to_child_in_frame = + const gz::math::Pose3d model_to_frame_in_model{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; + const gz::math::Pose3d model_to_child_in_model{0.1, 0, 0.1, 0, 0, 0}; + const gz::math::Pose3d frame_to_child_in_frame = model_to_frame_in_model.Inverse() * model_to_child_in_model; - const ignition::math::Pose3d child_to_joint_in_child{0, 0, 0, 0, 0, 0}; - const ignition::math::Pose3d frame_to_joint_in_frame = + const gz::math::Pose3d child_to_joint_in_child{0, 0, 0, 0, 0, 0}; + const gz::math::Pose3d frame_to_joint_in_frame = frame_to_child_in_frame * child_to_joint_in_child; - const ignition::math::Vector3d axis_in_frame{0.1, 1.23, 4.567}; - const ignition::math::Vector3d axis_in_joint = + const gz::math::Vector3d axis_in_frame{0.1, 1.23, 4.567}; + const gz::math::Vector3d axis_in_joint = frame_to_joint_in_frame.Inverse().Rot().RotateVector(axis_in_frame); - const ignition::math::Vector3d actual_axis{joint->axis.x, joint->axis.y, joint->axis.z}; + const gz::math::Vector3d actual_axis{joint->axis.x, joint->axis.y, joint->axis.z}; EXPECT_EQ("joint_revolute", joint->name); EXPECT_EQ(urdf::Joint::REVOLUTE, joint->type); diff --git a/sdformat_urdf/test/material_tests.cpp b/sdformat_urdf/test/material_tests.cpp index 2133616..730c379 100644 --- a/sdformat_urdf/test/material_tests.cpp +++ b/sdformat_urdf/test/material_tests.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include "sdf_paths.hpp" @@ -38,9 +38,9 @@ TEST(Material, material_blinn_phong) urdf::VisualConstSharedPtr visual = link->visual; ASSERT_NE(nullptr, visual); - const ignition::math::Vector4d ambient{0.3, 0, 0, 1}; - const ignition::math::Vector4d diffuse{0, 0.3, 0, 1}; - const ignition::math::Vector4d expected_color = + const gz::math::Vector4d ambient{0.3, 0, 0, 1}; + const gz::math::Vector4d diffuse{0, 0.3, 0, 1}; + const gz::math::Vector4d expected_color = 0.4 * ambient + 0.8 * diffuse; EXPECT_EQ(link->name + visual->name, visual->material->name); diff --git a/sdformat_urdf/test/pose_tests.cpp b/sdformat_urdf/test/pose_tests.cpp index b92698e..df86427 100644 --- a/sdformat_urdf/test/pose_tests.cpp +++ b/sdformat_urdf/test/pose_tests.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include "sdf_paths.hpp" @@ -52,38 +52,38 @@ TEST(Pose, pose_chain) urdf::JointConstSharedPtr joint_3 = model->getJoint("joint_3"); ASSERT_NE(nullptr, joint_3); - const ignition::math::Pose3d model_to_link_1_in_model{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}; - const ignition::math::Pose3d model_to_link_2_in_model{0.2, 0.3, 0.4, 0.5, 0.6, 0.7}; - const ignition::math::Pose3d model_to_link_3_in_model{0.3, 0.4, 0.5, 0.6, 0.7, 0.8}; - const ignition::math::Pose3d model_to_link_4_in_model{0.4, 0.5, 0.6, 0.7, 0.8, 0.9}; - const ignition::math::Pose3d link_2_to_joint_1_in_link_2 {0.9, 0.8, 0.7, 0.6, 0.5, 0.4}; - const ignition::math::Pose3d link_3_to_joint_2_in_link_3{0.8, 0.7, 0.6, 0.5, 0.4, 0.3}; - const ignition::math::Pose3d link_4_to_joint_3_in_link_4{0.7, 0.6, 0.5, 0.4, 0.3, 0.2}; + const gz::math::Pose3d model_to_link_1_in_model{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}; + const gz::math::Pose3d model_to_link_2_in_model{0.2, 0.3, 0.4, 0.5, 0.6, 0.7}; + const gz::math::Pose3d model_to_link_3_in_model{0.3, 0.4, 0.5, 0.6, 0.7, 0.8}; + const gz::math::Pose3d model_to_link_4_in_model{0.4, 0.5, 0.6, 0.7, 0.8, 0.9}; + const gz::math::Pose3d link_2_to_joint_1_in_link_2 {0.9, 0.8, 0.7, 0.6, 0.5, 0.4}; + const gz::math::Pose3d link_3_to_joint_2_in_link_3{0.8, 0.7, 0.6, 0.5, 0.4, 0.3}; + const gz::math::Pose3d link_4_to_joint_3_in_link_4{0.7, 0.6, 0.5, 0.4, 0.3, 0.2}; - const ignition::math::Pose3d model_to_joint_1_in_model = + const gz::math::Pose3d model_to_joint_1_in_model = model_to_link_2_in_model * link_2_to_joint_1_in_link_2; - const ignition::math::Pose3d model_to_joint_2_in_model = + const gz::math::Pose3d model_to_joint_2_in_model = model_to_link_3_in_model * link_3_to_joint_2_in_link_3; - const ignition::math::Pose3d model_to_joint_3_in_model = + const gz::math::Pose3d model_to_joint_3_in_model = model_to_link_4_in_model * link_4_to_joint_3_in_link_4; - const ignition::math::Pose3d link_1_to_joint_1_in_link_1 = + const gz::math::Pose3d link_1_to_joint_1_in_link_1 = model_to_link_1_in_model.Inverse() * model_to_joint_1_in_model; - const ignition::math::Pose3d joint_1_to_link_2_in_joint_1 = + const gz::math::Pose3d joint_1_to_link_2_in_joint_1 = model_to_joint_1_in_model.Inverse() * model_to_link_2_in_model; - const ignition::math::Pose3d joint_2_to_link_3_in_joint_2 = + const gz::math::Pose3d joint_2_to_link_3_in_joint_2 = model_to_joint_2_in_model.Inverse() * model_to_link_3_in_model; - const ignition::math::Pose3d joint_3_to_link_4_in_joint_3 = + const gz::math::Pose3d joint_3_to_link_4_in_joint_3 = model_to_joint_3_in_model.Inverse() * model_to_link_4_in_model; - const ignition::math::Pose3d joint_1_to_joint_2_in_joint_1 = + const gz::math::Pose3d joint_1_to_joint_2_in_joint_1 = model_to_joint_1_in_model.Inverse() * model_to_joint_2_in_model; - const ignition::math::Pose3d joint_2_to_joint_3_in_joint_2 = + const gz::math::Pose3d joint_2_to_joint_3_in_joint_2 = model_to_joint_2_in_model.Inverse() * model_to_joint_3_in_model; - EXPECT_POSE(ignition::math::Pose3d::Zero, link_1->inertial->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link_1->visual->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link_1->collision->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link_1->inertial->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link_1->visual->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link_1->collision->origin); EXPECT_POSE(link_1_to_joint_1_in_link_1, joint_1->parent_to_joint_origin_transform); @@ -117,10 +117,10 @@ TEST(Pose, pose_collision) urdf::LinkConstSharedPtr link = model->getRoot(); ASSERT_NE(nullptr, link); - const ignition::math::Pose3d expected_collision_pose{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; + const gz::math::Pose3d expected_collision_pose{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; - EXPECT_POSE(ignition::math::Pose3d::Zero, link->inertial->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->visual->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->inertial->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->visual->origin); EXPECT_POSE(expected_collision_pose, link->collision->origin); } @@ -136,16 +136,16 @@ TEST(Pose, pose_collision_in_frame) urdf::LinkConstSharedPtr link = model->getRoot(); ASSERT_NE(nullptr, link); - const ignition::math::Pose3d model_to_frame_in_model{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; - const ignition::math::Pose3d model_to_link_in_model{0, 0, 0, 0, 0, 0}; - const ignition::math::Pose3d frame_to_link_in_frame = + const gz::math::Pose3d model_to_frame_in_model{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; + const gz::math::Pose3d model_to_link_in_model{0, 0, 0, 0, 0, 0}; + const gz::math::Pose3d frame_to_link_in_frame = model_to_frame_in_model.Inverse() * model_to_link_in_model; - const ignition::math::Pose3d frame_to_collision_in_frame{0.2, 0.4, 0.8, 0.2, 0.3, 0.4}; - const ignition::math::Pose3d link_to_collision_in_link = + const gz::math::Pose3d frame_to_collision_in_frame{0.2, 0.4, 0.8, 0.2, 0.3, 0.4}; + const gz::math::Pose3d link_to_collision_in_link = frame_to_link_in_frame.Inverse() * frame_to_collision_in_frame; - EXPECT_POSE(ignition::math::Pose3d::Zero, link->inertial->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->visual->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->inertial->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->visual->origin); EXPECT_POSE(link_to_collision_in_link, link->collision->origin); } @@ -161,11 +161,11 @@ TEST(Pose, pose_inertial) urdf::LinkConstSharedPtr link = model->getRoot(); ASSERT_NE(nullptr, link); - const ignition::math::Pose3d expected_inertial_pose{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; + const gz::math::Pose3d expected_inertial_pose{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; EXPECT_POSE(expected_inertial_pose, link->inertial->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->visual->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->collision->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->visual->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->collision->origin); } TEST(Pose, pose_inertial_in_frame) @@ -182,17 +182,17 @@ TEST(Pose, pose_inertial_in_frame) urdf::LinkConstSharedPtr link = model->getRoot(); ASSERT_NE(nullptr, link); - const ignition::math::Pose3d model_to_frame_in_model{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; - const ignition::math::Pose3d model_to_link_in_model{0, 0, 0, 0, 0, 0}; - const ignition::math::Pose3d frame_to_link_in_frame = + const gz::math::Pose3d model_to_frame_in_model{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; + const gz::math::Pose3d model_to_link_in_model{0, 0, 0, 0, 0, 0}; + const gz::math::Pose3d frame_to_link_in_frame = model_to_frame_in_model.Inverse() * model_to_link_in_model; - const ignition::math::Pose3d frame_to_inertial_in_frame{0.2, 0.4, 0.8, 0.2, 0.3, 0.4}; - const ignition::math::Pose3d link_to_inertial_in_link = + const gz::math::Pose3d frame_to_inertial_in_frame{0.2, 0.4, 0.8, 0.2, 0.3, 0.4}; + const gz::math::Pose3d link_to_inertial_in_link = frame_to_link_in_frame.Inverse() * frame_to_inertial_in_frame; EXPECT_POSE(link_to_inertial_in_link, link->inertial->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->visual->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->collision->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->visual->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->collision->origin); } TEST(Pose, pose_joint) @@ -211,15 +211,15 @@ TEST(Pose, pose_joint) // In URDF joint is in parent link frame // The child link in URDF lives in the joint frame - const ignition::math::Pose3d model_to_parent_in_model{0, 0, 0, 0, 0, 0}; - const ignition::math::Pose3d model_to_child_in_model{0, 0, 0, 0, 0, 0}; - const ignition::math::Pose3d child_to_joint_in_child{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; + const gz::math::Pose3d model_to_parent_in_model{0, 0, 0, 0, 0, 0}; + const gz::math::Pose3d model_to_child_in_model{0, 0, 0, 0, 0, 0}; + const gz::math::Pose3d child_to_joint_in_child{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; - const ignition::math::Pose3d parent_to_child_in_parent = + const gz::math::Pose3d parent_to_child_in_parent = model_to_parent_in_model.Inverse() * model_to_child_in_model; - const ignition::math::Pose3d parent_to_joint_in_parent = + const gz::math::Pose3d parent_to_joint_in_parent = parent_to_child_in_parent * child_to_joint_in_child; - const ignition::math::Pose3d joint_to_child_in_joint = child_to_joint_in_child.Inverse(); + const gz::math::Pose3d joint_to_child_in_joint = child_to_joint_in_child.Inverse(); EXPECT_POSE(parent_to_joint_in_parent, joint->parent_to_joint_origin_transform); @@ -250,31 +250,31 @@ TEST(Pose, pose_joint_all) urdf::JointConstSharedPtr joint = model->getJoint("joint"); ASSERT_NE(nullptr, joint); - const ignition::math::Pose3d model_to_link_1_in_model{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}; - const ignition::math::Pose3d model_to_link_2_in_model{0.5, 0.6, 0.7, 0.8, 0.9, 1.0}; - const ignition::math::Pose3d link_2_to_joint_in_link_2{0.9, 1.0, 1.1, 1.2, 1.3, 1.4}; + const gz::math::Pose3d model_to_link_1_in_model{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}; + const gz::math::Pose3d model_to_link_2_in_model{0.5, 0.6, 0.7, 0.8, 0.9, 1.0}; + const gz::math::Pose3d link_2_to_joint_in_link_2{0.9, 1.0, 1.1, 1.2, 1.3, 1.4}; - const ignition::math::Pose3d model_to_joint_in_model = + const gz::math::Pose3d model_to_joint_in_model = model_to_link_2_in_model * link_2_to_joint_in_link_2; - const ignition::math::Pose3d link_1_to_joint_in_link_1 = + const gz::math::Pose3d link_1_to_joint_in_link_1 = model_to_link_1_in_model.Inverse() * model_to_joint_in_model; - const ignition::math::Pose3d joint_to_link_2_in_joint = + const gz::math::Pose3d joint_to_link_2_in_joint = model_to_joint_in_model.Inverse() * model_to_link_2_in_model; - const ignition::math::Pose3d link_1_to_visual_in_link_1{0.2, 0.3, 0.4, 0.5, 0.6, 0.7}; - const ignition::math::Pose3d link_1_to_collision_in_link_1{0.3, 0.4, 0.5, 0.6, 0.7, 0.8}; - const ignition::math::Pose3d link_1_to_inertial_in_link_1{0.4, 0.5, 0.6, 0.7, 0.8, 0.9}; + const gz::math::Pose3d link_1_to_visual_in_link_1{0.2, 0.3, 0.4, 0.5, 0.6, 0.7}; + const gz::math::Pose3d link_1_to_collision_in_link_1{0.3, 0.4, 0.5, 0.6, 0.7, 0.8}; + const gz::math::Pose3d link_1_to_inertial_in_link_1{0.4, 0.5, 0.6, 0.7, 0.8, 0.9}; - const ignition::math::Pose3d link_2_to_visual_in_link_2{0.6, 0.7, 0.8, 0.9, 1.0, 1.1}; - const ignition::math::Pose3d link_2_to_collision_in_link_2{0.7, 0.8, 0.9, 1.0, 1.1, 1.2}; - const ignition::math::Pose3d link_2_to_inertial_in_link_2{0.8, 0.9, 1.0, 1.1, 1.2, 1.3}; + const gz::math::Pose3d link_2_to_visual_in_link_2{0.6, 0.7, 0.8, 0.9, 1.0, 1.1}; + const gz::math::Pose3d link_2_to_collision_in_link_2{0.7, 0.8, 0.9, 1.0, 1.1, 1.2}; + const gz::math::Pose3d link_2_to_inertial_in_link_2{0.8, 0.9, 1.0, 1.1, 1.2, 1.3}; - const ignition::math::Pose3d joint_to_visual_in_joint = + const gz::math::Pose3d joint_to_visual_in_joint = joint_to_link_2_in_joint * link_2_to_visual_in_link_2; - const ignition::math::Pose3d joint_to_collision_in_joint = + const gz::math::Pose3d joint_to_collision_in_joint = joint_to_link_2_in_joint * link_2_to_collision_in_link_2; - const ignition::math::Pose3d joint_to_inertial_in_joint = + const gz::math::Pose3d joint_to_inertial_in_joint = joint_to_link_2_in_joint * link_2_to_inertial_in_link_2; EXPECT_POSE(link_1_to_visual_in_link_1, link_1->visual->origin); @@ -304,16 +304,16 @@ TEST(Pose, pose_joint_in_frame) // In URDF joint is in parent link frame // The child link in URDF lives in the joint frame - const ignition::math::Pose3d model_to_parent_in_model{0, 0, 0, 0, 0, 0}; - const ignition::math::Pose3d model_to_child_in_model{0, 0, 0, 0, 0, 0}; - const ignition::math::Pose3d model_to_frame_in_model{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; - const ignition::math::Pose3d frame_to_joint_in_frame{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; + const gz::math::Pose3d model_to_parent_in_model{0, 0, 0, 0, 0, 0}; + const gz::math::Pose3d model_to_child_in_model{0, 0, 0, 0, 0, 0}; + const gz::math::Pose3d model_to_frame_in_model{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; + const gz::math::Pose3d frame_to_joint_in_frame{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; - const ignition::math::Pose3d model_to_joint_in_model = + const gz::math::Pose3d model_to_joint_in_model = model_to_frame_in_model * frame_to_joint_in_frame; - const ignition::math::Pose3d parent_to_joint_in_parent = + const gz::math::Pose3d parent_to_joint_in_parent = model_to_parent_in_model.Inverse() * model_to_joint_in_model; - const ignition::math::Pose3d joint_to_child_in_joint = + const gz::math::Pose3d joint_to_child_in_joint = model_to_joint_in_model.Inverse() * model_to_child_in_model; EXPECT_POSE(parent_to_joint_in_parent, joint->parent_to_joint_origin_transform); @@ -339,9 +339,9 @@ TEST(Pose, pose_link) // URDF link C++ structure does not have an origin - root link members should be unaffected // by root link pose - EXPECT_POSE(ignition::math::Pose3d::Zero, link->inertial->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->visual->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->collision->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->inertial->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->visual->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->collision->origin); } TEST(Pose, pose_link_all) @@ -358,9 +358,9 @@ TEST(Pose, pose_link_all) // URDF link C++ structure does not have an origin - root link members should be unaffected // by root link pose - const ignition::math::Pose3d link_to_inertial_in_link{0.05, 0.1, 0.2, 0.4, 0.5, 0.6}; - const ignition::math::Pose3d link_to_collision_in_link{0.04, 0.8, 0.16, 0.3, 0.4, 0.5}; - const ignition::math::Pose3d link_to_visual_in_link{0.03, 0.6, 0.12, 0.2, 0.3, 0.4}; + const gz::math::Pose3d link_to_inertial_in_link{0.05, 0.1, 0.2, 0.4, 0.5, 0.6}; + const gz::math::Pose3d link_to_collision_in_link{0.04, 0.8, 0.16, 0.3, 0.4, 0.5}; + const gz::math::Pose3d link_to_visual_in_link{0.03, 0.6, 0.12, 0.2, 0.3, 0.4}; EXPECT_POSE(link_to_inertial_in_link, link->inertial->origin); EXPECT_POSE(link_to_visual_in_link, link->visual->origin); @@ -381,9 +381,9 @@ TEST(Pose, pose_link_in_frame) // URDF link C++ structure does not have an origin - root link members should be unaffected // by root link pose - EXPECT_POSE(ignition::math::Pose3d::Zero, link->inertial->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->visual->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->collision->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->inertial->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->visual->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->collision->origin); } TEST(Pose, pose_model) @@ -408,11 +408,11 @@ TEST(Pose, pose_visual) urdf::LinkConstSharedPtr link = model->getRoot(); ASSERT_NE(nullptr, link); - const ignition::math::Pose3d expected_visual_pose{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; + const gz::math::Pose3d expected_visual_pose{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; - EXPECT_POSE(ignition::math::Pose3d::Zero, link->inertial->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->inertial->origin); EXPECT_POSE(expected_visual_pose, link->visual->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->collision->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->collision->origin); } TEST(Pose, pose_visual_in_frame) @@ -427,15 +427,15 @@ TEST(Pose, pose_visual_in_frame) urdf::LinkConstSharedPtr link = model->getRoot(); ASSERT_NE(nullptr, link); - const ignition::math::Pose3d model_to_frame_in_model{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; - const ignition::math::Pose3d model_to_link_in_model{0, 0, 0, 0, 0, 0}; - const ignition::math::Pose3d frame_to_link_in_frame = + const gz::math::Pose3d model_to_frame_in_model{0.05, 0.1, 0.2, 0.1, 0.2, 0.3}; + const gz::math::Pose3d model_to_link_in_model{0, 0, 0, 0, 0, 0}; + const gz::math::Pose3d frame_to_link_in_frame = model_to_frame_in_model.Inverse() * model_to_link_in_model; - const ignition::math::Pose3d frame_to_visual_in_frame{0.2, 0.4, 0.8, 0.2, 0.3, 0.4}; - const ignition::math::Pose3d link_to_visual_in_link = + const gz::math::Pose3d frame_to_visual_in_frame{0.2, 0.4, 0.8, 0.2, 0.3, 0.4}; + const gz::math::Pose3d link_to_visual_in_link = frame_to_link_in_frame.Inverse() * frame_to_visual_in_frame; - EXPECT_POSE(ignition::math::Pose3d::Zero, link->inertial->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->inertial->origin); EXPECT_POSE(link_to_visual_in_link, link->visual->origin); - EXPECT_POSE(ignition::math::Pose3d::Zero, link->collision->origin); + EXPECT_POSE(gz::math::Pose3d::Zero, link->collision->origin); }