From 73b7772f8ef21897989a4bb0e838bdaeb350465a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 30 Nov 2021 09:00:27 -0500 Subject: [PATCH 01/11] serialization for Links --- gtdynamics/universal_robot/Link.h | 32 +++++++++++++++++++++++++++++++ tests/testLink.cpp | 21 ++++++++++++++++++++ 2 files changed, 53 insertions(+) diff --git a/gtdynamics/universal_robot/Link.h b/gtdynamics/universal_robot/Link.h index d05ce744..1a4a341e 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 { 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 { /// Unfix the link void unfix() { is_fixed_ = false; } + + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + 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 : public Testable {}; + +} // namespace gtsam 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 #include #include +#include #include #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); From b85bb931731989e128af98e073a3fed732dcc93d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 30 Nov 2021 10:53:17 -0500 Subject: [PATCH 02/11] serialize all the joints --- gtdynamics/universal_robot/HelicalJoint.h | 33 +++++++++-- gtdynamics/universal_robot/Joint.h | 49 ++++++++++++++++- gtdynamics/universal_robot/PrismaticJoint.h | 24 ++++++++ gtdynamics/universal_robot/RevoluteJoint.h | 24 ++++++++ tests/testHelicalJoint.cpp | 27 ++++++++- tests/testPrismaticJoint.cpp | 25 ++++++++- tests/testRevoluteJoint.cpp | 61 ++++++++++----------- 7 files changed, 199 insertions(+), 44 deletions(-) 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 ¶meters = JointParams()) + const LinkSharedPtr &parent_link, + const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, + double thread_pitch, + const JointParams ¶meters = 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 + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Joint); + } + + /// @} }; } // namespace gtdynamics + +namespace gtsam { + +template <> +struct traits + : public Testable {}; + +} // namespace gtsam diff --git a/gtdynamics/universal_robot/Joint.h b/gtdynamics/universal_robot/Joint.h index 9a630c7b..e1086761 100644 --- a/gtdynamics/universal_robot/Joint.h +++ b/gtdynamics/universal_robot/Joint.h @@ -20,9 +20,9 @@ #include #include #include -#include #include #include +#include #include #include @@ -80,6 +80,20 @@ struct JointParams { /// Constructor JointParams() {} + + /** Serialization function */ + friend class boost::serialization::access; + template + 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 { 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 { * @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 + 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 : public Testable {}; + +} // 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 + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Joint); + } + + /// @} }; } // namespace gtdynamics + +namespace gtsam { + +template <> +struct traits + : public Testable {}; + +} // 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 + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Joint); + } + + /// @} }; } // namespace gtdynamics + +namespace gtsam { + +template <> +struct traits + : public Testable {}; + +} // namespace gtsam diff --git a/tests/testHelicalJoint.cpp b/tests/testHelicalJoint.cpp index 11376767..5e3c988f 100644 --- a/tests/testHelicalJoint.cpp +++ b/tests/testHelicalJoint.cpp @@ -14,16 +14,17 @@ #include #include #include +#include +#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 namespace gtsam; /** * Construct a Screw joint via Parameters and ensure all values are as @@ -94,6 +95,26 @@ TEST(Joint, params_constructor) { j1->parameters().scalar_limits.value_limit_threshold)); } +BOOST_CLASS_EXPORT(gtdynamics::HelicalJoint) +BOOST_CLASS_EXPORT(gtdynamics::RevoluteJoint) + +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( + 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/testPrismaticJoint.cpp b/tests/testPrismaticJoint.cpp index f9d04193..5c0ef034 100644 --- a/tests/testPrismaticJoint.cpp +++ b/tests/testPrismaticJoint.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/universal_robot/PrismaticJoint.h" @@ -22,7 +23,7 @@ #include "gtdynamics/utils/utils.h" using namespace gtdynamics; -using gtsam::assert_equal, gtsam::Pose3, gtsam::Point3, gtsam::Rot3; +using namespace gtsam; /** * Construct a Prismatic joint via JointParams and ensure all values are as @@ -39,7 +40,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( 1, "j1", Pose3(Rot3::Rx(1.5707963268), Point3(0, 0, 2)), l1, l2, j1_axis, @@ -98,6 +99,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( + 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..8674bc19 100644 --- a/tests/testRevoluteJoint.cpp +++ b/tests/testRevoluteJoint.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/universal_robot/RevoluteJoint.h" @@ -23,21 +24,7 @@ #include "gtdynamics/utils/utils.h" #include "gtdynamics/utils/values.h" -using gtsam::assert_equal; -using gtsam::Matrix; -using gtsam::Matrix61; -using gtsam::Matrix66; -using gtsam::numericalDerivative11; -using gtsam::numericalDerivative21; -using gtsam::numericalDerivative22; -using gtsam::Point3; -using gtsam::Pose3; -using gtsam::Rot3; -using gtsam::Values; -using gtsam::Vector; -using gtsam::Vector3; -using gtsam::Vector6; - +using namespace gtsam; using namespace gtdynamics; auto robot = simple_urdf::getRobot(); @@ -53,17 +40,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 +82,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 +105,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 +140,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 +198,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); From 710e2b6e699248cb163b25095fd0feceb94365d6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 30 Nov 2021 10:53:45 -0500 Subject: [PATCH 03/11] serialize the Robot class --- gtdynamics/universal_robot/Robot.h | 46 ++++++++++++++++++++++++++++++ tests/testRobot.cpp | 31 ++++++++++++++++++++ 2 files changed, 77 insertions(+) diff --git a/gtdynamics/universal_robot/Robot.h b/gtdynamics/universal_robot/Robot.h index 7e0247a5..efb25382 100644 --- a/gtdynamics/universal_robot/Robot.h +++ b/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 &prior_link_name) const; + + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + 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 : public Testable {}; + +} // namespace gtsam diff --git a/tests/testRobot.cpp b/tests/testRobot.cpp index b7f10d9c..d494bc75 100644 --- a/tests/testRobot.cpp +++ b/tests/testRobot.cpp @@ -16,8 +16,14 @@ #include #include #include +#include #include +#include + +#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,31 @@ 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)); +} + +// 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); From 22a1dbb1e312c1eadba2a7f75402829541a33d22 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 30 Nov 2021 11:26:59 -0500 Subject: [PATCH 04/11] make robot class pickleable --- gtdynamics.i | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtdynamics.i b/gtdynamics.i index 1258513a..27895b63 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -305,6 +305,12 @@ class Robot { gtsam::Values forwardKinematics( const gtsam::Values &known_values, size_t t, const boost::optional &prior_link_name) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; #include From 3dab7bba29de64c7fc38200c86a78e33754ca386 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 30 Nov 2021 14:34:11 -0500 Subject: [PATCH 05/11] Update Ubuntu version to 20.04 --- .github/workflows/linux-ci.yml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index 6348fee4..cd30579c 100644 --- a/.github/workflows/linux-ci.yml +++ b/.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" From d3e58e940b5f14679d4ff3d0b7a34c04aa591a68 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Dec 2021 20:03:21 -0500 Subject: [PATCH 06/11] address review comments --- tests/testHelicalJoint.cpp | 5 ++++- tests/testPrismaticJoint.cpp | 5 ++++- tests/testRevoluteJoint.cpp | 16 +++++++++++++++- tests/testRobot.cpp | 8 ++++---- 4 files changed, 27 insertions(+), 7 deletions(-) diff --git a/tests/testHelicalJoint.cpp b/tests/testHelicalJoint.cpp index 5e3c988f..7e2a25d0 100644 --- a/tests/testHelicalJoint.cpp +++ b/tests/testHelicalJoint.cpp @@ -24,7 +24,10 @@ #include "gtdynamics/utils/utils.h" using namespace gtdynamics; -using namespace gtsam; +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 diff --git a/tests/testPrismaticJoint.cpp b/tests/testPrismaticJoint.cpp index 5c0ef034..d86e4d62 100644 --- a/tests/testPrismaticJoint.cpp +++ b/tests/testPrismaticJoint.cpp @@ -23,7 +23,10 @@ #include "gtdynamics/utils/utils.h" using namespace gtdynamics; -using namespace gtsam; +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 diff --git a/tests/testRevoluteJoint.cpp b/tests/testRevoluteJoint.cpp index 8674bc19..eb8cad54 100644 --- a/tests/testRevoluteJoint.cpp +++ b/tests/testRevoluteJoint.cpp @@ -24,7 +24,21 @@ #include "gtdynamics/utils/utils.h" #include "gtdynamics/utils/values.h" -using namespace gtsam; +using gtsam::assert_equal; +using gtsam::Matrix; +using gtsam::Matrix61; +using gtsam::Matrix66; +using gtsam::numericalDerivative11; +using gtsam::numericalDerivative21; +using gtsam::numericalDerivative22; +using gtsam::Point3; +using gtsam::Pose3; +using gtsam::Rot3; +using gtsam::Values; +using gtsam::Vector; +using gtsam::Vector3; +using gtsam::Vector6; + using namespace gtdynamics; auto robot = simple_urdf::getRobot(); diff --git a/tests/testRobot.cpp b/tests/testRobot.cpp index d494bc75..9cc43dfd 100644 --- a/tests/testRobot.cpp +++ b/tests/testRobot.cpp @@ -243,8 +243,8 @@ TEST(Robot, Equality) { Robot robot2 = CreateRobotFromFile( kSdfPath + std::string("test/four_bar_linkage_pure.sdf")); - // EXPECT(robot1 == robot2); - // EXPECT(robot1.equals(robot2)); + EXPECT(robot1 == robot2); + EXPECT(robot1.equals(robot2)); } // Declaration needed for serialization of derived class. @@ -258,8 +258,8 @@ TEST(Robot, Serialization) { using namespace gtsam::serializationTestHelpers; EXPECT(equalsObj(robot)); - // EXPECT(equalsXML(robot)); - // EXPECT(equalsBinary(robot)); + EXPECT(equalsXML(robot)); + EXPECT(equalsBinary(robot)); } int main() { From da13f7fd6b4f840cf9bb34f2ce8768709444fe23 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Dec 2021 20:03:54 -0500 Subject: [PATCH 07/11] update file brief for StaticsSlice.cpp --- gtdynamics/statics/StaticsSlice.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 */ From 8be7e9043c58b526e237b7f2c484eb951c31d2c6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 5 Dec 2021 22:50:59 -0500 Subject: [PATCH 08/11] address Gerry's comments --- gtdynamics/universal_robot/Joint.h | 13 +++++++++++++ gtdynamics/universal_robot/Link.h | 1 - gtdynamics/universal_robot/Robot.h | 1 + tests/testHelicalJoint.cpp | 1 - 4 files changed, 14 insertions(+), 2 deletions(-) diff --git a/gtdynamics/universal_robot/Joint.h b/gtdynamics/universal_robot/Joint.h index e1086761..e151fdae 100644 --- a/gtdynamics/universal_robot/Joint.h +++ b/gtdynamics/universal_robot/Joint.h @@ -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 + 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); + } }; /** @@ -85,6 +96,8 @@ struct JointParams { friend class boost::serialization::access; template 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); diff --git a/gtdynamics/universal_robot/Link.h b/gtdynamics/universal_robot/Link.h index 1a4a341e..ff79aed1 100644 --- a/gtdynamics/universal_robot/Link.h +++ b/gtdynamics/universal_robot/Link.h @@ -227,7 +227,6 @@ class Link : public boost::enable_shared_from_this { ar &BOOST_SERIALIZATION_NVP(bMlink_); ar &BOOST_SERIALIZATION_NVP(is_fixed_); ar &BOOST_SERIALIZATION_NVP(fixed_pose_); - ar &BOOST_SERIALIZATION_NVP(joints_); } /// @} diff --git a/gtdynamics/universal_robot/Robot.h b/gtdynamics/universal_robot/Robot.h index efb25382..6296ce6d 100644 --- a/gtdynamics/universal_robot/Robot.h +++ b/gtdynamics/universal_robot/Robot.h @@ -115,6 +115,7 @@ class Robot { /// 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 diff --git a/tests/testHelicalJoint.cpp b/tests/testHelicalJoint.cpp index 7e2a25d0..4b66251e 100644 --- a/tests/testHelicalJoint.cpp +++ b/tests/testHelicalJoint.cpp @@ -99,7 +99,6 @@ TEST(Joint, params_constructor) { } BOOST_CLASS_EXPORT(gtdynamics::HelicalJoint) -BOOST_CLASS_EXPORT(gtdynamics::RevoluteJoint) TEST(HelicalJoint, Serialization) { auto robot = simple_urdf::getRobot(); From 1b8707e27afa0cd13d8fe92fe60be38f30815f40 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 6 Dec 2021 10:50:28 -0500 Subject: [PATCH 09/11] Ubuntu 18 for GCC --- .github/workflows/linux-ci.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index cd30579c..7de42c54 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -17,12 +17,12 @@ 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-20.04-gcc-9, ubuntu-20.04-clang-10] + name: [ubuntu-18.04-gcc-9, ubuntu-20.04-clang-10] build_type: [Debug, Release] include: - - name: ubuntu-20.04-gcc-9 - os: ubuntu-20.04 + - name: ubuntu-18.04-gcc-9 + os: ubuntu-18.04 compiler: gcc version: "9" From fb36a65d2a308c151e9ad7ccf600259d637ef3f7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 18 Dec 2021 08:03:07 -0500 Subject: [PATCH 10/11] add inequality test --- tests/testRobot.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/tests/testRobot.cpp b/tests/testRobot.cpp index 9cc43dfd..a0bdad6b 100644 --- a/tests/testRobot.cpp +++ b/tests/testRobot.cpp @@ -245,6 +245,14 @@ TEST(Robot, Equality) { 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. From c5354171dc2c70d024f77078846d8b94d66c2a45 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 19 Dec 2021 20:10:47 -0500 Subject: [PATCH 11/11] remove pickle method since we no longer need it --- gtdynamics.i | 3 --- 1 file changed, 3 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index 27895b63..19c2a5cc 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -308,9 +308,6 @@ class Robot { // enabling serialization functionality void serialize() const; - - // enable pickling in python - void pickle() const; }; #include