SDF parsing (RobotLocomotion#7455)

* Drafting parser code

* Parses links

* Moves FrameCache to SDFParser

* Adds joint parsing

* Moves FrameCache code into drake/multibody/parsing/sdf

* Refactors namespaces

* Move files into multibody/parsing

* Removes sdf directory

* Removes ::sdf:: namespace

* Adds doc+<inertia> test

* Adds doc. Removes lint

* Adds more doc

* Removes more lint

* Removes unused model

* Addresses reviews

* Replaces parser object by free methods + Removes sdf.hh header from sdf_parser.h

* Removes unused headers + add TODO

* SDF --> Sdf

* SDF --> Sdf in comments

* Updates frame's nomenclature for clarity and to keep it consistent with SDF specs

* Addresses reviews

* Addresses reviews

* Addresses reviews

drake/multibody/parsing/BUILD.bazel

@@ -5,6 +5,7 @@ load(
     "drake_cc_googletest",
     "drake_cc_library",
 )
+load("//tools/install:install_data.bzl", "install_data")
 load("//tools/lint:lint.bzl", "add_lint_tests")
 
 package(
@@ -25,6 +26,37 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "sdf_spec",
+    srcs = [],
+    hdrs = [
+        "sdf_joint.h",
+        "sdf_link.h",
+        "sdf_model.h",
+        "sdf_spec.h",
+    ],
+    deps = [
+        "//drake/common:essential",
+    ],
+)
+
+drake_cc_library(
+    name = "sdf_parser",
+    srcs = [
+        "sdf_parser.cc",
+    ],
+    hdrs = [
+        "sdf_parser.h",
+    ],
+    deps = [
+        ":frame_cache",
+        ":sdf_spec",
+        "//drake/common:essential",
+        "@eigen",
+        "@sdformat",
+    ],
+)
+
 # === test/ ===
 
 drake_cc_googletest(
@@ -34,4 +66,18 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "sdf_parser_test",
+    data = [
+        ":test/models",
+    ],
+    deps = [
+        ":sdf_parser",
+        "//drake/common:find_resource",
+        "//drake/common/test_utilities:eigen_matrix_compare",
+    ],
+)
+
+install_data()
+
 add_lint_tests()

drake/multibody/parsing/frame_cache.h

@@ -42,7 +42,7 @@ struct FramedIsometry3 {
 template <typename T>
 class FrameCache {
  public:
-  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FrameCache);
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(FrameCache);
   /// Constructor that takes the @p root_frame.
   explicit FrameCache(const std::string& root_frame);
 
@@ -77,7 +77,8 @@ class FrameCache {
   Isometry3<T> RootTransform(std::string frame) const;
 
   // Name of the root frame of this cache.
-  const std::string root_frame_;
+  std::string root_frame_;
+
   // Map to keep all known frames' transforms.
   std::map<std::string, FramedIsometry3<T>> X_TS_cache_;
 };

drake/multibody/parsing/sdf_joint.h

@@ -0,0 +1,121 @@
+#pragma once
+
+#include <limits>
+#include <string>
+
+#include "drake/common/drake_assert.h"
+#include "drake/common/drake_copyable.h"
+#include "drake/common/eigen_types.h"
+
+namespace drake {
+namespace multibody {
+namespace parsing {
+
+/// A representation of a `<joint>` element in an SDF file.
+/// A joint connects two `<link>` elements imposing a kinematic relationship
+/// between them. The particulars of this kinematic relationship are determined
+/// by the joint's **type** attribute. Examples of joints's types include;
+/// revolute, prismatic, ball, among others.
+/// Throughout this documentation, we denote the frames of the parent and child
+/// links with P and C, respectively.
+/// For details on the specification of joints, including conventions and
+/// default values, please refer to the documentation for the
+/// <a href="http://sdformat.org/spec?ver=1.6&elem=joint">
+/// &lt;joint&gt; element</a>.
+///
+/// <h3> Definition of the Joint Frame J </h3>
+///
+/// The SDF specification defines a _joint frame_, which in this documentation
+/// we will refer to with the letter J. Per SDF specification version 1.4, the
+/// pose of this frame J is defined relative to the joint's child link's
+/// frame C, see the documentation for the
+/// <a href="http://sdformat.org/spec?ver=1.4&elem=joint#joint_pose">
+/// &lt;joint&gt;/&lt;pose&gt; element</a>.
+class SdfJoint {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(SdfJoint);
+
+  /// Creates a new joint object specification with the given `joint_name`.
+  /// Per SDF specification, `joint_name` must be unique within the scope of the
+  /// joint's model. Uniqueness is **not** enforced by %SdfJoint.
+  ///
+  /// @param[in] joint_name
+  ///   The name of this joint as specified in the
+  ///   <a href="http://sdformat.org/spec?ver=1.6&elem=joint#joint_name">
+  ///   &lt;name&gt; element</a> documentation.
+  /// @param[in] parent_link_name
+  ///   The name of the parent link as defined in the
+  ///   <a href="http://sdformat.org/spec?ver=1.6&elem=joint#joint_parent">
+  ///   &lt;parent&gt; element</a> documentation.
+  /// @param[in] child_link_name
+  ///   The name of the parent link as defined in the
+  ///   <a href="http://sdformat.org/spec?ver=1.6&elem=joint#joint_child">
+  ///   &lt;child&gt; element</a> documentation.
+  /// @param[in] joint_type
+  ///   The type of the joint. E.g: revolute, prismatic. See documentation on
+  ///   the <a href="http://sdformat.org/spec?ver=1.6&elem=joint#joint_type">
+  ///   &lt;type&gt; element</a> for details.
+  SdfJoint(
+      const std::string& joint_name,
+      const std::string& parent_link_name,
+      const std::string& child_link_name,
+      const std::string& joint_type)
+      : name_(joint_name),
+        parent_link_name_(parent_link_name),
+        child_link_name_(child_link_name),
+        joint_type_(joint_type) {}
+
+  /// Returns the name of `this` joint.
+  const std::string& name() const { return name_; }
+
+  /// Returns the name of `this` joint's parent link.
+  const std::string& parent_link() const { return parent_link_name_; }
+
+  /// Returns the name of `this` joint's child link.
+  const std::string& child_link() const { return child_link_name_; }
+
+  /// Returns the name of the type of `this` joint.
+  const std::string& joint_type() const { return joint_type_; }
+
+  /// Returns `true` if `this` joint type has an "axis" property, as it is the
+  /// case for revolute and prismatic joint types.
+  bool JointHasAxis() const {
+    return joint_type_ == "revolute" || joint_type_ == "prismatic";
+  }
+
+  /// Returns the axis of this joint expressed in the joint frame J as defined
+  /// in this class's documentation.
+  /// For details on how this frame is defined refer to the documenatation for
+  /// the <a href="http://sdformat.org/spec?ver=1.6&elem=joint#joint_axis">
+  /// &lt;axis&gt; element</a>.
+  /// @warning This method aborts for joint types that do not have an axis
+  /// property.
+  const Vector3<double>& get_axis() const {
+    DRAKE_DEMAND(JointHasAxis());
+    return axis_J_;
+  }
+
+  /// Sets the axis for this joint, expressed in the joint frame J as defined
+  /// in this class's documentation.
+  /// @warning This method aborts for joint types that do not have an axis
+  /// property.
+  /// @sa get_axis()
+  void set_axis(const Vector3<double>& axis) {
+    DRAKE_DEMAND(JointHasAxis());
+    axis_J_ = axis;
+  }
+
+ private:
+  // Name of the root frame of this cache.
+  std::string name_;
+  std::string parent_link_name_;
+  std::string child_link_name_;
+  std::string joint_type_;
+  // Joint's axis expressed in the joint's frame J.
+  Vector3<double> axis_J_{
+      Vector3<double>::Constant(std::numeric_limits<double>::quiet_NaN())};
+};
+
+}  // namespace parsing
+}  // namespace multibody
+}  // namespace drake

drake/multibody/parsing/sdf_link.h

@@ -0,0 +1,94 @@
+#pragma once
+
+#include <string>
+
+#include "drake/common/drake_copyable.h"
+#include "drake/common/eigen_types.h"
+
+namespace drake {
+namespace multibody {
+namespace parsing {
+
+/// A representation of a `<link>` element in an SDF file.
+/// For details on the specification of links, including conventions and default
+/// values, please refer to the documentation for the
+/// <a href="http://sdformat.org/spec?ver=1.6&elem=link">
+/// &lt;link&gt; element</a>.
+class SdfLink {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(SdfLink);
+
+  /// Creates a new link object specification with the given `link_name`.
+  /// Per SDF specification, `link_name` must be unique within the scope of the
+  /// link's model. Uniqueness is **not** enforced by %SdfLink.
+  explicit SdfLink(const std::string& link_name) : name_(link_name) {}
+
+  /// Returns the name of `this` link.
+  const std::string& name() const { return name_; }
+
+  /// Returns the mass for `this` link.
+  double mass() const { return mass_; }
+
+  /// Sets the mass for `this` link to `mass_value`.
+  void set_mass(double mass_value) { mass_ = mass_value; }
+
+  /// Returns the rotational inertia `I_Icm` of `this` link about its center of
+  /// mass and expressed in the "inertial" frame `Icm` defined within the
+  /// `<inertial>` element of an SDF file. The inertial frame `Icm` is defined
+  /// by elements `<frame>` and `<pose>` within the `<inertial>` element. By SDF
+  /// specification, frame `Icm` is located at the link's center of mass.
+  /// Refer to the documentation for the
+  /// <a href="http://sdformat.org/spec?ver=1.6&elem=link#link_inertial">
+  /// &lt;inertial&gt; element</a> for details on how the inertial frame is
+  /// defined.
+  const Matrix3<double>& get_inertia_matrix() const {
+    return I_Icm_;
+  }
+
+  /// Sets the inertia matrix for `this` link. The input matrix must represent
+  /// a valid inertia matrix about the link's COM, and must be expressed in the
+  /// inertial frame `Icm` which, by definition, is located at the link's center
+  /// of mass.
+  // TODO(amcastro-tri): check for inertia tensor invariants: SPD, triangle
+  // inequality.
+  void set_inertia_matrix(const Matrix3<double>& I_Icm) {
+    I_Icm_ = I_Icm;
+  }
+
+  /// Gets the pose `X_LIcm` of the inertial (`<inertial>`) frame `Icm` in the
+  /// link frame L.
+  const Isometry3<double>& get_inertial_frame_pose() const {
+    return X_LIcm_;
+  }
+
+  /// Sets the pose `X_LIcm` of the inertial (`<inertial>`) frame `Icm` in the
+  /// link frame L.
+  void set_inertial_frame_pose(const Isometry3<double>& X_LIcm) {
+    X_LIcm_ = X_LIcm;
+  }
+
+ private:
+  // Name of this link.
+  std::string name_;
+
+  // The mass of this link.
+  // TODO(amcastro-tri): default should ideally be provided by sdformat library.
+  double mass_{0.0};
+
+  // The pose of the <inertial> frame I in a measured-in frame L.
+  // TODO(amcastro-tri): default should ideally be provided by sdformat library.
+  Isometry3<double> X_LIcm_{Isometry3<double>::Identity()};
+
+  // Rotational inertia of this link about its center of mass.
+  // Icm denotes the <inertial> frame which, per SDF specification, must be at
+  // the link's center of mass. The "inertial" frame Icm may or not be aligned
+  // with the principal axes of the body.
+  // Therefore, I_Icm is the inertia of this link, about its center of mass,
+  // expressed in the "inertial" frame Icm.
+  // TODO(amcastro-tri): default value must be provided by sdformat library.
+  Matrix3<double> I_Icm_{Matrix3<double>::Zero()};
+};
+
+}  // namespace parsing
+}  // namespace multibody
+}  // namespace drake