Robot Serialization

.github/workflows/linux-ci.yml

@@ -17,17 +17,17 @@ jobs:
       matrix:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
-        name: [ubuntu-18.04-gcc-9, ubuntu-18.04-clang-10]
+        name: [ubuntu-20.04-gcc-9, ubuntu-20.04-clang-10]
 
         build_type: [Debug, Release]
         include:
-          - name: ubuntu-18.04-gcc-9
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-gcc-9
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
 
-          - name: ubuntu-18.04-clang-10
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-clang-10
+            os: ubuntu-20.04
             compiler: clang
             version: "10"
 

gtdynamics.i

@@ -305,6 +305,12 @@ class Robot {
   gtsam::Values forwardKinematics(
       const gtsam::Values &known_values, size_t t,
       const boost::optional<string> &prior_link_name) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 #include <gtdynamics/universal_robot/sdf.h>

gtdynamics/statics/StaticsSlice.cpp

@@ -6,8 +6,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file  testStaticsSlice.cpp
- * @brief Test Statics in single time slice.
+ * @file  StaticsSlice.cpp
+ * @brief Statics in single time slice.
  * @author: Frank Dellaert
  */
 

gtdynamics/universal_robot/HelicalJoint.h

@@ -39,6 +39,9 @@ class HelicalJoint : public Joint {
   }
 
  public:
+  /// Constructor for serialization
+  HelicalJoint() {}
+
   /**
    * @brief Create HelicalJoint using JointParams, joint name, joint pose in
    * world frame, screw axes, and parent and child links.
@@ -53,17 +56,39 @@ class HelicalJoint : public Joint {
    * @param[in] parameters    JointParams struct.
    */
   HelicalJoint(uint8_t id, const std::string &name, const gtsam::Pose3 &bTj,
-             const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link,
-             const gtsam::Vector3 &axis, double thread_pitch,
-             const JointParams &parameters = JointParams())
+               const LinkSharedPtr &parent_link,
+               const LinkSharedPtr &child_link, const gtsam::Vector3 &axis,
+               double thread_pitch,
+               const JointParams &parameters = JointParams())
       : Joint(id, name, bTj, parent_link, child_link,
               getScrewAxis(axis, thread_pitch), parameters) {}
-  
+
   /// Constructor directly from screwAxis
   using Joint::Joint;
 
   /// Return joint type for use in reconstructing robot from JointParams.
   Type type() const final override { return Type::Screw; }
+
+ private:
+  /// @name Advanced Interface
+  /// @{
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Joint);
+  }
+
+  /// @}
 };
 
 }  // namespace gtdynamics
+
+namespace gtsam {
+
+template <>
+struct traits<gtdynamics::HelicalJoint>
+    : public Testable<gtdynamics::HelicalJoint> {};
+
+}  // namespace gtsam

gtdynamics/universal_robot/Joint.h

@@ -20,9 +20,9 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/nonlinear/Expression.h>
-#include <gtsam/nonlinear/expressions.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/expressions.h>
 
 #include <boost/enable_shared_from_this.hpp>
 #include <boost/shared_ptr.hpp>
@@ -80,6 +80,20 @@ struct JointParams {
 
   /// Constructor
   JointParams() {}
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_NVP(velocity_limit);
+    ar &BOOST_SERIALIZATION_NVP(velocity_limit_threshold);
+    ar &BOOST_SERIALIZATION_NVP(acceleration_limit);
+    ar &BOOST_SERIALIZATION_NVP(acceleration_limit_threshold);
+    ar &BOOST_SERIALIZATION_NVP(torque_limit);
+    ar &BOOST_SERIALIZATION_NVP(torque_limit_threshold);
+    ar &BOOST_SERIALIZATION_NVP(damping_coefficient);
+    ar &BOOST_SERIALIZATION_NVP(spring_coefficient);
+  }
 };
 
 /// Joint is the base class for a joint connecting two Link objects.
@@ -213,6 +227,10 @@ class Joint : public boost::enable_shared_from_this<Joint> {
 
   bool operator!=(const Joint &other) const { return !(*this == other); }
 
+  bool equals(const Joint &other, double tol = 0) const {
+    return *this == other;
+  }
+
   friend std::ostream &operator<<(std::ostream &stream, const Joint &j);
 
   friend std::ostream &operator<<(std::ostream &stream,
@@ -408,8 +426,35 @@ class Joint : public boost::enable_shared_from_this<Joint> {
    * @brief Create expression that constraint the relation between
    * wrench and torque on each link.
    */
-  gtsam::Double_ torqueConstraint(uint64_t t=0) const;
+  gtsam::Double_ torqueConstraint(uint64_t t = 0) const;
+
+ private:
+  /// @name Advanced Interface
+  /// @{
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_NVP(name_);
+    ar &BOOST_SERIALIZATION_NVP(id_);
+    ar &BOOST_SERIALIZATION_NVP(jMp_);
+    ar &BOOST_SERIALIZATION_NVP(jMc_);
+    ar &BOOST_SERIALIZATION_NVP(parent_link_);
+    ar &BOOST_SERIALIZATION_NVP(child_link_);
+    ar &BOOST_SERIALIZATION_NVP(pScrewAxis_);
+    ar &BOOST_SERIALIZATION_NVP(cScrewAxis_);
+    ar &BOOST_SERIALIZATION_NVP(parameters_);
+  }
 
+  /// @}
 };
 
 }  // namespace gtdynamics
+
+namespace gtsam {
+
+template <>
+struct traits<gtdynamics::Joint> : public Testable<gtdynamics::Joint> {};
+
+}  // namespace gtsam

