Merge pull request #393 from borglab/fix-memory-leak

gtdynamics/universal_robot/Joint.cpp

@@ -28,24 +28,24 @@ namespace gtdynamics {
 
 /* ************************************************************************* */
 Joint::Joint(uint8_t id, const std::string &name, const Pose3 &bTj,
-             const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link,
+             const LinkWeakPtr &parent_link, const LinkWeakPtr &child_link,
              const Vector6 &jScrewAxis, const JointParams &parameters)
     : id_(id),
       name_(name),
       parent_link_(parent_link),
       child_link_(child_link),
-      jMp_(bTj.inverse() * parent_link->bMcom()),
-      jMc_(bTj.inverse() * child_link->bMcom()),
+      jMp_(bTj.inverse() * parent_link.lock()->bMcom()),
+      jMc_(bTj.inverse() * child_link.lock()->bMcom()),
       pScrewAxis_(-jMp_.inverse().AdjointMap() * jScrewAxis),
       cScrewAxis_(jMc_.inverse().AdjointMap() * jScrewAxis),
       parameters_(parameters) {}
 
 /* ************************************************************************* */
 bool Joint::isChildLink(const LinkSharedPtr &link) const {
-  if (link != child_link_ && link != parent_link_)
+  if (link != child_link_.lock() && link != parent_link_.lock())
     throw std::runtime_error("link " + link->name() +
                              " is not connected to this joint " + name_);
-  return link == child_link_;
+  return link == child_link_.lock();
 }
 
 /* ************************************************************************* */
@@ -160,7 +160,7 @@ gtsam::GaussianFactorGraph Joint::linearAFactors(
   const Pose3 T_wi2 = Pose(known_values, child()->id(), t);
   const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1;
   const Vector6 V_i2 = Twist(known_values, child()->id(), t);
-  const Vector6 S_i2_j = screwAxis(child_link_);
+  const Vector6 S_i2_j = screwAxis(child_link_.lock());
   const double v_j = JointVel(known_values, id(), t);
 
   // twist acceleration factor
@@ -183,7 +183,7 @@ gtsam::GaussianFactorGraph Joint::linearDynamicsFactors(
   const Pose3 T_wi1 = Pose(known_values, parent()->id(), t);
   const Pose3 T_wi2 = Pose(known_values, child()->id(), t);
   const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1;
-  const Vector6 S_i2_j = screwAxis(child_link_);
+  const Vector6 S_i2_j = screwAxis(child_link_.lock());
 
   // torque factor
   // S_i_j^T * F_i_j - tau = 0

gtdynamics/universal_robot/Joint.h

@@ -150,9 +150,12 @@ class Joint : public std::enable_shared_from_this<Joint> {
   /// Rest transform to child link CoM frame from joint frame.
   Pose3 jMc_;
 
-  using LinkSharedPtr = std::shared_ptr<Link>;
-  LinkSharedPtr parent_link_;
-  LinkSharedPtr child_link_;
+  // NOTE: We use a weak_ptr here since Link has shared_ptrs
+  // to joints, and this way we can avoid reference cyles.
+  // https://en.cppreference.com/w/cpp/memory/weak_ptr
+  using LinkWeakPtr = std::weak_ptr<Link>;
+  LinkWeakPtr parent_link_;
+  LinkWeakPtr child_link_;
 
   // Screw axis in parent and child CoM frames.
   Vector6 pScrewAxis_;
@@ -182,7 +185,7 @@ class Joint : public std::enable_shared_from_this<Joint> {
    * @param[in] parameters   The joint parameters.
    */
   Joint(uint8_t id, const std::string &name, const Pose3 &bTj,
-        const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link,
+        const LinkWeakPtr &parent_link, const LinkWeakPtr &child_link,
         const Vector6 &jScrewAxis,
         const JointParams &parameters = JointParams());
 
@@ -228,19 +231,19 @@ class Joint : public std::enable_shared_from_this<Joint> {
 
   /// Return the connected link other than the one provided.
   LinkSharedPtr otherLink(const LinkSharedPtr &link) const {
-    return isChildLink(link) ? parent_link_ : child_link_;
+    return isChildLink(link) ? parent_link_.lock() : child_link_.lock();
   }
 
   /// Return the links connected to this joint.
   std::vector<LinkSharedPtr> links() const {
-    return std::vector<LinkSharedPtr>{parent_link_, child_link_};
+    return std::vector<LinkSharedPtr>{parent_link_.lock(), child_link_.lock()};
   }
 
   /// Return a shared ptr to the parent link.
-  LinkSharedPtr parent() const { return parent_link_; }
+  LinkSharedPtr parent() const { return parent_link_.lock(); }
 
   /// Return a shared ptr to the child link.
-  LinkSharedPtr child() const { return child_link_; }
+  LinkSharedPtr child() const { return child_link_.lock(); }
 
   /// Return joint parameters.
   const JointParams &parameters() const { return parameters_; }

tests/make_joint.h

@@ -17,18 +17,25 @@
 #include <gtdynamics/universal_robot/Link.h>
 
 namespace gtdynamics {
-/// Create a joint with given rest transform cMp and screw-axis in child frame.
-JointConstSharedPtr make_joint(gtsam::Pose3 cMp, gtsam::Vector6 cScrewAxis) {
+/**
+ * Create a joint with given rest transform cMp and screw-axis in child frame.
+ *
+ * We return both the joint and its connected links so that
+ * the link pointers are alive in the test.
+ * This will be true in a Robot object since it holds
+ * both the joint and link pointers.
+ */
+std::pair<JointConstSharedPtr, std::vector<LinkSharedPtr>> make_joint(
+    gtsam::Pose3 cMp, gtsam::Vector6 cScrewAxis) {
   // create links
   std::string name = "l1";
   double mass = 100;
   gtsam::Matrix3 inertia = gtsam::Vector3(3, 2, 1).asDiagonal();
   gtsam::Pose3 bMcom;
   gtsam::Pose3 bMl;
 
-  auto l1 = std::make_shared<Link>(Link(1, name, mass, inertia, bMcom, bMl));
-  auto l2 =
-      std::make_shared<Link>(Link(2, name, mass, inertia, cMp.inverse(), bMl));
+  auto l1 = std::make_shared<Link>(1, name, mass, inertia, bMcom, bMl);
+  auto l2 = std::make_shared<Link>(2, name, mass, inertia, cMp.inverse(), bMl);
 
   // create joint
   JointParams joint_params;
@@ -40,7 +47,11 @@ JointConstSharedPtr make_joint(gtsam::Pose3 cMp, gtsam::Vector6 cScrewAxis) {
   gtsam::Pose3 jMc = bMj.inverse() * l2->bMcom();
   gtsam::Vector6 jScrewAxis = jMc.AdjointMap() * cScrewAxis;
 
-  return std::make_shared<const HelicalJoint>(1, "j1", bMj, l1, l2, jScrewAxis,
+  auto joint = std::make_shared<HelicalJoint>(1, "j1", bMj, l1, l2, jScrewAxis,
                                               joint_params);
+  l1->addJoint(joint);
+  l2->addJoint(joint);
+  std::vector<LinkSharedPtr> links{l1, l2};
+  return {joint, links};
 }
 }  // namespace gtdynamics

tests/testPoseFactor.cpp

@@ -49,7 +49,7 @@ TEST(PoseFactor, error) {
   Pose3 cMp = Pose3(Rot3(), Point3(-2, 0, 0));
   Vector6 screw_axis;
   screw_axis << 0, 0, 1, 0, 1, 0;
-  auto joint = make_joint(cMp, screw_axis);
+  auto [joint, links] = make_joint(cMp, screw_axis);
 
   // Create factor
   auto factor = PoseFactor(example::wTp_key, example::wTc_key, example::q_key,
@@ -77,7 +77,7 @@ TEST(PoseFactor, breaking) {
   Pose3 cMp = Pose3(Rot3(), Point3(-2, 0, 0));
   Vector6 screw_axis;
   screw_axis << 0, 0, 1, 0, 1, 0;
-  auto joint = make_joint(cMp, screw_axis);
+  auto [joint, links] = make_joint(cMp, screw_axis);
   auto factor = PoseFactor(example::wTp_key, example::wTc_key, example::q_key,
                            example::cost_model, joint);
 
@@ -111,7 +111,7 @@ TEST(PoseFactor, breaking_rr) {
 
   Vector6 screw_axis = (Vector6() << 1, 0, 0, 0, -1, 0).finished();
   Pose3 cMp = j1->relativePoseOf(l1, 0.0);
-  auto joint = make_joint(cMp, screw_axis);
+  auto [joint, links] = make_joint(cMp, screw_axis);
   auto factor = PoseFactor(example::wTp_key, example::wTc_key, example::q_key,
                            example::cost_model, joint);
 
@@ -129,7 +129,7 @@ TEST(PoseFactor, nonzero_rest) {
   Pose3 cMp = Pose3(Rot3::Rx(1), Point3(-2, 0, 0));
   Vector6 screw_axis;
   screw_axis << 0, 0, 1, 0, 1, 0;
-  auto joint = make_joint(cMp, screw_axis);
+  auto [joint, links] = make_joint(cMp, screw_axis);
   auto factor = PoseFactor(example::wTp_key, example::wTc_key, example::q_key,
                            example::cost_model, joint);
 

tests/testTorqueFactor.cpp

@@ -38,7 +38,7 @@ TEST(TorqueFactor, error) {
   gtsam::Vector6 screw_axis;
   screw_axis << 0, 0, 1, 0, 1, 0;
 
-  auto joint = make_joint(kMj, screw_axis);
+  auto [joint, links] = make_joint(kMj, screw_axis);
 
   // Create factor.
   auto cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1);

tests/testTwistAccelFactor.cpp

@@ -50,7 +50,7 @@ TEST(TwistAccelFactor, error) {
   gtsam::Vector6 screw_axis;
   screw_axis << 0, 0, 1, 0, 1, 0;
 
-  auto joint = make_joint(cMp, screw_axis);
+  auto [joint, links] = make_joint(cMp, screw_axis);
 
   // create factor
   auto factor = TwistAccelFactor(example::cost_model, joint, 0);
@@ -85,7 +85,7 @@ TEST(TwistAccelFactor, error_1) {
   gtsam::Pose3 cMp = gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(-1, 0, 0));
   gtsam::Vector6 screw_axis = (gtsam::Vector(6) << 0, 0, 1, 0, 1, 0).finished();
 
-  auto joint = make_joint(cMp, screw_axis);
+  auto [joint, links] = make_joint(cMp, screw_axis);
 
   auto factor = TwistAccelFactor(example::cost_model, joint, 0);
   double q = 0, qVel = 0, qAccel = -9.8;

tests/testTwistFactor.cpp

@@ -44,7 +44,7 @@ TEST(TwistFactor, error) {
   gtsam::Vector6 screw_axis;
   screw_axis << 0, 0, 1, 0, 1, 0;
 
-  auto joint = make_joint(cMp, screw_axis);
+  auto [joint, links] = make_joint(cMp, screw_axis);
 
   auto factor = TwistFactor(example::cost_model, joint, 0);
   double q = M_PI / 4, qVel = 10;

tests/testWrenchEquivalenceFactor.cpp

@@ -52,7 +52,7 @@ TEST(WrenchEquivalenceFactor, error_1) {
   Pose3 kMj = Pose3(Rot3(), Point3(-2, 0, 0));
   Vector6 screw_axis;
   screw_axis << 0, 0, 1, 0, 1, 0;
-  auto joint = make_joint(kMj, screw_axis);
+  auto [joint, links] = make_joint(kMj, screw_axis);
   auto factor = WrenchEquivalenceFactor(example::cost_model, joint, 777);
 
   // Check evaluateError.
@@ -79,7 +79,7 @@ TEST(WrenchEquivalenceFactor, error_2) {
   Pose3 kMj = Pose3(Rot3(), Point3(-2, 0, 0));
   Vector6 screw_axis;
   screw_axis << 0, 0, 1, 0, 1, 0;
-  auto joint = make_joint(kMj, screw_axis);
+  auto [joint, links] = make_joint(kMj, screw_axis);
   auto factor = WrenchEquivalenceFactor(example::cost_model, joint, 777);
 
   // Check evaluateError.
@@ -107,7 +107,7 @@ TEST(WrenchEquivalenceFactor, error_3) {
   Pose3 kMj = Pose3(Rot3(), Point3(0, 0, -2));
   Vector6 screw_axis;
   screw_axis << 1, 0, 0, 0, -1, 0;
-  auto joint = make_joint(kMj, screw_axis);
+  auto [joint, links] = make_joint(kMj, screw_axis);
   auto factor = WrenchEquivalenceFactor(example::cost_model, joint, 777);
 
   // Check evaluateError.

tests/testWrenchPlanarFactor.cpp

@@ -35,7 +35,7 @@ gtsam::noiseModel::Gaussian::shared_ptr cost_model =
     gtsam::noiseModel::Gaussian::Covariance(gtsam::I_3x3);
 const DynamicsSymbol wrench_key = WrenchKey(2, 1, 777);
 gtsam::Pose3 kMj;  // doesn't matter
-auto joint = make_joint(kMj, gtsam::Z_6x1);
+auto [joint, links] = make_joint(kMj, gtsam::Z_6x1);
 }  // namespace example
 
 // Test wrench planar factor for x-axis