Skip to content

Commit

Permalink
ign->gz
Browse files Browse the repository at this point in the history
Signed-off-by: Dharini Dutia <[email protected]>
  • Loading branch information
quarkytale committed Sep 7, 2022
1 parent ddf83ed commit 548fa58
Show file tree
Hide file tree
Showing 5 changed files with 119 additions and 119 deletions.
30 changes: 15 additions & 15 deletions sdformat_urdf/src/sdformat_urdf.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
#include <string>
#include <vector>

#include <ignition/math/Pose3.hh>
#include <gz/math/Pose3.hh>
#include <sdf/Error.hh>
#include <sdf/Collision.hh>
#include <sdf/Geometry.hh>
Expand All @@ -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);
Expand Down Expand Up @@ -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,
"<model> tags with <pose> are not currently supported by sdformat_urdf");
Expand All @@ -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;
}
Expand Down Expand Up @@ -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);

Expand All @@ -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());
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand All @@ -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::Inertial>();
urdf_link->inertial->mass = sdf_inertia.MassMatrix().Mass();
// URDF doesn't have link pose concept, so add SDF link pose to inertial
Expand All @@ -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());
Expand Down Expand Up @@ -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()) {
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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();
Expand Down
2 changes: 1 addition & 1 deletion sdformat_urdf/test/include/test_tools.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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, \
Expand Down
24 changes: 12 additions & 12 deletions sdformat_urdf/test/joint_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@
#include <urdf_model/types.h>
#include <sdformat_urdf/sdformat_urdf.hpp>

#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector3.hh>
#include <gz/math/Pose3.hh>
#include <gz/math/Vector3.hh>
#include <sdf/Types.hh>

#include "sdf_paths.hpp"
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down
8 changes: 4 additions & 4 deletions sdformat_urdf/test/material_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
#include <urdf_model/types.h>
#include <sdformat_urdf/sdformat_urdf.hpp>

#include <ignition/math/Vector4.hh>
#include <gz/math/Vector4.hh>
#include <sdf/Types.hh>

#include "sdf_paths.hpp"
Expand All @@ -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);
Expand Down
Loading

0 comments on commit 548fa58

Please sign in to comment.