diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml
index 6348fee4..7de42c54 100644
--- a/.github/workflows/linux-ci.yml
+++ b/.github/workflows/linux-ci.yml
@@ -17,7 +17,7 @@ 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-18.04-gcc-9, ubuntu-20.04-clang-10]
 
         build_type: [Debug, Release]
         include:
@@ -26,8 +26,8 @@ jobs:
             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"
 
diff --git a/gtdynamics.i b/gtdynamics.i
index 1258513a..19c2a5cc 100644
--- a/gtdynamics.i
+++ b/gtdynamics.i
@@ -305,6 +305,9 @@ 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;
 };
 
 #include <gtdynamics/universal_robot/sdf.h>
diff --git a/gtdynamics/statics/StaticsSlice.cpp b/gtdynamics/statics/StaticsSlice.cpp
index 9686f23c..7db77a0d 100644
--- a/gtdynamics/statics/StaticsSlice.cpp
+++ b/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
  */
 
diff --git a/gtdynamics/universal_robot/HelicalJoint.h b/gtdynamics/universal_robot/HelicalJoint.h
index fdcc1e72..3fb8e84b 100644
--- a/gtdynamics/universal_robot/HelicalJoint.h
+++ b/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
diff --git a/gtdynamics/universal_robot/Joint.h b/gtdynamics/universal_robot/Joint.h
index 9a630c7b..e151fdae 100644
--- a/gtdynamics/universal_robot/Joint.h
+++ b/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>
@@ -61,6 +61,17 @@ struct JointScalarLimit {
   double value_lower_limit = -M_PI_2;
   double value_upper_limit = M_PI_2;
   double value_limit_threshold = 1e-9;
+
+  JointScalarLimit() {}
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_NVP(value_lower_limit);
+    ar &BOOST_SERIALIZATION_NVP(value_upper_limit);
+    ar &BOOST_SERIALIZATION_NVP(value_limit_threshold);
+  }
 };
 
 /**
@@ -80,6 +91,22 @@ 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(effort_type);
+    ar &BOOST_SERIALIZATION_NVP(scalar_limits);
+    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 +240,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 +439,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
diff --git a/gtdynamics/universal_robot/Link.h b/gtdynamics/universal_robot/Link.h
index d05ce744..ff79aed1 100644
--- a/gtdynamics/universal_robot/Link.h
+++ b/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,33 @@ 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_);
+  }
+
+  /// @}
 };
 
 }  // namespace gtdynamics
+
+namespace gtsam {
+
+template <>
+struct traits<gtdynamics::Link> : public Testable<gtdynamics::Link> {};
+
+}  // namespace gtsam
diff --git a/gtdynamics/universal_robot/PrismaticJoint.h b/gtdynamics/universal_robot/PrismaticJoint.h
index 1375e712..2a8603a7 100644
--- a/gtdynamics/universal_robot/PrismaticJoint.h
+++ b/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
diff --git a/gtdynamics/universal_robot/RevoluteJoint.h b/gtdynamics/universal_robot/RevoluteJoint.h
index 1675bcea..a5d96fc3 100644
--- a/gtdynamics/universal_robot/RevoluteJoint.h
+++ b/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
diff --git a/gtdynamics/universal_robot/Robot.h b/gtdynamics/universal_robot/Robot.h
index 7e0247a5..6296ce6d 100644
--- a/gtdynamics/universal_robot/Robot.h
+++ b/gtdynamics/universal_robot/Robot.h
@@ -112,6 +112,33 @@ 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
+    // Needed since we are storing shared pointers as the values.
+    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 +163,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
diff --git a/tests/testHelicalJoint.cpp b/tests/testHelicalJoint.cpp
index 11376767..4b66251e 100644
--- a/tests/testHelicalJoint.cpp
+++ b/tests/testHelicalJoint.cpp
@@ -14,16 +14,20 @@
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/serializationTestHelpers.h>
 
+#include "gtdynamics/universal_robot/HelicalJoint.h"
 #include "gtdynamics/universal_robot/Link.h"
+#include "gtdynamics/universal_robot/RevoluteJoint.h"
 #include "gtdynamics/universal_robot/RobotModels.h"
-#include "gtdynamics/universal_robot/HelicalJoint.h"
 #include "gtdynamics/universal_robot/sdf.h"
 #include "gtdynamics/utils/utils.h"
 
 using namespace gtdynamics;
-
-using gtsam::assert_equal, gtsam::Pose3, gtsam::Point3, gtsam::Rot3;
+using gtsam::assert_equal;
+using gtsam::Point3;
+using gtsam::Pose3;
+using gtsam::Rot3;
 
 /**
  * Construct a Screw joint via Parameters and ensure all values are as
@@ -94,6 +98,25 @@ TEST(Joint, params_constructor) {
                       j1->parameters().scalar_limits.value_limit_threshold));
 }
 
+BOOST_CLASS_EXPORT(gtdynamics::HelicalJoint)
+
+TEST(HelicalJoint, Serialization) {
+  auto robot = simple_urdf::getRobot();
+  auto l1 = robot.link("l1");
+  auto l2 = robot.link("l2");
+
+  JointParams parameters;
+
+  auto j1 = boost::make_shared<HelicalJoint>(
+      123, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2,
+      gtsam::Vector3(1, 0, 0), 0.5, parameters);
+
+  using namespace gtsam::serializationTestHelpers;
+  EXPECT(equalsDereferenced(j1));
+  EXPECT(equalsDereferencedXML(j1));
+  EXPECT(equalsDereferencedBinary(j1));
+}
+
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
diff --git a/tests/testLink.cpp b/tests/testLink.cpp
index 0462cf3b..b1f67614 100644
--- a/tests/testLink.cpp
+++ b/tests/testLink.cpp
@@ -14,6 +14,7 @@
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/linear/VectorValues.h>
 
 #include "gtdynamics/universal_robot/Link.h"
@@ -27,6 +28,8 @@ using gtsam::Point3;
 using gtsam::Pose3;
 using gtsam::Rot3;
 
+using namespace gtsam::serializationTestHelpers;
+
 // Construct the same link via Params and ensure all values are as expected.
 TEST(Link, params_constructor) {
   Link l1(1, "l1", 100.0, gtsam::Vector3(3, 2, 1).asDiagonal(),
@@ -75,6 +78,24 @@ TEST(Link, NumJoints) {
   EXPECT_LONGS_EQUAL(2, l1->numJoints());
 }
 
+// Declaration needed for serialization of derived class.
+BOOST_CLASS_EXPORT(gtdynamics::RevoluteJoint)
+
+TEST(Link, Serialization) {
+  Link link(1, "l1", 100.0, gtsam::Vector3(3, 2, 1).asDiagonal(),
+            Pose3(Rot3(), Point3(0, 0, 1)), Pose3());
+  EXPECT(equalsObj(link));
+  EXPECT(equalsXML(link));
+  EXPECT(equalsBinary(link));
+
+  // Test link with joints
+  auto robot = simple_urdf::getRobot();
+  auto l1 = robot.link("l1");
+  EXPECT(equalsDereferenced(l1));
+  EXPECT(equalsDereferencedXML(l1));
+  EXPECT(equalsDereferencedBinary(l1));
+}
+
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
diff --git a/tests/testPrismaticJoint.cpp b/tests/testPrismaticJoint.cpp
index f9d04193..d86e4d62 100644
--- a/tests/testPrismaticJoint.cpp
+++ b/tests/testPrismaticJoint.cpp
@@ -14,6 +14,7 @@
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/serializationTestHelpers.h>
 
 #include "gtdynamics/universal_robot/Link.h"
 #include "gtdynamics/universal_robot/PrismaticJoint.h"
@@ -22,7 +23,10 @@
 #include "gtdynamics/utils/utils.h"
 
 using namespace gtdynamics;
-using gtsam::assert_equal, gtsam::Pose3, gtsam::Point3, gtsam::Rot3;
+using gtsam::assert_equal;
+using gtsam::Point3;
+using gtsam::Pose3;
+using gtsam::Rot3;
 
 /**
  * Construct a Prismatic joint via JointParams and ensure all values are as
@@ -39,7 +43,7 @@ TEST(Joint, params_constructor_prismatic) {
   parameters.scalar_limits.value_upper_limit = 2;
   parameters.scalar_limits.value_limit_threshold = 0;
 
-  const gtsam::Vector3 j1_axis = (gtsam::Vector(3) << 0, 0, 1).finished();
+  const gtsam::Vector3 j1_axis(0, 0, 1);
 
   auto j1 = boost::make_shared<PrismaticJoint>(
       1, "j1", Pose3(Rot3::Rx(1.5707963268), Point3(0, 0, 2)), l1, l2, j1_axis,
@@ -98,6 +102,26 @@ TEST(Joint, params_constructor_prismatic) {
                       j1->parameters().scalar_limits.value_limit_threshold));
 }
 
+BOOST_CLASS_EXPORT(gtdynamics::PrismaticJoint)
+
+TEST(PrismaticJoint, Serialization) {
+  auto robot = simple_urdf_prismatic::getRobot();
+  auto l1 = robot.link("l1");
+  auto l2 = robot.link("l2");
+
+  JointParams parameters;
+  const gtsam::Vector3 j1_axis(0, 0, 1);
+
+  auto j1 = boost::make_shared<PrismaticJoint>(
+      1, "j1", Pose3(Rot3::Rx(1.5707963268), Point3(0, 0, 2)), l1, l2, j1_axis,
+      parameters);
+
+  using namespace gtsam::serializationTestHelpers;
+  EXPECT(equalsDereferenced(j1));
+  EXPECT(equalsDereferencedXML(j1));
+  EXPECT(equalsDereferencedBinary(j1));
+}
+
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
diff --git a/tests/testRevoluteJoint.cpp b/tests/testRevoluteJoint.cpp
index bd5825f3..eb8cad54 100644
--- a/tests/testRevoluteJoint.cpp
+++ b/tests/testRevoluteJoint.cpp
@@ -15,6 +15,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/serializationTestHelpers.h>
 
 #include "gtdynamics/universal_robot/Link.h"
 #include "gtdynamics/universal_robot/RevoluteJoint.h"
@@ -53,17 +54,23 @@ JointParams getJointParams() {
   return parameters;
 }
 
+RevoluteJoint getRevoluteJoint() {
+  JointParams parameters = getJointParams();
+
+  const Vector3 axis(1, 0, 0);
+
+  RevoluteJoint joint(1, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, axis,
+                      parameters);
+
+  return joint;
+}
+
 /**
  * Construct a Revolute joint via Parameters and ensure all values are as
  * expected.
  */
 TEST(Joint, Constructor) {
-  JointParams parameters = getJointParams();
-
-  const Vector3 axis(1, 0, 0);
-
-  RevoluteJoint j1(1, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, axis,
-                   parameters);
+  RevoluteJoint j1 = getRevoluteJoint();
 
   // name
   EXPECT(assert_equal(j1.name(), "j1"));
@@ -89,11 +96,7 @@ TEST(Joint, Constructor) {
 
 // Test relativePoseOf and it derivatives
 TEST(Joint, RelativePoseOfDerivatives) {
-  const Vector3 axis(1, 0, 0);
-  JointParams parameters = getJointParams();
-
-  RevoluteJoint j1(1, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, axis,
-                   parameters);
+  RevoluteJoint j1 = getRevoluteJoint();
 
   // Rotating joint by -M_PI / 2
   double q = -M_PI / 2;
@@ -116,11 +119,8 @@ TEST(Joint, RelativePoseOfDerivatives) {
 
 // Test poseOf and it derivatives
 TEST(Joint, PoseOfDerivatives) {
-  const Vector3 axis(1, 0, 0);
-  JointParams parameters = getJointParams();
+  RevoluteJoint j1 = getRevoluteJoint();
 
-  RevoluteJoint j1(1, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, axis,
-                   parameters);
   Pose3 wT1(Rot3::Rx(4), Point3(1, 2, 3));
   Pose3 wT2(Rot3::Rx(5), Point3(6, 7, 8));
 
@@ -154,11 +154,9 @@ TEST(Joint, PoseOfDerivatives) {
 
 // Check values-based relativePoseOf, with derivatives.
 TEST(Joint, ValuesRelativePoseOf) {
-  const Vector3 axis(1, 0, 0);
   JointParams parameters = getJointParams();
+  RevoluteJoint j1 = getRevoluteJoint();
 
-  RevoluteJoint j1(1, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, axis,
-                   parameters);
   // Rotating joint by -M_PI / 2
   double q = -M_PI / 2;
   Pose3 T12(Rot3::Rx(q), Point3(0, 1, 1));
@@ -214,6 +212,17 @@ TEST(RevoluteJoint, ParentTchild) {
   EXPECT(assert_equal(expected_pTc, pTc, 1e-4));
 }
 
+BOOST_CLASS_EXPORT(gtdynamics::RevoluteJoint)
+
+TEST(RevoluteJoint, Serialization) {
+  RevoluteJoint j1 = getRevoluteJoint();
+
+  using namespace gtsam::serializationTestHelpers;
+  EXPECT(equalsObj(j1));
+  EXPECT(equalsXML(j1));
+  EXPECT(equalsBinary(j1));
+}
+
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
diff --git a/tests/testRobot.cpp b/tests/testRobot.cpp
index b7f10d9c..a0bdad6b 100644
--- a/tests/testRobot.cpp
+++ b/tests/testRobot.cpp
@@ -16,8 +16,14 @@
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/linear/VectorValues.h>
 
+#include <boost/serialization/export.hpp>
+
+#include "gtdynamics/universal_robot/HelicalJoint.h"
+#include "gtdynamics/universal_robot/PrismaticJoint.h"
+#include "gtdynamics/universal_robot/RevoluteJoint.h"
 #include "gtdynamics/universal_robot/Robot.h"
 #include "gtdynamics/universal_robot/RobotModels.h"
 #include "gtdynamics/universal_robot/sdf.h"
@@ -231,6 +237,39 @@ TEST(ForwardKinematics, FourBar) {
   THROWS_EXCEPTION(robot.forwardKinematics(wrong_vels));
 }
 
+TEST(Robot, Equality) {
+  Robot robot1 = CreateRobotFromFile(
+      kSdfPath + std::string("test/four_bar_linkage_pure.sdf"));
+  Robot robot2 = CreateRobotFromFile(
+      kSdfPath + std::string("test/four_bar_linkage_pure.sdf"));
+
+  EXPECT(robot1 == robot2);
+  EXPECT(robot1.equals(robot2));
+
+  // Check if not-equal works as expecred
+  JointSharedPtr j = robot1.joints()[0];
+  // Set the joint's parent link to default
+  *(j->parent()) = Link();
+
+  // robot1 should no longer equal robot2
+  EXPECT(!robot1.equals(robot2));
+}
+
+// Declaration needed for serialization of derived class.
+BOOST_CLASS_EXPORT(gtdynamics::RevoluteJoint)
+BOOST_CLASS_EXPORT(gtdynamics::HelicalJoint)
+BOOST_CLASS_EXPORT(gtdynamics::PrismaticJoint)
+
+TEST(Robot, Serialization) {
+  Robot robot = CreateRobotFromFile(
+      kSdfPath + std::string("test/four_bar_linkage_pure.sdf"));
+
+  using namespace gtsam::serializationTestHelpers;
+  EXPECT(equalsObj(robot));
+  EXPECT(equalsXML(robot));
+  EXPECT(equalsBinary(robot));
+}
+
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);