Automatic Moment of Inertia Calculations for Basic Shapes

CMakeLists.txt

@@ -9,7 +9,7 @@ project (sdformat14 VERSION 14.0.0)
 
 # The protocol version has nothing to do with the package version.
 # It represents the current version of SDFormat implemented by the software
-set (SDF_PROTOCOL_VERSION 1.10)
+set (SDF_PROTOCOL_VERSION 1.11)
 
 OPTION(SDFORMAT_DISABLE_CONSOLE_LOGFILE "Disable the sdformat console logfile" OFF)
 

include/sdf/Box.hh

@@ -17,8 +17,11 @@
 #ifndef SDF_BOX_HH_
 #define SDF_BOX_HH_
 
+#include <optional>
+
 #include <gz/math/Box.hh>
 #include <gz/math/Vector3.hh>
+#include <gz/math/MassMatrix3.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
@@ -65,6 +68,9 @@ namespace sdf
     /// \return A reference to a gz::math::Boxd object.
     public: gz::math::Boxd &Shape();
 
+    /// \brief Calculate an return the Mass Matrix values for the Box
+    public: std::optional< gz::math::MassMatrix3d >  MassMatrix(const double density);
+
     /// \brief Create and return an SDF element filled with data from this
     /// box.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Capsule.hh

@@ -17,7 +17,10 @@
 #ifndef SDF_CAPSULE_HH_
 #define SDF_CAPSULE_HH_
 
+#include <optional>
+
 #include <gz/math/Capsule.hh>
+#include <gz/math/MassMatrix3.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
@@ -72,6 +75,9 @@ namespace sdf
     /// \return A reference to a gz::math::Capsuled object.
     public: gz::math::Capsuled &Shape();
 
+    /// \brief Calculate an return the Mass Matrix values for the Capsule
+    public: std::optional< gz::math::MassMatrix3d >  MassMatrix(const double density);
+
     /// \brief Create and return an SDF element filled with data from this
     /// capsule.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Collision.hh

@@ -20,6 +20,7 @@
 #include <memory>
 #include <string>
 #include <gz/math/Pose3.hh>
+#include <gz/math/Vector3.hh>
 #include <gz/utils/ImplPtr.hh>
 #include "sdf/Element.hh"
 #include "sdf/SemanticPose.hh"
@@ -75,6 +76,16 @@ namespace sdf
     /// \param[in] _name Name of the collision.
     public: void SetName(const std::string &_name);
 
+    /// \brief Get the density of the collision.
+    /// The density of the collision must be unique within the scope of a Link.
+    /// \return Density of the collision.
+    public: double Density() const;
+
+    /// \brief Set the density of the collision.
+    /// The density of the collision must be unique within the scope of a Link.
+    /// \param[in] _name Density of the collision.
+    public: void SetDensity(const double density);
+
     /// \brief Get a pointer to the collisions's geometry.
     /// \return The collision's geometry.
     public: const Geometry *Geom() const;
@@ -119,6 +130,16 @@ namespace sdf
     /// \return SemanticPose object for this link.
     public: sdf::SemanticPose SemanticPose() const;
 
+    /// \brief Calculate and return the MassMatrix for the collision
+    /// \param[in] _xxyyzz A vector 3d representing the diagonal elements
+    /// of the mass matrix
+    /// \param[in] _xyxzyx A vector 3d representing the off-diagonal elements
+    /// of the mass matrix
+    /// \param[in] _mass A double representing the mass of the collision
+    /// \return Errors, which is a vector of Error objects. Each Error includes
+    /// an error code and message. An empty vector indicates no error
+    public: Errors MassMatrix(gz::math::Vector3d &_xxyyzz, gz::math::Vector3d &_xyxzyz, double &_mass);
+
     /// \brief Get a pointer to the SDF element that was used during
     /// load.
     /// \return SDF element pointer. The value will be nullptr if Load has

include/sdf/Cylinder.hh

@@ -17,7 +17,10 @@
 #ifndef SDF_CYLINDER_HH_
 #define SDF_CYLINDER_HH_
 
+#include <optional>
+
 #include <gz/math/Cylinder.hh>
+#include <gz/math/MassMatrix3.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
@@ -72,6 +75,9 @@ namespace sdf
     /// \return A reference to a gz::math::Cylinderd object.
     public: gz::math::Cylinderd &Shape();
 
+    /// \brief Calculate an return the Mass Matrix values for the Cylinder
+    public: std::optional< gz::math::MassMatrix3d >  MassMatrix(const double density);
+
     /// \brief Create and return an SDF element filled with data from this
     /// cylinder.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Ellipsoid.hh

@@ -17,6 +17,9 @@
 #ifndef SDF_ELLIPSOID_HH_
 #define SDF_ELLIPSOID_HH_
 
+#include <optional>
+
+#include <gz/math/MassMatrix3.hh>
 #include <gz/math/Ellipsoid.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
@@ -64,6 +67,9 @@ namespace sdf
     /// \return A reference to a gz::math::Ellipsoidd object.
     public: gz::math::Ellipsoidd &Shape();
 
