Merge pull request #1 from borglab/fix-build-2

Upgrade to sdformat15

Author: havriutkin

.github/workflows/linux-ci.yml

@@ -54,7 +54,7 @@ jobs:
           sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743
           sudo apt-get -y update
 
-          sudo apt-get -y install libtbb-dev libboost-all-dev libsdformat13-dev
+          sudo apt-get -y install libtbb-dev libboost-all-dev libsdformat15-dev
 
           # Install CppUnitLite.
           git clone https://github.com/borglab/CppUnitLite.git

.github/workflows/macos-ci.yml

@@ -44,7 +44,7 @@ jobs:
         run: |
           brew install boost
           brew tap osrf/simulation
-          brew install sdformat13
+          brew install sdformat15
           brew tap borglab/core
 
       - name: GTSAM

.github/workflows/python-ci.yml

@@ -54,7 +54,7 @@ jobs:
           sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
           sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743
           sudo apt-get -y update
-          sudo apt-get -y install libtbb-dev libboost-all-dev libsdformat13-dev
+          sudo apt-get -y install libtbb-dev libboost-all-dev libsdformat15-dev
 
       - name: Install System Dependencies (macOS)
         if: runner.os == 'macOS'
@@ -70,7 +70,7 @@ jobs:
           fi
           brew install boost
           brew tap osrf/simulation
-          brew install sdformat13
+          brew install sdformat15
 
       - name: Python Dependencies (Linux)
         if: runner.os == 'Linux'

CMakeLists.txt

@@ -46,7 +46,7 @@ endif()
 set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${GTSAM_DIR}/../GTSAMCMakeTools")
 
 # For parsing urdf/sdf files. # TODO: hardcoded sdformat version
-set(SDFormat_VERSION 13)
+set(SDFormat_VERSION 15)
 find_package(sdformat${SDFormat_VERSION} REQUIRED)
 
 # ##############################################################################

README.md

