Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix Memory Leak #393

Merged
merged 4 commits into from
Oct 22, 2024
Merged

Fix Memory Leak #393

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
f0f40c5
weak pointers to Links in Joint
varunagrawal Oct 22, 2024
476f5a1
update test fixture to return joint and links
varunagrawal Oct 22, 2024
a53290b
update tests
varunagrawal Oct 22, 2024
8238df2
improve syntax
varunagrawal Oct 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions gtdynamics/universal_robot/Joint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

/* ************************************************************************* */
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down
19 changes: 11 additions & 8 deletions gtdynamics/universal_robot/Joint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down Expand Up @@ -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());

Expand Down Expand Up @@ -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_; }
Expand Down
23 changes: 17 additions & 6 deletions tests/make_joint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
12 changes: 8 additions & 4 deletions tests/testPoseFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,8 @@ 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_and_links = make_joint(cMp, screw_axis);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

auto [joint, links] = ?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you. Updated!

auto joint = joint_and_links.first;

// Create factor
auto factor = PoseFactor(example::wTp_key, example::wTc_key, example::q_key,
Expand Down Expand Up @@ -77,7 +78,8 @@ 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_and_links = make_joint(cMp, screw_axis);
auto joint = joint_and_links.first;
auto factor = PoseFactor(example::wTp_key, example::wTc_key, example::q_key,
example::cost_model, joint);

Expand Down Expand Up @@ -111,7 +113,8 @@ 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_and_links = make_joint(cMp, screw_axis);
auto joint = joint_and_links.first;
auto factor = PoseFactor(example::wTp_key, example::wTc_key, example::q_key,
example::cost_model, joint);

Expand All @@ -129,7 +132,8 @@ 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_and_links = make_joint(cMp, screw_axis);
auto joint = joint_and_links.first;
auto factor = PoseFactor(example::wTp_key, example::wTc_key, example::q_key,
example::cost_model, joint);

Expand Down
3 changes: 2 additions & 1 deletion tests/testTorqueFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,8 @@ TEST(TorqueFactor, error) {
gtsam::Vector6 screw_axis;
screw_axis << 0, 0, 1, 0, 1, 0;

auto joint = make_joint(kMj, screw_axis);
auto joint_and_links = make_joint(kMj, screw_axis);
auto joint = joint_and_links.first;

// Create factor.
auto cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1);
Expand Down
6 changes: 4 additions & 2 deletions tests/testTwistAccelFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,8 @@ TEST(TwistAccelFactor, error) {
gtsam::Vector6 screw_axis;
screw_axis << 0, 0, 1, 0, 1, 0;

auto joint = make_joint(cMp, screw_axis);
auto joint_and_links = make_joint(cMp, screw_axis);
auto joint = joint_and_links.first;

// create factor
auto factor = TwistAccelFactor(example::cost_model, joint, 0);
Expand Down Expand Up @@ -85,7 +86,8 @@ 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_and_links = make_joint(cMp, screw_axis);
auto joint = joint_and_links.first;

auto factor = TwistAccelFactor(example::cost_model, joint, 0);
double q = 0, qVel = 0, qAccel = -9.8;
Expand Down
3 changes: 2 additions & 1 deletion tests/testTwistFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,8 @@ TEST(TwistFactor, error) {
gtsam::Vector6 screw_axis;
screw_axis << 0, 0, 1, 0, 1, 0;

auto joint = make_joint(cMp, screw_axis);
auto joint_and_links = make_joint(cMp, screw_axis);
auto joint = joint_and_links.first;

auto factor = TwistFactor(example::cost_model, joint, 0);
double q = M_PI / 4, qVel = 10;
Expand Down
9 changes: 6 additions & 3 deletions tests/testWrenchEquivalenceFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@ 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_and_links = make_joint(kMj, screw_axis);
auto joint = joint_and_links.first;
auto factor = WrenchEquivalenceFactor(example::cost_model, joint, 777);

// Check evaluateError.
Expand All @@ -79,7 +80,8 @@ 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_and_links = make_joint(kMj, screw_axis);
auto joint = joint_and_links.first;
auto factor = WrenchEquivalenceFactor(example::cost_model, joint, 777);

// Check evaluateError.
Expand Down Expand Up @@ -107,7 +109,8 @@ 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_and_links = make_joint(kMj, screw_axis);
auto joint = joint_and_links.first;
auto factor = WrenchEquivalenceFactor(example::cost_model, joint, 777);

// Check evaluateError.
Expand Down
3 changes: 2 additions & 1 deletion tests/testWrenchPlanarFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,8 @@ 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_and_links = make_joint(kMj, gtsam::Z_6x1);
auto joint = joint_and_links.first;
} // namespace example

// Test wrench planar factor for x-axis
Expand Down
Loading