+    /// \brief Calculate an return the Mass Matrix values for the Cylinder
+    public: std::optional< gz::math::MassMatrix3d >  MassMatrix(const double density);
+
     /// \brief Create and return an SDF element filled with data from this
     /// ellipsoid.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Geometry.hh

@@ -18,8 +18,10 @@
 #define SDF_GEOMETRY_HH_
 
 #include <vector>
+#include <optional>
 
 #include <gz/utils/ImplPtr.hh>
+#include <gz/math/MassMatrix3.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
 #include <sdf/sdf_config.h>
@@ -209,6 +211,15 @@ namespace sdf
     /// \param[in] _heightmap The heightmap shape.
     public: void SetHeightmapShape(const Heightmap &_heightmap);
 
+    /// \brief Calculate and return the Mass Matrix values for the Geometry
+    /// \param[in] _xxyyzz A vector 3d representing the diagonal elements
+    /// of the mass matrix
+    /// \param[in] _xyxzyx A vector 3d representing the off-diagonal elements
+    /// of the mass matrix
+    /// \param[in] _density The density of the geometry element.
+    /// \return Boolean that indicates whether the calculation was successfull
+    public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density);
+
     /// \brief Get a pointer to the SDF element that was used during
     /// load.
     /// \return SDF element pointer. The value will be nullptr if Load has

include/sdf/Sphere.hh

@@ -17,6 +17,9 @@
 #ifndef SDF_SPHERE_HH_
 #define SDF_SPHERE_HH_
 
+#include <optional>
+
+#include <gz/math/MassMatrix3.hh>
 #include <gz/math/Sphere.hh>
 #include <gz/utils/ImplPtr.hh>
 
@@ -65,6 +68,9 @@ namespace sdf
     /// not been called.
     public: sdf::ElementPtr Element() const;
 
+    /// \brief Calculate an return the Mass Matrix values for the Cylinder
+    public: std::optional< gz::math::MassMatrix3d >  MassMatrix(const double density);
+
     /// \brief Create and return an SDF element filled with data from this
     /// sphere.
     /// Note that parameter passing functionality is not captured with this

sdf/1.11/1_10.convert

@@ -0,0 +1,2 @@
+<convert name="sdf">
+</convert> <!-- End SDF -->

sdf/1.11/CMakeLists.txt

@@ -0,0 +1,88 @@
+set (sdfs
+  actor.sdf
+  air_pressure.sdf
+  air_speed.sdf
+  altimeter.sdf
+  atmosphere.sdf
+  audio_source.sdf
+  audio_sink.sdf
+  battery.sdf
+  box_shape.sdf
+  camera.sdf
+  capsule_shape.sdf
+  collision.sdf
+  contact.sdf
+  cylinder_shape.sdf
+  ellipsoid_shape.sdf
+  frame.sdf
+  forcetorque.sdf
+  geometry.sdf
+  gps.sdf
+  gripper.sdf
+  gui.sdf
+  heightmap_shape.sdf
+  image_shape.sdf
+  imu.sdf
+  inertial.sdf
+  joint.sdf
+  lidar.sdf
+  light.sdf
+  light_state.sdf
+  link.sdf
+  link_state.sdf
+  logical_camera.sdf
+  magnetometer.sdf
+  material.sdf
+  mesh_shape.sdf
+  model.sdf
+  model_state.sdf
+  navsat.sdf
+  noise.sdf
+  particle_emitter.sdf
+  physics.sdf
+  plane_shape.sdf
+  plugin.sdf
+  polyline_shape.sdf
+  population.sdf
+  pose.sdf
+  projector.sdf
+  ray.sdf
+  rfidtag.sdf
+  rfid.sdf
+  road.sdf
+  root.sdf
+  scene.sdf
+  sensor.sdf
+  spherical_coordinates.sdf
+  sphere_shape.sdf
+  sonar.sdf
+  state.sdf
+  surface.sdf
+  transceiver.sdf
+  visual.sdf
+  world.sdf
+)
+
+set (SDF_SCHEMA)
+
+foreach(FIL ${sdfs})
+  get_filename_component(ABS_FIL ${FIL} ABSOLUTE)
+  get_filename_component(FIL_WE ${FIL} NAME_WE)
+
+  list(APPEND SDF_SCHEMA "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd")
+
+  add_custom_command(
+    OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd"
+    COMMAND ${RUBY} ${CMAKE_SOURCE_DIR}/tools/xmlschema.rb
+    ARGS -s ${CMAKE_CURRENT_SOURCE_DIR} -i ${ABS_FIL} -o ${CMAKE_CURRENT_BINARY_DIR}
+    DEPENDS ${ABS_FIL}
+    COMMENT "Running xml schema compiler on ${FIL}"
+    VERBATIM)
+endforeach()
+
+add_custom_target(schema1_11 ALL DEPENDS ${SDF_SCHEMA})
+
+set_source_files_properties(${SDF_SCHEMA} PROPERTIES GENERATED TRUE)
+
+install(FILES 1_10.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.11)
+install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.11)

