diff --git a/gtdynamics/universal_robot/Joint.cpp b/gtdynamics/universal_robot/Joint.cpp index ff926268..24df3159 100644 --- a/gtdynamics/universal_robot/Joint.cpp +++ b/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 ¶meters) : 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 diff --git a/gtdynamics/universal_robot/Joint.h b/gtdynamics/universal_robot/Joint.h index 5591783e..58c3e67e 100644 --- a/gtdynamics/universal_robot/Joint.h +++ b/gtdynamics/universal_robot/Joint.h @@ -150,9 +150,12 @@ class Joint : public std::enable_shared_from_this { /// Rest transform to child link CoM frame from joint frame. Pose3 jMc_; - using LinkSharedPtr = std::shared_ptr; - 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; + 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 { * @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 ¶meters = JointParams()); @@ -228,19 +231,19 @@ class Joint : public std::enable_shared_from_this { /// 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 links() const { - return std::vector{parent_link_, child_link_}; + return std::vector{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 ¶meters() const { return parameters_; } diff --git a/tests/make_joint.h b/tests/make_joint.h index a9945ecf..3f5cc53e 100644 --- a/tests/make_joint.h +++ b/tests/make_joint.h @@ -17,8 +17,16 @@ #include 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> make_joint( + gtsam::Pose3 cMp, gtsam::Vector6 cScrewAxis) { // create links std::string name = "l1"; double mass = 100; @@ -26,9 +34,8 @@ JointConstSharedPtr make_joint(gtsam::Pose3 cMp, gtsam::Vector6 cScrewAxis) { gtsam::Pose3 bMcom; gtsam::Pose3 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)); + auto l1 = std::make_shared(1, name, mass, inertia, bMcom, bMl); + auto l2 = std::make_shared(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(1, "j1", bMj, l1, l2, jScrewAxis, + auto joint = std::make_shared(1, "j1", bMj, l1, l2, jScrewAxis, joint_params); + l1->addJoint(joint); + l2->addJoint(joint); + std::vector links{l1, l2}; + return {joint, links}; } } // namespace gtdynamics diff --git a/tests/testPoseFactor.cpp b/tests/testPoseFactor.cpp index 750a9451..05381061 100644 --- a/tests/testPoseFactor.cpp +++ b/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); diff --git a/tests/testTorqueFactor.cpp b/tests/testTorqueFactor.cpp index 0f9c5037..245701e6 100644 --- a/tests/testTorqueFactor.cpp +++ b/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); diff --git a/tests/testTwistAccelFactor.cpp b/tests/testTwistAccelFactor.cpp index b4c48b79..1b34d198 100644 --- a/tests/testTwistAccelFactor.cpp +++ b/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; diff --git a/tests/testTwistFactor.cpp b/tests/testTwistFactor.cpp index 11106453..ffa653af 100644 --- a/tests/testTwistFactor.cpp +++ b/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; diff --git a/tests/testWrenchEquivalenceFactor.cpp b/tests/testWrenchEquivalenceFactor.cpp index 825ebc44..0d4c7698 100644 --- a/tests/testWrenchEquivalenceFactor.cpp +++ b/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. diff --git a/tests/testWrenchPlanarFactor.cpp b/tests/testWrenchPlanarFactor.cpp index 046c97f1..e283f4d4 100644 --- a/tests/testWrenchPlanarFactor.cpp +++ b/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