@@ -13,7 +13,7 @@ GTDynamics is a library that allows the user to express the full kinodynamics co
 
 * [GTSAM4](https://github.com/borglab/gtsam)
 * [gtwrap](https://github.com/borglab/wrap)
-* [sdformat13](https://github.com/osrf/sdformat)
+* [sdformat15](https://github.com/osrf/sdformat)
 
 ## Installing SDFormat
 
@@ -26,9 +26,9 @@ Using Homebrew is the easiest way to get SDFormat installed and it also makes sw
 ```sh
 $ # Install homebrew.
 $ /bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)"
-$ # Set up the tap ind install sdformat13
+$ # Set up the tap and install sdformat15
 $ brew tap osrf/simulation
-$ brew install sdformat13
+$ brew install sdformat15
 ```
 
 ### Source
@@ -48,10 +48,10 @@ wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
 sudo apt-get update
 
 # Install SDFormat dependencies
-sudo apt-get install -y libtinyxml2-dev liburdfdom-dev libignition-cmake2-dev libignition-tools-dev libignition-math6-dev
+sudo apt-get install -y libtinyxml2-dev liburdfdom-dev libgz-cmake2-dev libgz-tools-dev libgz-math6-dev
 
 # Set the version to install
-export GTD_SDFormat_VERSION="13.0.0"
+export GTD_SDFormat_VERSION="15.0.0"
 
 # Download specific version of SDFormat
 wget http://osrf-distributions.s3.amazonaws.com/sdformat/releases/sdformat-${GTD_SDFormat_VERSION}.tar.bz2

docker/Dockerfile

@@ -10,17 +10,17 @@ WORKDIR /usr/src/
 # Make sure we have latest packages
 RUN apt-get -y update
 
-# Install basic dependencies for sdfformat
+# Install basic dependencies for sdformat
 RUN apt-get install -y ruby-dev build-essential libboost-all-dev cmake pkg-config wget lsb-release
 
 # Get Gazebo packages
 RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
 RUN wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -
 RUN apt-get update
-RUN apt-get install -y libtinyxml2-dev liburdfdom-dev libignition-cmake2-dev libignition-tools-dev libignition-math6-dev python3-psutil
+RUN apt-get install -y libtinyxml2-dev liburdfdom-dev libgz-cmake2-dev libgz-tools-dev libgz-math6-dev python3-psutil
 
 # Get sdfformat package
-ENV GTD_SDFormat_VERSION="10.5.0"
+ENV GTD_SDFormat_VERSION="15.0.0"
 RUN wget http://osrf-distributions.s3.amazonaws.com/sdformat/releases/sdformat-${GTD_SDFormat_VERSION}.tar.bz2
 RUN tar -xvjf sdformat-${GTD_SDFormat_VERSION}.tar.bz2
 RUN rm sdformat-${GTD_SDFormat_VERSION}.tar.bz2

examples/cmake_project_example/CMakeLists.txt

@@ -8,7 +8,7 @@ find_package(GTSAM REQUIRED)
 find_path(gtdynamics_include gtdynamics)
 find_library(gtdynamics_lib gtdynamics)
 # these next 3 lines are temporary until we create a proper cmake module for gtdynamics.
-set(SDFormat_VERSION 12)
+set(SDFormat_VERSION 15)
 find_package(sdformat${SDFormat_VERSION} REQUIRED)
 list(APPEND gtdynamics_lib ${SDFormat_LIBRARIES})
 

gtdynamics/universal_robot/sdf.cpp

@@ -78,9 +78,9 @@ sdf::Model GetSdf(const std::string &sdf_file_path,
   throw std::runtime_error("Model not found in: " + sdf_file_path);
 }
 
-gtsam::Pose3 Pose3FromIgnition(const ignition::math::Pose3d &ignition_pose) {
-  const auto &rot = ignition_pose.Rot();
-  const auto &pos = ignition_pose.Pos();
+gtsam::Pose3 Pose3FromGazebo(const gz::math::Pose3d &gazebo_pose) {
+  const auto &rot = gazebo_pose.Rot();
+  const auto &pos = gazebo_pose.Pos();
   return gtsam::Pose3(
       gtsam::Rot3(gtsam::Quaternion(rot.W(), rot.X(), rot.Y(), rot.Z())),
       gtsam::Point3(pos[0], pos[1], pos[2]));
@@ -115,7 +115,7 @@ Pose3 GetSdfLinkFrame(const sdf::Link *sdf_link) {
   }
 
   // Pose is updated from joint frame to base frame.
-  const Pose3 bMl = Pose3FromIgnition(raw_pose);
+  const Pose3 bMl = Pose3FromGazebo(raw_pose);
   return bMl;
 }
 
@@ -128,7 +128,7 @@ Pose3 GetJointFrame(const sdf::Joint &sdf_joint,
 
   // Get the pose of the joint in the parent or child link's frame depending on
   // the value of `frame_name`.
-  Pose3 lTj = Pose3FromIgnition(sdf_joint.RawPose());
+  Pose3 lTj = Pose3FromGazebo(sdf_joint.RawPose());
 
   if (frame_name.empty() || frame_name == child_sdf_link->Name()) {
     // If `frame_name` is empty or has the same name as the child_link, it means
@@ -175,12 +175,12 @@ LinkSharedPtr LinkFromSdf(uint8_t id, const sdf::Link &sdf_link) {
   // Get the pose of the link in the base frame
   // we only save the bMcom rest matrix as part of the Link class
   const Pose3 bMl = GetSdfLinkFrame(&sdf_link);
-  const Pose3 lMcom = Pose3FromIgnition(sdf_link.Inertial().Pose());
+  const Pose3 lMcom = Pose3FromGazebo(sdf_link.Inertial().Pose());
   const Pose3 bMcom = bMl * lMcom;
 
   return std::make_shared<Link>(id, sdf_link.Name(),
-                                  sdf_link.Inertial().MassMatrix().Mass(),
-                                  inertia, bMcom, bMl);
+                                sdf_link.Inertial().MassMatrix().Mass(),
+                                inertia, bMcom, bMl);
 }
 
 LinkSharedPtr LinkFromSdf(uint8_t id, const std::string &link_name,
@@ -214,20 +214,20 @@ JointSharedPtr JointFromSdf(uint8_t id, const LinkSharedPtr &parent_link,
   switch (sdf_joint.Type()) {
     case sdf::JointType::PRISMATIC:
       joint = std::make_shared<PrismaticJoint>(id, name, bMj, parent_link,
-                                                 child_link, axis, parameters);
+                                               child_link, axis, parameters);
       break;
     case sdf::JointType::REVOLUTE:
       joint = std::make_shared<RevoluteJoint>(id, name, bMj, parent_link,
-                                                child_link, axis, parameters);
+                                              child_link, axis, parameters);
       break;
     case sdf::JointType::SCREW:
       joint = std::make_shared<HelicalJoint>(
           id, name, bMj, parent_link, child_link, axis, sdf_joint.ThreadPitch(),
           parameters);
       break;
     case sdf::JointType::FIXED:
-      joint = std::make_shared<FixedJoint>(id, name, bMj, parent_link,
-                                             child_link);
+      joint =
+          std::make_shared<FixedJoint>(id, name, bMj, parent_link, child_link);
       break;
     default:
       throw std::runtime_error("Joint type for [" + name +
@@ -256,8 +256,8 @@ static LinkJointPair ExtractRobotFromSdf(const sdf::Model &sdf) {
     sdf::Joint sdf_joint = *sdf.JointByIndex(j);
 
     // Get this joint's parent and child links.
-    std::string parent_link_name = sdf_joint.ParentLinkName();
-    std::string child_link_name = sdf_joint.ChildLinkName();
+    std::string parent_link_name = sdf_joint.ParentName();
+    std::string child_link_name = sdf_joint.ChildName();
     if (parent_link_name == "world") {
       // This joint fixes the child link in the world frame.
       LinkSharedPtr child_link = name_to_link[child_link_name];

gtdynamics/universal_robot/sdf_internal.h

@@ -15,7 +15,7 @@
 
 #include <gtdynamics/universal_robot/Robot.h>
 
-#include <ignition/math/Pose3.hh>
+#include <gz/math/Pose3.hh>
 #include <sdf/sdf.hh>
 #include <string>
 
@@ -64,11 +64,11 @@ JointSharedPtr JointFromSdf(uint8_t id, const LinkSharedPtr &parent_link,
                             const sdf::Joint &sdf_joint);
 
 /**
- * Parse a ignition::math Pose object into a gtsam::Pose.
+ * Parse a gz::math Pose object into a gtsam::Pose.
  *
- * @param ignition_pose An ignition::math::Pose object to be parsed.
+ * @param gazebo_pose An gz::math::Pose object to be parsed.
  */
-gtsam::Pose3 Pose3FromIgnition(const ignition::math::Pose3d &ignition_pose);
+gtsam::Pose3 Pose3FromGazebo(const gz::math::Pose3d &gazebo_pose);
 
 /**
  * @fn Extract joint parameter values from an input sdf::Joint.

tests/testSdf.cpp

@@ -80,10 +80,10 @@ TEST(Sdf, load_and_parse_sdf_world_file) {
   EXPECT(assert_equal(0.03, l1.Inertial().Moi()(2, 2)));
 }
 
-TEST(Sdf, Pose3FromIgnition) {
-  ignition::math::Pose3d pose_to_parse(-1, 1, -1, M_PI / 2, 0, -M_PI);
+TEST(Sdf, Pose3FromGazebo) {
+  gz::math::Pose3d pose_to_parse(-1, 1, -1, M_PI / 2, 0, -M_PI);
 
-  gtsam::Pose3 parsed_pose = Pose3FromIgnition(pose_to_parse);
+  gtsam::Pose3 parsed_pose = Pose3FromGazebo(pose_to_parse);
 
   EXPECT(assert_equal(gtsam::Pose3(gtsam::Rot3::RzRyRx(M_PI / 2, 0, -M_PI),
                                    gtsam::Point3(-1, 1, -1)),