gtdynamics/universal_robot/Link.h

@@ -113,6 +113,10 @@ class Link : public boost::enable_shared_from_this<Link> {
 
   bool operator!=(const Link &other) const { return !(*this == other); }
 
+  bool equals(const Link &other, double tol = 0) const {
+    return *this == other;
+  }
+
   /// return a shared pointer of the link
   LinkSharedPtr shared(void) { return shared_from_this(); }
 
@@ -206,6 +210,34 @@ class Link : public boost::enable_shared_from_this<Link> {
 
   /// Unfix the link
   void unfix() { is_fixed_ = false; }
+
+  /// @name Advanced Interface
+  /// @{
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_NVP(id_);
+    ar &BOOST_SERIALIZATION_NVP(name_);
+    ar &BOOST_SERIALIZATION_NVP(mass_);
+    ar &BOOST_SERIALIZATION_NVP(centerOfMass_);
+    ar &BOOST_SERIALIZATION_NVP(inertia_);
+    ar &BOOST_SERIALIZATION_NVP(bMcom_);
+    ar &BOOST_SERIALIZATION_NVP(bMlink_);
+    ar &BOOST_SERIALIZATION_NVP(is_fixed_);
+    ar &BOOST_SERIALIZATION_NVP(fixed_pose_);
+    ar &BOOST_SERIALIZATION_NVP(joints_);
+  }
+
+  /// @}
 };
 
 }  // namespace gtdynamics
+
+namespace gtsam {
+
+template <>
+struct traits<gtdynamics::Link> : public Testable<gtdynamics::Link> {};
+
+}  // namespace gtsam

gtdynamics/universal_robot/PrismaticJoint.h

@@ -37,6 +37,9 @@ class PrismaticJoint : public Joint {
   }
 
  public:
+  /// Constructor for serialization
+  PrismaticJoint() {}
+
   /**
    * @brief Create PrismaticJoint using JointParams, joint name, joint pose in
    * world frame, screw axes, and parent and child links.
@@ -58,6 +61,27 @@ class PrismaticJoint : public Joint {
 
   /// Return joint type for use in reconstructing robot from JointParams.
   Type type() const final override { return Type::Prismatic; }
+
+ private:
+  /// @name Advanced Interface
+  /// @{
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Joint);
+  }
+
+  /// @}
 };
 
 }  // namespace gtdynamics
+
+namespace gtsam {
+
+template <>
+struct traits<gtdynamics::PrismaticJoint>
+    : public Testable<gtdynamics::PrismaticJoint> {};
+
+}  // namespace gtsam

gtdynamics/universal_robot/RevoluteJoint.h

@@ -36,6 +36,9 @@ class RevoluteJoint : public Joint {
   }
 
  public:
+  /// Constructor for serialization
+  RevoluteJoint() {}
+
   /**
    * @brief Create RevoluteJoint using JointParams, joint name, joint pose in
    * world frame, screw axes, and parent and child links.
@@ -57,6 +60,27 @@ class RevoluteJoint : public Joint {
 
   /// Return joint type for use in reconstructing robot from JointParams.
   Type type() const final override { return Type::Revolute; }
+
+ private:
+  /// @name Advanced Interface
+  /// @{
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Joint);
+  }
+
+  /// @}
 };
 
 }  // namespace gtdynamics
+
+namespace gtsam {
+
+template <>
+struct traits<gtdynamics::RevoluteJoint>
+    : public Testable<gtdynamics::RevoluteJoint> {};
+
+}  // namespace gtsam

gtdynamics/universal_robot/Robot.h

@@ -112,6 +112,32 @@ class Robot {
   /// Print links and joints of the robot, for debug purposes
   void print(const std::string &s = "") const;
 
+  /// Overload equality operator.
+  bool operator==(const Robot &other) const {
+    // Define comparators for easy std::map equality checking
+    auto link_comparator = [](decltype(*this->name_to_link_.begin()) a,
+                              decltype(a) b) {
+      // compare the key name and the underlying shared_ptr object
+      return a.first == b.first && (*a.second) == (*b.second);
+    };
+    auto joint_comparator = [](decltype(*this->name_to_joint_.begin()) a,
+                               decltype(a) b) {
+      // compare the key name and the underlying shared_ptr object
+      return a.first == b.first && (*a.second) == (*b.second);
+    };
+
+    return (this->name_to_link_.size() == other.name_to_link_.size() &&
+            std::equal(this->name_to_link_.begin(), this->name_to_link_.end(),
+                       other.name_to_link_.begin(), link_comparator) &&
+            this->name_to_joint_.size() == other.name_to_joint_.size() &&
+            std::equal(this->name_to_joint_.begin(), this->name_to_joint_.end(),
+                       other.name_to_joint_.begin(), joint_comparator));
+  }
+
+  bool equals(const Robot &other, double tol = 0) const {
+    return *this == other;
+  }
+
   /**
    * Calculate forward kinematics by performing BFS in the link-joint graph
    * (will throw an error when invalid joint angle specification detected).
@@ -136,5 +162,25 @@ class Robot {
   LinkSharedPtr findRootLink(
       const gtsam::Values &values,
       const boost::optional<std::string> &prior_link_name) const;
+
+  /// @name Advanced Interface
+  /// @{
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_NVP(name_to_link_);
+    ar &BOOST_SERIALIZATION_NVP(name_to_joint_);
+  }
+
+  /// @}
 };
 }  // namespace gtdynamics
+
+namespace gtsam {
+
+template <>
+struct traits<gtdynamics::Robot> : public Testable<gtdynamics::Robot> {};
+
+}  // namespace gtsam