sdf/1.11/actor.sdf

@@ -0,0 +1,86 @@
+<!-- Actor -->
+<element name="actor" required="*">
+  <description>A special kind of model which can have a scripted motion. This includes both global waypoint type animations and skeleton animations.</description>
+
+  <attribute name="name" type="string" default="__default__" required="1">
+    <description>A unique name for the actor.</description>
+  </attribute>
+
+  <include filename="pose.sdf" required="0"/>
+
+  <element name="skin" required="0">
+    <description>Skin file which defines a visual and the underlying skeleton which moves it.</description>
+
+    <element name="filename" type="string" default="__default__" required="1">
+      <description>Path to skin file, accepted formats: COLLADA, BVH.</description>
+    </element>
+
+    <element name="scale" type="double" default="1.0" required="0">
+      <description>Scale the skin's size.</description>
+    </element>
+  </element> <!-- End Skin -->
+
+  <element name="animation" required="*">
+    <description>Animation file defines an animation for the skeleton in the skin. The skeleton must be compatible with the skin skeleton.</description>
+
+    <attribute name="name" type="string" default="__default__" required="1">
+      <description>Unique name for animation.</description>
+    </attribute>
+
+    <element name="filename" type="string" default="__default__" required="1">
+      <description>Path to animation file. Accepted formats: COLLADA, BVH.</description>
+    </element>
+    <element name="scale" type="double" default="1.0" required="0">
+      <description>Scale for the animation skeleton.</description>
+    </element>
+    <element name="interpolate_x" type="bool" default="false" required="0">
+      <description>Set to true so the animation is interpolated on X.</description>
+    </element>
+  </element> <!-- End Animation -->
+
+  <element name="script" required="1">
+    <description>Adds scripted trajectories to the actor.</description>
+
+    <element name="loop" type="bool" default="true" required="0">
+      <description>Set this to true for the script to be repeated in a loop. For a fluid continuous motion, make sure the last waypoint matches the first one.</description>
+    </element>
+
+    <element name="delay_start" type="double" default="0.0" required="0">
+      <description>This is the time to wait before starting the script. If running in a loop, this time will be waited before starting each cycle.</description>
+    </element>
+
+    <element name="auto_start" type="bool" default="true" required="0">
+      <description>Set to true if the animation should start as soon as the simulation starts playing. It is useful to set this to false if the animation should only start playing only when triggered by a plugin, for example.</description>
+    </element>
+
+    <element name="trajectory" required="*">
+      <description>The trajectory contains a series of keyframes to be followed.</description>
+      <attribute name="id" type="int" default="0" required="1">
+        <description>Unique id for a trajectory.</description>
+      </attribute>
+
+      <attribute name="type" type="string" default="__default__" required="1">
+        <description>If it matches the type of an animation, they will be played at the same time.</description>
+      </attribute>
+
+      <attribute name="tension" type="double" default="0.0" required="0" min="0.0" max="1.0">
+        <description>The tension of the trajectory spline. The default value of zero equates to a Catmull-Rom spline, which may also cause the animation to overshoot keyframes. A value of one will cause the animation to stick to the keyframes.</description>
+      </attribute>
+
+      <element name="waypoint" required="*">
+        <description>Each point in the trajectory.</description>
+        <element name="time" type="double" default="0.0" required="1">
+          <description>The time in seconds, counted from the beginning of the script, when the pose should be reached.</description>
+        </element>
+        <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
+          <description>The pose which should be reached at the given time.</description>
+        </element> <!-- End Pose -->
+      </element> <!-- End Waypoint -->
+    </element> <!-- End Trajectory -->
+  </element> <!-- End Script -->
+
+  <include filename="link.sdf" required="+"/>
+  <include filename="joint.sdf" required="*"/>
+  <include filename="plugin.sdf" required="*"/>
+
+</element> <!-- End Actor -->

sdf/1.11/air_pressure.sdf

@@ -0,0 +1,15 @@
+<element name="air_pressure" required="0">
+  <description>These elements are specific to an air pressure sensor.</description>
+
+  <element name="reference_altitude" type="double" default="0.0" required="0">
+    <description>The initial altitude in meters. This value can be used by a sensor implementation to augment the altitude of the sensor. For example, if you are using simulation instead of creating a 1000 m mountain model on which to place your sensor, you could instead set this value to 1000 and place your model on a ground plane with a Z height of zero.</description>
+  </element>
+
+  <element name="pressure" required="0">
+    <description>
+      Noise parameters for the pressure data.
+    </description>
+    <include filename="noise.sdf" required="0"/>
+  </element>
+
+</element>

sdf/1.11/air_speed.sdf

@@ -0,0 +1,11 @@
+<element name="air_speed" required="0">
+  <description>These elements are specific to an air speed sensor. This sensor determines speed based on the differential between static and dynamic pressure.</description>
+
+  <element name="pressure" required="0">
+    <description>
+      Noise parameters for the pressure data.
+    </description>
+    <include filename="noise.sdf" required="0"/>
+  </element>
+
+</element>