From 5b13133e8cd76df3ccb58b43a46883d53ae7fcd1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 14 Feb 2022 16:37:18 -0500 Subject: [PATCH 01/17] test IK for Panda, initial check-in --- tests/testKinematicsSlice.cpp | 81 +++++++++++++++++++++++++++++++++++ 1 file changed, 81 insertions(+) diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index 4b3298091..9da2b2d27 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include "contactGoalsExample.h" @@ -81,6 +82,86 @@ TEST(Slice, InverseKinematics) { } } +// Values tonisFunction(const Slice& slice, const Robot& robot, +// const ContactGoals& contact_goals) const { +// auto graph = this->graph(slice, robot); + +// // Add prior. +// graph.add(jointAngleObjectives(slice, robot)); + +// graph.addPrior(PoseKey(0, slice.k), +// gtsam::Pose3(), nullptr); + +// auto initial_values = initialValues(slice, robot); + +// return optimize(graph, initial_values); +// } + +gtsam::Values ikWithPose(const Kinematics* self, const Slice& slice, + const Robot& robot, + const std::map constrained_poses, + const std::map desired_poses, + const gtsam::Vector7& initial) { + auto graph = self->graph(slice, robot); + + // Add prior on joint angles to constrain the solution + graph.add(self->jointAngleObjectives(slice, robot)); + + // Add priors on link poses with constrained poses from argument + // TODO: do real pose constraints - ask Yetong + for (auto&& kv : constrained_poses) { + const size_t link_index = kv.first; + const gtsam::Pose3& constrained_pose = kv.second; + graph.addPrior(PoseKey(link_index, slice.k), constrained_pose, + gtsam::noiseModel::Constrained::All(6)); + } + + // Add priors on link poses with desired poses from argument + for (auto&& kv : desired_poses) { + const size_t link_index = kv.first; + const gtsam::Pose3& desired_pose = kv.second; + graph.addPrior(PoseKey(link_index, slice.k), desired_pose, + nullptr); + } + + // TODO: make use of initial? + auto initial_values = self->initialValues(slice, robot); + + return self->optimize(graph, {}, initial_values); +} + +TEST(Slice, PandaIK) { + using gtsam::Pose3; + using gtsam::Rot3; + using gtsam::Values; + using gtsam::Vector7; + + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + // Create a slice. + const size_t k = 777; + const Slice slice(k); + + // Instantiate kinematics algorithms + KinematicsParameters parameters; + parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + Kinematics kinematics(parameters); + + Vector7 initial = Vector7::Zero(); + Rot3 sR7({{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}); + Pose3 sM7(sR7, Point3(0.0882972, 0.00213401, 0.933844)); + auto base_link = robot.link("link0"); + const Pose3 sM0 = base_link->bMcom(); + auto values = + ikWithPose(&kinematics, slice, robot, {{0, sM0}}, {{7, sM7}}, initial); + Vector7 optimal_q; + for (size_t j = 0; j < 7; j++) + optimal_q[j] = values.at(JointAngleKey(j, slice.k)); + EXPECT(assert_equal(initial, optimal_q)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 5197e2325b6ae05cdb051298c805bdd166f2f6e0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 14 Feb 2022 17:27:01 -0500 Subject: [PATCH 02/17] Make fix/unfix methods const --- gtdynamics/universal_robot/Robot.cpp | 4 ++-- gtdynamics/universal_robot/Robot.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtdynamics/universal_robot/Robot.cpp b/gtdynamics/universal_robot/Robot.cpp index 48ea48e63..6857f26cb 100644 --- a/gtdynamics/universal_robot/Robot.cpp +++ b/gtdynamics/universal_robot/Robot.cpp @@ -77,7 +77,7 @@ LinkSharedPtr Robot::link(const std::string &name) const { return name_to_link_.at(name); } -Robot Robot::fixLink(const std::string &name) { +Robot Robot::fixLink(const std::string &name) const { if (name_to_link_.find(name) == name_to_link_.end()) { throw std::runtime_error("no link named " + name); } @@ -87,7 +87,7 @@ Robot Robot::fixLink(const std::string &name) { return fixed_robot; } -Robot Robot::unfixLink(const std::string &name) { +Robot Robot::unfixLink(const std::string &name) const { if (name_to_link_.find(name) == name_to_link_.end()) { throw std::runtime_error("no link named " + name); } diff --git a/gtdynamics/universal_robot/Robot.h b/gtdynamics/universal_robot/Robot.h index 6296ce6d8..1ea88ef6a 100644 --- a/gtdynamics/universal_robot/Robot.h +++ b/gtdynamics/universal_robot/Robot.h @@ -89,7 +89,7 @@ class Robot { * @param name The name of the link to fix. * @return Robot */ - Robot fixLink(const std::string &name); + Robot fixLink(const std::string &name) const; /** * @brief Return a copy of this robot after unfixing the link corresponding to @@ -98,7 +98,7 @@ class Robot { * @param name The name of the link to unfix. * @return Robot */ - Robot unfixLink(const std::string &name); + Robot unfixLink(const std::string &name) const; /// Return the joint corresponding to the input string. JointSharedPtr joint(const std::string &name) const; From ea19ce1a563539316ff1ab6b7eaae82b55bfef8a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 14 Feb 2022 18:10:39 -0500 Subject: [PATCH 03/17] Removed logmap and use gtsam --- gtdynamics/universal_robot/Joint.cpp | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/gtdynamics/universal_robot/Joint.cpp b/gtdynamics/universal_robot/Joint.cpp index cbb3fd1bf..57bd5e63d 100644 --- a/gtdynamics/universal_robot/Joint.cpp +++ b/gtdynamics/universal_robot/Joint.cpp @@ -266,15 +266,6 @@ std::ostream &operator<<(std::ostream &os, const JointSharedPtr &j) { return j->to_stream(os); } -/* ************************************************************************* */ -// TODO(yetong): Remove the logmap and use the one in gtsam/slam/expressions.h. -template -gtsam::Expression::TangentVector> logmap( - const gtsam::Expression &x1, const gtsam::Expression &x2) { - return gtsam::Expression::TangentVector>( - gtsam::traits::Logmap, between(x1, x2)); -} - /* ************************************************************************* */ gtsam::Vector6_ Joint::poseConstraint(uint64_t t) const { using gtsam::Pose3_; @@ -291,7 +282,7 @@ gtsam::Vector6_ Joint::poseConstraint(uint64_t t) const { Pose3_ wTc_hat = wTp * pTc; // Return the error in tangent space - return gtdynamics::logmap(wTc, wTc_hat); + return gtsam::logmap(wTc, wTc_hat); } /* ************************************************************************* */ From 54b26c103231c3cc6247d43c600832956d06f6b5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 14 Feb 2022 18:10:58 -0500 Subject: [PATCH 04/17] Add constraints for fixed links --- gtdynamics/kinematics/KinematicsSlice.cpp | 21 +++++ tests/testKinematicsSlice.cpp | 95 ++++++++++++++--------- 2 files changed, 78 insertions(+), 38 deletions(-) diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 852ba8851..9353fcfad 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -18,6 +18,7 @@ #include #include #include +#include namespace gtdynamics { @@ -63,6 +64,26 @@ EqualityConstraints Kinematics::constraints(const Slice& slice, tolerance); } + // Constrain fixed links + for (auto&& link : robot.links()) { + if (link->isFixed()) { + using gtsam::Pose3_; + + // Get an expression for the unknown link pose. + Pose3_ bTcom(PoseKey(link->id(), slice.k)); + + // Kust make sure it does not move from its original rest pose + Pose3_ bMcom(link->bMcom()); + + // Create expression to calculate the error in tangent space + auto constraint_expr = gtsam::logmap(bTcom, bMcom); + + // Add constriant + constraints.emplace_shared>(constraint_expr, + tolerance); + } + } + return constraints; } diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index 9da2b2d27..e053ae334 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -24,21 +24,21 @@ using gtsam::Point3; using std::map; using std::string; +// Create a slice. +static const size_t k = 777; +static const Slice kSlice(k); + TEST(Slice, InverseKinematics) { // Load robot and establish contact/goal pairs using namespace contact_goals_example; - // Create a slice. - const size_t k = 777; - const Slice slice(k); - // Instantiate kinematics algorithms KinematicsParameters parameters; parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; Kinematics kinematics(parameters); // Create initial values - auto values = kinematics.initialValues(slice, robot, 0.0); + auto values = kinematics.initialValues(kSlice, robot, 0.0); EXPECT_LONGS_EQUAL(13 + 12, values.size()); // Set twists to zero for FK. TODO(frank): separate kinematics from velocity? @@ -57,16 +57,16 @@ TEST(Slice, InverseKinematics) { EXPECT(goal.satisfied(fk, k, 0.05)); } - auto graph = kinematics.graph(slice, robot); + auto graph = kinematics.graph(kSlice, robot); EXPECT_LONGS_EQUAL(12, graph.size()); - auto objectives = kinematics.pointGoalObjectives(slice, contact_goals); + auto objectives = kinematics.pointGoalObjectives(kSlice, contact_goals); EXPECT_LONGS_EQUAL(4, objectives.size()); - auto objectives2 = kinematics.jointAngleObjectives(slice, robot); + auto objectives2 = kinematics.jointAngleObjectives(kSlice, robot); EXPECT_LONGS_EQUAL(12, objectives2.size()); - auto result = kinematics.inverse(slice, robot, contact_goals); + auto result = kinematics.inverse(kSlice, robot, contact_goals); // Check that well-determined graph.add(objectives); @@ -82,52 +82,59 @@ TEST(Slice, InverseKinematics) { } } -// Values tonisFunction(const Slice& slice, const Robot& robot, +// Values tonisFunction(const Slice& kSlice, const Robot& robot, // const ContactGoals& contact_goals) const { -// auto graph = this->graph(slice, robot); +// auto graph = this->graph(kSlice, robot); // // Add prior. -// graph.add(jointAngleObjectives(slice, robot)); +// graph.add(jointAngleObjectives(kSlice, robot)); -// graph.addPrior(PoseKey(0, slice.k), +// graph.addPrior(PoseKey(0, k), // gtsam::Pose3(), nullptr); -// auto initial_values = initialValues(slice, robot); +// auto initial_values = initialValues(kSlice, robot); // return optimize(graph, initial_values); // } -gtsam::Values ikWithPose(const Kinematics* self, const Slice& slice, +gtsam::Values ikWithPose(const Kinematics* self, const Slice& kSlice, const Robot& robot, - const std::map constrained_poses, const std::map desired_poses, const gtsam::Vector7& initial) { - auto graph = self->graph(slice, robot); + auto graph = self->graph(kSlice, robot); // Add prior on joint angles to constrain the solution - graph.add(self->jointAngleObjectives(slice, robot)); - - // Add priors on link poses with constrained poses from argument - // TODO: do real pose constraints - ask Yetong - for (auto&& kv : constrained_poses) { - const size_t link_index = kv.first; - const gtsam::Pose3& constrained_pose = kv.second; - graph.addPrior(PoseKey(link_index, slice.k), constrained_pose, - gtsam::noiseModel::Constrained::All(6)); - } + graph.add(self->jointAngleObjectives(kSlice, robot)); // Add priors on link poses with desired poses from argument for (auto&& kv : desired_poses) { const size_t link_index = kv.first; const gtsam::Pose3& desired_pose = kv.second; - graph.addPrior(PoseKey(link_index, slice.k), desired_pose, - nullptr); + graph.addPrior(PoseKey(link_index, k), desired_pose, nullptr); } + // Robot kinematics constraints + auto constraints = self->constraints(kSlice, robot); + // TODO: make use of initial? - auto initial_values = self->initialValues(slice, robot); + auto initial_values = self->initialValues(kSlice, robot); - return self->optimize(graph, {}, initial_values); + return self->optimize(graph, constraints, initial_values); +} + +TEST(Slice, panda_constraints) { + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + // Instantiate kinematics algorithms + KinematicsParameters parameters; + parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + Kinematics kinematics(parameters); + + // We should only have 7 constraints plus one for the fixed link + auto constraints = kinematics.constraints(kSlice, robot); + EXPECT_LONGS_EQUAL(8, constraints.size()); } TEST(Slice, PandaIK) { @@ -140,25 +147,37 @@ TEST(Slice, PandaIK) { CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); const Robot robot = panda.fixLink("link0"); - // Create a slice. - const size_t k = 777; - const Slice slice(k); - // Instantiate kinematics algorithms KinematicsParameters parameters; parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; Kinematics kinematics(parameters); + // We should only have 7 unknown joint angles and That is still 7 factors. + auto graph = kinematics.graph(kSlice, robot); + EXPECT_LONGS_EQUAL(7, graph.size()); + + // We should only have 8 unknown links and 7 unknown joint angles, i.e., 15 + // values: + auto initial_values = kinematics.initialValues(kSlice, robot); + EXPECT_LONGS_EQUAL(15, initial_values.size()); + Vector7 initial = Vector7::Zero(); Rot3 sR7({{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}); Pose3 sM7(sR7, Point3(0.0882972, 0.00213401, 0.933844)); auto base_link = robot.link("link0"); + auto values = ikWithPose(&kinematics, kSlice, robot, {{7, sM7}}, initial); + + // Check that base link did not budge (much) const Pose3 sM0 = base_link->bMcom(); - auto values = - ikWithPose(&kinematics, slice, robot, {{0, sM0}}, {{7, sM7}}, initial); + EXPECT(assert_equal(sM0, values.at(PoseKey(0, k)))); + + // Check that desired pose was achieved + EXPECT(assert_equal(sM7, values.at(PoseKey(7, k)))); + + // Check joint angles Vector7 optimal_q; for (size_t j = 0; j < 7; j++) - optimal_q[j] = values.at(JointAngleKey(j, slice.k)); + optimal_q[j] = values.at(JointAngleKey(j, k)); EXPECT(assert_equal(initial, optimal_q)); } From 7e4440218e0fbd3b10a4e0349bd35494774c5a1e Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 16 Feb 2022 17:34:47 +0100 Subject: [PATCH 05/17] initial values function prototype --- tests/testKinematicsSlice.cpp | 81 +++++++++++++++++++++++++++++------ 1 file changed, 68 insertions(+), 13 deletions(-) diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index e053ae334..ac57c444a 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "contactGoalsExample.h" @@ -97,7 +98,25 @@ TEST(Slice, InverseKinematics) { // return optimize(graph, initial_values); // } -gtsam::Values ikWithPose(const Kinematics* self, const Slice& kSlice, +gtsam::Values initialValues(const Slice& slice, const Robot& robot, + const gtsam::Vector7& initial) { + gtsam::Values values; + + // Initialize all joint angles. + for (auto&& joint : robot.joints()) { + InsertJointAngle(&values, joint->id(), slice.k, initial(joint->id())); + } + + // Initialize poses with fk of initialized values + auto fk = robot.forwardKinematics(values, k); + for (auto&& link : robot.links()) { + InsertPose(&values, link->id(), slice.k, fk.at(PoseKey(link->id(), k))); + } + + return values; +} + +gtsam::Values inverseWithPose(const Kinematics* self, const Slice& kSlice, const Robot& robot, const std::map desired_poses, const gtsam::Vector7& initial) { @@ -110,18 +129,47 @@ gtsam::Values ikWithPose(const Kinematics* self, const Slice& kSlice, for (auto&& kv : desired_poses) { const size_t link_index = kv.first; const gtsam::Pose3& desired_pose = kv.second; - graph.addPrior(PoseKey(link_index, k), desired_pose, nullptr); + // TODO: put it as the kinematic parameters + graph.addPrior(PoseKey(link_index, k), desired_pose, gtsam::noiseModel::Isotropic::Sigma(6,0.00005)); } // Robot kinematics constraints auto constraints = self->constraints(kSlice, robot); - // TODO: make use of initial? - auto initial_values = self->initialValues(kSlice, robot); + auto initial_values = initialValues(kSlice, robot, initial); return self->optimize(graph, constraints, initial_values); } +TEST(Slice, initial_values) { + + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + gtsam::Vector7 initial; + initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; + + gtsam::Values initial_values = initialValues(kSlice, robot, initial); + + // We should only have 7 values for joints and 8 for link poses + EXPECT_LONGS_EQUAL(15, initial_values.size()) + + // check that joint angles are the same + gtsam::Vector7 actual_initial; + for (size_t j = 0; j < 7; j++) + actual_initial[j] = initial_values.at(JointAngleKey(j, k)); + double tol = 1e-5; + EXPECT(assert_equal(initial, actual_initial, tol)) + + // check that the last fk is the same + gtsam::Rot3 sR7 {{ 0.98161623, 0.13223102, -0.13763916}, + { 0.08587125, -0.9499891 , -0.30024463}, + {-0.17045735, 0.28290575, -0.94387956}}; + gtsam::Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); + EXPECT(assert_equal(sT7, initial_values.at(PoseKey(7,k)), tol)) +} + TEST(Slice, panda_constraints) { const Robot panda = CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); @@ -161,24 +209,31 @@ TEST(Slice, PandaIK) { auto initial_values = kinematics.initialValues(kSlice, robot); EXPECT_LONGS_EQUAL(15, initial_values.size()); - Vector7 initial = Vector7::Zero(); - Rot3 sR7({{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}); - Pose3 sM7(sR7, Point3(0.0882972, 0.00213401, 0.933844)); - auto base_link = robot.link("link0"); - auto values = ikWithPose(&kinematics, kSlice, robot, {{7, sM7}}, initial); + Rot3 sR7 {{ 0.98161623, 0.13223102, -0.13763916}, + { 0.08587125, -0.9499891 , -0.30024463}, + {-0.17045735, 0.28290575, -0.94387956}}; + Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); + Vector7 initial; + initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; + Vector7 noise; + noise << 0.04, -0.1, 0.07, 0.14, -0.05, 0.02, 0.1; + auto values = inverseWithPose(&kinematics, kSlice, robot, {{7, sT7}}, initial+noise); // Check that base link did not budge (much) + auto base_link = robot.link("link0"); const Pose3 sM0 = base_link->bMcom(); - EXPECT(assert_equal(sM0, values.at(PoseKey(0, k)))); + double tol = 1e-5; + EXPECT(assert_equal(sM0, values.at(PoseKey(0, k)), tol)); // Check that desired pose was achieved - EXPECT(assert_equal(sM7, values.at(PoseKey(7, k)))); + EXPECT(assert_equal(sT7, values.at(PoseKey(7, k)), tol)); - // Check joint angles + // Check that joint angles are not too different Vector7 optimal_q; for (size_t j = 0; j < 7; j++) optimal_q[j] = values.at(JointAngleKey(j, k)); - EXPECT(assert_equal(initial, optimal_q)); + tol = 0.001; + EXPECT(assert_equal(initial, optimal_q, tol)); } int main() { From e5343447c6dba658ffd9c846cd2a31af45155477 Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 23 Feb 2022 01:24:05 +0100 Subject: [PATCH 06/17] Added new functions in kinematics --- gtdynamics/kinematics/Kinematics.h | 46 ++++- gtdynamics/kinematics/KinematicsInterval.cpp | 19 +- gtdynamics/kinematics/KinematicsSlice.cpp | 80 +++++++- .../kinematics/KinematicsTrajectory.cpp | 18 +- tests/testKinematicsSlice.cpp | 190 +++++++++++------- 5 files changed, 254 insertions(+), 99 deletions(-) diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 4d8d681fb..4cf00b8c7 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -131,29 +131,49 @@ class Kinematics : public Optimizer { EqualityConstraints pointGoalConstraints( const CONTEXT& context, const ContactGoals& contact_goals) const; + /** + * @fn Create pose goal objectives. + * @param context Slice or Interval instance. + * @param pose_goals goals for poses + * @returns graph with pose goal factors. + */ + template + gtsam::NonlinearFactorGraph poseGoalObjectives( + const CONTEXT& context, const Robot& robot, + const gtsam::Values& pose_goals) const; + /** * @fn Factors that minimize joint angles. * @param context Slice or Interval instance. * @param robot Robot specification from URDF/SDF. + * @param joint_priors Values where the mean of the priors is specified * @returns graph with prior factors on joint angles. */ template - gtsam::NonlinearFactorGraph jointAngleObjectives(const CONTEXT& context, - const Robot& robot) const; + gtsam::NonlinearFactorGraph jointAngleObjectives( + const CONTEXT& context, const Robot& robot, + const gtsam::Values& joint_priors = gtsam::Values()) const; /** * @fn Initialize kinematics. * - * Use wTcom for poses and zero-mean noise for joint angles. + * If no values in initial_joints are given, use wTcom for poses and zero-mean + * noise for joint angles. + * If values are given, initialize joints with their given values, and + * zero-mean noise to the ones that without a given value. Poses are + * initialized with their fk values. + * * * @param context Slice or Interval instance. * @param robot Robot specification from URDF/SDF. * @param gaussian_noise time step to check (default 0.1). + * @param initial_joints initial values for joints * @returns values with identity poses and zero joint angles. */ template - gtsam::Values initialValues(const CONTEXT& context, const Robot& robot, - double gaussian_noise = 0.1) const; + gtsam::Values initialValues( + const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.1, + const gtsam::Values& initial_joints = gtsam::Values()) const; /** * @fn Inverse kinematics given a set of contact goals. @@ -169,6 +189,22 @@ class Kinematics : public Optimizer { const ContactGoals& contact_goals, bool contact_goals_as_constraints = true) const; + /** + * @fn Inverse kinematics given a set of desired poses + * @fn This function does inverse kinematics separately on each slice + * @param context Slice or Interval instance + * @param robot Robot specification from URDF/SDF + * @param goal_poses goals for EE poses + * @param joint_priors optional argument to put priors centered on given + * values. If empty, the priors will be centered on the origin + * @return values with poses and joint angles + */ + template + gtsam::Values inverseWithPose( + const CONTEXT& context, const Robot& robot, + const gtsam::Values& goal_poses, + const gtsam::Values& joint_priors = gtsam::Values()) const; + /** * Interpolate using inverse kinematics: the goals are linearly interpolated. * @param context Interval instance diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index 5e571736e..e132c67cc 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -44,6 +44,17 @@ EqualityConstraints Kinematics::constraints( return constraints; } +template <> +NonlinearFactorGraph Kinematics::poseGoalObjectives( + const Interval& interval, const Robot& robot, + const gtsam::Values& goal_poses) const { + NonlinearFactorGraph graph; + for (size_t k = interval.k_start; k <= interval.k_end; k++) { + graph.add(poseGoalObjectives(Slice(k), robot, goal_poses)); + } + return graph; +} + template <> NonlinearFactorGraph Kinematics::pointGoalObjectives( const Interval& interval, const ContactGoals& contact_goals) const { @@ -66,10 +77,10 @@ EqualityConstraints Kinematics::pointGoalConstraints( template <> NonlinearFactorGraph Kinematics::jointAngleObjectives( - const Interval& interval, const Robot& robot) const { + const Interval& interval, const Robot& robot, const Values& mean) const { NonlinearFactorGraph graph; for (size_t k = interval.k_start; k <= interval.k_end; k++) { - graph.add(jointAngleObjectives(Slice(k), robot)); + graph.add(jointAngleObjectives(Slice(k), robot, mean)); } return graph; } @@ -77,10 +88,10 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( template <> Values Kinematics::initialValues(const Interval& interval, const Robot& robot, - double gaussian_noise) const { + double gaussian_noise, const gtsam::Values& joint_priors) const { Values values; for (size_t k = interval.k_start; k <= interval.k_end; k++) { - values.insert(initialValues(Slice(k), robot, gaussian_noise)); + values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors)); } return values; } diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 9353fcfad..7bc3cce4b 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -119,23 +119,46 @@ EqualityConstraints Kinematics::pointGoalConstraints( return constraints; } +template <> +NonlinearFactorGraph Kinematics::poseGoalObjectives( + const Slice& slice, const Robot& robot, + const gtsam::Values& goal_poses) const { + gtsam::NonlinearFactorGraph graph; + + // Add priors on link poses with desired poses from argument + for (auto&& link : robot.links()) { + auto pose_key = PoseKey(link->id(), slice.k); + if (goal_poses.exists(pose_key)) { + const gtsam::Pose3& desired_pose = goal_poses.at(pose_key); + // TODO: use poseprior from unstable gtsam slam or create new factors, to + // add pose from link7 + graph.addPrior(pose_key, desired_pose, p_.p_cost_model); + } + } + + return graph; +} + template <> NonlinearFactorGraph Kinematics::jointAngleObjectives( - const Slice& slice, const Robot& robot) const { + const Slice& slice, const Robot& robot, const Values& mean) const { NonlinearFactorGraph graph; // Minimize the joint angles. for (auto&& joint : robot.joints()) { const gtsam::Key key = JointAngleKey(joint->id(), slice.k); - graph.addPrior(key, 0.0, p_.prior_q_cost_model); + double joint_mean = 0.0; + if (mean.exists(key)) joint_mean = mean.at(key); + graph.addPrior(key, joint_mean, p_.prior_q_cost_model); } return graph; } template <> -Values Kinematics::initialValues(const Slice& slice, const Robot& robot, - double gaussian_noise) const { +Values Kinematics::initialValues( + const Slice& slice, const Robot& robot, double gaussian_noise, + const gtsam::Values& initial_joints) const { Values values; auto sampler_noise_model = @@ -143,14 +166,33 @@ Values Kinematics::initialValues(const Slice& slice, const Robot& robot, gtsam::Sampler sampler(sampler_noise_model); // Initialize all joint angles. + bool any_value = false; for (auto&& joint : robot.joints()) { - InsertJointAngle(&values, joint->id(), slice.k, sampler.sample()[0]); + auto key = JointAngleKey(joint->id(), slice.k); + double value; + if (initial_joints.exists(key)) { + value = initial_joints.at(key); + any_value = true; + } else + value = sampler.sample()[0]; + InsertJointAngle(&values, joint->id(), slice.k, value); } - // Initialize all poses. - for (auto&& link : robot.links()) { - const gtsam::Vector6 xi = sampler.sample(); - InsertPose(&values, link->id(), slice.k, link->bMcom().expmap(xi)); + // Maybe fk takes a long time, so only compute it if there was a given initial + // joint value in this slice + if (any_value) { + // Initialize poses with fk of initialized values + auto fk = robot.forwardKinematics(values, slice.k); + for (auto&& link : robot.links()) { + InsertPose(&values, link->id(), slice.k, + fk.at(PoseKey(link->id(), slice.k))); + } + } else { + // Initialize all poses. + for (auto&& link : robot.links()) { + const gtsam::Vector6 xi = sampler.sample(); + InsertPose(&values, link->id(), slice.k, link->bMcom().expmap(xi)); + } } return values; @@ -182,4 +224,24 @@ Values Kinematics::inverse(const Slice& slice, const Robot& robot, return optimize(graph, constraints, initial_values); } + +template <> +gtsam::Values Kinematics::inverseWithPose( + const Slice& slice, const Robot& robot, const gtsam::Values& goal_poses, + const gtsam::Values& joint_priors) const { + auto graph = this->graph(slice, robot); + + // Add prior on joint angles to constrain the solution + graph.add(this->jointAngleObjectives(slice, robot, joint_priors)); + + // Add priors on link poses with desired poses from argument + graph.add(this->poseGoalObjectives(slice, robot, goal_poses)); + + // Robot kinematics constraints + auto constraints = this->constraints(slice, robot); + + auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors); + + return this->optimize(graph, constraints, initial_values); +} } // namespace gtdynamics diff --git a/gtdynamics/kinematics/KinematicsTrajectory.cpp b/gtdynamics/kinematics/KinematicsTrajectory.cpp index 83a9ae374..d632ab92d 100644 --- a/gtdynamics/kinematics/KinematicsTrajectory.cpp +++ b/gtdynamics/kinematics/KinematicsTrajectory.cpp @@ -35,8 +35,8 @@ NonlinearFactorGraph Kinematics::graph(const Trajectory& trajectory, } template <> -EqualityConstraints Kinematics::constraints(const Trajectory& trajectory, - const Robot& robot) const { +EqualityConstraints Kinematics::constraints( + const Trajectory& trajectory, const Robot& robot) const { EqualityConstraints constraints; for (auto&& phase : trajectory.phases()) { constraints.add(this->constraints(phase, robot)); @@ -66,21 +66,23 @@ EqualityConstraints Kinematics::pointGoalConstraints( template <> NonlinearFactorGraph Kinematics::jointAngleObjectives( - const Trajectory& trajectory, const Robot& robot) const { + const Trajectory& trajectory, const Robot& robot, + const Values& mean) const { NonlinearFactorGraph graph; for (auto&& phase : trajectory.phases()) { - graph.add(jointAngleObjectives(phase, robot)); + graph.add(jointAngleObjectives(phase, robot, mean)); } return graph; } template <> -Values Kinematics::initialValues(const Trajectory& trajectory, - const Robot& robot, - double gaussian_noise) const { +Values Kinematics::initialValues( + const Trajectory& trajectory, const Robot& robot, double gaussian_noise, + const gtsam::Values& initial_joints) const { Values values; for (auto&& phase : trajectory.phases()) { - values.insert(initialValues(phase, robot, gaussian_noise)); + values.insert( + initialValues(phase, robot, gaussian_noise, initial_joints)); } return values; } diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index ac57c444a..a275b7af4 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -83,74 +83,32 @@ TEST(Slice, InverseKinematics) { } } -// Values tonisFunction(const Slice& kSlice, const Robot& robot, -// const ContactGoals& contact_goals) const { -// auto graph = this->graph(kSlice, robot); - -// // Add prior. -// graph.add(jointAngleObjectives(kSlice, robot)); - -// graph.addPrior(PoseKey(0, k), -// gtsam::Pose3(), nullptr); - -// auto initial_values = initialValues(kSlice, robot); - -// return optimize(graph, initial_values); -// } - -gtsam::Values initialValues(const Slice& slice, const Robot& robot, - const gtsam::Vector7& initial) { - gtsam::Values values; - - // Initialize all joint angles. - for (auto&& joint : robot.joints()) { - InsertJointAngle(&values, joint->id(), slice.k, initial(joint->id())); +gtsam::Values jointVectorToValues(const Robot& robot, + const gtsam::Vector7& joints) { + gtsam::Values joint_values; + for (auto&& j : robot.joints()) { + size_t joint_id = j->id(); + joint_values.insert(JointAngleKey(joint_id, k), joints(joint_id)); } - // Initialize poses with fk of initialized values - auto fk = robot.forwardKinematics(values, k); - for (auto&& link : robot.links()) { - InsertPose(&values, link->id(), slice.k, fk.at(PoseKey(link->id(), k))); - } - - return values; -} - -gtsam::Values inverseWithPose(const Kinematics* self, const Slice& kSlice, - const Robot& robot, - const std::map desired_poses, - const gtsam::Vector7& initial) { - auto graph = self->graph(kSlice, robot); - - // Add prior on joint angles to constrain the solution - graph.add(self->jointAngleObjectives(kSlice, robot)); - - // Add priors on link poses with desired poses from argument - for (auto&& kv : desired_poses) { - const size_t link_index = kv.first; - const gtsam::Pose3& desired_pose = kv.second; - // TODO: put it as the kinematic parameters - graph.addPrior(PoseKey(link_index, k), desired_pose, gtsam::noiseModel::Isotropic::Sigma(6,0.00005)); - } - - // Robot kinematics constraints - auto constraints = self->constraints(kSlice, robot); - - auto initial_values = initialValues(kSlice, robot, initial); - - return self->optimize(graph, constraints, initial_values); + return joint_values; } TEST(Slice, initial_values) { - + // Load robot from urdf file const Robot panda = - CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); const Robot robot = panda.fixLink("link0"); + Kinematics kinematics; + + // Create Values where initial joint angles are stored gtsam::Vector7 initial; initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; + gtsam::Values initial_joints = jointVectorToValues(robot, initial); - gtsam::Values initial_values = initialValues(kSlice, robot, initial); + gtsam::Values initial_values = + kinematics.initialValues(kSlice, robot, 0.0, initial_joints); // We should only have 7 values for joints and 8 for link poses EXPECT_LONGS_EQUAL(15, initial_values.size()) @@ -163,11 +121,93 @@ TEST(Slice, initial_values) { EXPECT(assert_equal(initial, actual_initial, tol)) // check that the last fk is the same - gtsam::Rot3 sR7 {{ 0.98161623, 0.13223102, -0.13763916}, - { 0.08587125, -0.9499891 , -0.30024463}, - {-0.17045735, 0.28290575, -0.94387956}}; + gtsam::Rot3 sR7{{0.98161623, 0.13223102, -0.13763916}, + {0.08587125, -0.9499891, -0.30024463}, + {-0.17045735, 0.28290575, -0.94387956}}; gtsam::Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); - EXPECT(assert_equal(sT7, initial_values.at(PoseKey(7,k)), tol)) + EXPECT(assert_equal(sT7, initial_values.at(PoseKey(7, k)), tol)) +} + +TEST(Slice, JointAngleObjectives) { + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + // Instantiate kinematics algorithms + KinematicsParameters parameters; + parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + Kinematics kinematics(parameters); + + // Priors with means at 0 + auto joint_priors = kinematics.jointAngleObjectives(kSlice, robot); + EXPECT_LONGS_EQUAL(7, joint_priors.size()) + + // Check means at 0 + gtsam::Vector7 means_vector; + means_vector << 0, 0, 0, 0, 0, 0, 0; + gtsam::Values expected_means = jointVectorToValues(robot, means_vector); + gtsam::Values initial = + kinematics.initialValues(kSlice, robot, 0.0, expected_means); + double tol = 1e-5; + EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) + + // Define some means different than 0 + gtsam::Values means; + means.insert(JointAngleKey(0, k), 1.0); + means.insert(JointAngleKey(2, k), 1.0); + means.insert(JointAngleKey(4, k), 1.0); + means.insert(JointAngleKey(6, k), 1.0); + joint_priors = kinematics.jointAngleObjectives(kSlice, robot, means); + EXPECT_LONGS_EQUAL(7, joint_priors.size()) + + // check means + means_vector << 1, 0, 1, 0, 1, 0, 1; + expected_means = jointVectorToValues(robot, means_vector); + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) + + // Define means of all joints different than 0 + means.insert(JointAngleKey(1, k), 0.5); + means.insert(JointAngleKey(3, k), -1.0); + means.insert(JointAngleKey(5, k), 0.5); + joint_priors = kinematics.jointAngleObjectives(kSlice, robot, means); + EXPECT_LONGS_EQUAL(7, joint_priors.size()) + + // check means + means_vector << 1, 0.5, 1, -1, 1, 0.5, 1; + expected_means = jointVectorToValues(robot, means_vector); + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) +} + +TEST(Slice, PoseGoalObjectives) { + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + // Instantiate kinematics algorithms + KinematicsParameters parameters; + parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + Kinematics kinematics(parameters); + + // Add prior to pose + gtsam::Rot3 sR7{{0.98161623, 0.13223102, -0.13763916}, + {0.08587125, -0.9499891, -0.30024463}, + {-0.17045735, 0.28290575, -0.94387956}}; + gtsam::Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); + gtsam::Values goal_poses; + goal_poses.insert(PoseKey(7, k), sT7); + auto pose_priors = kinematics.poseGoalObjectives(kSlice, robot, goal_poses); + + gtsam::Vector7 initial; + initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; + gtsam::Values initial_joints = jointVectorToValues(robot, initial); + auto initial_values = + kinematics.initialValues(kSlice, robot, 0.0, initial_joints); + double tol = 1e-4; + GTSAM_PRINT(initial_values.at(PoseKey(7, k))); + EXPECT(assert_equal(sT7, initial_values.at(PoseKey(7, k)), tol)) + EXPECT_DOUBLES_EQUAL(0, pose_priors.error(initial_values), tol) } TEST(Slice, panda_constraints) { @@ -204,20 +244,24 @@ TEST(Slice, PandaIK) { auto graph = kinematics.graph(kSlice, robot); EXPECT_LONGS_EQUAL(7, graph.size()); - // We should only have 8 unknown links and 7 unknown joint angles, i.e., 15 - // values: - auto initial_values = kinematics.initialValues(kSlice, robot); - EXPECT_LONGS_EQUAL(15, initial_values.size()); - - Rot3 sR7 {{ 0.98161623, 0.13223102, -0.13763916}, - { 0.08587125, -0.9499891 , -0.30024463}, - {-0.17045735, 0.28290575, -0.94387956}}; + // Define the goal pose and add it to a values container + // This is the FK solution of {0.1,0.2,0.3,-0.4,0.5,0.6,0.7} + gtsam::Values goal_poses; + Rot3 sR7{{0.98161623, 0.13223102, -0.13763916}, + {0.08587125, -0.9499891, -0.30024463}, + {-0.17045735, 0.28290575, -0.94387956}}; Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); - Vector7 initial; + goal_poses.insert(PoseKey(7, k), sT7); + + // Give a noisy estimate of the original point + Vector7 initial, noise; initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; - Vector7 noise; noise << 0.04, -0.1, 0.07, 0.14, -0.05, 0.02, 0.1; - auto values = inverseWithPose(&kinematics, kSlice, robot, {{7, sT7}}, initial+noise); + gtsam::Values initial_joints = jointVectorToValues(robot, noise + initial); + + // Call IK solver + auto values = + kinematics.inverseWithPose(kSlice, robot, goal_poses, initial_joints); // Check that base link did not budge (much) auto base_link = robot.link("link0"); @@ -232,7 +276,7 @@ TEST(Slice, PandaIK) { Vector7 optimal_q; for (size_t j = 0; j < 7; j++) optimal_q[j] = values.at(JointAngleKey(j, k)); - tol = 0.001; + tol = 0.1; EXPECT(assert_equal(initial, optimal_q, tol)); } From 9a555805437989db84be35a2457fae76fd7f179f Mon Sep 17 00:00:00 2001 From: Joobz Date: Mon, 7 Mar 2022 20:18:37 +0100 Subject: [PATCH 07/17] joint angle limits --- gtdynamics/kinematics/Kinematics.h | 10 +++++ gtdynamics/kinematics/KinematicsInterval.cpp | 16 +++++-- gtdynamics/kinematics/KinematicsSlice.cpp | 18 ++++++++ tests/testKinematicsSlice.cpp | 46 ++++++++++++++++++-- 4 files changed, 83 insertions(+), 7 deletions(-) diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 4cf00b8c7..321a010fe 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -154,6 +154,16 @@ class Kinematics : public Optimizer { const CONTEXT& context, const Robot& robot, const gtsam::Values& joint_priors = gtsam::Values()) const; + /** + * @fn Factors that enforce joint angle limits. + * @param context Slice or Interval instance. + * @param robot Robot specification from URDF/SDF. + * @return graph with prior factors on joint angles. + */ + template + gtsam::NonlinearFactorGraph jointAngleLimits(const CONTEXT& context, + const Robot& robot) const; + /** * @fn Initialize kinematics. * diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index e132c67cc..aa7bcb8c5 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -86,9 +86,19 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( } template <> -Values Kinematics::initialValues(const Interval& interval, - const Robot& robot, - double gaussian_noise, const gtsam::Values& joint_priors) const { +NonlinearFactorGraph Kinematics::jointAngleLimits( + const Interval& interval, const Robot& robot) const { + NonlinearFactorGraph graph; + for (size_t k = interval.k_start; k <= interval.k_end; k++) { + graph.add(jointAngleLimits(Slice(k), robot)); + } + return graph; +} + +template <> +Values Kinematics::initialValues( + const Interval& interval, const Robot& robot, double gaussian_noise, + const gtsam::Values& joint_priors) const { Values values; for (size_t k = interval.k_start; k <= interval.k_end; k++) { values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors)); diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 7bc3cce4b..9d9df9b25 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -11,6 +11,7 @@ * @author: Frank Dellaert */ +#include #include #include #include @@ -155,6 +156,20 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( return graph; } +template <> +NonlinearFactorGraph Kinematics::jointAngleLimits( + const Slice& slice, const Robot& robot) const { + NonlinearFactorGraph graph; + for (auto&& joint : robot.joints()) { + graph.add(JointLimitFactor( + JointAngleKey(joint->id(), slice.k), p_.prior_q_cost_model, + joint->parameters().scalar_limits.value_lower_limit, + joint->parameters().scalar_limits.value_upper_limit, + joint->parameters().scalar_limits.value_limit_threshold)); + } + return graph; +} + template <> Values Kinematics::initialValues( const Slice& slice, const Robot& robot, double gaussian_noise, @@ -237,6 +252,9 @@ gtsam::Values Kinematics::inverseWithPose( // Add priors on link poses with desired poses from argument graph.add(this->poseGoalObjectives(slice, robot, goal_poses)); + // Add joint angle limits factors + graph.add(this->jointAngleLimits(slice, robot)); + // Robot kinematics constraints auto constraints = this->constraints(slice, robot); diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index a275b7af4..b84bfbd48 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -142,7 +142,7 @@ TEST(Slice, JointAngleObjectives) { auto joint_priors = kinematics.jointAngleObjectives(kSlice, robot); EXPECT_LONGS_EQUAL(7, joint_priors.size()) - // Check means at 0 + // Check that error from priors evaluated at 0 is 0 gtsam::Vector7 means_vector; means_vector << 0, 0, 0, 0, 0, 0, 0; gtsam::Values expected_means = jointVectorToValues(robot, means_vector); @@ -151,7 +151,7 @@ TEST(Slice, JointAngleObjectives) { double tol = 1e-5; EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) - // Define some means different than 0 + // Define some prior means different than 0 gtsam::Values means; means.insert(JointAngleKey(0, k), 1.0); means.insert(JointAngleKey(2, k), 1.0); @@ -160,7 +160,11 @@ TEST(Slice, JointAngleObjectives) { joint_priors = kinematics.jointAngleObjectives(kSlice, robot, means); EXPECT_LONGS_EQUAL(7, joint_priors.size()) - // check means + // check that error at 0 is now not 0 + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + EXPECT(tol < joint_priors.error(initial)) + + // Check that the evaluated error at the expected means is 0 means_vector << 1, 0, 1, 0, 1, 0, 1; expected_means = jointVectorToValues(robot, means_vector); initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); @@ -173,13 +177,47 @@ TEST(Slice, JointAngleObjectives) { joint_priors = kinematics.jointAngleObjectives(kSlice, robot, means); EXPECT_LONGS_EQUAL(7, joint_priors.size()) - // check means + // Check that the evaluated error at the expected means is 0 means_vector << 1, 0.5, 1, -1, 1, 0.5, 1; expected_means = jointVectorToValues(robot, means_vector); initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) } +TEST(Slice, jointAngleLimits) { + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + KinematicsParameters parameters; + parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + Kinematics kinematics(parameters); + + auto jointLimits = kinematics.jointAngleLimits(kSlice, robot); + + // get joint limits + gtsam::Vector7 lower_limits, upper_limits; + for(auto&& joint : robot.joints()){ + auto scalar_values = joint->parameters().scalar_limits; + lower_limits(joint->id()) = scalar_values.value_lower_limit; + upper_limits(joint->id()) = scalar_values.value_upper_limit; + } + auto ones = gtsam::Vector7::Ones(); + + const double tol = 1e-5; + // if lower than lower_limit, error must be greater than 0 + auto values = jointVectorToValues(robot, lower_limits-0.1*ones); + EXPECT(tol < jointLimits.error(values)) + + // if inside the limits, the error must be 0 + values = jointVectorToValues(robot, (lower_limits+upper_limits)/2); + EXPECT_DOUBLES_EQUAL(0.0, jointLimits.error(values), tol) + + // if upper than upper_limit, error must be greater than 0 + values = jointVectorToValues(robot, upper_limits+0.1*ones); + EXPECT(tol < jointLimits.error(values)) +} + TEST(Slice, PoseGoalObjectives) { const Robot panda = CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); From 9c70904e66d4be233e4bd4e6dd6c71ca386831fa Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 9 Mar 2022 00:28:32 +0100 Subject: [PATCH 08/17] Adressed comments --- gtdynamics/kinematics/Kinematics.h | 4 +- gtdynamics/kinematics/KinematicsInterval.cpp | 4 +- gtdynamics/kinematics/KinematicsSlice.cpp | 46 ++++++++++--------- .../kinematics/KinematicsTrajectory.cpp | 4 +- tests/testKinematicsSlice.cpp | 16 +++---- 5 files changed, 38 insertions(+), 36 deletions(-) diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 321a010fe..723ccbb24 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -183,7 +183,7 @@ class Kinematics : public Optimizer { template gtsam::Values initialValues( const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.1, - const gtsam::Values& initial_joints = gtsam::Values()) const; + const gtsam::Values& initial_joints = gtsam::Values(), bool use_fk = false) const; /** * @fn Inverse kinematics given a set of contact goals. @@ -210,7 +210,7 @@ class Kinematics : public Optimizer { * @return values with poses and joint angles */ template - gtsam::Values inverseWithPose( + gtsam::Values inverse( const CONTEXT& context, const Robot& robot, const gtsam::Values& goal_poses, const gtsam::Values& joint_priors = gtsam::Values()) const; diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index aa7bcb8c5..b814820c9 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -98,10 +98,10 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( template <> Values Kinematics::initialValues( const Interval& interval, const Robot& robot, double gaussian_noise, - const gtsam::Values& joint_priors) const { + const gtsam::Values& joint_priors, bool use_fk) const { Values values; for (size_t k = interval.k_start; k <= interval.k_end; k++) { - values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors)); + values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors, use_fk)); } return values; } diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 9d9df9b25..4d5480156 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -73,13 +73,13 @@ EqualityConstraints Kinematics::constraints(const Slice& slice, // Get an expression for the unknown link pose. Pose3_ bTcom(PoseKey(link->id(), slice.k)); - // Kust make sure it does not move from its original rest pose + // Just make sure it does not move from its original rest pose Pose3_ bMcom(link->bMcom()); // Create expression to calculate the error in tangent space auto constraint_expr = gtsam::logmap(bTcom, bMcom); - // Add constriant + // Add constraint constraints.emplace_shared>(constraint_expr, tolerance); } @@ -126,13 +126,20 @@ NonlinearFactorGraph Kinematics::poseGoalObjectives( const gtsam::Values& goal_poses) const { gtsam::NonlinearFactorGraph graph; + for (const gtsam::Key& key : goal_poses.keys()) { + const gtsam::Pose3& desired_pose = goal_poses.at(key); + // TODO(toni): use poseprior from unstable gtsam slam or create new + // factors, to add pose from link7 + graph.addPrior(key, desired_pose, p_.p_cost_model); + } + // Add priors on link poses with desired poses from argument for (auto&& link : robot.links()) { auto pose_key = PoseKey(link->id(), slice.k); if (goal_poses.exists(pose_key)) { const gtsam::Pose3& desired_pose = goal_poses.at(pose_key); - // TODO: use poseprior from unstable gtsam slam or create new factors, to - // add pose from link7 + // TODO(toni): use poseprior from unstable gtsam slam or create new + // factors, to add pose from link7 graph.addPrior(pose_key, desired_pose, p_.p_cost_model); } } @@ -148,9 +155,8 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( // Minimize the joint angles. for (auto&& joint : robot.joints()) { const gtsam::Key key = JointAngleKey(joint->id(), slice.k); - double joint_mean = 0.0; - if (mean.exists(key)) joint_mean = mean.at(key); - graph.addPrior(key, joint_mean, p_.prior_q_cost_model); + graph.addPrior(key, (mean.exists(key) ? mean.at(key) : 0.0), + p_.prior_q_cost_model); } return graph; @@ -171,9 +177,10 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( } template <> -Values Kinematics::initialValues( - const Slice& slice, const Robot& robot, double gaussian_noise, - const gtsam::Values& initial_joints) const { +Values Kinematics::initialValues(const Slice& slice, const Robot& robot, + double gaussian_noise, + const gtsam::Values& initial_joints, + bool use_fk) const { Values values; auto sampler_noise_model = @@ -181,21 +188,16 @@ Values Kinematics::initialValues( gtsam::Sampler sampler(sampler_noise_model); // Initialize all joint angles. - bool any_value = false; for (auto&& joint : robot.joints()) { auto key = JointAngleKey(joint->id(), slice.k); - double value; - if (initial_joints.exists(key)) { - value = initial_joints.at(key); - any_value = true; - } else - value = sampler.sample()[0]; - InsertJointAngle(&values, joint->id(), slice.k, value); + double angle = initial_joints.exists(key) ? initial_joints.at(key) + : sampler.sample()[0]; + InsertJointAngle(&values, joint->id(), slice.k, angle); } // Maybe fk takes a long time, so only compute it if there was a given initial // joint value in this slice - if (any_value) { + if (use_fk) { // Initialize poses with fk of initialized values auto fk = robot.forwardKinematics(values, slice.k); for (auto&& link : robot.links()) { @@ -241,12 +243,12 @@ Values Kinematics::inverse(const Slice& slice, const Robot& robot, } template <> -gtsam::Values Kinematics::inverseWithPose( +gtsam::Values Kinematics::inverse( const Slice& slice, const Robot& robot, const gtsam::Values& goal_poses, const gtsam::Values& joint_priors) const { auto graph = this->graph(slice, robot); - // Add prior on joint angles to constrain the solution + // Add prior on joint angles to prefer solution close to our initial estimates graph.add(this->jointAngleObjectives(slice, robot, joint_priors)); // Add priors on link poses with desired poses from argument @@ -258,7 +260,7 @@ gtsam::Values Kinematics::inverseWithPose( // Robot kinematics constraints auto constraints = this->constraints(slice, robot); - auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors); + auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors, true); return this->optimize(graph, constraints, initial_values); } diff --git a/gtdynamics/kinematics/KinematicsTrajectory.cpp b/gtdynamics/kinematics/KinematicsTrajectory.cpp index d632ab92d..a35ee1088 100644 --- a/gtdynamics/kinematics/KinematicsTrajectory.cpp +++ b/gtdynamics/kinematics/KinematicsTrajectory.cpp @@ -78,11 +78,11 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( template <> Values Kinematics::initialValues( const Trajectory& trajectory, const Robot& robot, double gaussian_noise, - const gtsam::Values& initial_joints) const { + const gtsam::Values& initial_joints, bool use_fk) const { Values values; for (auto&& phase : trajectory.phases()) { values.insert( - initialValues(phase, robot, gaussian_noise, initial_joints)); + initialValues(phase, robot, gaussian_noise, initial_joints, use_fk)); } return values; } diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index b84bfbd48..d7b56bcbc 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -94,7 +94,7 @@ gtsam::Values jointVectorToValues(const Robot& robot, return joint_values; } -TEST(Slice, initial_values) { +TEST(Slice, initialValues) { // Load robot from urdf file const Robot panda = CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); @@ -108,7 +108,7 @@ TEST(Slice, initial_values) { gtsam::Values initial_joints = jointVectorToValues(robot, initial); gtsam::Values initial_values = - kinematics.initialValues(kSlice, robot, 0.0, initial_joints); + kinematics.initialValues(kSlice, robot, 0.0, initial_joints, true); // We should only have 7 values for joints and 8 for link poses EXPECT_LONGS_EQUAL(15, initial_values.size()) @@ -147,7 +147,7 @@ TEST(Slice, JointAngleObjectives) { means_vector << 0, 0, 0, 0, 0, 0, 0; gtsam::Values expected_means = jointVectorToValues(robot, means_vector); gtsam::Values initial = - kinematics.initialValues(kSlice, robot, 0.0, expected_means); + kinematics.initialValues(kSlice, robot, 0.0, expected_means, true); double tol = 1e-5; EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) @@ -161,13 +161,13 @@ TEST(Slice, JointAngleObjectives) { EXPECT_LONGS_EQUAL(7, joint_priors.size()) // check that error at 0 is now not 0 - initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means, true); EXPECT(tol < joint_priors.error(initial)) // Check that the evaluated error at the expected means is 0 means_vector << 1, 0, 1, 0, 1, 0, 1; expected_means = jointVectorToValues(robot, means_vector); - initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means, true); EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) // Define means of all joints different than 0 @@ -180,7 +180,7 @@ TEST(Slice, JointAngleObjectives) { // Check that the evaluated error at the expected means is 0 means_vector << 1, 0.5, 1, -1, 1, 0.5, 1; expected_means = jointVectorToValues(robot, means_vector); - initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means, true); EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) } @@ -241,7 +241,7 @@ TEST(Slice, PoseGoalObjectives) { initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; gtsam::Values initial_joints = jointVectorToValues(robot, initial); auto initial_values = - kinematics.initialValues(kSlice, robot, 0.0, initial_joints); + kinematics.initialValues(kSlice, robot, 0.0, initial_joints, true); double tol = 1e-4; GTSAM_PRINT(initial_values.at(PoseKey(7, k))); EXPECT(assert_equal(sT7, initial_values.at(PoseKey(7, k)), tol)) @@ -299,7 +299,7 @@ TEST(Slice, PandaIK) { // Call IK solver auto values = - kinematics.inverseWithPose(kSlice, robot, goal_poses, initial_joints); + kinematics.inverse(kSlice, robot, goal_poses, initial_joints); // Check that base link did not budge (much) auto base_link = robot.link("link0"); From d2226e4f7826ef45472e50f90f6d39e80d35717c Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 9 Mar 2022 01:45:00 +0100 Subject: [PATCH 09/17] PoseGoals and addressed more comments --- gtdynamics/kinematics/Kinematics.h | 67 +++++++++++++++++--- gtdynamics/kinematics/KinematicsInterval.cpp | 6 +- gtdynamics/kinematics/KinematicsSlice.cpp | 45 +++++++------ tests/testKinematicsSlice.cpp | 25 ++++---- 4 files changed, 99 insertions(+), 44 deletions(-) diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 723ccbb24..4668fe8f1 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +66,56 @@ struct ContactGoal { ///< Map of link name to ContactGoal using ContactGoals = std::vector; +/** + * Similar to the previous struct ContactGoal but with poses instead of + * points. + * Desired world pose for a end-effector pose. + * + * This simple struct stores the robot link that holds the end-effector, the + * end-effector's pose in the final link's CoM frame, and a `goal_pose` in + * world coordinate frames. The goal is satisfied iff + * `pose_on_link.predict(values, k) == goal_pose`. + */ +struct PoseGoal { + LinkSharedPtr ee_link; ///< Link that hold end-effector + gtsam::Pose3 comTee; ///< In link's CoM frame. + gtsam::Pose3 goal_pose; ///< In world frame. + + /// Constructor + PoseGoal(const LinkSharedPtr& ee_link, const gtsam::Pose3& comTee, + const gtsam::Pose3& goal_pose) + : ee_link(ee_link), comTee(comTee), goal_pose(goal_pose) {} + + /// Return link associated with contact pose. + const LinkSharedPtr& link() const { return ee_link; } + + /// Return goal pose in ee_link COM frame. + const gtsam::Pose3& poseInCoM() const { return comTee; } + + /// Return CoM pose needed to achieve goal pose. + const gtsam::Pose3 sTcom() const { + return goal_pose.compose(poseInCoM().inverse()); + } + + /// Print to stream. + friend std::ostream& operator<<(std::ostream& os, const PoseGoal& cg); + + /// GTSAM-style print, works with wrapper. + void print(const std::string& s) const; + + /** + * @fn Check that the contact goal has been achieved for given values. + * @param values a GTSAM Values instance that should contain link pose. + * @param k time step to check (default 0). + * @param tol tolerance in 3D (default 1e-9). + */ + bool satisfied(const gtsam::Values& values, size_t k = 0, + double tol = 1e-9) const; +}; + +///< Map of time to PoseGoal +using PoseGoals = std::map; + /// Noise models etc specific to Kinematics class struct KinematicsParameters : public OptimizationParameters { using Isotropic = gtsam::noiseModel::Isotropic; @@ -132,21 +183,21 @@ class Kinematics : public Optimizer { const CONTEXT& context, const ContactGoals& contact_goals) const; /** - * @fn Create pose goal objectives. + * @fn Create a pose prior for a given link for each given pose. * @param context Slice or Interval instance. * @param pose_goals goals for poses * @returns graph with pose goal factors. */ template gtsam::NonlinearFactorGraph poseGoalObjectives( - const CONTEXT& context, const Robot& robot, - const gtsam::Values& pose_goals) const; + const CONTEXT& context, const PoseGoals& pose_goals) const; /** * @fn Factors that minimize joint angles. * @param context Slice or Interval instance. * @param robot Robot specification from URDF/SDF. - * @param joint_priors Values where the mean of the priors is specified + * @param joint_priors Values where the mean of the priors is specified. The + * default is an empty Values, meaning that no means are specified. * @returns graph with prior factors on joint angles. */ template @@ -183,7 +234,8 @@ class Kinematics : public Optimizer { template gtsam::Values initialValues( const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.1, - const gtsam::Values& initial_joints = gtsam::Values(), bool use_fk = false) const; + const gtsam::Values& initial_joints = gtsam::Values(), + bool use_fk = false) const; /** * @fn Inverse kinematics given a set of contact goals. @@ -204,15 +256,14 @@ class Kinematics : public Optimizer { * @fn This function does inverse kinematics separately on each slice * @param context Slice or Interval instance * @param robot Robot specification from URDF/SDF - * @param goal_poses goals for EE poses + * @param pose_goals goals for EE poses * @param joint_priors optional argument to put priors centered on given * values. If empty, the priors will be centered on the origin * @return values with poses and joint angles */ template gtsam::Values inverse( - const CONTEXT& context, const Robot& robot, - const gtsam::Values& goal_poses, + const CONTEXT& context, const Robot& robot, const PoseGoals& pose_goals, const gtsam::Values& joint_priors = gtsam::Values()) const; /** diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index b814820c9..e31c440a3 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -46,11 +46,11 @@ EqualityConstraints Kinematics::constraints( template <> NonlinearFactorGraph Kinematics::poseGoalObjectives( - const Interval& interval, const Robot& robot, - const gtsam::Values& goal_poses) const { + const Interval& interval, + const PoseGoals& pose_goals) const { NonlinearFactorGraph graph; for (size_t k = interval.k_start; k <= interval.k_end; k++) { - graph.add(poseGoalObjectives(Slice(k), robot, goal_poses)); + graph.add(poseGoalObjectives(Slice(k), pose_goals)); } return graph; } diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 4d5480156..230809fca 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -36,6 +36,21 @@ void ContactGoal::print(const std::string& s) const { std::cout << (s.empty() ? s : s + " ") << *this; } +std::ostream& operator<<(std::ostream& os, const PoseGoal& pg) { + os << "{" << pg.link()->name() << ", [" << pg.comTee << "], [" << pg.goal_pose << "]}"; + return os; +} + +void PoseGoal::print(const std::string& s) const { + std::cout << (s.empty() ? s : s + " ") << *this; +} + +bool PoseGoal::satisfied(const gtsam::Values& values, size_t k, + double tol) const { + return gtsam::equal( + Pose(values, link()->id(), k).compose(poseInCoM()), goal_pose, tol); +} + template <> NonlinearFactorGraph Kinematics::graph(const Slice& slice, const Robot& robot) const { @@ -122,26 +137,15 @@ EqualityConstraints Kinematics::pointGoalConstraints( template <> NonlinearFactorGraph Kinematics::poseGoalObjectives( - const Slice& slice, const Robot& robot, - const gtsam::Values& goal_poses) const { + const Slice& slice, const PoseGoals& pose_goals) const { gtsam::NonlinearFactorGraph graph; - for (const gtsam::Key& key : goal_poses.keys()) { - const gtsam::Pose3& desired_pose = goal_poses.at(key); + if (pose_goals.find(slice.k) != pose_goals.end()) { + auto pose_key = PoseKey(pose_goals.at(slice.k).link()->id(), slice.k); + const gtsam::Pose3& desired_pose = pose_goals.at(slice.k).sTcom(); // TODO(toni): use poseprior from unstable gtsam slam or create new // factors, to add pose from link7 - graph.addPrior(key, desired_pose, p_.p_cost_model); - } - - // Add priors on link poses with desired poses from argument - for (auto&& link : robot.links()) { - auto pose_key = PoseKey(link->id(), slice.k); - if (goal_poses.exists(pose_key)) { - const gtsam::Pose3& desired_pose = goal_poses.at(pose_key); - // TODO(toni): use poseprior from unstable gtsam slam or create new - // factors, to add pose from link7 - graph.addPrior(pose_key, desired_pose, p_.p_cost_model); - } + graph.addPrior(pose_key, desired_pose, p_.p_cost_model); } return graph; @@ -230,7 +234,7 @@ Values Kinematics::inverse(const Slice& slice, const Robot& robot, graph.add(pointGoalObjectives(slice, contact_goals)); } - // Traget joint angles. + // Target joint angles. graph.add(jointAngleObjectives(slice, robot)); // TODO(frank): allo pose prior as well. @@ -244,7 +248,7 @@ Values Kinematics::inverse(const Slice& slice, const Robot& robot, template <> gtsam::Values Kinematics::inverse( - const Slice& slice, const Robot& robot, const gtsam::Values& goal_poses, + const Slice& slice, const Robot& robot, const PoseGoals& pose_goals, const gtsam::Values& joint_priors) const { auto graph = this->graph(slice, robot); @@ -252,7 +256,7 @@ gtsam::Values Kinematics::inverse( graph.add(this->jointAngleObjectives(slice, robot, joint_priors)); // Add priors on link poses with desired poses from argument - graph.add(this->poseGoalObjectives(slice, robot, goal_poses)); + graph.add(this->poseGoalObjectives(slice, pose_goals)); // Add joint angle limits factors graph.add(this->jointAngleLimits(slice, robot)); @@ -260,7 +264,8 @@ gtsam::Values Kinematics::inverse( // Robot kinematics constraints auto constraints = this->constraints(slice, robot); - auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors, true); + auto initial_values = + this->initialValues(slice, robot, 0.1, joint_priors, true); return this->optimize(graph, constraints, initial_values); } diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index d7b56bcbc..9c9a4cfc7 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -188,7 +188,7 @@ TEST(Slice, jointAngleLimits) { const Robot panda = CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); const Robot robot = panda.fixLink("link0"); - + KinematicsParameters parameters; parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; Kinematics kinematics(parameters); @@ -197,7 +197,7 @@ TEST(Slice, jointAngleLimits) { // get joint limits gtsam::Vector7 lower_limits, upper_limits; - for(auto&& joint : robot.joints()){ + for (auto&& joint : robot.joints()) { auto scalar_values = joint->parameters().scalar_limits; lower_limits(joint->id()) = scalar_values.value_lower_limit; upper_limits(joint->id()) = scalar_values.value_upper_limit; @@ -206,15 +206,15 @@ TEST(Slice, jointAngleLimits) { const double tol = 1e-5; // if lower than lower_limit, error must be greater than 0 - auto values = jointVectorToValues(robot, lower_limits-0.1*ones); + auto values = jointVectorToValues(robot, lower_limits - 0.1 * ones); EXPECT(tol < jointLimits.error(values)) // if inside the limits, the error must be 0 - values = jointVectorToValues(robot, (lower_limits+upper_limits)/2); + values = jointVectorToValues(robot, (lower_limits + upper_limits) / 2); EXPECT_DOUBLES_EQUAL(0.0, jointLimits.error(values), tol) // if upper than upper_limit, error must be greater than 0 - values = jointVectorToValues(robot, upper_limits+0.1*ones); + values = jointVectorToValues(robot, upper_limits + 0.1 * ones); EXPECT(tol < jointLimits.error(values)) } @@ -233,9 +233,9 @@ TEST(Slice, PoseGoalObjectives) { {0.08587125, -0.9499891, -0.30024463}, {-0.17045735, 0.28290575, -0.94387956}}; gtsam::Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); - gtsam::Values goal_poses; - goal_poses.insert(PoseKey(7, k), sT7); - auto pose_priors = kinematics.poseGoalObjectives(kSlice, robot, goal_poses); + PoseGoals pose_goals = { + {k, PoseGoal(robot.links()[7], gtsam::Pose3(), sT7)}}; + auto pose_priors = kinematics.poseGoalObjectives(kSlice, pose_goals); gtsam::Vector7 initial; initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; @@ -284,13 +284,13 @@ TEST(Slice, PandaIK) { // Define the goal pose and add it to a values container // This is the FK solution of {0.1,0.2,0.3,-0.4,0.5,0.6,0.7} - gtsam::Values goal_poses; Rot3 sR7{{0.98161623, 0.13223102, -0.13763916}, {0.08587125, -0.9499891, -0.30024463}, {-0.17045735, 0.28290575, -0.94387956}}; Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); - goal_poses.insert(PoseKey(7, k), sT7); - + PoseGoals pose_goals = { + {k, PoseGoal(robot.links()[7], gtsam::Pose3(), sT7)}}; + // Give a noisy estimate of the original point Vector7 initial, noise; initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; @@ -298,8 +298,7 @@ TEST(Slice, PandaIK) { gtsam::Values initial_joints = jointVectorToValues(robot, noise + initial); // Call IK solver - auto values = - kinematics.inverse(kSlice, robot, goal_poses, initial_joints); + auto values = kinematics.inverse(kSlice, robot, pose_goals, initial_joints); // Check that base link did not budge (much) auto base_link = robot.link("link0"); From fdb0b632e9d2c6779c98a9f37173c7d14cb880fb Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 9 Mar 2022 17:45:14 +0100 Subject: [PATCH 10/17] Adressed comments --- gtdynamics/kinematics/Kinematics.h | 24 ++++++++++------------- gtdynamics/kinematics/KinematicsSlice.cpp | 15 ++++++++------ 2 files changed, 19 insertions(+), 20 deletions(-) diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 4668fe8f1..159a9c73a 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -72,29 +71,26 @@ using ContactGoals = std::vector; * Desired world pose for a end-effector pose. * * This simple struct stores the robot link that holds the end-effector, the - * end-effector's pose in the final link's CoM frame, and a `goal_pose` in + * end-effector's pose in the final link's CoM frame, and a `goal_pose` in * world coordinate frames. The goal is satisfied iff * `pose_on_link.predict(values, k) == goal_pose`. */ struct PoseGoal { - LinkSharedPtr ee_link; ///< Link that hold end-effector - gtsam::Pose3 comTee; ///< In link's CoM frame. - gtsam::Pose3 goal_pose; ///< In world frame. + LinkSharedPtr ee_link; ///< Link that hold end-effector + gtsam::Pose3 comTgoal; ///< Goal pose in link's CoM frame. + gtsam::Pose3 wTgoal; ///< Goal pose in world frame. /// Constructor - PoseGoal(const LinkSharedPtr& ee_link, const gtsam::Pose3& comTee, - const gtsam::Pose3& goal_pose) - : ee_link(ee_link), comTee(comTee), goal_pose(goal_pose) {} + PoseGoal(const LinkSharedPtr& ee_link, const gtsam::Pose3& comTgoal, + const gtsam::Pose3& wTgoal) + : ee_link(ee_link), comTgoal(comTgoal), wTgoal(wTgoal) {} /// Return link associated with contact pose. const LinkSharedPtr& link() const { return ee_link; } - /// Return goal pose in ee_link COM frame. - const gtsam::Pose3& poseInCoM() const { return comTee; } - /// Return CoM pose needed to achieve goal pose. - const gtsam::Pose3 sTcom() const { - return goal_pose.compose(poseInCoM().inverse()); + const gtsam::Pose3 wTcom() const { + return wTgoal.compose(comTgoal.inverse()); } /// Print to stream. @@ -197,7 +193,7 @@ class Kinematics : public Optimizer { * @param context Slice or Interval instance. * @param robot Robot specification from URDF/SDF. * @param joint_priors Values where the mean of the priors is specified. The - * default is an empty Values, meaning that no means are specified. + * default is an empty Values, meaning that the means will default to 0. * @returns graph with prior factors on joint angles. */ template diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 230809fca..377b6ad1a 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -37,7 +37,8 @@ void ContactGoal::print(const std::string& s) const { } std::ostream& operator<<(std::ostream& os, const PoseGoal& pg) { - os << "{" << pg.link()->name() << ", [" << pg.comTee << "], [" << pg.goal_pose << "]}"; + os << "{" << pg.link()->name() << ", [" << pg.comTgoal << "], [" << pg.wTgoal + << "]}"; return os; } @@ -47,8 +48,8 @@ void PoseGoal::print(const std::string& s) const { bool PoseGoal::satisfied(const gtsam::Values& values, size_t k, double tol) const { - return gtsam::equal( - Pose(values, link()->id(), k).compose(poseInCoM()), goal_pose, tol); + return gtsam::equal(Pose(values, link()->id(), k), wTcom(), + tol); } template <> @@ -140,9 +141,11 @@ NonlinearFactorGraph Kinematics::poseGoalObjectives( const Slice& slice, const PoseGoals& pose_goals) const { gtsam::NonlinearFactorGraph graph; - if (pose_goals.find(slice.k) != pose_goals.end()) { - auto pose_key = PoseKey(pose_goals.at(slice.k).link()->id(), slice.k); - const gtsam::Pose3& desired_pose = pose_goals.at(slice.k).sTcom(); + auto it = pose_goals.find(slice.k); // short for "iterator" + if (it != pose_goals.end()) { + const auto& pose_goal = it->second; + auto pose_key = PoseKey(pose_goal.link()->id(), slice.k); + const gtsam::Pose3& desired_pose = pose_goal.wTcom(); // TODO(toni): use poseprior from unstable gtsam slam or create new // factors, to add pose from link7 graph.addPrior(pose_key, desired_pose, p_.p_cost_model); From 0cf03b550c8661b10f4c885a8500efdf2db9d177 Mon Sep 17 00:00:00 2001 From: lprice8 Date: Mon, 16 May 2022 11:23:42 -0400 Subject: [PATCH 11/17] added gpmp and changed kinematics --- gtdynamics/kinematics/Kinematics.h | 8 +- gtdynamics/kinematics/KinematicsSlice.cpp | 4 +- gtdynamics/pandarobot/CMakeLists.txt | 2 +- gtdynamics/pandarobot/gpmp/CMakeLists.txt | 0 .../pandarobot/gpmp/GaussianProcessPrior.h | 114 ++++++++++++++++++ gtdynamics/pandarobot/ikfast/PandaIKFast.cpp | 3 +- gtdynamics/pandarobot/ikfast/PandaIKFast.h | 2 +- .../pandarobot/ikfast/ikfast61_panda.cpp | 47 -------- models/urdfs/panda/panda.urdf | 14 +-- 9 files changed, 130 insertions(+), 64 deletions(-) create mode 100644 gtdynamics/pandarobot/gpmp/CMakeLists.txt create mode 100644 gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 159a9c73a..059200d81 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -120,10 +120,10 @@ struct KinematicsParameters : public OptimizationParameters { prior_q_cost_model; // joint angle prior factor // TODO(yetong): replace noise model with tolerance. - KinematicsParameters() - : p_cost_model(Isotropic::Sigma(6, 1e-4)), - g_cost_model(Isotropic::Sigma(3, 0.01)), - prior_q_cost_model(Isotropic::Sigma(1, 0.5)) {} + KinematicsParameters(double p_cost_model_sigma = 1e-4, double g_cost_model_sigma = 1e-2, double prior_q_cost_model_sigma = 0.5) + : p_cost_model(Isotropic::Sigma(6, p_cost_model_sigma)), + g_cost_model(Isotropic::Sigma(3, g_cost_model_sigma)), + prior_q_cost_model(Isotropic::Sigma(1, prior_q_cost_model_sigma)) {} }; /// All things kinematics, zero velocities/twists, and no forces. diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 377b6ad1a..f1448c802 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -175,10 +175,10 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( NonlinearFactorGraph graph; for (auto&& joint : robot.joints()) { graph.add(JointLimitFactor( - JointAngleKey(joint->id(), slice.k), p_.prior_q_cost_model, + JointAngleKey(joint->id(), slice.k), gtsam::noiseModel::Isotropic::Sigma(1, 0.00001), joint->parameters().scalar_limits.value_lower_limit, joint->parameters().scalar_limits.value_upper_limit, - joint->parameters().scalar_limits.value_limit_threshold)); + 0.04)); } return graph; } diff --git a/gtdynamics/pandarobot/CMakeLists.txt b/gtdynamics/pandarobot/CMakeLists.txt index 565781db9..2d34e3e1a 100644 --- a/gtdynamics/pandarobot/CMakeLists.txt +++ b/gtdynamics/pandarobot/CMakeLists.txt @@ -1,5 +1,5 @@ # add cablerobot subfolders to gtdynamics' SOURCE_SUBDIRS list -list(APPEND SOURCE_SUBDIRS pandarobot/ikfast) +list(APPEND SOURCE_SUBDIRS pandarobot/ikfast pandarobot/gpmp) set(SOURCE_SUBDIRS ${SOURCE_SUBDIRS} PARENT_SCOPE) # add wrapper interface file diff --git a/gtdynamics/pandarobot/gpmp/CMakeLists.txt b/gtdynamics/pandarobot/gpmp/CMakeLists.txt new file mode 100644 index 000000000..e69de29bb diff --git a/gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h b/gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h new file mode 100644 index 000000000..afd5319cc --- /dev/null +++ b/gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h @@ -0,0 +1,114 @@ +#pragma once + +#include +#include +#include + +#include +namespace gtdynamics { +class GaussianProcessPrior + : public gtsam::NoiseModelFactor4 { + private: + double delta_t_; + + using This = GaussianProcessPrior; + using Base = gtsam::NoiseModelFactor4; + + public: + GaussianProcessPrior() {} /* Default constructor only for serialization */ + + /// Constructor + /// @param delta_t is the time between the two states + GaussianProcessPrior(gtsam::Key thetaKey1, gtsam::Key thetadKey1, + gtsam::Key thetaKey2, gtsam::Key thetadKey2, + double delta_t, double Qc_model) + : // TODO:define calcQ + Base(gtsam::noiseModel::Gaussian::Covariance(calcQ(Qc_model, delta_t)), + thetaKey1, thetadKey1, thetaKey2, thetadKey2), + delta_t_(delta_t) {} + + virtual ~GaussianProcessPrior() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// factor error function + gtsam::Vector evaluateError( + const double& theta1, const double& thetad1, const double& theta2, + const double& thetad2, boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none) const override { + // using namespace gtsam; + + // state vector + gtsam::Vector2 error; + error << -(theta2 - theta1) + delta_t_ * thetad1, -(thetad2 - thetad1); + + // Jacobians + if (H1) *H1 = (gtsam::Matrix(2, 1) << 1, 0).finished(); + if (H2) *H2 = (gtsam::Matrix(2, 1) << delta_t_, 1).finished(); + if (H3) *H3 = (gtsam::Matrix(2, 1) << -1, 0).finished(); + if (H4) *H4 = (gtsam::Matrix(2, 1) << 0, -1).finished(); + + // transition matrix & error + return error; + } + + // double calcF(gtsam::Matrix error, gtsam::Matrix Qinv) { + // return (1/2 * error.transposed() * Qinv * error); + // } + + gtsam::Matrix calcQ(double Qc, double delta_t) { + return (gtsam::Matrix(2, 2) << 1.0 / 3 * pow(delta_t, 3.0) * Qc, + 1.0 / 2 * pow(delta_t, 2.0) * Qc, 1.0 / 2 * pow(delta_t, 2.0) * Qc, + delta_t * Qc) + .finished(); + } + + // gtsam::Matrix calcQ_inv(double Qc, double delta_t) { + // return (gtsam::Matrix(2*Qc.rows(), 2*Qc.rows()) << + // 12.0 * pow(delta_t, -3.0) / Qc_inv, (-6.0) * pow(delta_t, -2.0) / + // Qc_inv, + // (-6.0) * pow(delta_t, -2.0) / Qc_inv, 4.0 * pow(delta_t, -1.0) / + // Qc_inv).finished(); + // } + + /** demensions */ + size_t dim() const override { return 1; } + + /** number of variables attached to this factor */ + size_t size() const { return 4; } + + /** equals specialized to this factor */ + virtual bool equals(const gtsam::NonlinearFactor& expected, + double tol = 1e-9) const override { + const This* e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && + fabs(this->delta_t_ - e->delta_t_) < tol; + } + + /** print contents */ + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "4-way Gaussian Process Factor Linear(" << 1 << ")" + << std::endl; + Base::print("", keyFormatter); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(delta_t_); + } + +}; // GaussianProcessPrior + +} // namespace gtdynamics \ No newline at end of file diff --git a/gtdynamics/pandarobot/ikfast/PandaIKFast.cpp b/gtdynamics/pandarobot/ikfast/PandaIKFast.cpp index 5629de7be..fed0987c3 100644 --- a/gtdynamics/pandarobot/ikfast/PandaIKFast.cpp +++ b/gtdynamics/pandarobot/ikfast/PandaIKFast.cpp @@ -10,7 +10,6 @@ //----------------------------------------------------------------------------// #define IKFAST_HAS_LIBRARY // Build IKFast with API functions -#define IKFAST_NO_MAIN // Don't include main() from IKFast #define IKFAST_NAMESPACE panda_internal #define IK_VERSION 61 @@ -64,7 +63,7 @@ std::vector PandaIKFast::inverse(const Pose3& bTe, double theta7) { &theta7, solutions); if (!success) { - fprintf(stderr, "Error: (inverse PandaIKFast) failed to get ik solution\n"); + //fprintf(stderr, "Error: (inverse PandaIKFast) failed to get ik solution\n"); return std::vector(); } diff --git a/gtdynamics/pandarobot/ikfast/PandaIKFast.h b/gtdynamics/pandarobot/ikfast/PandaIKFast.h index 4e0fafd4b..440085c21 100644 --- a/gtdynamics/pandarobot/ikfast/PandaIKFast.h +++ b/gtdynamics/pandarobot/ikfast/PandaIKFast.h @@ -44,7 +44,7 @@ class PandaIKFast { * @return std::vector -- std vector containing the different * solutions for joint angles' values */ - static std::vector inverse(const gtsam::Pose3& bRe, + static std::vector inverse(const gtsam::Pose3& bTe, double theta7); }; diff --git a/gtdynamics/pandarobot/ikfast/ikfast61_panda.cpp b/gtdynamics/pandarobot/ikfast/ikfast61_panda.cpp index d11411504..34851e91e 100644 --- a/gtdynamics/pandarobot/ikfast/ikfast61_panda.cpp +++ b/gtdynamics/pandarobot/ikfast/ikfast61_panda.cpp @@ -12769,50 +12769,3 @@ IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; } #ifdef IKFAST_NAMESPACE } // end namespace #endif - -#ifndef IKFAST_NO_MAIN -#include -#include -#ifdef IKFAST_NAMESPACE -using namespace IKFAST_NAMESPACE; -#endif -int main(int argc, char** argv) -{ - if( argc != 12+GetNumFreeParameters()+1 ) { - printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" - "Returns the ik solutions given the transformation of the end effector specified by\n" - "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n" - "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters()); - return 1; - } - - IkSolutionList solutions; - std::vector vfree(GetNumFreeParameters()); - IkReal eerot[9],eetrans[3]; - eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]); - eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]); - eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]); - for(std::size_t i = 0; i < vfree.size(); ++i) - vfree[i] = atof(argv[13+i]); - bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); - - if( !bSuccess ) { - fprintf(stderr,"Failed to get ik solution\n"); - return -1; - } - - printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions()); - std::vector solvalues(GetNumJoints()); - for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) { - const IkSolutionBase& sol = solutions.GetSolution(i); - printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size()); - std::vector vsolfree(sol.GetFree().size()); - sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); - for( std::size_t j = 0; j < solvalues.size(); ++j) - printf("%.15f, ", solvalues[j]); - printf("\n"); - } - return 0; -} - -#endif diff --git a/models/urdfs/panda/panda.urdf b/models/urdfs/panda/panda.urdf index a24f1480f..b18787700 100644 --- a/models/urdfs/panda/panda.urdf +++ b/models/urdfs/panda/panda.urdf @@ -48,7 +48,7 @@ - + @@ -78,7 +78,7 @@ - + @@ -108,7 +108,7 @@ - + @@ -138,7 +138,7 @@ - + @@ -168,7 +168,7 @@ - + @@ -198,7 +198,7 @@ - + @@ -228,7 +228,7 @@ - + From 54c72df3420ffdc7fb4ec8b0cea9c4d45e8a8057 Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 6 Jul 2022 11:51:05 +0200 Subject: [PATCH 12/17] small update --- gtdynamics/kinematics/Kinematics.h | 3 +- gtdynamics/kinematics/KinematicsSlice.cpp | 4 +- gtdynamics/pandarobot/CMakeLists.txt | 2 +- .../tests/testGaussianProcessPrior.cpp | 87 +++++++++++++++++++ 4 files changed, 92 insertions(+), 4 deletions(-) create mode 100644 gtdynamics/pandarobot/tests/testGaussianProcessPrior.cpp diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 059200d81..0197a3416 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -181,7 +181,8 @@ class Kinematics : public Optimizer { /** * @fn Create a pose prior for a given link for each given pose. * @param context Slice or Interval instance. - * @param pose_goals goals for poses + * @param pose_goals an object of PoseGoals: map of time k to desired pose at + * that time, which will be used as mean of the prior * @returns graph with pose goal factors. */ template diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index f1448c802..9a01cb11b 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -175,10 +175,10 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( NonlinearFactorGraph graph; for (auto&& joint : robot.joints()) { graph.add(JointLimitFactor( - JointAngleKey(joint->id(), slice.k), gtsam::noiseModel::Isotropic::Sigma(1, 0.00001), + JointAngleKey(joint->id(), slice.k), gtsam::noiseModel::Isotropic::Sigma(1, 0.001), joint->parameters().scalar_limits.value_lower_limit, joint->parameters().scalar_limits.value_upper_limit, - 0.04)); + 0.04));//joint->parameters().scalar_limits.value_limit_threshold)); } return graph; } diff --git a/gtdynamics/pandarobot/CMakeLists.txt b/gtdynamics/pandarobot/CMakeLists.txt index 2d34e3e1a..a8d489361 100644 --- a/gtdynamics/pandarobot/CMakeLists.txt +++ b/gtdynamics/pandarobot/CMakeLists.txt @@ -1,4 +1,4 @@ -# add cablerobot subfolders to gtdynamics' SOURCE_SUBDIRS list +# add pandarobot subfolders to gtdynamics' SOURCE_SUBDIRS list list(APPEND SOURCE_SUBDIRS pandarobot/ikfast pandarobot/gpmp) set(SOURCE_SUBDIRS ${SOURCE_SUBDIRS} PARENT_SCOPE) diff --git a/gtdynamics/pandarobot/tests/testGaussianProcessPrior.cpp b/gtdynamics/pandarobot/tests/testGaussianProcessPrior.cpp new file mode 100644 index 000000000..bdc456c7c --- /dev/null +++ b/gtdynamics/pandarobot/tests/testGaussianProcessPrior.cpp @@ -0,0 +1,87 @@ + +/** +* @file testGaussianProcessPrior.cpp +* @author Wanli Qian, Toni Jubes, Frank Dellaert +**/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace gtdynamics; + + +/* ************************************************************************** */ + +TEST(GaussianProcessPrior, Optimization) { + + /** + * A simple graph: + * + * p1 p2 + * | | + * x1 x2 + * \ / + * gp + * / \ + * v1 v2 + * + * p1 and p2 are pose prior factor to fix the poses, gp is the GP factor + * that get correct velocity of v1 and v2 + */ + + noiseModel::Isotropic::shared_ptr model_prior = + noiseModel::Isotropic::Sigma(3, 0.001); + + //noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc); + + double theta1 = 0; + double theta1d = 5; + double theta2 = 4; + double theta2d = 6; + + double EXPECTEDfi = 7; + gtsam::Vector2 expected_error; + expected_error << 1 , -1; + + double key1 = 0; + double key2 = 1; + double key3 = 2; + double key4 = 3; + double delta_t = 1; + double Qc = 2; + + auto factor = GaussianProcessPrior(key1,key2,key3,key4,delta_t, Qc); + gtsam::Values v; + v.insert(key1,theta1); + v.insert(key2,theta1d); + v.insert(key3,theta2); + v.insert(key4,theta2d); + gtsam::Vector2 actual_error = factor.evaluateError(theta1,theta1d,theta2,theta2d); + double ACTUALfi = factor.error(v); + +EXPECT(assert_equal(expected_error,actual_error,1e-06)) +EXPECT_DOUBLES_EQUAL(EXPECTEDfi,ACTUALfi,1e-06) + +} + +/* ************************************************************************** */ +/* main function */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} \ No newline at end of file From 27f43736afab9014b2134416a76a06f71d562c0c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Feb 2026 11:32:15 -0500 Subject: [PATCH 13/17] # Conflicts: # gtdynamics/kinematics/Kinematics.h # gtdynamics/kinematics/KinematicsInterval.cpp # gtdynamics/kinematics/KinematicsSlice.cpp # gtdynamics/kinematics/KinematicsTrajectory.cpp # gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h # gtdynamics/universal_robot/Joint.cpp --- .github/copilot-instructions.md | 11 + .github/workflows/linux-ci.yml | 181 +- .github/workflows/macos-ci.yml | 156 +- .github/workflows/python-ci.yml | 137 - .gitignore | 9 +- CI-workflows.md | 49 + CMakeLists.txt | 110 +- README.md | 107 +- Config.cmake.in => cmake/Config.cmake.in | 0 cmake/HandleTBB.cmake | 28 + docker/Dockerfile | 6 +- examples/CMakeLists.txt | 26 +- examples/README,md | 17 + examples/cmake_project_example/CMakeLists.txt | 18 + examples/cmake_project_example/main.cpp | 52 + examples/example_a1_walking/CMakeLists.txt | 15 +- examples/example_a1_walking/main.cpp | 452 ++- examples/example_a1_walking/main_combined.cpp | 422 +++ examples/example_a1_walking/sim.py | 112 +- .../CMakeLists.txt | 2 +- .../main.cpp | 14 +- .../sim.py | 4 +- .../CMakeLists.txt | 81 + .../CartPoleUtils.cpp | 294 ++ .../CartPoleUtils.h | 116 + .../QuadrupedUtils.cpp | 623 +++ .../QuadrupedUtils.h | 195 + .../example_constraint_manifold/SerialChain.h | 176 + .../main_arm_kinematic_planning.cpp | 451 +++ .../main_cablerobot.cpp | 185 + .../main_cartpole.cpp | 149 + .../main_connected_poses.cpp | 312 ++ .../main_quadruped.cpp | 201 + .../main_range_constraint.cpp | 252 ++ .../quadruped.ipynb | 3365 +++++++++++++++++ .../quadruped_plot.py | 72 + .../quadruped_visualize.py | 140 + .../example_forward_dynamics/CMakeLists.txt | 2 +- examples/example_forward_dynamics/main.cpp | 5 +- .../CMakeLists.txt | 2 +- .../main.cpp | 22 +- .../example_full_kinodynamic_balancing/sim.py | 4 +- .../CMakeLists.txt | 2 +- .../main_rotate.cpp | 19 +- .../main_walk_forward.cpp | 21 +- .../example_full_kinodynamic_walking/sim.py | 4 +- .../vision60.urdf | 480 --- .../CMakeLists.txt | 2 +- .../inverted_pendulum.py | 40 +- .../main.cpp | 13 +- .../sim.py | 4 +- examples/example_quadruped_mp/CMakeLists.txt | 4 +- .../{kinematic_mp_sim.py => sim.py} | 4 +- examples/example_quadruped_mp/vision60.urdf | 480 --- .../example_spider_walking/CMakeLists.txt | 2 +- examples/example_spider_walking/main.cpp | 11 +- .../example_spider_walking/main_rotate.cpp | 19 +- examples/example_spider_walking/sim.py | 8 +- examples/scripts/CMakeLists.txt | 2 + .../alejandro_yetong_01_id_four_bar.cpp | 18 +- .../scripts}/constrainedExample.h | 6 +- .../scripts}/forward_dynamics_simple.cpp | 13 +- .../gerry00_ForwardDynamicsPrismatic.cpp | 11 +- .../scripts}/gerry00_visualize.m | 0 .../nithya00_constrained_opt_benchmark.cpp | 127 + .../nithya00_constrained_opt_benchmark.py | 0 .../stephanie01_ForwardDynamicsThreeLink.cpp | 14 +- .../scripts}/stephanie02_spider_sdf.cpp | 6 +- gtdynamics.i | 177 +- gtdynamics/CMakeLists.txt | 49 +- .../cablerobot/factors/CableLengthFactor.h | 19 +- .../cablerobot/factors/CableTensionFactor.cpp | 15 +- .../cablerobot/factors/CableTensionFactor.h | 33 +- .../factors/CableVelocityFactor.cpp | 5 +- .../cablerobot/factors/CableVelocityFactor.h | 25 +- gtdynamics/cablerobot/factors/PriorFactor.h | 4 +- gtdynamics/cablerobot/src/cdpr_planar.py | 70 +- .../cablerobot/src/cdpr_planar_controller.py | 6 +- gtdynamics/cablerobot/src/test_cdpr_planar.py | 12 +- gtdynamics/cmopt/CMakeLists.txt | 0 gtdynamics/cmopt/ConstraintManifold.cpp | 149 + gtdynamics/cmopt/ConstraintManifold.h | 296 ++ gtdynamics/cmopt/LMManifoldOptimizer.cpp | 156 + gtdynamics/cmopt/LMManifoldOptimizer.h | 124 + gtdynamics/cmopt/LMManifoldOptimizerState.cpp | 361 ++ gtdynamics/cmopt/LMManifoldOptimizerState.h | 317 ++ gtdynamics/cmopt/ManifoldOptimizer.cpp | 322 ++ gtdynamics/cmopt/ManifoldOptimizer.h | 230 ++ gtdynamics/cmopt/MultiJacobian.cpp | 231 ++ gtdynamics/cmopt/MultiJacobian.h | 160 + gtdynamics/cmopt/NonlinearMOptimizer.cpp | 65 + gtdynamics/cmopt/NonlinearMOptimizer.h | 98 + gtdynamics/cmopt/README.md | 149 + gtdynamics/cmopt/Retractor.cpp | 420 ++ gtdynamics/cmopt/Retractor.h | 428 +++ gtdynamics/cmopt/TspaceBasis.cpp | 628 +++ gtdynamics/cmopt/TspaceBasis.h | 569 +++ gtdynamics/config.h.in | 1 + .../constrained_optimizer/CMakeLists.txt | 0 .../ConstrainedOptBenchmark.cpp | 593 +++ .../ConstrainedOptBenchmark.h | 178 + .../ConstrainedOptProblem.cpp | 92 + .../ConstrainedOptProblem.h | 105 + .../ConstrainedOptimizer.cpp | 60 + .../ConstrainedOptimizer.h | 126 + .../constrained_optimizer/IPOptOptimizer.cpp | 433 +++ .../constrained_optimizer/IPOptOptimizer.h | 160 + .../constrained_optimizer/SQPOptimizer.cpp | 550 +++ .../constrained_optimizer/SQPOptimizer.h | 211 ++ gtdynamics/constraints/CMakeLists.txt | 0 .../LinearInequalityConstraint.cpp | 87 + .../constraints/LinearInequalityConstraint.h | 130 + gtdynamics/dynamics/Chain.cpp | 208 +- gtdynamics/dynamics/Chain.h | 130 +- gtdynamics/dynamics/ChainDynamicsGraph.cpp | 221 ++ gtdynamics/dynamics/ChainDynamicsGraph.h | 99 + gtdynamics/dynamics/Dynamics.h | 2 +- gtdynamics/dynamics/DynamicsGraph.cpp | 143 +- gtdynamics/dynamics/DynamicsGraph.h | 93 +- gtdynamics/dynamics/OptimizerSetting.cpp | 2 +- gtdynamics/dynamics/OptimizerSetting.h | 16 +- gtdynamics/dynamics/Simulator.h | 24 +- gtdynamics/factors/BiasedFactor.h | 92 + gtdynamics/factors/CollocationFactors.h | 432 ++- gtdynamics/factors/ConstVarFactor.cpp | 123 + gtdynamics/factors/ConstVarFactor.h | 107 + .../ContactDynamicsFrictionConeFactor.h | 28 +- .../factors/ContactDynamicsMomentFactor.h | 39 +- gtdynamics/factors/ContactEqualityFactor.h | 66 +- gtdynamics/factors/ContactHeightFactor.h | 15 +- .../factors/ContactKinematicsAccelFactor.h | 14 +- .../factors/ContactKinematicsTwistFactor.h | 18 +- gtdynamics/factors/ContactPointFactor.h | 191 +- gtdynamics/factors/ForwardKinematicsFactor.h | 7 +- gtdynamics/factors/GeneralPriorFactor.h | 190 + gtdynamics/factors/JointLimitFactor.h | 28 +- gtdynamics/factors/JointMeasurementFactor.h | 37 +- gtdynamics/factors/MinTorqueFactor.h | 20 +- gtdynamics/factors/PenaltyFactor.h | 140 + gtdynamics/factors/PointGoalFactor.h | 18 +- gtdynamics/factors/PoseFactor.h | 19 +- .../factors/PreintegratedContactFactors.h | 37 +- gtdynamics/factors/SmoothPenaltyFactor.cpp | 96 + gtdynamics/factors/SmoothPenaltyFactor.h | 49 + gtdynamics/factors/SubstituteFactor.cpp | 130 + gtdynamics/factors/SubstituteFactor.h | 143 + gtdynamics/factors/TorqueFactor.h | 11 +- gtdynamics/factors/TwistAccelFactor.h | 7 +- gtdynamics/factors/TwistFactor.h | 7 +- gtdynamics/factors/WrenchEquivalenceFactor.h | 17 +- gtdynamics/factors/WrenchFactor.h | 17 +- gtdynamics/factors/WrenchPlanarFactor.h | 23 +- .../factors/JRCollocationFactors.h | 1 - .../factors/PneumaticActuatorFactors.h | 88 +- .../jumpingrobot/factors/PneumaticFactors.h | 75 +- .../src/actuation_graph_builder.py | 10 +- .../jumpingrobot/src/jr_graph_builder.py | 21 +- gtdynamics/jumpingrobot/src/jr_simulator.py | 38 +- gtdynamics/jumpingrobot/src/jr_values.py | 42 +- gtdynamics/jumpingrobot/src/jr_visualizer.py | 68 +- gtdynamics/jumpingrobot/src/jumping_robot.py | 126 +- .../jumpingrobot/src/robot_graph_builder.py | 16 +- gtdynamics/jumpingrobot/src/test_jr_values.py | 5 +- .../tests/testPneumaticActuatorFactors.cpp | 10 +- .../tests/testPneumaticFactors.cpp | 3 +- gtdynamics/kinematics/Kinematics.h | 12 +- gtdynamics/kinematics/KinematicsInterval.cpp | 32 +- gtdynamics/kinematics/KinematicsPhase.cpp | 2 - gtdynamics/kinematics/KinematicsSlice.cpp | 39 +- .../kinematics/KinematicsTrajectory.cpp | 19 +- .../AugmentedLagrangianOptimizer.cpp | 95 - .../optimizer/AugmentedLagrangianOptimizer.h | 56 - gtdynamics/optimizer/ConstrainedOptimizer.h | 69 - gtdynamics/optimizer/ConvexIQPSolver.cpp | 236 ++ gtdynamics/optimizer/ConvexIQPSolver.h | 42 + gtdynamics/optimizer/EqualityConstraint-inl.h | 66 - gtdynamics/optimizer/EqualityConstraint.cpp | 46 - gtdynamics/optimizer/EqualityConstraint.h | 166 - gtdynamics/optimizer/HistoryLMOptimizer.h | 92 + gtdynamics/optimizer/MutableLMOptimizer.cpp | 353 ++ gtdynamics/optimizer/MutableLMOptimizer.h | 162 + gtdynamics/optimizer/Optimizer.cpp | 38 +- gtdynamics/optimizer/Optimizer.h | 4 +- .../optimizer/PenaltyMethodOptimizer.cpp | 54 - gtdynamics/optimizer/PenaltyMethodOptimizer.h | 67 - gtdynamics/optimizer/QPSolver.cpp | 57 + gtdynamics/optimizer/QPSolver.h | 34 + .../pandarobot/gpmp/GaussianProcessPrior.h | 29 +- gtdynamics/statics/StaticWrenchFactor.cpp | 14 +- gtdynamics/statics/StaticWrenchFactor.h | 30 +- gtdynamics/statics/Statics.cpp | 11 +- gtdynamics/statics/Statics.h | 38 +- gtdynamics/statics/StaticsSlice.cpp | 26 +- gtdynamics/universal_robot/FixedJoint.h | 80 + gtdynamics/universal_robot/HelicalJoint.h | 4 +- gtdynamics/universal_robot/Joint.cpp | 156 +- gtdynamics/universal_robot/Joint.h | 114 +- gtdynamics/universal_robot/Link.cpp | 53 +- gtdynamics/universal_robot/Link.h | 103 +- gtdynamics/universal_robot/PrismaticJoint.h | 4 +- gtdynamics/universal_robot/RevoluteJoint.h | 4 +- gtdynamics/universal_robot/Robot.cpp | 80 +- gtdynamics/universal_robot/Robot.h | 81 +- gtdynamics/universal_robot/RobotModels.h | 51 +- gtdynamics/universal_robot/RobotTypes.h | 12 +- gtdynamics/universal_robot/sdf.cpp | 161 +- gtdynamics/universal_robot/sdf.h | 9 +- gtdynamics/universal_robot/sdf_internal.h | 17 +- gtdynamics/utils/ChainInitializer.cpp | 65 + gtdynamics/utils/ChainInitializer.h | 38 + gtdynamics/utils/DynamicsSymbol.cpp | 25 +- gtdynamics/utils/DynamicsSymbol.h | 111 +- gtdynamics/utils/FootContactConstraintSpec.h | 2 +- gtdynamics/utils/GraphUtils.cpp | 204 + gtdynamics/utils/GraphUtils.h | 238 ++ ...ize_solution_utils.cpp => Initializer.cpp} | 83 +- gtdynamics/utils/Initializer.h | 223 ++ gtdynamics/utils/JsonSaver.h | 43 +- gtdynamics/utils/Phase.cpp | 2 +- gtdynamics/utils/Phase.h | 9 +- gtdynamics/utils/PointOnLink.h | 14 +- gtdynamics/utils/Timer.h | 62 + gtdynamics/utils/Trajectory.cpp | 20 +- gtdynamics/utils/Trajectory.h | 7 +- gtdynamics/utils/WalkCycle.cpp | 16 +- gtdynamics/utils/initialize_solution_utils.h | 214 -- gtdynamics/utils/utils.cpp | 79 +- gtdynamics/utils/utils.h | 66 +- gtdynamics/utils/values.cpp | 10 + gtdynamics/utils/values.h | 55 +- models/urdfs/cart_pole.urdf | 2 +- {urdfs => models/urdfs}/nao.urdf | 0 models/urdfs/plane.urdf | 24 + python/CMakeLists.txt | 10 +- python/README.md | 58 + python/gtdynamics/__init__.py | 2 + python/gtdynamics/preamble/gtdynamics.h | 4 - python/gtdynamics/sim.py | 2 +- .../notebooks/cmopt_benchmark_dashboard.ipynb | 3295 ++++++++++++++++ python/requirements.txt | 2 - python/templates/gtdynamics.tpl | 7 - python/templates/pyproject.toml.in | 49 + python/templates/setup.py.in | 57 - python/tests/test_chain.py | 56 +- python/tests/test_dynamics_graph.py | 2 +- python/tests/test_factors.py | 53 +- python/tests/test_forward_kinematics.py | 7 +- python/tests/test_four_bar.py | 8 +- python/tests/test_link.py | 4 +- python/tests/test_panda.py | 9 +- python/tests/test_print.py | 21 +- python/tests/test_robot.py | 103 +- scripts/CMakeLists.txt | 2 - .../nithya00_constrained_opt_benchmark.cpp | 96 - tests/ManifoldOptScenarios.h | 43 + tests/constrainedExample.h | 80 +- tests/gtsamConstrainedExample.h | 111 + tests/make_joint.h | 29 +- tests/testAugmentedLagrangianOptimizer.cpp | 84 +- tests/testChain.cpp | 1366 ++++++- tests/testCollocationFactor.cpp | 29 +- tests/testConstBiasFactor.cpp | 58 + tests/testConstVarFactor.cpp | 103 + tests/testConstraintManifold.cpp | 484 +++ .../testContactDynamicsFrictionConeFactor.cpp | 15 +- tests/testContactDynamicsMomentFactor.cpp | 5 +- tests/testContactEqualityFactor.cpp | 15 +- tests/testContactHeightFactor.cpp | 3 +- tests/testContactKinematicsAccelFactor.cpp | 5 +- tests/testContactKinematicsTwistFactor.cpp | 13 +- tests/testContactPointFactor.cpp | 129 +- tests/testDynamics.cpp | 8 +- tests/testDynamicsGraph.cpp | 63 +- tests/testDynamicsSymbol.cpp | 25 + tests/testEqualityConstraint.cpp | 169 - tests/testFixedJoint.cpp | 50 + tests/testForwardKinematicsFactor.cpp | 10 +- tests/testGeneralPriorFactor.cpp | 67 + tests/testGraphUtils.cpp | 60 + tests/testHelicalJoint.cpp | 56 +- ...eSolutionUtils.cpp => testInitializer.cpp} | 40 +- tests/testJointMeasurementFactor.cpp | 9 +- tests/testKinematicsConstraints.cpp | 87 + tests/testKinematicsInterval.cpp | 6 +- tests/testKinematicsPhase.cpp | 3 +- tests/testKinematicsSlice.cpp | 2 +- tests/testLink.cpp | 29 +- tests/testManifoldOptimizer_rot2.cpp | 338 ++ tests/testMinTorqueFactor.cpp | 3 +- tests/testMultiJacobian.cpp | 161 + tests/testMutableLMOptimizer.cpp | 58 + tests/testObjectiveFactors.cpp | 6 +- tests/testPenaltyMethodOptimizer.cpp | 30 +- tests/testPhase.cpp | 32 +- tests/testPointGoalFactor.cpp | 5 +- tests/testPoseFactor.cpp | 15 +- tests/testPreintegratedContactFactors.cpp | 3 +- tests/testPrismaticJoint.cpp | 58 +- tests/testRetractor.cpp | 91 + tests/testRevoluteJoint.cpp | 15 +- tests/testRobot.cpp | 84 +- tests/testSdf.cpp | 191 +- tests/testSimulator.cpp | 9 +- tests/testSpiderWalking.cpp | 9 +- tests/testStaticWrenchFactor.cpp | 7 +- tests/testStatics.cpp | 14 +- tests/testStaticsSlice.cpp | 17 +- tests/testSubstituteFactor.cpp | 159 + tests/testTimer.cpp | 37 + tests/testTorqueFactor.cpp | 8 +- tests/testTrajectory.cpp | 28 +- tests/testTspaceBasis.cpp | 190 + tests/testTwistAccelFactor.cpp | 8 +- tests/testTwistFactor.cpp | 14 +- tests/testUniversalRobotChanges.cpp | 101 + tests/testUtils.cpp | 34 +- tests/testValues.cpp | 17 +- tests/testWalkCycle.cpp | 31 +- tests/testWrenchEquivalenceFactor.cpp | 21 +- tests/testWrenchFactor.cpp | 23 +- tests/testWrenchPlanarFactor.cpp | 4 +- tests/walkCycleExample.h | 18 +- 322 files changed, 30087 insertions(+), 4576 deletions(-) create mode 100644 .github/copilot-instructions.md mode change 100755 => 100644 .github/workflows/macos-ci.yml delete mode 100644 .github/workflows/python-ci.yml create mode 100644 CI-workflows.md rename Config.cmake.in => cmake/Config.cmake.in (100%) create mode 100644 cmake/HandleTBB.cmake create mode 100644 examples/README,md create mode 100644 examples/cmake_project_example/CMakeLists.txt create mode 100644 examples/cmake_project_example/main.cpp create mode 100644 examples/example_a1_walking/main_combined.cpp create mode 100644 examples/example_constraint_manifold/CMakeLists.txt create mode 100644 examples/example_constraint_manifold/CartPoleUtils.cpp create mode 100644 examples/example_constraint_manifold/CartPoleUtils.h create mode 100644 examples/example_constraint_manifold/QuadrupedUtils.cpp create mode 100644 examples/example_constraint_manifold/QuadrupedUtils.h create mode 100644 examples/example_constraint_manifold/SerialChain.h create mode 100644 examples/example_constraint_manifold/main_arm_kinematic_planning.cpp create mode 100644 examples/example_constraint_manifold/main_cablerobot.cpp create mode 100644 examples/example_constraint_manifold/main_cartpole.cpp create mode 100644 examples/example_constraint_manifold/main_connected_poses.cpp create mode 100644 examples/example_constraint_manifold/main_quadruped.cpp create mode 100644 examples/example_constraint_manifold/main_range_constraint.cpp create mode 100644 examples/example_constraint_manifold/quadruped.ipynb create mode 100644 examples/example_constraint_manifold/quadruped_plot.py create mode 100644 examples/example_constraint_manifold/quadruped_visualize.py delete mode 100644 examples/example_full_kinodynamic_walking/vision60.urdf rename examples/example_quadruped_mp/{kinematic_mp_sim.py => sim.py} (94%) delete mode 100644 examples/example_quadruped_mp/vision60.urdf create mode 100644 examples/scripts/CMakeLists.txt rename {scripts => examples/scripts}/alejandro_yetong_01_id_four_bar.cpp (86%) rename {scripts => examples/scripts}/constrainedExample.h (89%) rename {scripts => examples/scripts}/forward_dynamics_simple.cpp (85%) rename {scripts => examples/scripts}/gerry00_ForwardDynamicsPrismatic.cpp (90%) rename {scripts => examples/scripts}/gerry00_visualize.m (100%) create mode 100644 examples/scripts/nithya00_constrained_opt_benchmark.cpp rename {scripts => examples/scripts}/nithya00_constrained_opt_benchmark.py (100%) rename {scripts => examples/scripts}/stephanie01_ForwardDynamicsThreeLink.cpp (85%) rename {scripts => examples/scripts}/stephanie02_spider_sdf.cpp (88%) create mode 100644 gtdynamics/cmopt/CMakeLists.txt create mode 100644 gtdynamics/cmopt/ConstraintManifold.cpp create mode 100644 gtdynamics/cmopt/ConstraintManifold.h create mode 100644 gtdynamics/cmopt/LMManifoldOptimizer.cpp create mode 100644 gtdynamics/cmopt/LMManifoldOptimizer.h create mode 100644 gtdynamics/cmopt/LMManifoldOptimizerState.cpp create mode 100644 gtdynamics/cmopt/LMManifoldOptimizerState.h create mode 100644 gtdynamics/cmopt/ManifoldOptimizer.cpp create mode 100644 gtdynamics/cmopt/ManifoldOptimizer.h create mode 100644 gtdynamics/cmopt/MultiJacobian.cpp create mode 100644 gtdynamics/cmopt/MultiJacobian.h create mode 100644 gtdynamics/cmopt/NonlinearMOptimizer.cpp create mode 100644 gtdynamics/cmopt/NonlinearMOptimizer.h create mode 100644 gtdynamics/cmopt/README.md create mode 100644 gtdynamics/cmopt/Retractor.cpp create mode 100644 gtdynamics/cmopt/Retractor.h create mode 100644 gtdynamics/cmopt/TspaceBasis.cpp create mode 100644 gtdynamics/cmopt/TspaceBasis.h create mode 100644 gtdynamics/constrained_optimizer/CMakeLists.txt create mode 100644 gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.cpp create mode 100644 gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h create mode 100644 gtdynamics/constrained_optimizer/ConstrainedOptProblem.cpp create mode 100644 gtdynamics/constrained_optimizer/ConstrainedOptProblem.h create mode 100644 gtdynamics/constrained_optimizer/ConstrainedOptimizer.cpp create mode 100644 gtdynamics/constrained_optimizer/ConstrainedOptimizer.h create mode 100644 gtdynamics/constrained_optimizer/IPOptOptimizer.cpp create mode 100644 gtdynamics/constrained_optimizer/IPOptOptimizer.h create mode 100644 gtdynamics/constrained_optimizer/SQPOptimizer.cpp create mode 100644 gtdynamics/constrained_optimizer/SQPOptimizer.h create mode 100644 gtdynamics/constraints/CMakeLists.txt create mode 100644 gtdynamics/constraints/LinearInequalityConstraint.cpp create mode 100644 gtdynamics/constraints/LinearInequalityConstraint.h create mode 100644 gtdynamics/dynamics/ChainDynamicsGraph.cpp create mode 100644 gtdynamics/dynamics/ChainDynamicsGraph.h create mode 100644 gtdynamics/factors/BiasedFactor.h create mode 100644 gtdynamics/factors/ConstVarFactor.cpp create mode 100644 gtdynamics/factors/ConstVarFactor.h create mode 100644 gtdynamics/factors/GeneralPriorFactor.h create mode 100644 gtdynamics/factors/PenaltyFactor.h create mode 100644 gtdynamics/factors/SmoothPenaltyFactor.cpp create mode 100644 gtdynamics/factors/SmoothPenaltyFactor.h create mode 100644 gtdynamics/factors/SubstituteFactor.cpp create mode 100644 gtdynamics/factors/SubstituteFactor.h delete mode 100644 gtdynamics/optimizer/AugmentedLagrangianOptimizer.cpp delete mode 100644 gtdynamics/optimizer/AugmentedLagrangianOptimizer.h delete mode 100644 gtdynamics/optimizer/ConstrainedOptimizer.h create mode 100644 gtdynamics/optimizer/ConvexIQPSolver.cpp create mode 100644 gtdynamics/optimizer/ConvexIQPSolver.h delete mode 100644 gtdynamics/optimizer/EqualityConstraint-inl.h delete mode 100644 gtdynamics/optimizer/EqualityConstraint.cpp delete mode 100644 gtdynamics/optimizer/EqualityConstraint.h create mode 100644 gtdynamics/optimizer/HistoryLMOptimizer.h create mode 100644 gtdynamics/optimizer/MutableLMOptimizer.cpp create mode 100644 gtdynamics/optimizer/MutableLMOptimizer.h delete mode 100644 gtdynamics/optimizer/PenaltyMethodOptimizer.cpp delete mode 100644 gtdynamics/optimizer/PenaltyMethodOptimizer.h create mode 100644 gtdynamics/optimizer/QPSolver.cpp create mode 100644 gtdynamics/optimizer/QPSolver.h create mode 100644 gtdynamics/universal_robot/FixedJoint.h create mode 100644 gtdynamics/utils/ChainInitializer.cpp create mode 100644 gtdynamics/utils/ChainInitializer.h create mode 100644 gtdynamics/utils/GraphUtils.cpp create mode 100644 gtdynamics/utils/GraphUtils.h rename gtdynamics/utils/{initialize_solution_utils.cpp => Initializer.cpp} (83%) create mode 100644 gtdynamics/utils/Initializer.h create mode 100644 gtdynamics/utils/Timer.h delete mode 100644 gtdynamics/utils/initialize_solution_utils.h rename {urdfs => models/urdfs}/nao.urdf (100%) create mode 100644 models/urdfs/plane.urdf create mode 100644 python/README.md create mode 100644 python/notebooks/cmopt_benchmark_dashboard.ipynb delete mode 100644 python/requirements.txt create mode 100644 python/templates/pyproject.toml.in delete mode 100644 python/templates/setup.py.in delete mode 100644 scripts/CMakeLists.txt delete mode 100644 scripts/nithya00_constrained_opt_benchmark.cpp create mode 100644 tests/ManifoldOptScenarios.h create mode 100644 tests/gtsamConstrainedExample.h create mode 100644 tests/testConstBiasFactor.cpp create mode 100644 tests/testConstVarFactor.cpp create mode 100644 tests/testConstraintManifold.cpp delete mode 100644 tests/testEqualityConstraint.cpp create mode 100644 tests/testFixedJoint.cpp create mode 100644 tests/testGeneralPriorFactor.cpp create mode 100644 tests/testGraphUtils.cpp rename tests/{testInitializeSolutionUtils.cpp => testInitializer.cpp} (89%) create mode 100644 tests/testKinematicsConstraints.cpp create mode 100644 tests/testManifoldOptimizer_rot2.cpp create mode 100644 tests/testMultiJacobian.cpp create mode 100644 tests/testMutableLMOptimizer.cpp create mode 100644 tests/testRetractor.cpp create mode 100644 tests/testSubstituteFactor.cpp create mode 100644 tests/testTimer.cpp create mode 100644 tests/testTspaceBasis.cpp create mode 100644 tests/testUniversalRobotChanges.cpp diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md new file mode 100644 index 000000000..b2179615f --- /dev/null +++ b/.github/copilot-instructions.md @@ -0,0 +1,11 @@ +For reviewing PRs: +* All functions in header files should have doxygen-style API docs, /** */ style, except small functions like getters which can have single line /// comments, no need for @brief, @params etc +* Use /// for single-line comments rather than /** */ +* Use meaningful variable names, e.g. `measurement` not `msm`, avoid abbreviations. +* Flag overly complex or long/functions: break up in smaller functions +* On Windows it is necessary to explicitly export all functions from the library which should be externally accessible. To do this, include the macro `GTSAM_EXPORT` in your class or function definition. +* If we add a C++ function to a `.i` file to expose it to the wrapper, we must ensure that the parameter names match exactly between the declaration in the header file and the declaration in the `.i`. Similarly, if we change any parameter names in a wrapped function in a header file, or change any parameter names in a `.i` file, we must change the corresponding function in the other file to reflect those changes. +* Classes are Uppercase, methods and functions lowerMixedCase. +* Public fields in structs keep plain names (no trailing underscore). +* Apart from those naming conventions, we adopt Google C++ style. +* After any code change, always run relevant tests via `make -j6 testXXX.run` in the build folder $WORKSPACE/build. If in VS code, ask for escalated permissions if needed. diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index c1c8dddbd..9da95021f 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -2,89 +2,174 @@ name: Linux CI on: [pull_request] +# Cancels any in-progress workflow runs for the same PR when a new push is made, +# allowing the runner to become available more quickly for the latest changes. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: - name: ${{ matrix.name }} ${{ matrix.build_type }} + name: ${{ matrix.name }} ${{ matrix.build_type }} (${{ matrix.build_type == 'Release' && 'native+py' || 'no-native C++-only' }}) runs-on: ${{ matrix.os }} env: - CTEST_OUTPUT_ON_FAILURE: ON - CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} + PYTHON_VERSION: ${{ matrix.python_version }} + GTSAM_INSTALL_DIR: ${{ github.workspace }}/gtsam_install strategy: fail-fast: false 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-20.04-clang-10] - + name: [ubuntu-24.04-gcc-14, ubuntu-24.04-clang-16] build_type: [Debug, Release] + python_version: [3] include: - - name: ubuntu-18.04-gcc-9 - os: ubuntu-18.04 + - name: ubuntu-24.04-gcc-14 + os: ubuntu-24.04 compiler: gcc - version: "9" - - - name: ubuntu-20.04-clang-10 - os: ubuntu-20.04 + version: "14" + - name: ubuntu-24.04-clang-16 + os: ubuntu-24.04 compiler: clang - version: "10" + version: "16" steps: - - name: Setup Compiler + - name: Checkout GTDynamics Repository + uses: actions/checkout@v4 + + - name: Install System Dependencies run: | + set -e + NTHREADS="$(nproc)" + echo "NTHREADS=${NTHREADS}" >> $GITHUB_ENV + sudo apt-get -y update if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV - else sudo apt-get install -y clang-${{ matrix.version }} g++-multilib - echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV - echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV fi - - - name: Install Dependencies - run: | - # For SDFormat sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743 sudo apt-get -y update - - sudo apt-get -y install libtbb-dev libboost-all-dev libsdformat10-dev - - # Install CppUnitLite. - git clone https://github.com/borglab/CppUnitLite.git - cd CppUnitLite && mkdir build && cd $_ - cmake .. && sudo make -j4 install - cd ../../ - - - name: GTSAM + sudo apt-get -y install libtbb-dev libboost-all-dev libsdformat15-dev + + # GTDynamics C++ tests use CppUnitLite (not provided by default on the runner). + git clone https://github.com/borglab/CppUnitLite.git /tmp/CppUnitLite + cd /tmp/CppUnitLite + mkdir build && cd build + # CppUnitLite's CMakeLists uses an old minimum CMake version; newer CMake + # requires explicitly opting into older policy compatibility. + # Force a modern C++ standard for newer Boost headers (Homebrew Boost requires C++11+). + cmake -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DCMAKE_CXX_STANDARD=17 -DCMAKE_CXX_STANDARD_REQUIRED=ON .. + sudo make -j${NTHREADS} install + cd ${{ github.workspace }} + sudo rm -rf /tmp/CppUnitLite + + - name: Python Dependencies (Release only) + if: matrix.build_type == 'Release' + shell: bash run: | - sudo add-apt-repository -y ppa:borglab/gtsam-develop - sudo apt-get -y update - sudo apt-get -y install libgtsam-no-tbb-dev libgtsam-no-tbb-unstable-dev - - - name: Checkout - uses: actions/checkout@master + set -e + python${{ env.PYTHON_VERSION }} -m venv venv + source venv/bin/activate + echo "VENV_PYTHON_EXECUTABLE=${{ github.workspace }}/venv/bin/python" >> $GITHUB_ENV + echo "${{ github.workspace }}/venv/bin" >> $GITHUB_PATH + python -m pip install --upgrade pip + python -m pip install -U "setuptools<70" wheel numpy pyparsing pyyaml "pybind11-stubgen>=2.5.1" + + - name: Build & Install GTSAM + shell: bash + run: | + set -e + git clone https://github.com/borglab/gtsam.git /tmp/gtsam_source + cd /tmp/gtsam_source + mkdir build && cd $_ + if [ "${{ matrix.build_type }}" = "Release" ]; then + if [ "${{ matrix.compiler }}" = "gcc" ]; then + MARCH_NATIVE=OFF + else + MARCH_NATIVE=ON + fi + BUILD_PYTHON=ON + PYTHON_ARGS="-D PYTHON_EXECUTABLE=${{ env.VENV_PYTHON_EXECUTABLE }}" + else + MARCH_NATIVE=OFF + BUILD_PYTHON=OFF + PYTHON_ARGS="" + fi + cmake -D GTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ + -D GTSAM_BUILD_WITH_MARCH_NATIVE=${MARCH_NATIVE} \ + -D GTSAM_BUILD_PYTHON=${BUILD_PYTHON} \ + -D GTSAM_WITH_TBB=OFF \ + -D CMAKE_CXX_FLAGS="-faligned-new" \ + -D CMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + ${PYTHON_ARGS} \ + -D CMAKE_INSTALL_PREFIX=${{ env.GTSAM_INSTALL_DIR }} \ + .. + sudo make -j${NTHREADS} install + if [ "${{ matrix.build_type }}" = "Release" ]; then + make -j${NTHREADS} python-install + fi + sudo ldconfig + cd ${{ github.workspace }} + sudo rm -rf /tmp/gtsam_source - - name: Build Directory - run: mkdir build + - name: Create build directory + run: mkdir build - - name: Configure + - name: Configure GTDynamics run: | - cmake -DGTDYNAMICS_BUILD_PYTHON=OFF -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF .. + set -e + if [ "${{ matrix.build_type }}" = "Release" ]; then + BUILD_PYTHON=ON + PYTHON_ARGS="-DPYTHON_EXECUTABLE=${{ env.VENV_PYTHON_EXECUTABLE }}" + else + BUILD_PYTHON=OFF + PYTHON_ARGS="" + fi + cmake -DGTDYNAMICS_BUILD_PYTHON=${BUILD_PYTHON} \ + ${PYTHON_ARGS} \ + -DGTSAM_DIR="${{ env.GTSAM_INSTALL_DIR }}/lib/cmake/GTSAM" \ + -DCMAKE_PREFIX_PATH="${{ env.GTSAM_INSTALL_DIR }}" \ + -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + .. working-directory: ./build - name: Build - run: make -j$(nproc) + run: make -j${NTHREADS} working-directory: ./build - - name: Test - run: make -j$(nproc) check + - name: Test C++ + run: | + set -e + if [ "${{ matrix.compiler }}" = "gcc" ]; then + export LD_LIBRARY_PATH="${{ env.GTSAM_INSTALL_DIR }}/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" + if [ -d "${{ env.GTSAM_INSTALL_DIR }}/lib64" ]; then + export LD_LIBRARY_PATH="${{ env.GTSAM_INSTALL_DIR }}/lib64:$LD_LIBRARY_PATH" + fi + fi + make -j${NTHREADS} check + working-directory: ./build + + - name: Install Python Wrappers (Release only) + shell: bash + if: matrix.build_type == 'Release' + run: | + set -e + make -j${NTHREADS} python-install working-directory: ./build - - - name: Install - run: sudo make -j$(nproc) install + + - name: Test Python (Release only) + if: matrix.build_type == 'Release' + shell: bash + run: | + set -e + source ${{ github.workspace }}/venv/bin/activate + export LD_LIBRARY_PATH="${{ env.GTSAM_INSTALL_DIR }}/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" + make -j${NTHREADS} python-test working-directory: ./build diff --git a/.github/workflows/macos-ci.yml b/.github/workflows/macos-ci.yml old mode 100755 new mode 100644 index 52ec07f9b..ef9374724 --- a/.github/workflows/macos-ci.yml +++ b/.github/workflows/macos-ci.yml @@ -2,34 +2,43 @@ name: macOS CI on: [pull_request] +# Cancels any in-progress workflow runs for the same PR when a new push is made, +# allowing the runner to become available more quickly for the latest changes. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: - name: ${{ matrix.name }} ${{ matrix.build_type }} + name: ${{ matrix.name }} ${{ matrix.build_type }} (${{ matrix.build_type == 'Release' && 'native+py' || 'no-native C++-only' }}) runs-on: ${{ matrix.os }} env: - CTEST_OUTPUT_ON_FAILURE: ON - CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} - GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} + PYTHON_VERSION: ${{ matrix.python_version }} + GTSAM_INSTALL_DIR: ${{ github.workspace }}/gtsam_install + strategy: fail-fast: false 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: [macOS-10.15-xcode-11.3.1] - + name: [macos-15-xcode-16.4] build_type: [Debug, Release] - build_unstable: [ON] + python_version: [3] include: - - name: macOS-10.15-xcode-11.3.1 - os: macOS-10.15 + - name: macos-15-xcode-16.4 + os: macos-15 compiler: xcode - version: "11.3.1" + version: "16.4" steps: - - name: Setup Compiler + - name: Checkout GTDynamics Repository + uses: actions/checkout@v4 + + - name: Install System Dependencies run: | + set -e + NTHREADS="$(sysctl -n hw.ncpu)" + echo "NTHREADS=${NTHREADS}" >> $GITHUB_ENV if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV @@ -39,41 +48,118 @@ jobs: echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi - - - name: Dependencies - run: | brew install boost brew tap osrf/simulation - brew install sdformat10 + brew install sdformat15 - - name: GTSAM - run: | - git clone https://github.com/borglab/gtsam.git - cd gtsam + # GTDynamics C++ tests use CppUnitLite (install it once per job). + git clone https://github.com/borglab/CppUnitLite.git /tmp/CppUnitLite + cd /tmp/CppUnitLite mkdir build && cd build - cmake -D GTSAM_BUILD_EXAMPLES_ALWAYS=OFF .. - make -j$(sysctl -n hw.physicalcpu) install - cd ../../ # go back to home directory + # CppUnitLite's CMakeLists uses an old minimum CMake version; newer CMake + # requires explicitly opting into older policy compatibility. + # Force a modern C++ standard for newer Boost headers (Homebrew Boost requires C++11+). + cmake -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DCMAKE_CXX_STANDARD=17 -DCMAKE_CXX_STANDARD_REQUIRED=ON .. + sudo make -j${NTHREADS} install + cd ${{ github.workspace }} + rm -rf /tmp/CppUnitLite - - name: Checkout - uses: actions/checkout@v2 + - name: Python Dependencies (Release only) + if: matrix.build_type == 'Release' + shell: bash + run: | + set -e + python${{ env.PYTHON_VERSION }} -m venv venv + source venv/bin/activate + echo "VENV_PYTHON_EXECUTABLE=${{ github.workspace }}/venv/bin/python" >> $GITHUB_ENV + echo "${{ github.workspace }}/venv/bin" >> $GITHUB_PATH + python -m pip install --upgrade pip + python -m pip install -U "setuptools<70" wheel numpy pyparsing pyyaml "pybind11-stubgen>=2.5.1" + + - name: Build & Install GTSAM + shell: bash + run: | + set -e + git clone https://github.com/borglab/gtsam.git /tmp/gtsam_source + cd /tmp/gtsam_source + mkdir build && cd $_ + if [ "${{ matrix.build_type }}" = "Release" ]; then + MARCH_NATIVE=ON + BUILD_PYTHON=ON + PYTHON_ARGS="-D PYTHON_EXECUTABLE=${{ env.VENV_PYTHON_EXECUTABLE }}" + else + MARCH_NATIVE=OFF + BUILD_PYTHON=OFF + PYTHON_ARGS="" + fi + cmake -D GTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ + -D GTSAM_BUILD_WITH_MARCH_NATIVE=${MARCH_NATIVE} \ + -D GTSAM_BUILD_PYTHON=${BUILD_PYTHON} \ + -D GTSAM_WITH_TBB=OFF \ + -D CMAKE_CXX_FLAGS="-faligned-new" \ + -D CMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + ${PYTHON_ARGS} \ + -D CMAKE_INSTALL_PREFIX=${{ env.GTSAM_INSTALL_DIR }} \ + .. + make -j${NTHREADS} install + if [ "${{ matrix.build_type }}" = "Release" ]; then + make -j${NTHREADS} python-install + fi + cd ${{ github.workspace }} + rm -rf /tmp/gtsam_source - - name: Build Directory - run: mkdir ./build + - name: Create build directory + run: mkdir build - - name: Configure + - name: Configure GTDynamics + shell: bash run: | - cmake -DGTDYNAMICS_BUILD_PYTHON=OFF -DGTDYNAMICS_BUILD_CABLE_ROBOT=ON -DGTDYNAMICS_BUILD_JUMPING_ROBOT=ON .. + set -e + if [ "${{ matrix.build_type }}" = "Release" ]; then + BUILD_PYTHON=ON + PYTHON_ARGS="-DPYTHON_EXECUTABLE=${{ env.VENV_PYTHON_EXECUTABLE }}" + else + BUILD_PYTHON=OFF + PYTHON_ARGS="" + fi + cmake -DGTDYNAMICS_BUILD_PYTHON=${BUILD_PYTHON} \ + ${PYTHON_ARGS} \ + -DGTSAM_DIR="${{ env.GTSAM_INSTALL_DIR }}/lib/cmake/GTSAM" \ + -DCMAKE_PREFIX_PATH="${{ env.GTSAM_INSTALL_DIR }};$(brew --prefix)" \ + -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + .. working-directory: ./build - name: Build - run: make -j$(sysctl -n hw.physicalcpu) + run: make -j${NTHREADS} working-directory: ./build - - name: Test - run: make -j$(sysctl -n hw.physicalcpu) check + - name: Test C++ + shell: bash + run: | + # Mirror the python-test linker settings: GTSAM is installed to a local prefix. + set -e + export DYLD_LIBRARY_PATH="${{ env.GTSAM_INSTALL_DIR }}/lib${DYLD_LIBRARY_PATH:+:$DYLD_LIBRARY_PATH}" + make -j${NTHREADS} check working-directory: ./build - - name: Install - run: make -j$(sysctl -n hw.physicalcpu) install + - name: Install Python Wrappers (Release only) + shell: bash + if: matrix.build_type == 'Release' + run: | + set -e + make -j${NTHREADS} python-install + working-directory: ./build + + - name: Test Python (Release only) + if: matrix.build_type == 'Release' + # For macOS, set DYLD_LIBRARY_PATH so the dynamic linker can find the installed GTSAM libraries. + # Also, ensure the venv is active for the context of running the tests. + shell: bash + run: | + set -e + source ${{ github.workspace }}/venv/bin/activate # Ensure venv Python and its site-packages are primary + export DYLD_LIBRARY_PATH="${{ env.GTSAM_INSTALL_DIR }}/lib${DYLD_LIBRARY_PATH:+:$DYLD_LIBRARY_PATH}" + echo "DYLD_LIBRARY_PATH is set to: $DYLD_LIBRARY_PATH" + make -j${NTHREADS} python-test working-directory: ./build diff --git a/.github/workflows/python-ci.yml b/.github/workflows/python-ci.yml deleted file mode 100644 index 806e3e71f..000000000 --- a/.github/workflows/python-ci.yml +++ /dev/null @@ -1,137 +0,0 @@ -name: Python CI - -on: [pull_request] - -jobs: - build: - name: ${{ matrix.name }} ${{ matrix.build_type }} Python ${{ matrix.python_version }} - runs-on: ${{ matrix.os }} - - env: - CTEST_OUTPUT_ON_FAILURE: ON - CTEST_PARALLEL_LEVEL: 2 - CMAKE_BUILD_TYPE: ${{ matrix.build_type }} - PYTHON_VERSION: ${{ matrix.python_version }} - - strategy: - fail-fast: false - 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, macOS-10.15-xcode-11.3.1] - - build_type: [Debug, Release] - python_version: [3] - include: - - name: ubuntu-18.04-gcc-9 - os: ubuntu-18.04 - compiler: gcc - version: "9" - - - name: ubuntu-18.04-clang-10 - os: ubuntu-18.04 - compiler: clang - version: "10" - - - name: ubuntu-18.04-clang-10 - os: ubuntu-18.04 - compiler: clang - version: "10" - build_type: Debug - python_version: "3" - - - name: macOS-10.15-xcode-11.3.1 - os: macOS-10.15 - compiler: xcode - version: "11.3.1" - - steps: - - name: Install Dependencies (Linux) - if: runner.os == 'Linux' - run: | - if [ "${{ matrix.compiler }}" = "gcc" ]; then - sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib - echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV - echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV - - else - sudo apt-get install -y clang-${{ matrix.version }} g++-multilib - echo "CC=clang" >> $GITHUB_ENV - echo "CXX=clang++" >> $GITHUB_ENV - fi - - # For SDFormat - sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' - sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743 - sudo apt-get -y update - - sudo apt-get -y install libtbb-dev libboost-all-dev libsdformat10-dev - - - name: Install Dependencies (macOS) - if: runner.os == 'macOS' - run: | - if [ "${{ matrix.compiler }}" = "gcc" ]; then - brew install gcc@${{ matrix.version }} - echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV - echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV - else - sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app - echo "CC=clang" >> $GITHUB_ENV - echo "CXX=clang++" >> $GITHUB_ENV - fi - - brew install boost - brew tap osrf/simulation - brew install sdformat10 - - - name: Python Dependencies - run: | - # Install dependencies for gtwrap - pip3 install -U setuptools numpy pyparsing pyyaml - - - name: GTSAM (Linux) - if: runner.os == 'Linux' - run: | - # Install gtsam - git clone https://github.com/borglab/gtsam.git /home/runner/work/gtsam - cd /home/runner/work/gtsam - mkdir build && cd $_ - cmake -D GTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_PYTHON=ON .. - sudo make -j$(nproc) install && sudo make python-install - sudo ldconfig - cd $GITHUB_WORKSPACE # go back to home directory - - - name: GTSAM (macOS) - if: runner.os == 'macOS' - run: | - # Install gtsam - git clone https://github.com/borglab/gtsam.git - cd gtsam - mkdir build && cd $_ - cmake -D GTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_PYTHON=ON .. - make -j$(sysctl -n hw.physicalcpu) install && make python-install - cd $GITHUB_WORKSPACE # go back to home directory - - - name: Checkout - uses: actions/checkout@v2 - - - name: Build Directory - run: mkdir build - - - name: Configure - run: | - cmake -DGTDYNAMICS_BUILD_PYTHON=ON -DGTDYNAMICS_BUILD_CABLE_ROBOT=ON -DGTDYNAMICS_BUILD_JUMPING_ROBOT=ON .. - working-directory: ./build - - - name: Build - run: make -j4 - working-directory: ./build - - - name: Test - run: make -j4 python-test - working-directory: ./build - - - name: Install - run: sudo make -j4 python-install - working-directory: ./build diff --git a/.gitignore b/.gitignore index 4f80b3744..dbd6de3b0 100644 --- a/.gitignore +++ b/.gitignore @@ -10,7 +10,9 @@ __pycache__/ # Distribution / packaging .Python -build/ +build*/ +tmp/ +vs_build/ develop-eggs/ dist/ downloads/ @@ -110,9 +112,12 @@ venv.bak/ # test-data v-rep/test_data/ - +data/ +figures/ visualization/*.json # gerry00 visualization data scripts/gerry00_rpr_viz.gif traj.csv + +results/ diff --git a/CI-workflows.md b/CI-workflows.md new file mode 100644 index 000000000..cde757bdd --- /dev/null +++ b/CI-workflows.md @@ -0,0 +1,49 @@ +# CI Workflows + +GTDynamics has two GitHub Actions workflows that run on every pull request: + +- `.github/workflows/linux-ci.yml` +- `.github/workflows/macos-ci.yml` + +They are intentionally kept as similar as possible: both build **GTSAM from source** first, then build/test **GTDynamics** against that installed GTSAM (C++ always; Python in Release only). This avoids relying on a prebuilt GTSAM package, so CI picks up fixes from GTSAM's default branch as soon as they land. + +## Platforms and toolchains + +- **Ubuntu 24.04:** gcc-14 and clang-16 +- **macOS 15:** Xcode 16.4 + +Both platforms test **Debug** and **Release**. + +## Build / test matrix behavior + +- **Release:** + - Builds GTSAM with Python wrappers (`GTSAM_BUILD_PYTHON=ON`) + - Builds GTDynamics with Python wrappers (`GTDYNAMICS_BUILD_PYTHON=ON`) + - Runs **C++ tests** (`make check`) and **Python tests** (`make python-test`) +- **Debug:** + - Builds GTSAM without Python wrappers + - Builds GTDynamics without Python wrappers + - Runs **C++ tests only** + +## Python environment (same on macOS and Linux) + +In Release builds, both workflows create a Python virtual environment (`venv`) and pass its interpreter to CMake via `PYTHON_EXECUTABLE`. This ensures: + +- The locally built `gtsam` Python package (from the GTSAM build) is installed into the same environment used to install/test `gtdynamics`. +- CI does not accidentally try to fetch `gtsam` from PyPI during `pip install .`. + +## CPU flags / Eigen alignment consistency + +Eigen's alignment requirements can change when AVX is enabled (e.g., via `-march=native`). CI keeps GTSAM and GTDynamics consistent in two ways: + +- Release builds enable `-march=native` for clang on Linux; it is disabled for gcc on Linux. +- GTDynamics' CMake configuration matches `GTSAM_BUILD_WITH_MARCH_NATIVE` to the installed `gtsam` target's exported compile options, so the downstream build stays consistent even if the workflow does not explicitly set the flag for GTDynamics. + +## Runtime library paths + +Because GTSAM is installed under a workspace prefix in CI (not a system default), tests need appropriate dynamic loader search paths: + +- **macOS:** `DYLD_LIBRARY_PATH` is set to `${GTSAM_INSTALL_DIR}/lib` during C++ and Python tests. +- **Linux:** `LD_LIBRARY_PATH` is set to `${GTSAM_INSTALL_DIR}/lib` for Python tests, and for gcc C++ tests as needed. + +For local "build from source like CI" instructions, see the top-level `README.md` (section at the end). diff --git a/CMakeLists.txt b/CMakeLists.txt index 4544e4569..f9a0a4d5d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.9...3.29) enable_testing() project(gtdynamics LANGUAGES CXX C @@ -7,11 +7,35 @@ project(gtdynamics add_compile_options(-faligned-new) -# Enforce c++11 standards -add_compile_options(-std=c++11) # CMake 3.1 and earlier -set(CMAKE_CXX_STANDARD 11) +# Configuration options should appear even if dependency checks fail. +option(GTDYNAMICS_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON) +option(GTDYNAMICS_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" OFF) +option(GTDYNAMICS_WITH_SUITESPARSE "Enable SuiteSparse-based routines and IFOPT/IPOpt optimizer" OFF) +option(GTDYNAMICS_BUILD_CABLE_ROBOT "Build Cable Robot" OFF) +option(GTDYNAMICS_BUILD_JUMPING_ROBOT "Build Jumping Robot" OFF) +option(GTDYNAMICS_BUILD_PANDA_ROBOT "Build Panda Robot" OFF) +option(GTDYNAMICS_BUILD_PYTHON "Build Python wrapper" ON) +option(GTDYNAMICS_BUILD_EXAMPLES "Build all examples" ON) +mark_as_advanced(CLEAR + GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + GTDYNAMICS_WITH_TBB + GTDYNAMICS_BUILD_PANDA_ROBOT + GTDYNAMICS_BUILD_PYTHON + GTDYNAMICS_BUILD_EXAMPLES +) +mark_as_advanced( + GTDYNAMICS_WITH_SUITESPARSE + GTDYNAMICS_BUILD_CABLE_ROBOT + GTDYNAMICS_BUILD_JUMPING_ROBOT +) + +# Enforce c++17 standards +add_compile_options(-std=c++17) # CMake 3.1 and earlier +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + # ############################################################################## # Find Dependencies # # ############################################################################## @@ -23,6 +47,36 @@ set(GTSAM_LIBS gtsam) find_package(GTSAMCMakeTools) include(GtsamMakeConfigFile) + +# Keep GTDynamics' CPU-vectorization/alignment assumptions consistent with the +# installed GTSAM we are linking against. In particular, Eigen's required +# alignment changes (e.g., 16 -> 32 bytes) when AVX is enabled via -march=native. +# +# GtsamBuildTypes defines/uses the cache option `GTSAM_BUILD_WITH_MARCH_NATIVE` +# to control whether `-march=native` is added. If it disagrees with how the +# installed `gtsam` target was compiled, Eigen may assert on some platforms. +if(TARGET gtsam) + get_target_property(_gtsam_iface_compile_options gtsam INTERFACE_COMPILE_OPTIONS) + set(_gtsam_uses_march_native OFF) + if(_gtsam_iface_compile_options) + foreach(_opt IN LISTS _gtsam_iface_compile_options) + if(_opt MATCHES ".*march=native.*") + set(_gtsam_uses_march_native ON) + break() + endif() + endforeach() + endif() + + # Force the cache option to match the detected GTSAM target setting. + set(GTSAM_BUILD_WITH_MARCH_NATIVE ${_gtsam_uses_march_native} + CACHE BOOL "Build with -march=native (matched to installed GTSAM)" FORCE) + message(STATUS "Matched GTSAM_BUILD_WITH_MARCH_NATIVE to installed GTSAM: ${GTSAM_BUILD_WITH_MARCH_NATIVE}") + + unset(_gtsam_iface_compile_options) + unset(_gtsam_uses_march_native) + unset(_opt) +endif() + include(GtsamBuildTypes) include(GtsamTesting) @@ -31,32 +85,36 @@ set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${GTSAM_DIR}/../GTSAMCMakeTools") # Boost is automatically added when we add GTSAM above. # This ensures both GTSAM and GTDynamics have the same Boost version. -include_directories(${Boost_INCLUDE_DIR}) - -if(NOT (${Boost_VERSION} LESS 105600)) - message( - "Ignoring Boost restriction on optional lvalue assignment from rvalues") - add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES - -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) +if(Boost_FOUND) + include_directories(${Boost_INCLUDE_DIR}) + + if(NOT (Boost_VERSION LESS 105600)) + message( + "Ignoring Boost restriction on optional lvalue assignment from rvalues") + add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES + -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) + endif() endif() # For Python wrapper. set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${GTSAM_DIR}/../GTSAMCMakeTools") -# For parsing urdf/sdf files. -set(SDFormat_VERSION 10) +# For parsing urdf/sdf files. # TODO: hardcoded sdformat version +set(SDFormat_VERSION 15) find_package(sdformat${SDFormat_VERSION} REQUIRED) # ############################################################################## # Dynamics library # ############################################################################## -option(GTDYNAMICS_BUILD_CABLE_ROBOT "Build Cable Robot" ON) -option(GTDYNAMICS_BUILD_JUMPING_ROBOT "Build Jumping Robot" ON) -option(GTDYNAMICS_BUILD_PANDA_ROBOT "Build Panda Robot" ON) +if(GTDYNAMICS_ENABLE_BOOST_SERIALIZATION) + add_compile_definitions(GTSAM_ENABLE_BOOST_SERIALIZATION) + add_compile_definitions(GTDYNAMICS_ENABLE_BOOST_SERIALIZATION) +endif() + +include(cmake/HandleTBB.cmake) # TBB add_subdirectory(gtdynamics) -option(GTDYNAMICS_BUILD_PYTHON "Build Python wrapper" ON) message(STATUS "Build Python Wrapper: ${GTDYNAMICS_BUILD_PYTHON}") if(GTDYNAMICS_BUILD_PYTHON) @@ -66,12 +124,6 @@ endif() # Process subdirectories. add_subdirectory(tests) -option(GTDYNAMICS_BUILD_SCRIPTS "Build all scripts" ON) -if(GTDYNAMICS_BUILD_SCRIPTS) - add_subdirectory(scripts) -endif() - -option(GTDYNAMICS_BUILD_EXAMPLES "Build all examples" ON) if(GTDYNAMICS_BUILD_EXAMPLES) add_subdirectory(examples) endif() @@ -79,17 +131,25 @@ endif() message(STATUS "===============================================================") message(STATUS "================ Configuration Options ======================") message(STATUS "Project : ${PROJECT_NAME}") +message(STATUS "Build Type : ${CMAKE_BUILD_TYPE}") message(STATUS "CMAKE_CXX_COMPILER_ID type : ${CMAKE_CXX_COMPILER_ID}") message(STATUS "CMAKE_CXX_COMPILER_VERSION : ${CMAKE_CXX_COMPILER_VERSION}") message(STATUS "GTSAM Version : ${GTSAM_VERSION}") message(STATUS "Boost Version : ${Boost_VERSION}") message(STATUS "SDFormat Version : ${sdformat${SDFormat_VERSION}_VERSION}") + +if(TBB_FOUND) + message(STATUS "Use Intel TBB : Yes (${TBB_VERSION})") +else() +message(STATUS "Use Intel TBB : NO") +endif(TBB_FOUND) +message(STATUS "Enable SuiteSparse : ${GTDYNAMICS_WITH_SUITESPARSE}") + message(STATUS "Build Python : ${GTDYNAMICS_BUILD_PYTHON}") if(GTDYNAMICS_BUILD_PYTHON) message(STATUS "Python Version : ${WRAP_PYTHON_VERSION}") endif() message(STATUS "Build march=native : ${GTSAM_BUILD_WITH_MARCH_NATIVE}") -message(STATUS "Build Scripts : ${GTDYNAMICS_BUILD_SCRIPTS}") message(STATUS "Build Examples : ${GTDYNAMICS_BUILD_EXAMPLES}") message(STATUS "Build Robots") message(STATUS " Cable Robot : ${GTDYNAMICS_BUILD_CABLE_ROBOT}") @@ -115,7 +175,7 @@ endif() # Configure the config file that is includes the exports configure_package_config_file( - ${CMAKE_SOURCE_DIR}/Config.cmake.in + ${CMAKE_SOURCE_DIR}/cmake/Config.cmake.in "${CMAKE_BINARY_DIR}/gtdynamicsConfig.cmake" INSTALL_DESTINATION "${INSTALL_CMAKE_DIR}" INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX}) diff --git a/README.md b/README.md index 044f36466..a1fe5b507 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ GTDynamics is a library that allows the user to express the full kinodynamics co * [GTSAM4](https://github.com/borglab/gtsam) * [gtwrap](https://github.com/borglab/wrap) -* [sdformat10](https://github.com/osrf/sdformat) +* [sdformat15](https://github.com/osrf/sdformat) ## Installing SDFormat @@ -26,9 +26,9 @@ Using Homebrew is the easiest way to get SDFormat installed and it also makes sw ```sh $ # Install homebrew. $ /bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)" -$ # Set up the tap ind install sdformat10 +$ # Set up the tap and install sdformat15 $ brew tap osrf/simulation -$ brew install sdformat10 +$ brew install sdformat15 ``` ### Source @@ -48,10 +48,10 @@ wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - sudo apt-get update # Install SDFormat dependencies -sudo apt-get install -y libtinyxml2-dev liburdfdom-dev libignition-cmake2-dev libignition-tools-dev libignition-math6-dev +sudo apt-get install -y libtinyxml2-dev liburdfdom-dev libgz-cmake2-dev libgz-tools-dev libgz-math6-dev # Set the version to install -export GTD_SDFormat_VERSION="10.5.0" +export GTD_SDFormat_VERSION="15.0.0" # Download specific version of SDFormat wget http://osrf-distributions.s3.amazonaws.com/sdformat/releases/sdformat-${GTD_SDFormat_VERSION}.tar.bz2 @@ -72,9 +72,15 @@ $ git clone https://github.com/borglab/GTDynamics.git $ cd GTDynamics $ mkdir build; cd build # We can specify the install path with -DCMAKE_INSTALL_PREFIX +# To compile with Python wrapper support, ensure that GTSAM was built with Python +# and use -DGTDYNAMICS_BUILD_PYTHON=ON +$ # If GTSAM is installed to a non-system prefix, point CMake to it, e.g.: +$ # -DGTSAM_DIR=/path/to/gtsam_install/lib/cmake/GTSAM +$ # -DCMAKE_PREFIX_PATH=/path/to/gtsam_install $ cmake -DCMAKE_INSTALL_PREFIX=../install .. $ make $ sudo make install +# Run make python-install for installation with Python ``` ## Running Tests @@ -85,27 +91,90 @@ $ make check ## Running Examples -The `/examples` directory contains example projects that demonstrate how to include GTDynamics in your application. To run an example, ensure that the `CMAKE_PREFIX_PATH` is set to the GTDynamics install directory. +The `examples` directory contains various full example projects demonstrating the use of GTDynamics for various robotic applications. + +We recommend going through the examples to get a better understanding of how to use GTDynamics for your own use cases. + +*NOTE* The examples are made to run within the GTDynamics source folder and are not standalone projects. + +1. Build GTDynamics and navigate to the `build` folder: + ```sh + $ cd build + ``` + +2. Run the example using: + ```sh + $ make example_XXX.run + ``` + where `XXX` corresponds to the example name. Example names align with folder names in `examples`, but some of them have added suffixes. For example, `make example_forward_dynamics.run` or `make example_spider_walking_forward`. + +3. Run the simulation using: + ```sh + $ make example_XXX.sim + ``` + where `XXX` corresponds to the folder name in `examples`. For example, `make example_quadruped_mp.sim` or `make example_spider_walking.sim`. Make sure you have `pybullet` and `matplotlib` installed in your Python environment. + +## Constrained Optimization (GTSAM-backed) + +GTDynamics now routes some constrained optimization methods through GTSAM's constrained optimizers. + +Typical entry points: + +- **Optimizer API:** set `OptimizationParameters::Method` to `PENALTY` or `AUGMENTED_LAGRANGIAN` and call `Optimizer::optimize(...)`. +- **Benchmark helpers:** use `OptimizePenaltyMethod(...)` / `OptimizeAugmentedLagrangian(...)` in `gtdynamics/optimizer/OptimizationBenchmark.h`. + +### Benchmark: Kuka Arm (Trajectory Optimization) + +We ran the Kuka arm example to compare **Soft Constraints** vs **Penalty** vs **Augmented Lagrangian**. + +Run it from the build directory: -1. Navigate to the example's subdirectory and create a build directory. e.g. ```sh -cd GTDynamics/examples/example_forward_dynamics -mkdir build; cd build +$ make example_constraint_manifold_arm.run ``` +Example output (times in seconds; your machine will differ): + +``` +Soft Constraint 0.334 s +Penalty Method 2.045 s +Augmented Lagrangian 1.663 s +Constraint Manifold (F) 0.107 s +Constraint Manifold (I) 0.061 s +``` + +**Change note (Feb 1, 2026):** GTDynamics now uses GTSAM's nonlinear inequality constraints +(`gtsam::NonlinearInequalityConstraint` / `NonlinearInequalityConstraints`) with the convention +**g(x) <= 0**. The legacy GTDynamics nonlinear inequality classes were removed; linear inequality +support remains in `gtdynamics/constraints/LinearInequalityConstraint.{h,cpp}`. + +## Including GTDynamics With CMake + +The `examples/cmake_project_example` directory contains an example CMake-based project that demonstrates how to include GTDynamics in your application. + +Use this as a template when you want to set up your own project that uses GTDynamics (e.g. separate git repo, ROS, personal libraries, etc). + +To build the project: + +1. Navigate to the example's subdirectory and create a build directory. e.g. + ```sh + cd GTDynamics/examples/cmake_project_example + mkdir build; cd build + ``` + 2. Make the example. -If GTDynamics was installed to `~/JohnDoe/gtdynamics_install`, then run the cmake command with: + If GTDynamics was installed to `~/JohnDoe/GTDynamics/install`, then run the cmake command with: -```sh -cmake -DCMAKE_PREFIX_PATH=~/JohnDoe/gtdynamics_install .. -make -``` + ```sh + cmake -DCMAKE_PREFIX_PATH=~/JohnDoe/GTDynamics/install .. + make + ``` 3. Run the example! -```sh -./exec -``` + ```sh + ./example + ``` ## Python Wrapper (Recommended use case) @@ -127,6 +196,9 @@ To compile and install the GTDynamics python library: 2. In the GTDynamics build directory, run `cmake` with the flag `GTDYNAMICS_BUILD_PYTHON=ON`. It is highly advised for the GTDynamics CMake prefix to match the CMake prefix used for GTSAM. Again, use the `CMAKE_INSTALL_PREFIX=/path/to/install/dir` flag to specify the updated prefix. ```sh + # If GTSAM was installed to a non-system prefix, also provide: + # -DGTSAM_DIR=/path/to/install/dir/lib/cmake/GTSAM + # -DCMAKE_PREFIX_PATH=/path/to/install/dir cmake -DGTDYNAMICS_BUILD_PYTHON=ON -DCMAKE_INSTALL_PREFIX=/path/to/install/dir .. ``` @@ -165,4 +237,3 @@ Please cite the following paper if you use this code as part of any published re Eprint = {arXiv:2011.06194}, } ``` - diff --git a/Config.cmake.in b/cmake/Config.cmake.in similarity index 100% rename from Config.cmake.in rename to cmake/Config.cmake.in diff --git a/cmake/HandleTBB.cmake b/cmake/HandleTBB.cmake new file mode 100644 index 000000000..e0e15f651 --- /dev/null +++ b/cmake/HandleTBB.cmake @@ -0,0 +1,28 @@ +############################################################################### +if (GTDYNAMICS_WITH_TBB) + # Find TBB + find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) + + # Set up variables if we're using TBB + if(TBB_FOUND) + set(GTDYNAMICS_USE_TBB 1) # This will go into config.h + + if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) + set(TBB_GREATER_EQUAL_2020 1) + else() + set(TBB_GREATER_EQUAL_2020 0) + endif() + # all definitions and link requisites will go via imported targets: + # tbb & tbbmalloc + list(APPEND GTDYNAMICS_ADDITIONAL_LIBRARIES TBB::tbb TBB::tbbmalloc) + else() + set(GTDYNAMICS_USE_TBB 0) # This will go into config.h + endif() + + ############################################################################### + # Prohibit Timing build mode in combination with TBB + if(GTDYNAMICS_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) + message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") + endif() + +endif() diff --git a/docker/Dockerfile b/docker/Dockerfile index ff0ff0246..beaaa8406 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -10,17 +10,17 @@ WORKDIR /usr/src/ # Make sure we have latest packages RUN apt-get -y update -# Install basic dependencies for sdfformat +# Install basic dependencies for sdformat RUN apt-get install -y ruby-dev build-essential libboost-all-dev cmake pkg-config wget lsb-release # Get Gazebo packages RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' RUN wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add - RUN apt-get update -RUN apt-get install -y libtinyxml2-dev liburdfdom-dev libignition-cmake2-dev libignition-tools-dev libignition-math6-dev python3-psutil +RUN apt-get install -y libtinyxml2-dev liburdfdom-dev libgz-cmake2-dev libgz-tools-dev libgz-math6-dev python3-psutil # Get sdfformat package -ENV GTD_SDFormat_VERSION="10.5.0" +ENV GTD_SDFormat_VERSION="15.0.0" RUN wget http://osrf-distributions.s3.amazonaws.com/sdformat/releases/sdformat-${GTD_SDFormat_VERSION}.tar.bz2 RUN tar -xvjf sdformat-${GTD_SDFormat_VERSION}.tar.bz2 RUN rm sdformat-${GTD_SDFormat_VERSION}.tar.bz2 diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 68fe47df3..288b2f7d2 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -2,6 +2,7 @@ # Variable to hold all the example directories set(EXAMPLE_SUBDIRS + scripts example_a1_walking example_cart_pole_trajectory_optimization example_forward_dynamics @@ -10,9 +11,32 @@ set(EXAMPLE_SUBDIRS example_inverted_pendulum_trajectory_optimization # example_jumping_robot # Python based example example_quadruped_mp - example_spider_walking) + example_spider_walking + example_constraint_manifold + # example_talos_walking # In progress + ) # Add each example subdirectory for compilation foreach(EXAMPLE ${EXAMPLE_SUBDIRS}) add_subdirectory(${EXAMPLE}) endforeach() + +# --- Process Python Animation Scripts --- +# This automatically finds 'animate.py' scripts and creates .animate targets. +message(STATUS "Searching for python animation scripts...") +foreach(EXAMPLE_NAME ${EXAMPLE_SUBDIRS}) + set(ANIMATION_SCRIPT "${CMAKE_CURRENT_SOURCE_DIR}/${EXAMPLE_NAME}/sim.py") + + if(EXISTS ${ANIMATION_SCRIPT}) + message(STATUS " -- Found animation script for ${EXAMPLE_NAME}, creating '.sim' target") + add_custom_target(${EXAMPLE_NAME}.sim + COMMAND + python3 ${ANIMATION_SCRIPT} + WORKING_DIRECTORY + "${CMAKE_CURRENT_BINARY_DIR}" + COMMENT + "Running animation for ${EXAMPLE_NAME}..." + VERBATIM + ) + endif() +endforeach() \ No newline at end of file diff --git a/examples/README,md b/examples/README,md new file mode 100644 index 000000000..6bd80f174 --- /dev/null +++ b/examples/README,md @@ -0,0 +1,17 @@ +# Examples build status + +## GTD Examples Status + +| Name | Cpp Status | Animation Status | +| :--- | :--- | :--- | +| `quadruped_mp` | 🟢 Working | 🟢 Working | +| `a1_walking_forward` | 🔴 Runtime Error | ⚫️ Not started | +| `a1_walking_forward_combined` | 🔴 Runtime Error | ⚫️ Not started | +| `cart_pole_trajectory_optimization` | 🟢 Working | 🟢 Working | +| `forward_dynamics` | 🟢 Working | ⚫️ Not Implemented | +| `full_kinodynamic_balancing` | 🟢 Working | 🟢 Working | +| `full_kinodynamic_walking_walk_forward` | 🟢 Working | 🟡 Runs With Errors | +| `full_kinodynamic_walking_rotate` | 🟢 Working | 🟡 Runs With Errors | +| `inverted_pendulum_trajectory_optimization` | 🟢 Working | 🔴 Not Working | +| `spider_walking_forward` | 🟢 Working | 🔴 Not Working | +| `spider_walking_rotate` | 🟢 Working | 🔴 Not Working | \ No newline at end of file diff --git a/examples/cmake_project_example/CMakeLists.txt b/examples/cmake_project_example/CMakeLists.txt new file mode 100644 index 000000000..caecffad5 --- /dev/null +++ b/examples/cmake_project_example/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.9...3.29) +project(example C CXX) + +# gtsam +find_package(GTSAM REQUIRED) + +# gtdynamics +find_path(gtdynamics_include gtdynamics) +find_library(gtdynamics_lib gtdynamics) +# these next 3 lines are temporary until we create a proper cmake module for gtdynamics. +set(SDFormat_VERSION 15) +find_package(sdformat${SDFormat_VERSION} REQUIRED) +list(APPEND gtdynamics_lib ${SDFormat_LIBRARIES}) + +# Build Executable +add_executable(${PROJECT_NAME} main.cpp) +target_link_libraries(${PROJECT_NAME} PUBLIC ${gtdynamics_lib} gtsam) +target_include_directories(${PROJECT_NAME} PUBLIC ${gtdynamics_include}) diff --git a/examples/cmake_project_example/main.cpp b/examples/cmake_project_example/main.cpp new file mode 100644 index 000000000..d9a4c7e87 --- /dev/null +++ b/examples/cmake_project_example/main.cpp @@ -0,0 +1,52 @@ +/** + * @file main.cpp + * @brief gtdynamics cmake Project Example + * This code demonstrates a sample project that imports gtdynamics with cmake. + * This cpp file should be the same as `example_forward_dynamics/main.cpp`. + */ + +#include +#include +#include +#include +#include +#include + +using namespace gtdynamics; +int main(int argc, char** argv) { + // Load the robot + auto simple_rr = CreateRobotFromFile( + kSdfPath + std::string("test/simple_rr.sdf"), "simple_rr_sdf"); + simple_rr.print(); + + gtsam::Vector3 gravity(0, 0, -9.8); + + // Build a nonlinear factor graph of kinodynamics constraints. + auto graph_builder = DynamicsGraph(gravity); + auto kdfg = graph_builder.dynamicsFactorGraph(simple_rr, + 0 // timestep + ); + + // Specify the forward dynamics priors and add them to the factor graph. + gtsam::Values known_values; + for (auto&& joint : simple_rr.joints()) { + int j = joint->id(); + InsertJointAngle(&known_values, j, 0, 0.0); + InsertJointVel(&known_values, j, 0, 0.0); + InsertTorque(&known_values, j, 0, 0.0); + } + auto fd_priors = + graph_builder.forwardDynamicsPriors(simple_rr, 0, known_values); + kdfg.add(fd_priors); + + // Initialize solution. + Initializer initializer; + auto init_values = initializer.ZeroValues(simple_rr, 0); + + // Compute the forward dynamics. + gtsam::LevenbergMarquardtOptimizer optimizer(kdfg, init_values); + gtsam::Values results = optimizer.optimize(); + graph_builder.printValues(results); + + return 0; +} diff --git a/examples/example_a1_walking/CMakeLists.txt b/examples/example_a1_walking/CMakeLists.txt index e2af57c2d..7f39e04af 100644 --- a/examples/example_a1_walking/CMakeLists.txt +++ b/examples/example_a1_walking/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.9...3.29) project(example_a1_walking C CXX) # Build Executables @@ -9,8 +9,21 @@ add_executable(${WALK_FORWARD} main.cpp) target_link_libraries(${WALK_FORWARD} PUBLIC gtdynamics) target_include_directories(${WALK_FORWARD} PUBLIC ${CMAKE_PREFIX_PATH}/include) +# Walk forward one meter. +set(WALK_FORWARD_COMBINED ${PROJECT_NAME}_forward_combined) +add_executable(${WALK_FORWARD_COMBINED} main_combined.cpp) +target_link_libraries(${WALK_FORWARD_COMBINED} PUBLIC gtdynamics) +target_include_directories(${WALK_FORWARD_COMBINED} PUBLIC ${CMAKE_PREFIX_PATH}/include) + add_custom_target( ${WALK_FORWARD}.run COMMAND ./${WALK_FORWARD} DEPENDS ${WALK_FORWARD} WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/examples/${PROJECT_NAME}) + + add_custom_target( + ${WALK_FORWARD_COMBINED}.run + COMMAND ./${WALK_FORWARD_COMBINED} + DEPENDS ${WALK_FORWARD_COMBINED} + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/examples/${PROJECT_NAME}) + \ No newline at end of file diff --git a/examples/example_a1_walking/main.cpp b/examples/example_a1_walking/main.cpp index a02bd4cc6..91c1ffb73 100644 --- a/examples/example_a1_walking/main.cpp +++ b/examples/example_a1_walking/main.cpp @@ -11,16 +11,16 @@ * @author: Dan Barladeanu */ +#include #include #include +#include #include +#include #include #include -#include #include -#include -#include #include #include #include @@ -39,37 +39,191 @@ using namespace gtdynamics; // Returns a Trajectory object for a single a1 walk cycle. Trajectory getTrajectory(const Robot& robot, size_t repeat) { - vector rlfr = {robot.link("RL_lower"),robot.link("FR_lower")}; - vector rrfl = {robot.link("RR_lower"),robot.link("FL_lower")}; + vector rlfr = {robot.link("RL_lower"), robot.link("FR_lower")}; + vector rrfl = {robot.link("RR_lower"), robot.link("FL_lower")}; auto all_feet = rlfr; all_feet.insert(all_feet.end(), rrfl.begin(), rrfl.end()); - // Create three different FootContactConstraintSpecs, one for all the feet on the - // ground, one with RL and FR, one with RR and FL + // Create three different FootContactConstraintSpecs, one for all the feet on + // the ground, one with RL and FR, one with RR and FL const Point3 contact_in_com(0, 0, -0.07); - auto stationary = boost::make_shared(all_feet, contact_in_com); - auto RLFR = boost::make_shared(rlfr, contact_in_com); - auto RRFL = boost::make_shared(rrfl, contact_in_com); + auto stationary = + std::make_shared(all_feet, contact_in_com); + auto RLFR = std::make_shared(rlfr, contact_in_com); + auto RRFL = std::make_shared(rrfl, contact_in_com); - //FootContactVector states = {noRL, noRR, noFR, noFL}; - FootContactVector states = {stationary, RRFL, stationary, RLFR}; - std::vector phase_lengths = {1,5,1,5}; + // FootContactVector states = {noRL, noRR, noFR, noFL}; + FootContactVector states = {RRFL, stationary, RLFR, stationary}; + std::vector phase_lengths = {25, 5, 25, 5}; WalkCycle walk_cycle(states, phase_lengths); return Trajectory(walk_cycle, repeat); } -int main(int argc, char** argv) { +int massGraph() { + gttic_(start1); // need to build gtsam with timing for this + + // Load Unitree A1 robot urdf. + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + double sigma_dynamics = 1e-3; // std of dynamics constraints. + double sigma_objectives = 1e-3; // std of additional objectives. + + // Noise models. + auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics), + dynamics_model_1 = Isotropic::Sigma(1, sigma_dynamics), + objectives_model_6 = Isotropic::Sigma(6, sigma_objectives), + objectives_model_1 = Isotropic::Sigma(1, sigma_objectives); + + // Env parameters. + gtsam::Vector3 gravity(0, 0, -9.8); + double mu = 1.0; + + // OptimizerSetting opt(sigma_dynamics); + OptimizerSetting opt(1e-3, 1e-3, 1e-3, 1e-2); + DynamicsGraph graph_builder(opt, gravity); + + // Create the trajectory, 1 walk cycle. + auto trajectory = getTrajectory(robot, 1); + + // Create multi-phase trajectory factor graph + auto collocation = CollocationScheme::Euler; + auto graph = + trajectory.multiPhaseFactorGraph(robot, graph_builder, collocation, mu); + + // Build the objective factors. + double ground_height = 1.0; + const Point3 step(0.25, 0, 0); + gtsam::NonlinearFactorGraph objectives = trajectory.contactPointObjectives( + robot, Isotropic::Sigma(3, 1e-6), step, ground_height); + + // Get final time step. + int K = trajectory.getEndTimeStep(trajectory.numPhases() - 1); + + // Add body goal objectives to the factor graph. + auto base_link = robot.link("trunk"); + for (int k = 0; k <= K; k++) { + objectives.add( + LinkObjectives(base_link->id(), k) + .pose(Pose3(Rot3(), Point3(0, 0, 0.4)), Isotropic::Sigma(6, 1e-2)) + .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-2))); + } + + // Add link and joint boundary conditions to FG. + trajectory.addBoundaryConditions(&objectives, robot, dynamics_model_6, + dynamics_model_6, objectives_model_6, + objectives_model_1, objectives_model_1); + + // Constrain all Phase keys to have duration of 1 /240. + const double desired_dt = 1. / 20; + trajectory.addIntegrationTimeFactors(&objectives, desired_dt, 1e-30); + + // Add min torque objectives. + // trajectory.addMinimumTorqueFactors(&objectives, robot, Unit::Create(1)); + + // Add prior on A1 lower joint angles + auto prior_model_angles = Isotropic::Sigma(1, 1e-2); + auto prior_model_hip = Isotropic::Sigma(1, 1e-2); + for (auto&& joint : robot.joints()) + if (joint->name().find("lower") < 100) + for (int k = 0; k <= K; k++) { + // std::cout << joint->name() << joint->name().find("lower") << + // std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(-1.4, prior_model_angles)); + } + + // Add prior on A1 lower joint angles + /*for (auto&& joint : robot.joints()) + if (joint->name().find("upper") < 100) + for (int k = 0; k <= K; k++) { + //std::cout << joint->name() << joint->name().find("lower") << + std::endl; objectives.add(JointObjectives(joint->id(), k).torque(0.0, + prior_model_torques)); + } + + // Add prior on A1 lower joint angles + for (auto&& joint : robot.joints()) + if (joint->name().find("hip") < 100) + for (int k = 0; k <= K; k++) { + //std::cout << joint->name() << joint->name().find("lower") << + std::endl; objectives.add(JointObjectives(joint->id(), k).torque(0.0, + prior_model_torques)); + } */ - gttic_(start); //need to build gtsam with timing for this + // Add prior on A1 hip joint angles + // prior_model = Isotropic::Sigma(1, 1e-3); + for (auto&& joint : robot.joints()) + if (joint->name().find("hip") < 100) + for (int k = 0; k <= K; k++) { + // std::cout << joint->name() << joint->name().find("hip") << std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(0.0, prior_model_hip)); + } + + // Add prior on A1 hip joint angles + // prior_model = Isotropic::Sigma(1, 1e-3); + for (auto&& joint : robot.joints()) + if (joint->name().find("upper") < 100) + for (int k = 0; k <= K; k++) { + // std::cout << joint->name() << joint->name().find("hip") << std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(0.7, prior_model_angles)); + } + // Add objectives to factor graph. + graph.add(objectives); + + // Initialize solution. + double gaussian_noise = 1e-30; + Initializer initializer; + gtsam::Values init_vals = trajectory.multiPhaseInitialValues( + robot, initializer, gaussian_noise, desired_dt); + + gttic_(optimization1); + + std::cout << "graph size = " << graph.size() << std::endl; + std::cout << "graph keys size = " << graph.keys().size() << std::endl; + std::cout << "init vals size = " << init_vals.size() << std::endl; + + // Optimize! + gtsam::LevenbergMarquardtParams params; + // params.setVerbosityLM("SUMMARY"); + params.setlambdaInitial(1e10); + params.setlambdaLowerBound(1e-7); + params.setlambdaUpperBound(1e10); + params.setAbsoluteErrorTol(1.0); + gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params); + auto results = optimizer.optimize(); + + gttoc_(optimization1); + gttoc_(start1); + + // gtsam::tictoc_print_(); //need to build gtsam with timing for this + // Write results to traj file + trajectory.writeToFile(robot, "a1_traj_DG_mass.csv", results); + + return 0; +} + +int oldGraph() { + gttic_(start2); // need to build gtsam with timing for this // Load Unitree A1 robot urdf. auto robot = - CreateRobotFromFile(std::string("/home/dan/.local/lib/python3.8/site-packages/pybullet_data/a1/a1.urdf"), "a1"); + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + // set masses and inertias + for (auto&& link : robot.links()) { + if (link->name().find("trunk") == std::string::npos) { + link->setMass(0.0); + link->setInertia(gtsam::Matrix3::Zero()); + } + } - double sigma_dynamics = 1e-6; // std of dynamics constraints. - double sigma_objectives = 1e-7; // std of additional objectives. + double sigma_dynamics = 1e-3; // std of dynamics constraints. + double sigma_objectives = 1e-3; // std of additional objectives. // Noise models. auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics), @@ -81,7 +235,8 @@ int main(int argc, char** argv) { gtsam::Vector3 gravity(0, 0, -9.8); double mu = 1.0; - OptimizerSetting opt(sigma_dynamics); + // OptimizerSetting opt(sigma_dynamics); + OptimizerSetting opt(1e-3, 1e-3, 1e-3, 1e-2); DynamicsGraph graph_builder(opt, gravity); // Create the trajectory, 1 walk cycle. @@ -94,9 +249,9 @@ int main(int argc, char** argv) { // Build the objective factors. double ground_height = 1.0; - const Point3 step(0.5, 0, 0); - gtsam::NonlinearFactorGraph objectives = - trajectory.contactPointObjectives(robot, Isotropic::Sigma(3, 1e-7), step, ground_height); + const Point3 step(0.25, 0, 0); + gtsam::NonlinearFactorGraph objectives = trajectory.contactPointObjectives( + robot, Isotropic::Sigma(3, 1e-6), step, ground_height); // Get final time step. int K = trajectory.getEndTimeStep(trajectory.numPhases() - 1); @@ -106,8 +261,8 @@ int main(int argc, char** argv) { for (int k = 0; k <= K; k++) { objectives.add( LinkObjectives(base_link->id(), k) - .pose(Pose3(Rot3(), Point3(0, 0, 0.4)), Isotropic::Sigma(6, 1e-6)) - .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-5))); + .pose(Pose3(Rot3(), Point3(0, 0, 0.4)), Isotropic::Sigma(6, 1e-2)) + .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-2))); } // Add link and joint boundary conditions to FG. @@ -116,44 +271,248 @@ int main(int argc, char** argv) { objectives_model_1, objectives_model_1); // Constrain all Phase keys to have duration of 1 /240. - const double desired_dt = 1. / 240; + const double desired_dt = 1. / 20; trajectory.addIntegrationTimeFactors(&objectives, desired_dt, 1e-30); // Add min torque objectives. - trajectory.addMinimumTorqueFactors(&objectives, robot, Unit::Create(1)); + // trajectory.addMinimumTorqueFactors(&objectives, robot, Unit::Create(1)); // Add prior on A1 lower joint angles - auto prior_model = Isotropic::Sigma(1, 1e-6); + auto prior_model_angles = Isotropic::Sigma(1, 1e-2); + auto prior_model_hip = Isotropic::Sigma(1, 1e-2); for (auto&& joint : robot.joints()) if (joint->name().find("lower") < 100) for (int k = 0; k <= K; k++) { - //std::cout << joint->name() << joint->name().find("lower") << std::endl; - objectives.add(JointObjectives(joint->id(), k).angle(-1.4, prior_model)); + // std::cout << joint->name() << joint->name().find("lower") << + // std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(-1.4, prior_model_angles)); + } + + // Add prior on A1 lower joint angles + /*for (auto&& joint : robot.joints()) + if (joint->name().find("upper") < 100) + for (int k = 0; k <= K; k++) { + //std::cout << joint->name() << joint->name().find("lower") << + std::endl; objectives.add(JointObjectives(joint->id(), k).torque(0.0, + prior_model_torques)); + } + + // Add prior on A1 lower joint angles + for (auto&& joint : robot.joints()) + if (joint->name().find("hip") < 100) + for (int k = 0; k <= K; k++) { + //std::cout << joint->name() << joint->name().find("lower") << + std::endl; objectives.add(JointObjectives(joint->id(), k).torque(0.0, + prior_model_torques)); + } */ + + // Add prior on A1 hip joint angles + // prior_model = Isotropic::Sigma(1, 1e-3); + for (auto&& joint : robot.joints()) + if (joint->name().find("hip") < 100) + for (int k = 0; k <= K; k++) { + // std::cout << joint->name() << joint->name().find("hip") << std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(0.0, prior_model_hip)); + } + + // Add prior on A1 hip joint angles + // prior_model = Isotropic::Sigma(1, 1e-3); + for (auto&& joint : robot.joints()) + if (joint->name().find("upper") < 100) + for (int k = 0; k <= K; k++) { + // std::cout << joint->name() << joint->name().find("hip") << std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(0.7, prior_model_angles)); } // Add objectives to factor graph. graph.add(objectives); + // Initialize solution. + double gaussian_noise = 1e-30; + Initializer initializer; + gtsam::Values init_vals = trajectory.multiPhaseInitialValues( + robot, initializer, gaussian_noise, desired_dt); + + gttic_(optimization2); + + std::cout << "graph size = " << graph.size() << std::endl; + std::cout << "graph keys size = " << graph.keys().size() << std::endl; + std::cout << "init vals size = " << init_vals.size() << std::endl; + + // Optimize! + gtsam::LevenbergMarquardtParams params; + // params.setVerbosityLM("SUMMARY"); + params.setlambdaInitial(1e10); + params.setlambdaLowerBound(1e-7); + params.setlambdaUpperBound(1e10); + params.setAbsoluteErrorTol(1.0); + gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params); + auto results = optimizer.optimize(); + + gttoc_(optimization2); + gttoc_(start2); + + // gtsam::tictoc_print_(); //need to build gtsam with timing for this + // Write results to traj file + trajectory.writeToFile(robot, "a1_traj_DG_massless.csv", results); + + return 0; +} + +int newGraph() { + gttic_(start3); // need to build gtsam with timing for this + + // Load Unitree A1 robot urdf. + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + // set masses and inertias + for (auto&& link : robot.links()) { + if (link->name().find("trunk") == std::string::npos) { + link->setMass(0.0); + link->setInertia(gtsam::Matrix3::Zero()); + } + } + + double sigma_dynamics = 1e-3; // std of dynamics constraints. + double sigma_objectives = 1e-3; // std of additional objectives. + + // Noise models. + auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics), + dynamics_model_1 = Isotropic::Sigma(1, sigma_dynamics), + objectives_model_6 = Isotropic::Sigma(6, sigma_objectives), + objectives_model_1 = Isotropic::Sigma(1, sigma_objectives); + + // Env parameters. + gtsam::Vector3 gravity(0, 0, -9.8); + double mu = 1.0; + + // OptimizerSetting opt(sigma_dynamics); + OptimizerSetting opt; + ChainDynamicsGraph graph_builder(robot, opt, gravity); + + // Create the trajectory, 1 walk cycle. + auto trajectory = getTrajectory(robot, 1); + + // Create multi-phase trajectory factor graph + auto collocation = CollocationScheme::Euler; + auto graph = + trajectory.multiPhaseFactorGraph(robot, graph_builder, collocation, mu); + + // Build the objective factors. + double ground_height = 1.0; + const Point3 step(0.25, 0, 0); + gtsam::NonlinearFactorGraph objectives = trajectory.contactPointObjectives( + robot, Isotropic::Sigma(3, 1e-6), step, ground_height); + + // Get final time step. + int K = trajectory.getEndTimeStep(trajectory.numPhases() - 1); + + // boundary conditions, using this instead addBoundaryConditions because we + // dont have link twist and accel variables + for (auto&& link : robot.links()) { + // Initial link poses should be bMcom + const int i = link->id(); + if (i == 0) + objectives.add(LinkObjectives(i, 0) + .pose(link->bMcom(), Isotropic::Sigma(6, 1e-3)) + .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 1e-3))); + if (i == 3 || i == 6 || i == 9 || i == 12) + objectives.add( + LinkObjectives(i, 0).pose(link->bMcom(), Isotropic::Sigma(6, 1e-3))); + } + // initial and end joint velocity and accelerations should be zero + objectives.add( + JointsAtRestObjectives(robot, objectives_model_1, objectives_model_1, 0)); + objectives.add( + JointsAtRestObjectives(robot, objectives_model_1, objectives_model_1, K)); + + // Add body goal objectives to the factor graph. + auto base_link = robot.link("trunk"); + for (int k = 0; k <= K; k++) { + objectives.add( + LinkObjectives(base_link->id(), k) + .pose(Pose3(Rot3(), Point3(0, 0, 0.4)), Isotropic::Sigma(6, 1e-2)) + .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-2))); + } + + // Constrain all Phase keys to have duration of 1 /240. + const double desired_dt = 1. / 20; + trajectory.addIntegrationTimeFactors(&objectives, desired_dt, 1e-30); + + // Add min torque objectives. + // trajectory.addMinimumTorqueFactors(&objectives, robot, Unit::Create(1)); + + // Add prior on A1 lower joint angles + auto prior_model_angles = Isotropic::Sigma(1, 1e-2); + auto prior_model_hip = Isotropic::Sigma(1, 1e-2); + // auto prior_model_torques = Isotropic::Sigma(1, 1e-1); + for (auto&& joint : robot.joints()) + if (joint->name().find("lower") < 100) + for (int k = 0; k <= K; k++) { + // std::cout << joint->name() << joint->name().find("lower") << + // std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(-1.4, prior_model_angles)); + } + + // Add prior on A1 lower joint angles + // auto prior_model = Isotropic::Sigma(1, 1e-3); + /* for (auto&& joint : robot.joints()) + if (joint->name().find("upper") < 100) + for (int k = 0; k <= K; k++) { + //std::cout << joint->name() << joint->name().find("lower") << + std::endl; objectives.add(JointObjectives(joint->id(), k).torque(0.0, + prior_model_torques)); + } + + // Add prior on A1 lower joint angles + for (auto&& joint : robot.joints()) + if (joint->name().find("hip") < 100) + for (int k = 0; k <= K; k++) { + //std::cout << joint->name() << joint->name().find("lower") << + std::endl; objectives.add(JointObjectives(joint->id(), k).torque(0.0, + prior_model_torques)); + } */ + // Add prior on A1 hip joint angles - prior_model = Isotropic::Sigma(1, 1e-7); + // prior_model = Isotropic::Sigma(1, 1e-3); for (auto&& joint : robot.joints()) if (joint->name().find("hip") < 100) for (int k = 0; k <= K; k++) { - //std::cout << joint->name() << joint->name().find("hip") << std::endl; - objectives.add(JointObjectives(joint->id(), k).angle(0.0, prior_model)); + // std::cout << joint->name() << joint->name().find("hip") << std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(0.0, prior_model_hip)); + } + + // Add prior on A1 hip joint angles + for (auto&& joint : robot.joints()) + if (joint->name().find("upper") < 100) + for (int k = 0; k <= K; k++) { + // std::cout << joint->name() << joint->name().find("hip") << std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(0.7, prior_model_angles)); } // Add objectives to factor graph. - graph.add(objectives); - + graph.add(objectives); + // Initialize solution. - double gaussian_noise = 1e-3; - gtsam::Values init_vals = - trajectory.multiPhaseInitialValues(robot, gaussian_noise, desired_dt); + double gaussian_noise = 1e-30; + ChainInitializer initializer; + gtsam::Values init_vals = trajectory.multiPhaseInitialValues( + robot, initializer, gaussian_noise, desired_dt); + + gttic_(optimization3); - gttic_(optimization); + std::cout << "graph size = " << graph.size() << std::endl; + std::cout << "graph keys size = " << graph.keys().size() << std::endl; + std::cout << "init vals size = " << init_vals.size() << std::endl; // Optimize! gtsam::LevenbergMarquardtParams params; - //params.setVerbosityLM("SUMMARY"); + // params.setVerbosityLM("SUMMARY"); params.setlambdaInitial(1e10); params.setlambdaLowerBound(1e-7); params.setlambdaUpperBound(1e10); @@ -161,12 +520,19 @@ int main(int argc, char** argv) { gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params); auto results = optimizer.optimize(); - gttoc_(optimization); - gttoc_(start); + gttoc_(optimization3); + gttoc_(start3); - gtsam::tictoc_print_(); //need to build gtsam with timing for this + gtsam::tictoc_print_(); // need to build gtsam with timing for this // Write results to traj file - trajectory.writeToFile(robot, "a1_traj.csv", results); + trajectory.writeToFile(robot, "a1_traj_CDG_massless.csv", results); return 0; } + +int main(int argc, char** argv) { + int a = massGraph(); + int b = oldGraph(); + int c = newGraph(); + return 0; +} \ No newline at end of file diff --git a/examples/example_a1_walking/main_combined.cpp b/examples/example_a1_walking/main_combined.cpp new file mode 100644 index 000000000..debdddac9 --- /dev/null +++ b/examples/example_a1_walking/main_combined.cpp @@ -0,0 +1,422 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file main_combined.cpp + * @brief Unitree A1 trajectory optimization with pre-specified footholds. + * @author: Dan Barladeanu + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using std::string; +using std::vector; + +using gtsam::Point3; +using gtsam::Pose3; +using gtsam::Rot3; +using gtsam::Vector6; +using gtsam::noiseModel::Isotropic; +using gtsam::noiseModel::Unit; + +using namespace gtdynamics; + +// Returns a Trajectory object for a single a1 walk cycle. +Trajectory getTrajectory(const Robot& robot, size_t repeat) { + vector rlfr = {robot.link("RL_lower"), robot.link("FR_lower")}; + vector rrfl = {robot.link("RR_lower"), robot.link("FL_lower")}; + auto all_feet = rlfr; + all_feet.insert(all_feet.end(), rrfl.begin(), rrfl.end()); + + // Create three different FootContactConstraintSpecs, one for all the feet on + // the ground, one with RL and FR, one with RR and FL + const Point3 contact_in_com(0, 0, -0.07); + auto stationary = + std::make_shared(all_feet, contact_in_com); + auto RLFR = std::make_shared(rlfr, contact_in_com); + auto RRFL = std::make_shared(rrfl, contact_in_com); + + // FootContactVector states = {noRL, noRR, noFR, noFL}; + FootContactVector states = {stationary, RLFR, stationary, RRFL}; + std::vector phase_lengths = {2, 10, 2, 10}; + + WalkCycle walk_cycle(states, phase_lengths); + + return Trajectory(walk_cycle, repeat); +} + +int CombinedRun(bool add_mass_to_body) { + // gttic_(start1); //need to build gtsam with timing for this + + // Load Unitree A1 robot urdf. + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + // Load Unitree A1 robot urdf. + auto robot_massless = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + // set masses and inertias + double total_mass = 0.0; + for (auto&& link : robot_massless.links()) { + if (link->name().find("trunk") == std::string::npos) { + link->setMass(0.0); + link->setInertia(gtsam::Matrix3::Zero()); + } + } + + if (add_mass_to_body) { + robot_massless.link("trunk")->setMass(12.458); + std::cout << "robot body mass set to " + << robot_massless.link("trunk")->mass() << std::endl; + } + + double sigma_dynamics = 5e-5; // std of dynamics constraints. + double sigma_objectives = 1e-3; // std of additional objectives. + + // Noise models. + auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics), + dynamics_model_1 = Isotropic::Sigma(1, sigma_dynamics), + objectives_model_6 = Isotropic::Sigma(6, sigma_objectives), + objectives_model_1 = Isotropic::Sigma(1, sigma_objectives); + + // Env parameters. + gtsam::Vector3 gravity(0, 0, -9.8); + double mu = 1.0; + + OptimizerSetting opt(sigma_dynamics, sigma_dynamics, sigma_dynamics, + sigma_dynamics); + opt.p_cost_model = gtsam::noiseModel::Constrained::All(6); + // opt.q_col_cost_model = Isotropic::Sigma(1, 1e-2); + // opt.v_col_cost_model = Isotropic::Sigma(1, 1e-2); + // opt.cm_cost_model = gtsam::noiseModel::Constrained::All(3); + // opt.f_cost_model = gtsam::noiseModel::Constrained::All(6); + // opt.fa_cost_model = gtsam::noiseModel::Constrained::All(6); + // opt.t_cost_model = gtsam::noiseModel::Constrained::All(1); + // OptimizerSetting opt(1e-3, 1e-3, 1e-3, 1e-2); + DynamicsGraph graph_mass_builder(opt, gravity); + DynamicsGraph graph_massless_builder(opt, gravity); + ChainDynamicsGraph chain_graph_massless_builder(robot_massless, opt, gravity); + + // Create the trajectory, 1 walk cycle. + auto trajectory = getTrajectory(robot, 3); + + // Create multi-phase trajectory factor graph + auto collocation = CollocationScheme::Euler; + std::cout << "graph_DG_mass" << std::endl; + auto graph_DG_mass = trajectory.multiPhaseFactorGraph( + robot, graph_mass_builder, collocation, mu); + std::cout << "graph_DG_massless" << std::endl; + auto graph_DG_massless = trajectory.multiPhaseFactorGraph( + robot_massless, graph_massless_builder, collocation, mu); + std::cout << "graph_CDG_massless" << std::endl; + auto graph_CDG_massless = trajectory.multiPhaseFactorGraph( + robot_massless, chain_graph_massless_builder, collocation, mu); + + // Build the objective factors. + double ground_height = -0.4; + const Point3 step(0.18, 0.0, 0); + gtsam::NonlinearFactorGraph objectives = trajectory.contactPointObjectives( + robot, Isotropic::Sigma(3, 8e-6), step, ground_height); + + // Get final time step. + int K = trajectory.getEndTimeStep(trajectory.numPhases() - 1); + + // Add body goal objectives to the factor graph. + auto base_link = robot.link("trunk"); + std::cout << "base link mass = " << base_link->mass() << std::endl; + for (int k = 0; k <= K; k++) { + objectives.add(LinkObjectives(base_link->id(), k) + .pose(base_link->bMcom(), Isotropic::Sigma(6, 5e-5)) + .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-5)) + .twistAccel(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-5))); + } + + // Add prior on A1 lower joint angles + auto prior_model_lower = Isotropic::Sigma(1, 5e-5); + auto prior_model_upper = Isotropic::Sigma(1, 5e-5); + auto prior_model_hip = Isotropic::Sigma(1, 5e-5); + auto prior_model_va = Isotropic::Sigma(1, 1e-1); + for (auto&& joint : robot.joints()) + if (joint->name().find("lower") < 100) + for (int k = 0; k <= K; k++) { + // std::cout << joint->name() << joint->name().find("lower") << + // std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(-1.4, prior_model_lower)); + //.velocity(0.0, prior_model_va).acceleration(0.0, prior_model_va)); + } + + // Add prior on A1 hip joint angles + // prior_model = Isotropic::Sigma(1, 1e-3); + for (auto&& joint : robot.joints()) + if (joint->name().find("hip") < 100) + for (int k = 0; k <= K; k++) { + // std::cout << joint->name() << joint->name().find("hip") << std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(0.0, prior_model_hip)); + //.velocity(0.0, prior_model_va).acceleration(0.0, prior_model_va)); + } + + // Add prior on A1 hip joint angles + // prior_model = Isotropic::Sigma(1, 1e-3); + for (auto&& joint : robot.joints()) + if (joint->name().find("upper") < 100) + for (int k = 0; k <= K; k++) { + // std::cout << joint->name() << joint->name().find("hip") << std::endl; + objectives.add( + JointObjectives(joint->id(), k).angle(0.9, prior_model_upper)); + //.velocity(0.0, prior_model_va).acceleration(0.0, prior_model_va)); + } + + // Constrain all Phase keys to have duration of 1 /20. + const double desired_dt = 1. / 24; + trajectory.addIntegrationTimeFactors(&objectives, desired_dt, 1e-30); + + // Add objectives to factor graph. + graph_DG_mass.add(objectives); + graph_DG_massless.add(objectives); + graph_CDG_massless.add(objectives); + + gtsam::NonlinearFactorGraph boundary_objectives_DG; + gtsam::NonlinearFactorGraph boundary_objectives_CDG; + + // Add link and joint boundary conditions to FG. + trajectory.addBoundaryConditions( + &boundary_objectives_DG, robot, dynamics_model_6, dynamics_model_6, + objectives_model_6, objectives_model_1, objectives_model_1); + + // boundary conditions, using this instead addBoundaryConditions because we + // dont have link twist and accel variables + for (auto&& link : robot_massless.links()) { + // Initial link poses should be bMcom + const int i = link->id(); + if (i == 0) + boundary_objectives_CDG.add(LinkObjectives(i, 0) + .pose(link->bMcom(), dynamics_model_6) + .twist(gtsam::Z_6x1, dynamics_model_6)); + if (i == 3 || i == 6 || i == 9 || i == 12) + boundary_objectives_CDG.add( + LinkObjectives(i, 0).pose(link->bMcom(), dynamics_model_6)); + } + // initial and end joint velocity and accelerations should be zero + boundary_objectives_CDG.add( + JointsAtRestObjectives(robot, objectives_model_1, objectives_model_1, 0)); + boundary_objectives_CDG.add( + JointsAtRestObjectives(robot, objectives_model_1, objectives_model_1, K)); + + // Add objectives to factor graph. + graph_DG_mass.add(boundary_objectives_DG); + graph_DG_massless.add(boundary_objectives_DG); + graph_CDG_massless.add(boundary_objectives_CDG); + + // Initialize solution. + double gaussian_noise = 1e-20; + Initializer initializer; + ChainInitializer chain_initializer; + gtsam::Values init_vals_DG = trajectory.multiPhaseInitialValues( + robot, initializer, gaussian_noise, desired_dt); + gtsam::Values init_vals_CDG = trajectory.multiPhaseInitialValues( + robot_massless, chain_initializer, gaussian_noise, desired_dt); + + // Optimize! + gtsam::LevenbergMarquardtParams params; + // params.setVerbosityLM("SUMMARY"); + params.setlambdaInitial(1e10); + params.setlambdaLowerBound(1e-7); + params.setlambdaUpperBound(1e10); + params.setAbsoluteErrorTol(1.0); + + gttic_(optimization1); + + std::cout << "graph_DG_mass size = " << graph_DG_mass.size() << std::endl; + std::cout << "graph_DG_mass keys size = " << graph_DG_mass.keys().size() + << std::endl; + std::cout << "graph_DG_mass init vals size = " << init_vals_DG.size() + << std::endl; + + // Optimize! + gtsam::LevenbergMarquardtOptimizer optimizer_DG_mass(graph_DG_mass, + init_vals_DG, params); + auto results_DG_mass = optimizer_DG_mass.optimize(); + + gttoc_(optimization1); + // gttoc_(start1); + + // gtsam::tictoc_print_(); //need to build gtsam with timing for this + // Write results to traj file + trajectory.writeToFile(robot, "a1_traj_DG_mass.csv", results_DG_mass); + + gttic_(optimization2); + + std::cout << "graph_DG_massless size = " << graph_DG_massless.size() + << std::endl; + std::cout << "graph_DG_massless keys size = " + << graph_DG_massless.keys().size() << std::endl; + std::cout << "graph_DG_massless init vals size = " << init_vals_DG.size() + << std::endl; + + // Optimize! + gtsam::LevenbergMarquardtOptimizer optimizer_DG_massless( + graph_DG_massless, init_vals_DG, params); + auto results_DG_massless = optimizer_DG_massless.optimize(); + + gttoc_(optimization2); + + // gtsam::tictoc_print_(); //need to build gtsam with timing for this + // Write results to traj file + trajectory.writeToFile(robot_massless, "a1_traj_DG_massless.csv", + results_DG_massless); + + gttic_(optimization3); + + std::cout << "graph_CDG_massless size = " << graph_CDG_massless.size() + << std::endl; + std::cout << "graph_CDG_massless keys size = " + << graph_CDG_massless.keys().size() << std::endl; + std::cout << "graph_CDG_massless init vals size = " << init_vals_CDG.size() + << std::endl; + + // Optimize! + gtsam::LevenbergMarquardtOptimizer optimizer_CDG_massless( + graph_CDG_massless, init_vals_CDG, params); + auto results_CDG_massless = optimizer_CDG_massless.optimize(); + + gttoc_(optimization3); + + // gtsam::tictoc_print_(); //need to build gtsam with timing for this + // Write results to traj file + trajectory.writeToFile(robot_massless, "a1_traj_CDG_massless.csv", + results_CDG_massless); + + gtsam::tictoc_print_(); + + std::ofstream FileLocations("locations.txt"); + std::ofstream FileTorques; + std::ofstream FileWrenchesBody; + std::ofstream FileWrenchesGround; + if (add_mass_to_body) { + FileWrenchesBody.open("body_wrenches_body_full_mass.txt"); + FileWrenchesGround.open("ground_wrenches_body_full_mass.txt"); + FileTorques.open("torques_body_full_mass.txt"); + } else { + FileWrenchesBody.open("body_wrenches_body_only_mass.txt"); + FileWrenchesGround.open("ground_wrenches_body_only_mass.txt"); + FileTorques.open("torques_body_only_mass.txt"); + } + + const int link_to_test = 0; + const int joint_to_test = 0; + for (int i = 0; i < K; ++i) { + // std::cout << results_DG_massless.at(WrenchKey(0,0,i)) << + // std::endl; std::cout << + // results_CDG_massless.at(WrenchKey(0,0,i)) << std::endl; + gtsam::Pose3 pose1 = + results_DG_mass.at(PoseKey(link_to_test, i)); + gtsam::Pose3 pose2 = + results_DG_massless.at(PoseKey(link_to_test, i)); + gtsam::Pose3 pose3 = + results_CDG_massless.at(PoseKey(link_to_test, i)); + FileLocations << pose1.translation().x() << "," << pose1.translation().y() + << "," << pose1.translation().z() << "," + << pose2.translation().x() << "," << pose2.translation().y() + << "," << pose2.translation().z() << "," + << pose3.translation().x() << "," << pose3.translation().y() + << "," << pose3.translation().z() << "\n"; + gtsam::Vector6 wrench1 = + results_DG_mass.at(WrenchKey(0, joint_to_test, i)); + gtsam::Vector6 wrench2 = + results_DG_massless.at(WrenchKey(0, joint_to_test, i)); + gtsam::Vector6 wrench3 = + results_CDG_massless.at(WrenchKey(0, joint_to_test, i)); + FileWrenchesBody << wrench1[0] << "," << wrench1[1] << "," << wrench1[2] + << "," << wrench1[3] << "," << wrench1[4] << "," + << wrench1[5] << "," << wrench2[0] << "," << wrench2[1] + << "," << wrench2[2] << "," << wrench2[3] << "," + << wrench2[4] << "," << wrench2[5] << "," << wrench3[0] + << "," << wrench3[1] << "," << wrench3[2] << "," + << wrench3[3] << "," << wrench3[4] << "," << wrench3[5] + << "\n"; + + double angle0 = results_CDG_massless.at(JointAngleKey(0, i)); + double angle1 = results_CDG_massless.at(JointAngleKey(1, i)); + double angle2 = results_CDG_massless.at(JointAngleKey(2, i)); + Pose3 T0 = robot.joint("FL_hip_joint")->parentTchild(angle0); + Pose3 T1 = robot.joint("FL_upper_joint")->parentTchild(angle1); + Pose3 T2 = robot.joint("FL_lower_joint")->parentTchild(angle2); + Pose3 T3 = Pose3(Rot3(), Point3(0.0, 0.0, -0.07)); + Pose3 T_body_contact = T0 * T1 * T2 * T3; + + double torque0_dg_mass = results_DG_mass.at(TorqueKey(0, i)); + double torque1_dg_mass = results_DG_mass.at(TorqueKey(1, i)); + double torque2_dg_mass = results_DG_mass.at(TorqueKey(2, i)); + + double torque0_dg_massless = results_DG_mass.at(TorqueKey(0, i)); + double torque1_dg_massless = results_DG_mass.at(TorqueKey(1, i)); + double torque2_dg_massless = results_DG_mass.at(TorqueKey(2, i)); + + gtsam::Vector6 wrench6 = + results_CDG_massless.at(WrenchKey(0, 0, i)); + + double torque0_cdg_massless = (-1) * wrench6.transpose() * T0.AdjointMap() * + robot.joint("FL_hip_joint")->cScrewAxis(); + double torque1_cdg_massless = (-1) * wrench6.transpose() * + (T0 * T1).AdjointMap() * + robot.joint("FL_upper_joint")->cScrewAxis(); + double torque2_cdg_massless = (-1) * wrench6.transpose() * + (T0 * T1 * T2).AdjointMap() * + robot.joint("FL_lower_joint")->cScrewAxis(); + + FileTorques << torque0_dg_mass << ',' << torque1_dg_mass << ',' + << torque2_dg_mass << ',' << torque0_dg_massless << ',' + << torque1_dg_massless << ',' << torque2_dg_massless << ',' + << torque0_cdg_massless << ',' << torque1_cdg_massless << ',' + << torque2_cdg_massless << '\n'; + + if (i < 13 || i > 25) continue; + + gtsam::Vector6 wrench4 = + results_DG_mass.at(ContactWrenchKey(3, 0, i)) + .transpose() * + T3.AdjointMap(); + gtsam::Vector6 wrench5 = + results_DG_massless.at(ContactWrenchKey(3, 0, i)) + .transpose() * + T3.AdjointMap(); + gtsam::Vector6 wrench7 = wrench6.transpose() * T_body_contact.AdjointMap(); + FileWrenchesGround << wrench4[0] << "," << wrench4[1] << "," << wrench4[2] + << "," << wrench4[3] << "," << wrench4[4] << "," + << wrench4[5] << "," << wrench5[0] << "," << wrench5[1] + << "," << wrench5[2] << "," << wrench5[3] << "," + << wrench5[4] << "," << wrench5[5] << "," << wrench7[0] + << "," << wrench7[1] << "," << wrench7[2] << "," + << wrench7[3] << "," << wrench7[4] << "," << wrench7[5] + << "\n"; + } + FileLocations.close(); + FileWrenchesBody.close(); + FileWrenchesGround.close(); + return 0; +} + +int main(int argc, char** argv) { + int a = CombinedRun(false); + int b = CombinedRun(true); + return 0; +} \ No newline at end of file diff --git a/examples/example_a1_walking/sim.py b/examples/example_a1_walking/sim.py index 896165d7e..92335ffad 100644 --- a/examples/example_a1_walking/sim.py +++ b/examples/example_a1_walking/sim.py @@ -1,4 +1,5 @@ """Run kinematic motion planning using GTDynamics outputs.""" +# TODO(Varun): This whole file needs some cleaning up import time @@ -12,11 +13,12 @@ _ = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) # To load plane SDF. +print(pybullet_data.getDataPath()) p.setGravity(0, 0, -9.8) -p.setRealTimeSimulation(0) -planeId = p.loadURDF("plane.urdf") -p.changeDynamics(planeId, -1, lateralFriction=1) -robot = p.loadURDF(gtd.URDF_PATH + "/a1/a1.urdf",[0,0,0.48],[0,0,0,1]) +p.setRealTimeSimulation(1) +planeId = p.loadURDF(gtd.URDF_PATH + "plane.urdf", useFixedBase=True) +p.changeDynamics(planeId, -1, lateralFriction=0.4) +robot = p.loadURDF(gtd.URDF_PATH + "/a1/a1.urdf",[0,0,0.45],[0,0,0,1.0]) robot_id = robot @@ -28,29 +30,41 @@ joint_to_jid_map[jinfo[1].decode("utf-8")] = jinfo[0] #Read walk forward trajectory file from build (after running the example) -df = pd.read_csv(gtd.URDF_PATH + '/../../build/examples/example_a1_walking/a1_traj.csv') +df = pd.read_csv('a1_traj_DG_mass.csv') pos, orn = p.getBasePositionAndOrientation(robot_id) print("Init Base\n\tPos: {}\n\tOrn: {}".format(pos, p.getEulerFromQuaternion(orn))) +# for i in range(0,20,1): + # print(i, p.getJointInfo(robot_id, i)) + #import pdb;pdb.set_trace() + #print(i, p.getLinkInfo(robot_id, i)[1]) +#print(p.getDynamicsInfo(planeId,-1)) debug_iters = 1 -max_traj_replays = 200 +max_traj_replays = 1 num_traj_replays = 0 t = 0 t_f = None ts = [] all_pos_sim = [] +all_torques_hip = [] +all_torques_upper = [] +all_torques_lower = [] link_dict = {} -link_to_num = {10:1, 5: 1, 20: 1, 15: 3} +link_to_num = {10:1, 5: 4, 20: 10, 15: 7} constrained = [] i = 1 k = 0 +# for i in range(0,10,1): + # print(i) + # time.sleep(0.5) + while True: if num_traj_replays == max_traj_replays: break @@ -69,7 +83,7 @@ for joint_id in joint_to_jid_map.values(): p.setJointMotorControl2(robot_id,joint_id, controlMode=p.VELOCITY_CONTROL, - force=500) + force=50000) for joint, angle in jangles.items(): @@ -78,42 +92,47 @@ jointIndex=joint_to_jid_map[joint], controlMode=p.POSITION_CONTROL, targetPosition=angle, - targetVelocity=target_velocity) + targetVelocity=0.0) # Update body CoM coordinate frame. new_pos, new_orn = p.getBasePositionAndOrientation(robot_id) new_pos = np.array(new_pos) new_R = np.array(p.getMatrixFromQuaternion(new_orn)).reshape(3, 3) - # print(i) + t_hip = p.getJointState(bodyUniqueId=robot_id, jointIndex = 6)[3] + t_upper= p.getJointState(bodyUniqueId=robot_id, jointIndex = 8)[3] + t_lower = p.getJointState(bodyUniqueId=robot_id, jointIndex = 9)[3] # Detect collision points and constrain them. '''cp = np.asarray(p.getContactPoints(bodyA=planeId, bodyB=robot_id)) + #import pdb;pdb.set_trace() if cp.shape[0] > 1 and i > 1: new_cps = set(cp[:, 4]) - change = list(df.loc[i][np.arange(0, 12)] - - df.loc[i-1][np.arange(0, 12)]) - # import pdb;pdb.set_trace() + change = list(df.loc[i][np.arange(0, 12)] - df.loc[i-1][np.arange(0, 12)]) + #import pdb;pdb.set_trace() # Initial collision just_collided = [ x for x in new_cps if x not in constrained and x in link_to_num.keys()] # print(new_cps) for x in just_collided: - if (link_to_num[x] < 4 and change[link_to_num[x]] >= 0) or (link_to_num[x] >= 4 and change[link_to_num[x]] <= 0): + print(change[link_to_num[x]] ) + # if (link_to_num[x] < 4 and change[link_to_num[x]] >= 0) or (link_to_num[x] >= 4 and change[link_to_num[x]] <= 0): + if (change[link_to_num[x]] >= 0.01) or (change[link_to_num[x]] <= -0.01): link_dict[x] = p.createConstraint(robot_id, x, planeId, -1, p.JOINT_POINT2POINT, [ 0, 0, 0], [0, 0, 0], p.getLinkState(robot_id, x)[0]) constrained.append(x) - print(link_dict) + #print(link_dict) # Wants to lift for x in constrained: - if (link_to_num[x] < 4 and change[link_to_num[x]] <= 0) or (link_to_num[x] >= 4 and change[link_to_num[x]] >= 0) and link_dict.get(x) != None: + #if (link_to_num[x] < 4 and change[link_to_num[x]] <= 0) or (link_to_num[x] >= 4 and change[link_to_num[x]] >= 0) and link_dict.get(x) != None: + if (change[link_to_num[x]] >= 0.01) or (change[link_to_num[x]] <= -0.01) and link_dict.get(x) != None: numConstraints_before = p.getNumConstraints() p.removeConstraint(link_dict[x]) if numConstraints_before != p.getNumConstraints(): constrained.remove(x) ''' - '''if (i % debug_iters) == 0: + if (i % debug_iters) == 0: # print("\tIter {} Base\n\t\tPos: {}\n\t\tOrn: {}".format( # i, new_pos, p.getEulerFromQuaternion(new_orn))) @@ -122,10 +141,10 @@ 1, 0, 1], lineWidth=2.5) else: p.addUserDebugLine(pos, new_pos, lineColorRGB=[ - 0, 1, 1], lineWidth=2.5) + 0, 1, 1], lineWidth=2.5) pos, orn = new_pos, new_orn - bod_debug_line_x = p.addUserDebugLine( + '''bod_debug_line_x = p.addUserDebugLine( np.array([0, 0, 0]) + new_pos, np.matmul(new_R, np.array([0.05, 0, 0])) + new_pos, lineColorRGB=[1, 0, 1], lineWidth=1, lifeTime=1.5) @@ -139,37 +158,70 @@ lineColorRGB=[1, 0, 1], lineWidth=1, lifeTime=1.5)''' p.stepSimulation() - time.sleep(1. / 20.) + time.sleep(1. /24.) - ts.append(t) - t += 1. / 240. - all_pos_sim.append(new_pos) + t += 1. / 24. i += 1 k += 1 - + if t >2 and t < 12: + all_pos_sim.append(new_pos) + #if t_hip > 80 or t_upper > 80 or t_lower > 80: continue + ts.append(t-2) + all_torques_hip.append(t_hip) + all_torques_upper.append(t_upper) + all_torques_lower.append(t_lower) + +torques = np.zeros((len(all_torques_hip),4)) +torques[:,0] = np.array(ts) +torques[:,1] = np.array(all_torques_hip) +torques[:,2] = np.array(all_torques_upper) +torques[:,3] = np.array(all_torques_lower) +np.savetxt('/home/dan/Desktop/Projects/GTDynamics/build/examples/example_a1_walking/torques_pyb.txt', torques) pos, orn = p.getBasePositionAndOrientation(robot_id) print("Final Base\n\tPos: {}\n\tOrn: {}".format(pos, p.getEulerFromQuaternion(orn))) -fig, axs = plt.subplots(3, 1, sharex=True) -fig.subplots_adjust(hspace=0) +fig, axs = plt.subplots(6, 1, sharex=True) +fig.subplots_adjust(hspace=0.1) axs[0].plot(ts, [p[0] for p in all_pos_sim]) -axs[0].axvline(x=t_f, color='k', linestyle='--') +#axs[0].axvline(x=t_f, color='k', linestyle='--') axs[0].set_ylabel('x (m.)') +axs[0].set_title('Robot Body location Vs. Time') axs[1].plot(ts, [p[1] for p in all_pos_sim]) -axs[1].axvline(x=t_f, color='k', linestyle='--') +#axs[1].axvline(x=t_f, color='k', linestyle='--') axs[1].set_ylabel('y (m.)') +axs[1].set_ylim((-0.2,0.2)) axs[2].plot(ts, [p[2] for p in all_pos_sim]) -axs[2].axvline(x=t_f, color='k', linestyle='--') +#axs[2].axvline(x=t_f, color='k', linestyle='--') axs[2].set_ylabel('z (m.)') +axs[2].set_ylim((0.2,0.45)) + +axs[3].plot(ts, [t for t in all_torques_hip]) +#axs[2].axvline(x=t_f, color='k', linestyle='--') +axs[3].set_ylabel('tau hip') +#axs[3].set_ylim((-10,10)) + +axs[4].plot(ts, [t for t in all_torques_upper]) +#axs[2].axvline(x=t_f, color='k', linestyle='--') +axs[4].set_ylabel('tau upper') +#axs[3].set_ylim((-10,10)) + +axs[5].plot(ts, [t for t in all_torques_lower]) +#axs[2].axvline(x=t_f, color='k', linestyle='--') +axs[5].set_ylabel('tau lower') +#axs[3].set_ylim((-10,10)) plt.xlabel("time (s.)") -#plt.show() +plt.show() +# uncomment to output pdflatex +#plt.rc('pgf', texsystem='pdflatex') +#plt.savefig('/home/dan/Desktop/my_papers/thesis/create_simulation/pyb_sim.pgf') + # i = 0 # while True: # i += 1 diff --git a/examples/example_cart_pole_trajectory_optimization/CMakeLists.txt b/examples/example_cart_pole_trajectory_optimization/CMakeLists.txt index 4ff373a34..b8dc99f6f 100644 --- a/examples/example_cart_pole_trajectory_optimization/CMakeLists.txt +++ b/examples/example_cart_pole_trajectory_optimization/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.9...3.29) project(example_cart_pole_trajectory_optimization C CXX) # Build Executable diff --git a/examples/example_cart_pole_trajectory_optimization/main.cpp b/examples/example_cart_pole_trajectory_optimization/main.cpp index f4a130edf..76f8e1b9e 100644 --- a/examples/example_cart_pole_trajectory_optimization/main.cpp +++ b/examples/example_cart_pole_trajectory_optimization/main.cpp @@ -15,14 +15,12 @@ #include #include #include -#include +#include #include #include #include #include -#include -#include #include #include #include @@ -93,7 +91,8 @@ int main(int argc, char** argv) { graph.emplace_shared(TorqueKey(j0_id, t), control_model); // Initialize solution. - auto init_vals = ZeroValuesTrajectory(cp, t_steps, 0, 0.0); + Initializer initializer; + auto init_vals = initializer.ZeroValuesTrajectory(cp, t_steps, 0, 0.0); gtsam::LevenbergMarquardtParams params; params.setMaxIterations(40); params.setVerbosityLM("SUMMARY"); @@ -113,7 +112,12 @@ int main(int argc, char** argv) { JointAccelKey(j1_id, t), TorqueKey(j1_id, t)}; std::vector vals = {std::to_string(t_elapsed)}; for (auto&& k : keys) vals.push_back(std::to_string(results.atDouble(k))); - traj_file << boost::algorithm::join(vals, ",") << "\n"; + + std::string vals_str = ""; + for (size_t j = 0; j < vals.size(); j++) { + vals_str += vals[j] + (j != vals.size() - 1 ? "," : ""); + } + traj_file << vals_str << "\n"; } traj_file.close(); diff --git a/examples/example_cart_pole_trajectory_optimization/sim.py b/examples/example_cart_pole_trajectory_optimization/sim.py index 32784df7e..d4a2ba42b 100644 --- a/examples/example_cart_pole_trajectory_optimization/sim.py +++ b/examples/example_cart_pole_trajectory_optimization/sim.py @@ -6,12 +6,14 @@ import pybullet as p import pybullet_data +import gtdynamics as gtd + # pylint: disable=I1101, C0103 _ = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) # To load plane SDF. p.setGravity(0, 0, -9.8) -robot = p.loadURDF("cart_pole.urdf", [0, 0, 0], [0, 0, 0, 1], False, True) +robot = p.loadURDF(gtd.URDF_PATH + "cart_pole.urdf", [0, 0, 0], [0, 0, 0, 1], False, True) t = 0 diff --git a/examples/example_constraint_manifold/CMakeLists.txt b/examples/example_constraint_manifold/CMakeLists.txt new file mode 100644 index 000000000..cd0634fb7 --- /dev/null +++ b/examples/example_constraint_manifold/CMakeLists.txt @@ -0,0 +1,81 @@ +cmake_minimum_required(VERSION 3.9...3.29) +project(example_constraint_manifold C CXX) + +# Build Executable + + +# Connecte Poses. +set(CONNECTED_POSES ${PROJECT_NAME}_connected_poses) +add_executable(${CONNECTED_POSES} main_connected_poses.cpp) +target_link_libraries(${CONNECTED_POSES} PUBLIC gtdynamics) +target_include_directories(${CONNECTED_POSES} PUBLIC ${CMAKE_PREFIX_PATH}/include) + +add_custom_target( + ${CONNECTED_POSES}.run + COMMAND ./${CONNECTED_POSES} + DEPENDS ${CONNECTED_POSES} + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/examples/${PROJECT_NAME}) + + +# Range Constraint. +set(RANGE_CONSTRAINT ${PROJECT_NAME}_range_constraint) +add_executable(${RANGE_CONSTRAINT} main_range_constraint.cpp) +target_link_libraries(${RANGE_CONSTRAINT} PUBLIC gtdynamics) +target_include_directories(${RANGE_CONSTRAINT} PUBLIC ${CMAKE_PREFIX_PATH}/include) + +add_custom_target( + ${RANGE_CONSTRAINT}.run + COMMAND ./${RANGE_CONSTRAINT} + DEPENDS ${RANGE_CONSTRAINT} + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/examples/${PROJECT_NAME}) + + +# Arm Planning. +set(ARM ${PROJECT_NAME}_arm) +add_executable(${ARM} main_arm_kinematic_planning.cpp SerialChain.h) +target_link_libraries(${ARM} PUBLIC gtdynamics) +target_include_directories(${ARM} PUBLIC ${CMAKE_PREFIX_PATH}/include) + +add_custom_target( + ${ARM}.run + COMMAND ./${ARM} + DEPENDS ${ARM} + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/examples/${PROJECT_NAME}) + +# Arm Planning. +set(CABLE_ROBOT ${PROJECT_NAME}_cable_robot) +add_executable(${CABLE_ROBOT} main_cablerobot.cpp) +target_link_libraries(${CABLE_ROBOT} PUBLIC gtdynamics) +target_include_directories(${CABLE_ROBOT} PUBLIC ${CMAKE_PREFIX_PATH}/include) + +add_custom_target( + ${CABLE_ROBOT}.run + COMMAND ./${CABLE_ROBOT} + DEPENDS ${CABLE_ROBOT} + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/examples/${PROJECT_NAME}) + +# Cart pole planning. +set(CART_POLE ${PROJECT_NAME}_cart_pole) +add_executable(${CART_POLE} main_cartpole.cpp CartPoleUtils.h CartPoleUtils.cpp) +target_link_libraries(${CART_POLE} PUBLIC gtdynamics) +target_include_directories(${CART_POLE} PUBLIC ${CMAKE_PREFIX_PATH}/include) + +add_custom_target( + ${CART_POLE}.run + COMMAND ./${CART_POLE} + DEPENDS ${CART_POLE} + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/examples/${PROJECT_NAME}) + + +# Quadruped planning. +set(QUADRUPED_MP ${PROJECT_NAME}_quadruped_mp) +add_executable(${QUADRUPED_MP} main_quadruped.cpp QuadrupedUtils.h QuadrupedUtils.cpp) +target_link_libraries(${QUADRUPED_MP} PUBLIC gtdynamics) +target_include_directories(${QUADRUPED_MP} PUBLIC ${CMAKE_PREFIX_PATH}/include) + +add_custom_target( + ${QUADRUPED_MP}.run + COMMAND ./${QUADRUPED_MP} + DEPENDS ${QUADRUPED_MP} + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/examples/${PROJECT_NAME}) + diff --git a/examples/example_constraint_manifold/CartPoleUtils.cpp b/examples/example_constraint_manifold/CartPoleUtils.cpp new file mode 100644 index 000000000..58ab8db27 --- /dev/null +++ b/examples/example_constraint_manifold/CartPoleUtils.cpp @@ -0,0 +1,294 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file CartPoleUtils.cpp + * @brief Cart pole utilities implementations. + * @author Yetong Zhang + */ + +#include "CartPoleUtils.h" + +#include +#include + +namespace gtdynamics { + +using gtsam::Key; +using gtsam::KeyVector; +using gtsam::NonlinearFactorGraph; +using gtsam::Values; + +/* ************************************************************************* */ +NonlinearFactorGraph CartPole::getDynamicsGraph(size_t num_steps) const { + NonlinearFactorGraph graph; + // kinodynamic constraints at each step + for (size_t k = 0; k <= num_steps; k++) { + graph.add(graph_builder.dynamicsFactorGraph(robot, k)); + } + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph CartPole::getUnactuatedGraph(size_t num_steps) const { + NonlinearFactorGraph graph; + for (int k = 0; k <= num_steps; k++) + graph.addPrior(TorqueKey(j1_id, k), 0.0, + graph_builder.opt().prior_t_cost_model); + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph CartPole::initStateGraph() const { + // Initial conditions + NonlinearFactorGraph graph; + graph.addPrior(JointAngleKey(j0_id, 0), X_i[0], prior_model); + graph.addPrior(JointVelKey(j0_id, 0), X_i[1], prior_model); + graph.addPrior(JointAccelKey(j0_id, 0), X_i[2], prior_model); + graph.addPrior(JointAngleKey(j1_id, 0), X_i[3], prior_model); + graph.addPrior(JointVelKey(j1_id, 0), X_i[4], prior_model); + // graph.addPrior(JointAccelKey(j1_id, 0), X_i[5], prior_model); + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph CartPole::finalStateGraph(size_t num_steps) const { + NonlinearFactorGraph graph; + graph.addPrior(JointAngleKey(j0_id, num_steps), X_T[0], pos_objectives_model); + graph.addPrior(JointAngleKey(j1_id, num_steps), X_T[3], pos_objectives_model); + graph.addPrior(JointVelKey(j0_id, num_steps), X_T[1], objectives_model); + graph.addPrior(JointAccelKey(j0_id, num_steps), X_T[2], objectives_model); + graph.addPrior(JointVelKey(j1_id, num_steps), X_T[4], objectives_model); + graph.addPrior(JointAccelKey(j1_id, num_steps), X_T[5], objectives_model); + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph CartPole::minTorqueCosts(size_t num_steps) const { + NonlinearFactorGraph graph; + for (int k = 0; k <= num_steps; k++) + graph.emplace_shared(TorqueKey(j0_id, k), control_model); + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph CartPole::getCollocation(size_t num_steps, double dt) { + NonlinearFactorGraph graph; + for (int k = 0; k < num_steps; k++) { + graph.add( + graph_builder.collocationFactors(robot, k, dt, collocation_scheme)); + } + return graph; +} + +/* ************************************************************************* */ +Values CartPole::getInitValuesZero(size_t num_steps) { + Initializer initializer; + Values values0 = initializer.ZeroValues(robot, 0, 0.0); + Values known_values; + for (const auto& joint : robot.joints()) { + InsertJointAngle(&known_values, joint->id(), + JointAngle(values0, joint->id())); + InsertJointVel(&known_values, joint->id(), JointVel(values0, joint->id())); + InsertTorque(&known_values, joint->id(), 0.0); + } + for (const auto& link : robot.links()) { + InsertPose(&known_values, link->id(), Pose(values0, link->id())); + InsertTwist(&known_values, link->id(), Twist(values0, link->id())); + } + values0 = graph_builder.linearSolveFD(robot, 0, known_values); + + Values values; + for (size_t k = 0; k <= num_steps; k++) { + for (const auto& joint : robot.joints()) { + InsertJointAngle(&values, joint->id(), k, + JointAngle(values0, joint->id())); + InsertJointVel(&values, joint->id(), k, JointVel(values0, joint->id())); + InsertJointAccel(&values, joint->id(), k, + JointAccel(values0, joint->id())); + InsertTorque(&values, joint->id(), k, Torque(values0, joint->id())); + for (const auto& link : joint->links()) { + InsertWrench(&values, link->id(), joint->id(), k, + Wrench(values0, link->id(), joint->id())); + } + } + for (const auto& link : robot.links()) { + InsertPose(&values, link->id(), k, Pose(values0, link->id())); + InsertTwist(&values, link->id(), k, Twist(values0, link->id())); + InsertTwistAccel(&values, link->id(), k, TwistAccel(values0, link->id())); + } + } + return values; +} + +/* ************************************************************************* */ +Values CartPole::getInitValues(size_t num_steps, std::string option) { + if (option == "zero") { + return getInitValuesZero(num_steps); + } else if (option == "interp") { + return getInitValuesInterp(num_steps); + } else if (option == "infeasible") { + return getInitValuesInfeasible(num_steps); + } else { + return getInitValuesZero(num_steps); + } +} + +/* ************************************************************************* */ +Values CartPole::getInitValuesInterp(size_t num_steps) { + Initializer initializer; + + double init_q0 = X_i(0); + double terminal_q0 = X_T(0); + double init_q1 = X_i(3); + double terminal_q1 = X_T(3); + + Values init_values; + for (size_t k = 0; k <= num_steps; k++) { + double q0 = + init_q0 + (terminal_q0 - init_q0) * (double)k / (double)num_steps; + double q1 = + init_q1 + (terminal_q1 - init_q1) * (double)k / (double)num_steps; + Values known_values; + InsertJointAngle(&known_values, j0_id, k, q0); + InsertJointAngle(&known_values, j1_id, k, q1); + known_values = robot.forwardKinematics(known_values, k); + InsertTorque(&known_values, j0_id, k, 0.0); + InsertTorque(&known_values, j1_id, k, 0.0); + Values values = graph_builder.linearSolveFD(robot, k, known_values); + init_values.insert(values); + } + return init_values; +} + +/* ************************************************************************* */ +Values CartPole::getInitValuesInfeasible(size_t num_steps) { + Initializer initializer; + auto init_values_infeasible = + initializer.ZeroValuesTrajectory(robot, num_steps, 0, 0.0); + return init_values_infeasible; +} +/* ************************************************************************* */ +void CartPole::exportTrajectory(const Values& results, size_t num_steps, + double dt, std::string file_name) const { + std::ofstream traj_file; + traj_file.open(file_name); + traj_file << "t,x,xdot,xddot,xtau,theta,thetadot,thetaddot,thetatau\n"; + double t_elapsed = 0; + for (int t = 0; t <= num_steps; t++, t_elapsed += dt) { + std::vector keys = { + JointAngleKey(j0_id, t), JointVelKey(j0_id, t), + JointAccelKey(j0_id, t), TorqueKey(j0_id, t), + JointAngleKey(j1_id, t), JointVelKey(j1_id, t), + JointAccelKey(j1_id, t), TorqueKey(j1_id, t)}; + std::vector vals = {std::to_string(t_elapsed)}; + for (auto&& k : keys) { + vals.push_back(std::to_string(results.atDouble(k))); + } + std::string vals_str = ""; + for (size_t j = 0; j < vals.size(); j++) { + vals_str += vals[j] + (j != vals.size() - 1 ? "," : ""); + } + traj_file << vals_str << "\n"; + } + traj_file.close(); +} + +/* ************************************************************************* */ +void CartPole::printJointAngles(const Values& values, size_t num_steps) const { + for (size_t k = 0; k <= num_steps; k++) { + std::cout << "step " << k << ":"; + for (const auto& joint : robot.joints()) { + double angle = JointAngle(values, joint->id(), k); + std::cout << "\t" << angle; + } + std::cout << "\n"; + } +} + +/* ************************************************************************* */ +OptimizerSetting CartPole::getOptSetting() const { + auto opt = gtdynamics::OptimizerSetting(); + // opt.bp_cost_model = gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + // opt.bv_cost_model = gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + // opt.ba_cost_model = gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + // opt.p_cost_model = gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + // opt.v_cost_model = gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + // opt.a_cost_model = gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + // opt.f_cost_model = gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + // opt.fa_cost_model = gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + // opt.t_cost_model = gtsam::noiseModel::Isotropic::Sigma(1, sigma_dynamics); + // opt.cp_cost_model = gtsam::noiseModel::Isotropic::Sigma(1, sigma_dynamics); + // opt.cfriction_cost_model = + // gtsam::noiseModel::Isotropic::Sigma(1, sigma_dynamics); + // opt.cv_cost_model = gtsam::noiseModel::Isotropic::Sigma(3, sigma_dynamics); + // opt.ca_cost_model = gtsam::noiseModel::Isotropic::Sigma(3, sigma_dynamics); + // opt.planar_cost_model = + // gtsam::noiseModel::Isotropic::Sigma(3, sigma_dynamics); + // opt.prior_q_cost_model = + // gtsam::noiseModel::Isotropic::Sigma(1, sigma_dynamics); + // opt.prior_qv_cost_model = + // gtsam::noiseModel::Isotropic::Sigma(1, sigma_dynamics); + // opt.prior_qa_cost_model = + // gtsam::noiseModel::Isotropic::Sigma(1, sigma_dynamics); + // opt.prior_t_cost_model = + // gtsam::noiseModel::Isotropic::Sigma(1, sigma_dynamics); + // opt.q_col_cost_model = gtsam::noiseModel::Isotropic::Sigma(1, + // sigma_collocation); opt.v_col_cost_model = + // gtsam::noiseModel::Isotropic::Sigma(1, sigma_collocation); + // opt.time_cost_model = gtsam::noiseModel::Isotropic::Sigma(1, + // sigma_dynamics); opt.pose_col_cost_model = + // gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + // opt.twist_col_cost_model = + // gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + return opt; +} + +/* ************************************************************************* */ +BasisKeyFunc CartPole::getBasisKeyFunc(bool unactuated_as_constraint) const { + if (unactuated_as_constraint) { + BasisKeyFunc basis_key_func = + [=](const KeyVector& keys) -> KeyVector { + KeyVector basis_keys; + for (const Key& key : keys) { + auto symb = DynamicsSymbol(key); + if (symb.label() == "q") { + basis_keys.push_back(key); + } + if (symb.label() == "v") { + basis_keys.push_back(key); + } + if (symb.label() == "T" and symb.jointIdx() == j0_id) { + basis_keys.push_back(key); + } + } + return basis_keys; + }; + return basis_key_func; + } else { + BasisKeyFunc basis_key_func = + [](const KeyVector& keys) -> KeyVector { + KeyVector basis_keys; + for (const Key& key : keys) { + auto symb = DynamicsSymbol(key); + if (symb.label() == "q") { + basis_keys.push_back(key); + } + if (symb.label() == "v") { + basis_keys.push_back(key); + } + if (symb.label() == "T") { + basis_keys.push_back(key); + } + } + return basis_keys; + }; + return basis_key_func; + } +} + +} // namespace gtdynamics diff --git a/examples/example_constraint_manifold/CartPoleUtils.h b/examples/example_constraint_manifold/CartPoleUtils.h new file mode 100644 index 000000000..8fb0c9d09 --- /dev/null +++ b/examples/example_constraint_manifold/CartPoleUtils.h @@ -0,0 +1,116 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file CartPoleUtils.h + * @brief Utility functions for cart pole experiments. + * @author Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace gtdynamics { + +using gtsam::NonlinearFactorGraph; +using gtsam::SharedNoiseModel; +using gtsam::Values; +using gtsam::Vector; +using gtsam::Vector3; +using gtsam::Vector6; + +class CartPole { +public: + // Cart-pole dynamic planning scenario setting. + gtdynamics::Robot robot = + gtdynamics::CreateRobotFromFile(gtdynamics::kUrdfPath + + std::string("cart_pole.urdf")) + .fixLink("l0"); + int j0_id = robot.joint("j0")->id(), j1_id = robot.joint("j1")->id(); + Vector6 X_i = Vector6::Constant(6, 0); + Vector6 X_T = (Vector(6) << 1, 0, 0, M_PI, 0, 0).finished(); + Vector3 gravity = Vector3(0, 0, -9.8); + gtdynamics::CollocationScheme collocation_scheme = gtdynamics::CollocationScheme::Trapezoidal; + + double sigma_dynamics = 1e-5; + double sigma_prior = 1e-7; + double sigma_collocation = 1e-5; + double sigma_pos_objective = 1e-5; + double sigma_objectives = 5e-3; + double sigma_min_torque = 2e1; + SharedNoiseModel prior_model = gtsam::noiseModel::Isotropic::Sigma(1, sigma_prior); // prior constraints. + SharedNoiseModel pos_objectives_model = + gtsam::noiseModel::Isotropic::Sigma(1, sigma_pos_objective); // Pos objectives. + SharedNoiseModel objectives_model = + gtsam::noiseModel::Isotropic::Sigma(1, sigma_objectives); // Additional objectives. + SharedNoiseModel control_model = + gtsam::noiseModel::Isotropic::Sigma(1, sigma_min_torque); // Controls. + + gtdynamics::OptimizerSetting opt = getOptSetting(); + gtdynamics::DynamicsGraph graph_builder = gtdynamics::DynamicsGraph(opt, gravity); + +public: + /// Kinodynamic constraints + NonlinearFactorGraph getDynamicsGraph(size_t num_steps) const; + + /// Set the 2nd joint to be unactuated + NonlinearFactorGraph getUnactuatedGraph(size_t num_steps) const; + + /** Cost for planning, includes terminal state objectives, control costs.*/ + NonlinearFactorGraph minTorqueCosts(size_t num_steps) const; + + /// Priors on initial state. + NonlinearFactorGraph initStateGraph() const; + + /// Priors on final state. + NonlinearFactorGraph finalStateGraph(size_t num_steps) const; + + /** Collocation between each step, benchmark will be made by either treating + * collocation as costs or constraints. */ + NonlinearFactorGraph getCollocation(size_t num_steps, double dt); + + /// Initial values for trajectory optimization. + Values getInitValues(size_t num_steps, std::string option = "zero"); + + /// Export trajectory to external file. + void exportTrajectory(const Values& results, size_t num_steps, double dt, std::string file_name = "traj.csv") const; + + /// Print joint angles for all steps. + void printJointAngles(const Values& values, size_t num_steps) const; + + /// Return function that select basis keys for constraint manifolds. + BasisKeyFunc getBasisKeyFunc(bool unactuated_as_constraint = true) const; + +protected: + /// Default optimizer setting. + gtdynamics::OptimizerSetting getOptSetting() const; + + /// Initial values seet as rest pose for all steps. + Values getInitValuesZero(size_t num_steps); + + /// Initial values seet as rest pose for all steps. + Values getInitValuesInterp(size_t num_steps); + + /// Infeasible initial values. + Values getInitValuesInfeasible(size_t num_steps); + +}; + +} // namespace gtdynamics diff --git a/examples/example_constraint_manifold/QuadrupedUtils.cpp b/examples/example_constraint_manifold/QuadrupedUtils.cpp new file mode 100644 index 000000000..ea51d2564 --- /dev/null +++ b/examples/example_constraint_manifold/QuadrupedUtils.cpp @@ -0,0 +1,623 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file QuadrupedUtils.cpp + * @brief Quadruped utilities implementations. + * @author: Yetong Zhang + */ + +#include "QuadrupedUtils.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace gtdynamics { + +using namespace gtsam; + +/* ************************************************************************* */ +Vector3 get_contact_force(const Pose3 &pose, const Vector6 wrench, + OptionalJacobian<3, 6> H_pose, + OptionalJacobian<3, 6> H_wrench) { + Vector3 force_l(wrench(3), wrench(4), wrench(5)); + if (H_pose || H_wrench) { + Matrix36 J_fl_wrench; + J_fl_wrench << Z_3x3, I_3x3; + + Matrix36 J_rot_pose; + Rot3 rot = pose.rotation(J_rot_pose); + + Matrix33 H_rot, H_fl; + Vector3 force_w = rot.rotate(force_l, H_rot, H_fl); + + if (H_pose) { + *H_pose = H_rot * J_rot_pose; + } + if (H_wrench) { + *H_wrench = H_fl * J_fl_wrench; + } + + return force_w; + } else { + return pose.rotation().rotate(force_l); + } +} + +/* ************************************************************************* */ +Vector6_ ContactRedundancyConstraint(int t, + const std::vector &contact_ids, + const double &a, const double &b) { + std::vector error; + for (size_t i = 0; i < 4; i++) { + auto link_id = contact_ids.at(i); + Vector6_ c_wrench(gtdynamics::ContactWrenchKey(link_id, 0, t)); + Pose3_ pose(gtdynamics::PoseKey(link_id, t)); + Vector3_ c_force(get_contact_force, pose, c_wrench); + Matrix63 H; + if (i == 0) { + H << 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, a, b, 0; + } else if (i == 1) { + H << 0, 0, -1, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, a, b, 0; + } else if (i == 2) { + H << 0, 0, -1, 0, 0, 0, 1, 0, 0, 0, -1, 0, 0, 0, 0, -a, -b, 0; + } else if (i == 3) { + H << 0, 0, 1, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, -1, 0, -a, -b, 0; + } + const std::function f = [H](const Vector3 &F) { + return H * F; + }; + error.emplace_back(linearExpression(f, c_force, H)); + } + + return error[0] + error[1] + error[2] + error[3]; +} + +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr ContactRedundancyFactor( + int t, const std::vector &contact_ids, const double &a, + const double &b, const noiseModel::Base::shared_ptr &cost_model, + bool express_redundancy) { + if (express_redundancy) { + Vector6_ expected_redundancy = + ContactRedundancyConstraint(t, contact_ids, a, b); + Vector6_ redundancy(ContactRedundancyKey(t)); + return std::make_shared>( + cost_model, Vector6::Zero(), expected_redundancy - redundancy); + } else { + return std::make_shared>( + cost_model, Vector6::Zero(), + ContactRedundancyConstraint(t, contact_ids, a, b)); + } +} + +/* ************************************************************************* */ +NonlinearFactorGraph contact_q_factors( + const int k, const gtdynamics::PointOnLinks &contact_points, + const std::vector &contact_in_world, + const noiseModel::Base::shared_ptr &cost_model) { + NonlinearFactorGraph graph; + // Add contact factors. + for (size_t contact_idx = 0; contact_idx < contact_points.size(); + contact_idx++) { + const auto &cp = contact_points.at(contact_idx); + gtdynamics::FixedContactPointFactor contact_pose_factor( + gtdynamics::PoseKey(cp.link->id(), k), cost_model, + contact_in_world.at(contact_idx), cp.point); + graph.add(contact_pose_factor); + } + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph Vision60Robot::DynamicsFactors( + const int k, const std::optional &contact_points) const { + NonlinearFactorGraph graph; + + for (auto &&link : robot.links()) { + int i = link->id(); + if (!link->isFixed()) { + const auto &connected_joints = link->joints(); + std::vector wrench_keys; + + // Add wrench keys for joints. + for (auto &&joint : connected_joints) + wrench_keys.push_back(WrenchKey(i, joint->id(), k)); + + // Add wrench keys for contact points. + if (contact_points) { + for (auto &&cp : *contact_points) { + if (cp.link->id() != i) continue; + // TODO(frank): allow multiple contact points on one link, id = 0,1,.. + auto wrench_key = ContactWrenchKey(i, 0, k); + wrench_keys.push_back(wrench_key); + + graph.emplace_shared( + wrench_key, opt().cm_cost_model, + Pose3(Rot3(), -cp.point)); + } + } + + // add wrench factor for link + graph.add( + WrenchFactor(opt().fa_cost_model, link, wrench_keys, k, gravity)); + } + } + + for (auto &&joint : robot.joints()) { + auto j = joint->id(), child_id = joint->child()->id(); + auto const_joint = joint; + graph.add(WrenchEquivalenceFactor(opt().f_cost_model, const_joint, k)); + graph.add(TorqueFactor(opt().t_cost_model, const_joint, k)); + } + return graph; +} + +/* ************************************************************************* */ +KeyVector FindBasisKeys4C(const KeyVector &keys) { + KeyVector basis_keys; + for (const Key &key : keys) { + auto symb = gtdynamics::DynamicsSymbol(key); + if (symb.label() == "p" && symb.linkIdx() == 0) { + basis_keys.push_back(key); + } else if (symb.label() == "V" && symb.linkIdx() == 0) { + basis_keys.push_back(key); + } else if (symb.label() == "A" && symb.linkIdx() == 0) { + basis_keys.push_back(key); + } + } + return basis_keys; +} + +/* ************************************************************************* */ +KeyVector findBasisKeysRedundancy(const KeyVector &keys) { + KeyVector basis_keys; + for (const Key &key : keys) { + auto symb = gtdynamics::DynamicsSymbol(key); + if (symb.label() == "p" && symb.linkIdx() == 0) { + basis_keys.push_back(key); + } else if (symb.label() == "V" && symb.linkIdx() == 0) { + basis_keys.push_back(key); + } else if (symb.label() == "A" && symb.linkIdx() == 0) { + basis_keys.push_back(key); + } else if (symb.label() == "CR") { + basis_keys.push_back(key); + } + } + return basis_keys; +} + +/* ************************************************************************* */ +gtdynamics::OptimizerSetting Vision60Robot::getOptSetting() const { + auto opt = gtdynamics::OptimizerSetting(); + opt.bp_cost_model = noiseModel::Isotropic::Sigma(6, sigma_dynamics); + opt.bv_cost_model = noiseModel::Isotropic::Sigma(6, sigma_dynamics); + opt.ba_cost_model = noiseModel::Isotropic::Sigma(6, sigma_dynamics); + opt.p_cost_model = noiseModel::Isotropic::Sigma(6, sigma_dynamics); + opt.v_cost_model = noiseModel::Isotropic::Sigma(6, sigma_dynamics); + opt.a_cost_model = noiseModel::Isotropic::Sigma(6, sigma_dynamics); + opt.f_cost_model = noiseModel::Isotropic::Sigma(6, sigma_dynamics); + opt.fa_cost_model = noiseModel::Isotropic::Sigma(6, sigma_dynamics); + opt.t_cost_model = noiseModel::Isotropic::Sigma(1, sigma_dynamics); + opt.cp_cost_model = noiseModel::Isotropic::Sigma(1, sigma_dynamics); + opt.cfriction_cost_model = + noiseModel::Isotropic::Sigma(1, sigma_dynamics); + opt.cv_cost_model = noiseModel::Isotropic::Sigma(3, sigma_dynamics); + opt.ca_cost_model = noiseModel::Isotropic::Sigma(3, sigma_dynamics); + opt.planar_cost_model = + noiseModel::Isotropic::Sigma(3, sigma_dynamics); + opt.prior_q_cost_model = + noiseModel::Isotropic::Sigma(1, sigma_dynamics); + opt.prior_qv_cost_model = + noiseModel::Isotropic::Sigma(1, sigma_dynamics); + opt.prior_qa_cost_model = + noiseModel::Isotropic::Sigma(1, sigma_dynamics); + opt.prior_t_cost_model = + noiseModel::Isotropic::Sigma(1, sigma_dynamics); + opt.q_col_cost_model = noiseModel::Isotropic::Sigma(1, sigma_dynamics); + opt.v_col_cost_model = noiseModel::Isotropic::Sigma(1, sigma_dynamics); + opt.time_cost_model = noiseModel::Isotropic::Sigma(1, sigma_dynamics); + opt.pose_col_cost_model = + noiseModel::Isotropic::Sigma(6, sigma_dynamics); + opt.twist_col_cost_model = + noiseModel::Isotropic::Sigma(6, sigma_dynamics); + return opt; +} + +/* ************************************************************************* */ +NonlinearFactorGraph Vision60Robot::getConstraintsGraphStepQ( + const int t) const { + NonlinearFactorGraph graph = graph_builder.qFactors(robot, t); + graph.add(contact_q_factors(t, contact_points, contact_in_world, + cpoint_cost_model)); + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph Vision60Robot::getConstraintsGraphStepV( + const int t) const { + return graph_builder.vFactors(robot, t, contact_points); +} + +/* ************************************************************************* */ +NonlinearFactorGraph Vision60Robot::getConstraintsGraphStepAD( + const int t) const { + NonlinearFactorGraph graph = graph_builder.aFactors(robot, t, contact_points); + graph.add(DynamicsFactors(t, contact_points)); + graph.add(ContactRedundancyFactor(t, contact_ids, a, b, redundancy_model, + express_redundancy)); + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph Vision60Robot::getConstraintsGraphStep(const int t) const { + NonlinearFactorGraph graph; + graph.add(getConstraintsGraphStepQ(t)); + graph.add(getConstraintsGraphStepV(t)); + graph.add(getConstraintsGraphStepAD(t)); + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph Vision60Robot::getConstraintsGraphTrajectory( + const int num_steps) const { + NonlinearFactorGraph graph; + for (int t = 0; t <= num_steps; t++) { + graph.add(getConstraintsGraphStep(t)); + } + return graph; +} + +template +Values SubValues(const Values &values, const CONTAINER &keys) { + Values sub_values; + for (const Key &key : keys) { + sub_values.insert(key, values.at(key)); + } + return sub_values; +} + +/* ************************************************************************* */ +Values Vision60Robot::getInitValuesStep(const int t, const Pose3 &base_pose, + const Vector6 &base_twist, + const Vector6 &base_accel, + Values init_values_t) const { + if (init_values_t.size() == 0) { + init_values_t = nominal_values; + Vector6 zero_vec6 = Vector6::Zero(); + for (auto &&joint : robot.joints()) { + int j = joint->id(); + InsertJointAccel(&init_values_t, j, 0.0); + InsertTorque(&init_values_t, j, 0.0); + InsertWrench(&init_values_t, joint->parent()->id(), j, zero_vec6); + InsertWrench(&init_values_t, joint->child()->id(), j, zero_vec6); + } + for (const auto &cp : contact_points) { + int i = cp.link->id(); + init_values_t.insert(ContactWrenchKey(i, 0, 0), zero_vec6); + } + for (auto &&link : robot.links()) { + InsertTwistAccel(&init_values_t, link->id(), zero_vec6); + } + if (express_redundancy) { + init_values_t.insert(ContactRedundancyKey(t), zero_vec6); + } + } + + Values known_values; + LevenbergMarquardtParams lm_params; + // lm_params.setVerbosityLM("SUMMARY"); + lm_params.setlambdaUpperBound(1e20); + + // solve q level + NonlinearFactorGraph graph_q = getConstraintsGraphStepQ(t); + graph_q.addPrior(PoseKey(base_id, t), base_pose, + graph_builder.opt().p_cost_model); + + Values init_values_q = SubValues(init_values_t, graph_q.keys()); + LevenbergMarquardtOptimizer optimizer_q(graph_q, init_values_q, lm_params); + auto results_q = optimizer_q.optimize(); + if (graph_q.error(results_q) > 1e-5) { + std::cout << "solving q fails! error: " << graph_q.error(results_q) << "\n"; + } + known_values.insert(results_q); + + // solve v level + NonlinearFactorGraph graph_v = getConstraintsGraphStepV(t); + graph_v.addPrior(TwistKey(base_id, t), base_twist, + graph_builder.opt().v_cost_model); + graph_v = ConstVarGraph(graph_v, known_values); + Values init_values_v = SubValues(init_values_t, graph_v.keys()); + LevenbergMarquardtOptimizer optimizer_v(graph_v, init_values_v, lm_params); + auto results_v = optimizer_v.optimize(); + if (graph_v.error(results_v) > 1e-5) { + std::cout << "solving v fails! error: " << graph_v.error(results_v) << "\n"; + } + known_values.insert(results_v); + + // solve a and dynamics level + NonlinearFactorGraph graph_ad = getConstraintsGraphStepAD(t); + graph_ad.addPrior(TwistAccelKey(base_id, t), base_accel, + graph_builder.opt().a_cost_model); + Vector6 zero_vec6 = Vector6::Zero(); + if (express_redundancy) { + graph_ad.addPrior(ContactRedundancyKey(t), zero_vec6, + redundancy_model); + } + graph_ad = ConstVarGraph(graph_ad, known_values); + Values init_values_ad = SubValues(init_values_t, graph_ad.keys()); + LevenbergMarquardtOptimizer optimizer_ad(graph_ad, init_values_ad, lm_params); + auto results_ad = optimizer_ad.optimize(); + if (graph_ad.error(results_ad) > 1e-5) { + std::cout << "solving ad fails! error: " << graph_ad.error(results_ad) + << "\n"; + } + known_values.insert(results_ad); + + return known_values; +} + +/* ************************************************************************* */ +Values Vision60Robot::getInitValuesTrajectory( + const size_t num_steps, double dt, const Pose3 &base_pose_init, + const std::vector &des_poses, + std::vector &des_poses_t, + const std::string initialization_technique) const { + // Initialize solution. + Values init_vals; + Initializer initializer; + + // solve 1 step value + Values init_values_0 = getInitValuesStep(0, base_pose_init); + + // copy for all steps + if (initialization_technique == "zero") { + for (int t = 0; t <= num_steps; t++) { + for (const Key &key : init_values_0.keys()) { + init_vals.insert(key + t, init_values_0.at(key)); + } + } + } else if (initialization_technique == "interp") { + /// interpolate base pose + auto interp_values = initializer.InitializeSolutionInterpolationMultiPhase( + robot, "body", base_pose_init, des_poses, des_poses_t, dt, 0.0, + contact_points); + Values base_link_values; + for (int t = 0; t <= num_steps; t++) { + base_link_values.insert(PoseKey(base_id, t), + interp_values.at(PoseKey(base_id, t))); + } + /// compute base twist + Vector6 zero_vector6 = Vector6::Zero(); + base_link_values.insert(TwistKey(base_id, 0), zero_vector6); + for (int t = 0; t < num_steps; t++) { + Pose3 pose_prev = base_link_values.at(PoseKey(base_id, t)); + Pose3 pose_curr = base_link_values.at(PoseKey(base_id, t + 1)); + Pose3 pose_rel = pose_prev.inverse().compose(pose_curr); + Vector6 twist_interval = Pose3::Logmap(pose_rel) / dt; + Vector6 twist_prev = base_link_values.at(TwistKey(base_id, t)); + // Vector6 twist_curr = 2 * twist_interval - twist_prev; + Vector6 twist_curr = twist_interval; + base_link_values.insert(TwistKey(base_id, t + 1), twist_curr); + } + /// compute base accel + base_link_values.insert(TwistAccelKey(base_id, 0), zero_vector6); + for (int t = 0; t < num_steps; t++) { + Vector6 twist_prev = base_link_values.at(TwistKey(base_id, t)); + Vector6 twist_curr = + base_link_values.at(TwistKey(base_id, t + 1)); + Vector6 twist_rel = twist_curr - twist_prev; + Vector6 accel_interval = twist_rel / dt; + Vector6 accel_prev = + base_link_values.at(TwistAccelKey(base_id, t)); + // Vector6 accel_curr = 2 * accel_interval - accel_prev; + Vector6 accel_curr = accel_interval; + base_link_values.insert(TwistAccelKey(base_id, t + 1), accel_curr); + } + /// solve kinodynamics for each step + Values prev_values = init_values_0; + init_vals = init_values_0; + for (int t = 1; t <= num_steps; t++) { + Values init_values_t; + for (const Key &key : prev_values.keys()) { + init_values_t.insert(key + 1, prev_values.at(key)); + } + Values values_t = getInitValuesStep( + t, Pose(base_link_values, base_id, t), + Twist(base_link_values, base_id, t), + TwistAccel(base_link_values, base_id, t), init_values_t); + prev_values = values_t; + init_vals.insert(values_t); + + // std::cout << "pose: " << Pose(init_vals, base_id, t) << "\n"; + } + } + + // if (initialization_technique == "interp") + // // TODO(aescontrela): Figure out why the linearly interpolated initial + // // trajectory fails to optimize. My initial guess is that the optimizer + // has + // // a difficult time optimizing the trajectory when the initial solution + // lies + // // in the infeasible region. This would make sense if I were using an IPM + // to + // // solve this problem... + // init_vals = initializer.InitializeSolutionInterpolationMultiPhase( + // robot, "body", base_pose_init, des_poses, des_poses_t, dt, 0.0, + // contact_points); + // else if (initialization_technique == "zeros") + // init_vals = initializer.ZeroValuesTrajectory(robot, t_steps, 0, 0.0, + // contact_points); + // else if (initialization_technique == "inverse_kinematics") + // init_vals = initializer.InitializeSolutionInverseKinematics( + // robot, "body", base_pose_init, des_poses, des_poses_t, dt, 0.0, + // contact_points); + + return init_vals; +} + +/* ************************************************************************* */ +Values Vision60Robot::getNominalConfiguration(const double height) const { + Values qd_values; + qd_values.insert(PoseKey(base_id, 0), + Pose3(Rot3::Identity(), Point3(0, 0, height))); + for (const auto &joint : robot.joints()) { + qd_values.insert(JointAngleKey(joint->id(), 0), 0.0); + } + std::string base_name = "body"; + Values fk_values = robot.forwardKinematics(qd_values, 0, base_name); + return fk_values; +} + +/* ************************************************************************* */ +NonlinearFactorGraph Vision60Robot::collocationCosts(const int num_steps, + double dt) const { + NonlinearFactorGraph graph; + for (int t = 0; t < num_steps; t++) { + graph.add(gtdynamics::FixTimeTrapezoidalPoseCollocationFactor( + PoseKey(base_id, t), PoseKey(base_id, t + 1), TwistKey(base_id, t), + TwistKey(base_id, t + 1), dt, graph_builder.opt().pose_col_cost_model)); + graph.add(gtdynamics::FixTimeTrapezoidalTwistCollocationFactor( + TwistKey(base_id, t), TwistKey(base_id, t + 1), + TwistAccelKey(base_id, t), TwistAccelKey(base_id, t + 1), dt, + graph_builder.opt().twist_col_cost_model)); + } + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph Vision60Robot::minTorqueCosts(const int num_steps) const { + NonlinearFactorGraph graph; + for (int t = 0; t <= num_steps; t++) { + for (auto &&joint : robot.joints()) + graph.add(gtdynamics::MinTorqueFactor(TorqueKey(joint->id(), t), + min_torque_nm)); + } + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph Vision60Robot::frictionConeCosts( + const int num_steps) const { + NonlinearFactorGraph graph; + for (int t = 0; t <= num_steps; t++) { + for (auto &&cp : contact_points) { + int i = cp.link->id(); + auto wrench_key = ContactWrenchKey(i, 0, t); + + // Add contact dynamics constraints. + graph.emplace_shared( + PoseKey(i, t), wrench_key, opt().cfriction_cost_model, mu, gravity); + } + } + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph Vision60Robot::boundaryCosts( + const Pose3 &init_pose, const Vector6 &init_twist, + const std::vector &des_poses, const std::vector &des_poses_t, + double dt) const { + NonlinearFactorGraph graph; + + graph.addPrior(PoseKey(base_id, 0), init_pose, + noiseModel::Isotropic::Sigma(6, sigma_dynamics)); + graph.addPrior( + TwistKey(base_id, 0), init_twist, + noiseModel::Isotropic::Sigma(6, sigma_dynamics)); + for (size_t i = 0; i < des_poses.size(); i++) + graph.addPrior( + PoseKey(base_id, static_cast(std::ceil(des_poses_t[i] / dt))), + des_poses[i], des_pose_nm); + + return graph; +} + +/* ************************************************************************* */ +void Vision60Robot::printJointAngles(const Values &values, int t) const { + for (const auto &joint : robot.joints()) { + double q = JointAngle(values, joint->id(), t); + std::cout << joint->name() << "\t" << joint->parent()->name() << "\t" + << joint->child()->name() << "\t" << q << "\n"; + } +} + +/* ************************************************************************* */ +void Vision60Robot::exportTrajectory(const Values &results, + const size_t num_steps, + std::string file_path) { + const std::filesystem::path output_path(file_path); + if (!output_path.parent_path().empty()) { + std::filesystem::create_directories(output_path.parent_path()); + } + + // Log the joint angles, velocities, accels, torques, and current goal pose. + std::vector jnames; + for (auto &&joint : robot.joints()) jnames.push_back(joint->name()); + std::string jnames_str = ""; + for (size_t j = 0; j < jnames.size(); j++) { + jnames_str += jnames[j] + (j != jnames.size() - 1 ? "," : ""); + } + std::ofstream traj_file; + traj_file.open(output_path); + if (!traj_file.is_open()) { + throw std::runtime_error("Failed to open trajectory file: " + file_path); + } + // angles, vels, accels, torques. + traj_file << jnames_str << "," << jnames_str << "," << jnames_str << "," + << jnames_str << ",base_x" + << ",base_y" + << ",base_z" + << ",base_qx" + << ",base_qy" + << ",base_qz" + << ",base_qw" + << "\n"; + for (int t = 0; t <= num_steps; t++) { + std::vector vals; + for (auto &&joint : robot.joints()) + vals.push_back(std::to_string(JointAngle(results, joint->id(), t))); + for (auto &&joint : robot.joints()) + vals.push_back(std::to_string(JointVel(results, joint->id(), t))); + for (auto &&joint : robot.joints()) + vals.push_back(std::to_string(JointAccel(results, joint->id(), t))); + for (auto &&joint : robot.joints()) + vals.push_back(std::to_string(Torque(results, joint->id(), t))); + + Pose3 bp = Pose(results, base_id, t); + vals.push_back(std::to_string(bp.x())); + vals.push_back(std::to_string(bp.y())); + vals.push_back(std::to_string(bp.z())); + vals.push_back(std::to_string(bp.rotation().toQuaternion().x())); + vals.push_back(std::to_string(bp.rotation().toQuaternion().y())); + vals.push_back(std::to_string(bp.rotation().toQuaternion().z())); + vals.push_back(std::to_string(bp.rotation().toQuaternion().w())); + + std::string vals_str = ""; + for (size_t j = 0; j < vals.size(); j++) { + vals_str += vals[j] + (j != vals.size() - 1 ? "," : ""); + } + traj_file << vals_str << "\n"; + } + traj_file.close(); +} + +} // namespace gtsam diff --git a/examples/example_constraint_manifold/QuadrupedUtils.h b/examples/example_constraint_manifold/QuadrupedUtils.h new file mode 100644 index 000000000..52643b174 --- /dev/null +++ b/examples/example_constraint_manifold/QuadrupedUtils.h @@ -0,0 +1,195 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file QuadrupedUtils.h + * @brief Utilities for quadruped trajectory optimization experiments. + * @author: Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gtdynamics/dynamics/DynamicsGraph.h" +#include "gtdynamics/dynamics/OptimizerSetting.h" + +namespace gtdynamics { + +/// Key to represent contact redundancy. +inline DynamicsSymbol ContactRedundancyKey(int t = 0) { + return DynamicsSymbol::SimpleSymbol("CR", t); +} + +/** For a quadruped in standing phase, provided with the configuration, + * velocity, and accelerations, there are 6 degrees of freedom in dynamics. + * The Contact redundancy constraint imposes additional constraints to make + * the solution unique. */ +gtsam::Vector6_ ContactRedundancyConstraint(int t, + const std::vector &contact_ids, + const double &a, const double &b); +/** For a quadruped in standing phase, provided with the configuration, + * velocity, and accelerations, there are 6 degrees of freedom in dynamics. + * The Contact redundancy factor imposes additional costs to make the + * optimal solution unique. */ +gtsam::NoiseModelFactor::shared_ptr ContactRedundancyFactor( + int t, const std::vector &contact_ids, const double &a, + const double &b, const gtsam::noiseModel::Base::shared_ptr &cost_model, + bool express_redundancy = false); + +/// q-level contact factors with fixed contact points. +NonlinearFactorGraph contact_q_factors( + const int k, const PointOnLinks &contact_points, + const std::vector &contact_in_world, + const gtsam::noiseModel::Base::shared_ptr &cost_model); + +/** Function to manually define the basis keys for each constraint manifold. */ +KeyVector FindBasisKeys4C(const KeyVector &keys); + +/** Function to manually define the basis keys for each constraint manifold. */ +KeyVector findBasisKeysRedundancy(const KeyVector &keys); + +/// Class of utilities for Vision60 robot. +class Vision60Robot { + public: + Robot robot = CreateRobotFromFile(kUrdfPath + std::string("vision60.urdf")); + gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished(); + double mu = 2.0; + gtsam::Point3 contact_in_com = gtsam::Point3(0.14, 0, 0); + PointOnLinks contact_points{ + PointOnLink(robot.link("lower0"), contact_in_com), + PointOnLink(robot.link("lower1"), contact_in_com), + PointOnLink(robot.link("lower2"), contact_in_com), + PointOnLink(robot.link("lower3"), contact_in_com)}; + int lower0_id = robot.link("lower0")->id(); + int lower1_id = robot.link("lower1")->id(); + int lower2_id = robot.link("lower2")->id(); + int lower3_id = robot.link("lower3")->id(); + std::vector contact_ids{lower0_id, lower1_id, lower2_id, lower3_id}; + LinkSharedPtr base_link = robot.link("body"); + int base_id = base_link->id(); + + double nominal_height = Pose(getNominalConfiguration(), lower0_id, 0) + .transformFrom(contact_in_com) + .z() * + -1; + Values nominal_values = getNominalConfiguration(nominal_height); + + std::vector contact_in_world{ + Pose(nominal_values, lower0_id, 0).transformFrom(contact_in_com), + Pose(nominal_values, lower1_id, 0).transformFrom(contact_in_com), + Pose(nominal_values, lower2_id, 0).transformFrom(contact_in_com), + Pose(nominal_values, lower3_id, 0).transformFrom(contact_in_com)}; + double a = 0.5 * (contact_in_world.at(0).x() - contact_in_world.at(1).x()); + double b = 0.5 * (contact_in_world.at(0).y() - contact_in_world.at(2).y()); + + double sigma_dynamics = 1e-2; // Variance of dynamics constraints. + double sigma_objectives = 1e0; // Variance of additional objectives. + gtsam::SharedNoiseModel des_pose_nm = + gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + gtsam::SharedNoiseModel min_torque_nm = + gtsam::noiseModel::Isotropic::Sigma(1, 1e1); + gtsam::SharedNoiseModel cpoint_cost_model = + gtsam::noiseModel::Isotropic::Sigma(3, sigma_dynamics); + gtsam::SharedNoiseModel redundancy_model = + gtsam::noiseModel::Isotropic::Sigma(6, sigma_dynamics); + + DynamicsGraph graph_builder = DynamicsGraph(getOptSetting(), gravity); + + bool express_redundancy = false; + + protected: + OptimizerSetting getOptSetting() const; + + Values getNominalConfiguration(const double height = 0) const; + + NonlinearFactorGraph getConstraintsGraphStepQ(const int t) const; + + NonlinearFactorGraph getConstraintsGraphStepV(const int t) const; + + NonlinearFactorGraph getConstraintsGraphStepAD(const int t) const; + + public: + Vision60Robot() = default; + + /// Dynamics factors without friction cone factors (as they are moved to + /// costs). + NonlinearFactorGraph DynamicsFactors( + const int k, const std::optional &contact_points) const; + + /// Kinodynamic constraints at the specified time step. + NonlinearFactorGraph getConstraintsGraphStep(const int t) const; + + /// The constraints only consist of the kinodynamic constraints at each time + /// step. + NonlinearFactorGraph getConstraintsGraphTrajectory(const int num_steps) const; + + /// Return values of one step satisfying kinodynamic constraints. + Values getInitValuesStep( + const int t = 0, + const gtsam::Pose3 &base_pose = gtsam::Pose3(gtsam::Rot3::Identity(), + gtsam::Point3(0, 0, 0.0)), + const gtsam::Vector6 &base_twist = gtsam::Vector6::Zero(), + const gtsam::Vector6 &base_accel = gtsam::Vector6::Zero(), + Values init_values_t = Values()) const; + + /// Return values of trajectory satisfying kinodynamic constraints. + Values getInitValuesTrajectory( + const size_t num_steps, double dt, const gtsam::Pose3 &base_pose_init, + const std::vector &des_poses, + std::vector &des_poses_t, + const std::string initialization_technique = "interp") const; + + /// Costs for collocation across steps. + NonlinearFactorGraph collocationCosts(const int num_steps, double dt) const; + + /// Costs for min torque objectives. + NonlinearFactorGraph minTorqueCosts(const int num_steps) const; + + /// Costs for contact force within friction cone. + NonlinearFactorGraph frictionConeCosts(const int num_steps) const; + + /// Costs for init condition and reaching target poses. + NonlinearFactorGraph boundaryCosts(const gtsam::Pose3 &init_pose, + const gtsam::Vector6 &init_twist, + const std::vector &des_poses, + const std::vector &des_poses_t, + double dt) const; + + /// Print joint angles. + void printJointAngles(const Values &values, int t = 0) const; + + /// Return optimizer setting. + const OptimizerSetting &opt() const { return graph_builder.opt(); } + + /// Return function that select basis keys for constraint manifolds. + BasisKeyFunc getBasisKeyFunc() const { + if (express_redundancy) { + return &findBasisKeysRedundancy; + } else { + return &FindBasisKeys4C; + } + } + + /// Save trajectory to file. + void exportTrajectory(const Values &results, const size_t num_steps, + std::string file_path = "traj.csv"); +}; + +} // namespace gtdynamics diff --git a/examples/example_constraint_manifold/SerialChain.h b/examples/example_constraint_manifold/SerialChain.h new file mode 100644 index 000000000..e83979865 --- /dev/null +++ b/examples/example_constraint_manifold/SerialChain.h @@ -0,0 +1,176 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SerialChain.h + * @brief Serial chain kinematics as a manifold. By specifying the joint angles, + * we can compute poses of all the links. + * @author Yetong Zhang + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +template +/** Kinematic serial chain manifold, with the basis variables as the joint + angles. */ +class SerialChain { + protected: + std::shared_ptr robot_; + std::string base_name_; // name of base link + std::shared_ptr values_; + + public: + using VectorP = Eigen::Matrix; + enum { dimension = P }; + + /** Constructor. + * @param robot robot representing serial chain + * @param base_name name of base link + * @param base_pose pose of base link + * @param joint_angles angles of all the joints. + */ + SerialChain(const std::shared_ptr& robot, + const std::string& base_name, const Pose3& base_pose, + const Values& joint_angles) + : robot_(robot), + base_name_(base_name), + values_(std::make_shared(joint_angles)) { + values_ = std::make_shared( + robot_->forwardKinematics(*values_, 0, base_name_)); + } + + /** Constructor with new joint angles. */ + SerialChain

createWithNewAngles(const Values& new_joint_angles) const { + return SerialChain( + robot_, base_name_, + gtdynamics::Pose(*values_, robot_->link(base_name_)->id()), + new_joint_angles); + } + + /** return joint angle with Jacobian given joint id. */ + double jointById(const size_t& id, OptionalJacobian<1, P> H = {}) const { + if (H) { + H->setZero(); + (*H)(id - 1) = 1; + } + return gtdynamics::JointAngle(*values_, id); + } + + /** return joint angle with Jacobian given joint name. */ + double joint(const std::string& name, OptionalJacobian<1, P> H = {}) const { + return jointById(robot_->joint(name)->id(), H); + } + + /** return link pose with Jacobian given link name. */ + Pose3 linkPose(const std::string& name, OptionalJacobian<6, P> H = {}) const { + Pose3 wTe = gtdynamics::Pose(*values_, robot_->link(name)->id()); + + if (H) { + auto parent_link = robot_->link(base_name_); + auto joint = parent_link->joints().at(0); + // find all joints between base and this link + while (parent_link->name() != name) { + auto child_link = joint->otherLink(parent_link); + auto wTp = gtdynamics::Pose(*values_, parent_link->id()); + auto wTc = gtdynamics::Pose(*values_, child_link->id()); + + double q = gtdynamics::JointAngle(*values_, joint->id()); + Matrix J_wTc_q; + joint->poseOf(child_link, wTp, q, {}, J_wTc_q); + + if (child_link->name() == name) { + (*H).col(joint->id() - 1) = J_wTc_q; + break; + } else { + Pose3 cTe = wTc.inverse() * (wTe); + Matrix H_wTc; + wTc.compose(cTe, H_wTc, {}); + (*H).col(joint->id() - 1) = H_wTc * J_wTc_q; + } + + // next link and joint + parent_link = child_link; + if (joint == child_link->joints().at(0)) { + joint = child_link->joints().at(1); + } else { + joint = child_link->joints().at(0); + } + } + } + + return wTe; + } + + /// retraction with optional derivatives. + SerialChain retract(const VectorP& v, // TODO: use xi + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) const { + if (H1) { + *H1 = Matrix::Identity(P, P); + } + if (H2) { + *H2 = Matrix::Identity(P, P); + } + Values new_joint_angles; + for (const auto& joint : robot_->joints()) { + double q = gtdynamics::JointAngle(*values_, joint->id()); + q += v(joint->id() - 1); + gtdynamics::InsertJointAngle(&new_joint_angles, joint->id(), q); + } + return createWithNewAngles(new_joint_angles); + } + + /// localCoordinates with optional derivatives. + VectorP localCoordinates(const SerialChain& g, OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) const { + if (H1) { + *H1 = -Matrix::Identity(P, P); + } + if (H2) { + *H2 = Matrix::Identity(P, P); + } + VectorP v; + for (const auto& joint : robot_->joints()) { + double q1 = gtdynamics::JointAngle(*values_, joint->id()); + double q2 = gtdynamics::JointAngle(*(g.values_), joint->id()); + v(joint->id() - 1) = q2 - q1; + } + return v; + } + + /// print + void print(const std::string& s = "") const { + for (const auto& joint : robot_->joints()) { + std::cout << "joint: " << joint->name() + << "\tangle: " << gtdynamics::JointAngle(*values_, joint->id()) + << "\n"; + } + } + + /// equals + bool equals(const SerialChain& other, double tol = 1e-8) const { + for (const auto& joint : robot_->joints()) { + double q1 = gtdynamics::JointAngle(*values_, joint->id()); + double q2 = gtdynamics::JointAngle(*(other.values_), joint->id()); + if (abs(q2 - q1) > 1e-8) { + return false; + } + } + return true; + } +}; + +// Specialize SerialChain

traits to use a Retract/Local +template +struct traits> : internal::Manifold> {}; +} // namespace gtsam diff --git a/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp b/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp new file mode 100644 index 000000000..4099d5e85 --- /dev/null +++ b/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp @@ -0,0 +1,451 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file main_arm_kinematic_planning.cpp + * @brief KUKA arm kinematic trajectory benchmark using constrained optimization + * methods. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "SerialChain.h" + +using namespace gtsam; +using namespace gtdynamics; + +namespace { +constexpr size_t kNumSteps = 10; +const auto kJointNoise = noiseModel::Isotropic::Sigma(1, 1.0); +const auto kJointLimitNoise = noiseModel::Isotropic::Sigma(1, 1e-2); +const auto kTargetNoise = noiseModel::Isotropic::Sigma(6, 1e-2); +const std::string kSdfFile = "kuka_world.sdf"; +const std::string kRobotName = "lbr_iiwa"; +const std::string kBaseName = "lbr_iiwa_link_0"; +const std::string kEeName = "lbr_iiwa_link_7"; +const Pose3 kTargetPose = Pose3(Rot3(), Point3(0, 0.2, 1.0)); +const Pose3 kBasePose; +constexpr double kJointLimitLow = -3.14; +constexpr double kJointLimitHigh = 3.14; + +Robot& KukaRobot() { + static Robot robot = CreateRobotFromFile(kSdfPath + kSdfFile, kRobotName); + return robot; +} + +struct ArmBenchmarkArgs { + ConstrainedOptBenchmark::ParsedCli benchmark_cli; + bool cm_i_only = false; + bool cm_f_only = false; + bool skip_cm_f = false; +}; + +void PrintUsage(const char* program_name) { + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "arm"; + ConstrainedOptBenchmark::PrintUsage(std::cout, program_name, defaults); + std::cout << "Legacy compatibility options:\n" + << " --cm-i-only Run only CM(I) benchmark variant.\n" + << " --cm-f-only Run only CM(F) benchmark variant.\n" + << " --skip-cm-f Skip CM(F) in mixed run.\n"; +} + +ArmBenchmarkArgs ParseArgs(int argc, char** argv) { + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "arm"; + ArmBenchmarkArgs args{ + ConstrainedOptBenchmark::ParseCli(argc, argv, defaults)}; + + std::vector remaining; + for (const auto& arg : args.benchmark_cli.unknownArgs) { + if (arg == "--cm-i-only") { + args.cm_i_only = true; + } else if (arg == "--cm-f-only") { + args.cm_f_only = true; + } else if (arg == "--skip-cm-f") { + args.skip_cm_f = true; + } else { + remaining.push_back(arg); + } + } + args.benchmark_cli.unknownArgs = remaining; + + if (args.cm_i_only && args.cm_f_only) { + throw std::invalid_argument("Cannot combine --cm-i-only and --cm-f-only."); + } + + if (args.cm_i_only) { + args.benchmark_cli.runOptions.methods = { + ConstrainedOptBenchmark::Method::CM_I}; + } else if (args.cm_f_only) { + args.benchmark_cli.runOptions.methods = { + ConstrainedOptBenchmark::Method::CM_F}; + } else if (args.skip_cm_f) { + args.benchmark_cli.runOptions.methods.erase( + ConstrainedOptBenchmark::Method::CM_F); + args.benchmark_cli.runOptions.methods.insert( + ConstrainedOptBenchmark::Method::CM_I); + } + + return args; +} +} // namespace + +/** Functor version of JointLimitFactor, for creating expressions. Compute error + * for joint limit error, to reproduce joint limit factor in expressions. */ +class JointLimitFunctor { + protected: + double low_, high_; + + public: + JointLimitFunctor(const double& low, const double& high) + : low_(low), high_(high) {} + + double operator()(const double& q, + OptionalJacobian<1, 1> H_q = nullptr) const { + if (q < low_) { + if (H_q) *H_q = -I_1x1; + return low_ - q; + } else if (q <= high_) { + if (H_q) *H_q = Z_1x1; + return 0.0; + } else { + if (H_q) *H_q = I_1x1; + return q - high_; + } + } +}; + +/** Function to manually define the basis keys for each constraint manifold. */ +KeyVector FindBasisKeys(const KeyVector& keys) { + KeyVector basis_keys; + for (const Key& key : keys) { + auto symbol = DynamicsSymbol(key); + if (symbol.label() == "q") { + basis_keys.push_back(key); + } + } + return basis_keys; +} + +/** Factor graph of all kinematic constraints. Include kinematic constraints at + * each time step, and the priors for the first step. */ +NonlinearFactorGraph get_constraints_graph() { + auto& robot = KukaRobot(); + NonlinearFactorGraph constraints_graph; + // kinematics constraints at each step + auto graph_builder = DynamicsGraph(); + for (size_t k = 0; k <= kNumSteps; k++) { + constraints_graph.add(graph_builder.qFactors(robot, k)); + } + + // // prior constraints at first step + size_t k0 = 0; + for (const auto& joint : robot.joints()) { + constraints_graph.addPrior(JointAngleKey(joint->id(), k0), 0.0, + graph_builder.opt().prior_q_cost_model); + } + return constraints_graph; +} + +/** Cost function for planning, includes cost of rotation joints, joint limit + * costs, and cost for reaching target pose at end-effector. */ +NonlinearFactorGraph get_costs() { + auto& robot = KukaRobot(); + NonlinearFactorGraph costs; + // rotation costs + NonlinearFactorGraph rotation_costs; + for (size_t k = 0; k < kNumSteps; k++) { + for (const auto& joint : robot.joints()) { + rotation_costs.emplace_shared>( + JointAngleKey(joint->id(), k), JointAngleKey(joint->id(), k + 1), 0.0, + kJointNoise); + } + } + // joint limit costs; + NonlinearFactorGraph joint_limit_costs; + for (size_t k = 0; k <= kNumSteps; k++) { + for (const auto& joint : robot.joints()) { + joint_limit_costs.emplace_shared( + JointAngleKey(joint->id(), k), kJointLimitNoise, kJointLimitLow, + kJointLimitHigh, 0.0); + } + } + // target cost + NonlinearFactorGraph target_costs; + const auto& ee_link = robot.link(kEeName); + costs.addPrior(PoseKey(ee_link->id(), kNumSteps), kTargetPose, kTargetNoise); + costs.add(rotation_costs); + costs.add(target_costs); + costs.add(joint_limit_costs); + return costs; +} + +/** Initial values specified by 0 for all joint angles. */ +Values get_init_values() { + auto& robot = KukaRobot(); + Values init_values; + Values fk_values; + size_t k0 = 0; + for (const auto& joint : robot.joints()) { + InsertJointAngle(&fk_values, joint->id(), k0, 0.0); + } + fk_values = robot.forwardKinematics(fk_values, k0); + // fk_values.print("", GTDKeyFormatter); + for (size_t k = 0; k <= kNumSteps; k++) { + for (const auto& joint : robot.joints()) { + double angle = JointAngle(fk_values, joint->id(), k0); + InsertJointAngle(&init_values, joint->id(), k, angle); + } + for (const auto& link : robot.links()) { + Pose3 pose = Pose(fk_values, link->id(), k0); + InsertPose(&init_values, link->id(), k, pose); + } + } + return init_values; +} + +/** Initial values of serial chain specified by 0 for all joint angles. */ +Values get_init_values_sc() { + auto& robot = KukaRobot(); + Values init_values; + auto shared_robot = std::make_shared(robot); + Values joint_angles; + for (const auto& joint : robot.joints()) { + gtdynamics::InsertJointAngle(&joint_angles, joint->id(), 0.0); + } + for (size_t k = 1; k <= kNumSteps; k++) { + Key sc_key = k; + init_values.insert(sc_key, SerialChain<7>(shared_robot, kBaseName, + kBasePose, joint_angles)); + } + return init_values; +} + +/** Same cost function, but imposed on the serial chain manifold. */ +NonlinearFactorGraph get_costs_sc() { + auto& robot = KukaRobot(); + NonlinearFactorGraph costs; + // rotation costs + Expression> state1(1); + for (const auto& joint : robot.joints()) { + auto joint_func = std::bind(&SerialChain<7>::joint, std::placeholders::_1, + joint->name(), std::placeholders::_2); + Expression curr_joint(joint_func, state1); + costs.addExpressionFactor(kJointNoise, 0.0, curr_joint); + } + for (size_t k = 1; k < kNumSteps; k++) { + Key curr_key = k; + Key next_key = k + 1; + Expression> curr_state(curr_key); + Expression> next_state(next_key); + for (const auto& joint : robot.joints()) { + auto joint_func = std::bind(&SerialChain<7>::joint, std::placeholders::_1, + joint->name(), std::placeholders::_2); + Expression curr_joint(joint_func, curr_state); + Expression next_joint(joint_func, next_state); + Expression joint_rotation_expr = next_joint - curr_joint; + costs.addExpressionFactor(kJointNoise, 0.0, joint_rotation_expr); + } + } + + // joint limit costs + JointLimitFunctor joint_limit_functor(kJointLimitLow, kJointLimitHigh); + for (size_t k = 1; k <= kNumSteps; k++) { + Key curr_key = k; + Expression> curr_state(curr_key); + for (const auto& joint : robot.joints()) { + auto joint_func = std::bind(&SerialChain<7>::joint, std::placeholders::_1, + joint->name(), std::placeholders::_2); + Expression curr_joint(joint_func, curr_state); + Expression joint_limit_error(joint_limit_functor, curr_joint); + costs.addExpressionFactor(kJointLimitNoise, 0.0, + joint_limit_error); + } + } + + // target costs + Key last_key = kNumSteps; + Expression> last_state(last_key); + auto pose_func = std::bind(&SerialChain<7>::linkPose, std::placeholders::_1, + kEeName, std::placeholders::_2); + Expression target_pose_expr(pose_func, last_state); + costs.addExpressionFactor(kTargetNoise, kTargetPose, target_pose_expr); + return costs; +} + +/** Kinematic trajectory optimization using manually defined serial chain + * manifold. */ +Values optimize_serial_chain_manifold(const NonlinearFactorGraph& costs, + const Values& init_values) { + LevenbergMarquardtParams params; + params.setVerbosityLM("SUMMARY"); + size_t graph_dim = 0; + for (const auto& factor : costs) { + graph_dim += factor->dim(); + } + std::cout << "dimension: " << graph_dim << " x " << init_values.dim() << "\n"; + LevenbergMarquardtOptimizer optimizer(costs, init_values, params); + gttic_(serial_chain_manifold); + auto result = optimizer.optimize(); + gttoc_(serial_chain_manifold); + return result; +} + +/** Print joint angles for all steps. */ +void print_joint_angles(const Values& values) { + auto& robot = KukaRobot(); + for (size_t k = 0; k <= kNumSteps; k++) { + std::cout << "step " << k << ":"; + for (const auto& joint : robot.joints()) { + double angle = JointAngle(values, joint->id(), k); + std::cout << "\t" << angle; + } + std::cout << "\n"; + } +} + +/** Print joint angles of serial chain for all steps. */ +void print_joint_angles_sc(const Values& values) { + auto& robot = KukaRobot(); + for (size_t k = 1; k <= kNumSteps; k++) { + Key state_key = k; + auto state = values.at>(state_key); + std::cout << "step " << k << ":"; + for (const auto& joint : robot.joints()) { + double angle = state.joint(joint->name()); + std::cout << "\t" << angle; + } + std::cout << "\n"; + } +} + +void ExportJointAnglesCsv(const Values& values, const std::string& file_path) { + auto& robot = KukaRobot(); + std::ofstream out(file_path); + if (!out.is_open()) { + throw std::runtime_error("Failed to open trajectory file: " + file_path); + } + + out << "step"; + for (const auto& joint : robot.joints()) { + out << "," << joint->name(); + } + out << "\n"; + + for (size_t k = 0; k <= kNumSteps; ++k) { + out << k; + for (const auto& joint : robot.joints()) { + out << "," << JointAngle(values, joint->id(), k); + } + out << "\n"; + } +} + +/** Benchmark constrained optimizers on a KUKA arm kinematic planning problem. + */ +void kinematic_planning(const ArmBenchmarkArgs& args) { + auto& robot = KukaRobot(); + robot.fixLink(kBaseName); + auto constraints_graph = get_constraints_graph(); + auto costs = get_costs(); + auto init_values = get_init_values(); + auto constraints = + gtsam::NonlinearEqualityConstraints::FromCostGraph(constraints_graph); + + auto runOptions = args.benchmark_cli.runOptions; + runOptions.constraintUnitScale = 1.0; + runOptions.softMu = 1.0; + runOptions.cmFRetractorMaxIterations = 10; + runOptions.cmIRetractorMaxIterations = 1; + + if (args.cm_i_only) { + std::cout << "[BENCH] I-only mode enabled.\n"; + } else if (args.cm_f_only) { + std::cout << "[BENCH] F-only mode enabled.\n"; + } else if (args.skip_cm_f) { + std::cout << "[BENCH] Skipping CM(F); running CM(I) with other methods.\n"; + } + + LevenbergMarquardtParams baseLmParams; + ConstrainedOptBenchmark runner(runOptions); + runner.setProblemFactory( + [=]() { return EConsOptProblem(costs, constraints, init_values); }); + runner.setOuterLmBaseParams(baseLmParams); + runner.setOuterLmConfig([&](ConstrainedOptBenchmark::Method method, + LevenbergMarquardtParams* params) { + if (args.cm_i_only || + (args.skip_cm_f && method == ConstrainedOptBenchmark::Method::CM_I)) { + params->linearSolverType = + gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY; + params->setMaxIterations(20); + params->relativeErrorTol = 1e-3; + params->setlambdaUpperBound(1e2); + } + if (args.cm_f_only) { + params->linearSolverType = + gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY; + params->setMaxIterations(30); + params->relativeErrorTol = 1e-3; + params->setlambdaUpperBound(1e2); + } + }); + runner.setMoptFactory([](ConstrainedOptBenchmark::Method) { + auto moptParams = + ConstrainedOptBenchmark::DefaultMoptParamsSV(&FindBasisKeys); + auto* retractLm = + &moptParams.cc_params->retractor_creator->params()->lm_params; + retractLm->linearSolverType = + gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY; + retractLm->setlambdaUpperBound(1e2); + return moptParams; + }); + runner.setResultCallback( + [&](ConstrainedOptBenchmark::Method method, const Values& result) { + ExportJointAnglesCsv(result, ConstrainedOptBenchmark::MethodDataPath( + runOptions, method, "_traj.csv")); + }); + + ExportJointAnglesCsv( + init_values, std::string(kDataPath) + runOptions.id + "_init_traj.csv"); + + std::ostringstream latexOs; + runner.run(latexOs); + std::cout << latexOs.str(); +} + +int main(int argc, char** argv) { + try { + const ArmBenchmarkArgs args = ParseArgs(argc, argv); + if (!args.benchmark_cli.unknownArgs.empty()) { + throw std::invalid_argument("Unknown option: " + + args.benchmark_cli.unknownArgs.front()); + } + kinematic_planning(args); + return 0; + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + PrintUsage(argv[0]); + return 1; + } +} diff --git a/examples/example_constraint_manifold/main_cablerobot.cpp b/examples/example_constraint_manifold/main_cablerobot.cpp new file mode 100644 index 000000000..a17ede1bb --- /dev/null +++ b/examples/example_constraint_manifold/main_cablerobot.cpp @@ -0,0 +1,185 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file main_cablerobot.cpp + * @brief Cable robot kinematic trajectory benchmark using constrained + * optimization methods. + * @author Yetong Zhang + * @author Gerry Chen + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace gtsam; +using namespace gtdynamics; + +namespace { +constexpr size_t kNumSteps = 10; +const auto kJointNoise = noiseModel::Isotropic::Sigma(1, 1.0); +const auto kJointLimitNoise = noiseModel::Isotropic::Sigma(1, 1e-2); +const auto kTargetNoise = noiseModel::Isotropic::Sigma(6, 1e-2); + +constexpr double kJointLimitLow = 0.0; +constexpr double kJointLimitHigh = 3.6; + +const Key kEeId = 1; +const Pose3 kTargetPose = Pose3(Rot3(), Point3(0.2, 0.5, 0.0)); + +} // namespace + +namespace { +const double kCdprWidth = 3, kCdprHeight = 2; +const std::array kCdprFrameMountPoints = { + Point3{kCdprWidth, 0, 0}, // + Point3{kCdprWidth, kCdprHeight, 0}, Point3{0, kCdprHeight, 0}, + Point3{0, 0, 0}}; +const double kCdprEeWidth = 0.1, kCdprEeHeight = 0.1; +const std::array kCdprEeMountPoints = { + Point3{kCdprEeWidth, 0, 0}, Point3{kCdprEeWidth, kCdprEeHeight, 0}, + Point3{0, kCdprEeHeight, 0}, Point3{0, 0, 0}}; +} // namespace + +/** Factor graph of all kinematic constraints. */ +NonlinearFactorGraph get_constraints_graph() { + NonlinearFactorGraph constraints_graph; + auto graph_builder = DynamicsGraph(); // + // kinematics constraints at each step + for (size_t k = 0; k <= kNumSteps; ++k) { + for (size_t ci = 0; ci < 4; ++ci) { + constraints_graph.emplace_shared( + JointAngleKey(ci, k), PoseKey(kEeId, k), + gtsam::noiseModel::Isotropic::Sigma(1, 0.001), + kCdprFrameMountPoints[ci], kCdprEeMountPoints[ci]); + } + } + // constraints_graph.print("constraints_graph", GTDKeyFormatter); + // // prior constraints at first step + constraints_graph.addPrior(PoseKey(kEeId, 0), + Pose3(Rot3(), Point3(1.5, 1.0, 0)), + gtsam::noiseModel::Isotropic::Sigma(6, 0.001)); + return constraints_graph; +} + +/** Build joint rotation, limit, and terminal pose costs. */ +NonlinearFactorGraph get_costs() { + NonlinearFactorGraph costs; + // rotation costs + NonlinearFactorGraph rotation_costs; + for (size_t k = 0; k < kNumSteps; k++) { + for (size_t ci = 0; ci < 4; ++ci) { + rotation_costs.emplace_shared>( + JointAngleKey(ci, k), JointAngleKey(ci, k + 1), 0.0, kJointNoise); + } + } + // joint limit costs; + NonlinearFactorGraph joint_limit_costs; + for (size_t k = 0; k <= kNumSteps; k++) { + for (size_t ci = 0; ci < 4; ++ci) { + joint_limit_costs.emplace_shared( + JointAngleKey(ci, k), kJointLimitNoise, kJointLimitLow, + kJointLimitHigh, 0.0); + } + } + // target cost + NonlinearFactorGraph target_costs; + costs.addPrior(PoseKey(kEeId, kNumSteps), kTargetPose, kTargetNoise); + costs.add(rotation_costs); + costs.add(target_costs); + costs.add(joint_limit_costs); + return costs; +} + +/** Build an initial guess for cable lengths and end-effector pose. */ +Values get_init_values() { + Values initValues; + for (size_t k = 0; k <= kNumSteps; k++) { + for (size_t ci = 0; ci < 4; ++ci) { + InsertJointAngle(&initValues, ci, k, 1.8); + } + InsertPose(&initValues, kEeId, k, Pose3(Rot3(), Point3(1.5, 1.0, 0))); + } + return initValues; +} + +/** Print joint angles for all steps. */ +void print_joint_angles(const Values& values) { + for (size_t k = 0; k <= kNumSteps; k++) { + std::cout << "step " << k << ": "; + for (size_t ci = 0; ci < 4; ++ci) { + double angle = JointAngle(values, ci, k); + std::cout << "\t" << angle; + } + std::cout << "\n"; + } +} + +/** Compare simple kinematic planning tasks of a cable robot using (1) dynamics + * factor graph (2) constraint manifold */ +void kinematic_planning(const ConstrainedOptBenchmark::Options& runOptions) { + // Create constrained optimization problem. + auto constraintsGraph = get_constraints_graph(); + auto costs = get_costs(); + auto initValues = get_init_values(); + auto constraints = + gtsam::NonlinearEqualityConstraints::FromCostGraph(constraintsGraph); + auto problem = EConsOptProblem(costs, constraints, initValues); + + LevenbergMarquardtParams lmParams; + lmParams.linearSolverType = gtsam::NonlinearOptimizerParams::MULTIFRONTAL_QR; + ConstrainedOptBenchmark runner(runOptions); + runner.setProblemFactory( + [=]() { return EConsOptProblem(costs, constraints, initValues); }); + runner.setOuterLmBaseParams(lmParams); + runner.setMoptFactory([](ConstrainedOptBenchmark::Method) { + auto moptParams = ConstrainedOptBenchmark::DefaultMoptParams(); + moptParams.cc_params->retractor_creator->params() + ->lm_params.linearSolverType = + gtsam::NonlinearOptimizerParams::MULTIFRONTAL_QR; + moptParams.cc_params->retractor_creator->params() + ->lm_params.setlambdaUpperBound(1e2); + return moptParams; + }); + + std::ostringstream latexOs; + runner.run(latexOs); + std::cout << latexOs.str(); +} + +int main(int argc, char** argv) { + try { + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "cable_robot"; + auto parsed = ConstrainedOptBenchmark::ParseCli(argc, argv, defaults); + if (!parsed.unknownArgs.empty()) { + throw std::invalid_argument("Unknown option: " + + parsed.unknownArgs.front()); + } + kinematic_planning(parsed.runOptions); + return 0; + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "cable_robot"; + ConstrainedOptBenchmark::PrintUsage(std::cerr, argv[0], defaults); + return 1; + } +} diff --git a/examples/example_constraint_manifold/main_cartpole.cpp b/examples/example_constraint_manifold/main_cartpole.cpp new file mode 100644 index 000000000..c4d7633a4 --- /dev/null +++ b/examples/example_constraint_manifold/main_cartpole.cpp @@ -0,0 +1,149 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file main_cartpole.cpp + * @brief Dynamic trajectory planning problem of rotating up a cart-pole. + * @author Yetong Zhang + */ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "CartPoleUtils.h" + +using namespace gtsam; +using namespace gtdynamics; + +namespace { +constexpr double kConstraintUnitScale = 1e-3; +constexpr double kHorizonTime = 2.0; +constexpr double kDt = 1.0 / 100.0; + +struct CartPoleArgs { + ConstrainedOptBenchmark::ParsedCli benchmark_cli; +}; + +void PrintUsage(const char* program_name) { + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "cartpole"; + defaults.enableNumSteps = true; + defaults.defaultNumSteps = static_cast(std::ceil(kHorizonTime / kDt)); + defaults.defaultMethods = {ConstrainedOptBenchmark::Method::CM_I}; + ConstrainedOptBenchmark::PrintUsage(std::cout, program_name, defaults); +} + +CartPoleArgs ParseArgs(int argc, char** argv) { + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "cartpole"; + defaults.enableNumSteps = true; + defaults.defaultNumSteps = static_cast(std::ceil(kHorizonTime / kDt)); + defaults.defaultMethods = {ConstrainedOptBenchmark::Method::CM_I}; + return CartPoleArgs{ConstrainedOptBenchmark::ParseCli(argc, argv, defaults)}; +} +} // namespace + +/** Cart-pole dynamic planning benchmark with shared constrained optimizer + * runner. */ +void dynamic_planning(size_t numSteps, + const ConstrainedOptBenchmark::Options& runOptions) { + CartPole cartpole; + + NonlinearFactorGraph dynamicConstraintsGraph = + cartpole.getDynamicsGraph(numSteps); + NonlinearFactorGraph unactuatedGraph = cartpole.getUnactuatedGraph(numSteps); + NonlinearFactorGraph initStateGraph = cartpole.initStateGraph(); + + NonlinearFactorGraph finalStateGraph = cartpole.finalStateGraph(numSteps); + NonlinearFactorGraph collocationCosts = + cartpole.getCollocation(numSteps, kDt); + NonlinearFactorGraph minTorqueCosts = cartpole.minTorqueCosts(numSteps); + + NonlinearFactorGraph constraintsGraph; + constraintsGraph.add(dynamicConstraintsGraph); + constraintsGraph.add(unactuatedGraph); + constraintsGraph.add(initStateGraph); + + NonlinearFactorGraph costs; + costs.add(finalStateGraph); + costs.add(collocationCosts); + costs.add(minTorqueCosts); + + auto evaluateCosts = [=](const Values& values) { + std::cout << "collocation costs:\t" << collocationCosts.error(values) + << "\n"; + std::cout << "final state costs:\t" << finalStateGraph.error(values) + << "\n"; + std::cout << "min torque costs:\t" << minTorqueCosts.error(values) << "\n"; + }; + + auto initValues = cartpole.getInitValues(numSteps, "zero"); + auto constraints = + gtsam::NonlinearEqualityConstraints::FromCostGraph(constraintsGraph); + + evaluateCosts(initValues); + cartpole.exportTrajectory( + initValues, numSteps, kDt, + std::string(kDataPath) + runOptions.id + "_init_traj.csv"); + + LevenbergMarquardtParams lmParams; + lmParams.setlambdaUpperBound(1e10); + lmParams.setMaxIterations(30); + + ConstrainedOptBenchmark runner(runOptions); + runner.setProblemFactory( + [=]() { return EConsOptProblem(costs, constraints, initValues); }); + runner.setOuterLmBaseParams(lmParams); + runner.setMoptFactory([&](ConstrainedOptBenchmark::Method) { + auto moptParams = ConstrainedOptBenchmark::DefaultMoptParamsSV( + cartpole.getBasisKeyFunc(true)); + auto retractorParams = moptParams.cc_params->retractor_creator->params(); + retractorParams->check_feasible = true; + retractorParams->lm_params.linearSolverType = + gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY; + return moptParams; + }); + runner.setResultCallback( + [&](ConstrainedOptBenchmark::Method method, const Values& result) { + evaluateCosts(result); + cartpole.exportTrajectory(result, numSteps, kDt, + ConstrainedOptBenchmark::MethodDataPath( + runOptions, method, "_traj.csv")); + }); + + std::ostringstream latexOs; + runner.run(latexOs); + std::cout << latexOs.str(); +} + +int main(int argc, char** argv) { + try { + const CartPoleArgs args = ParseArgs(argc, argv); + if (!args.benchmark_cli.unknownArgs.empty()) { + throw std::invalid_argument("Unknown option: " + + args.benchmark_cli.unknownArgs.front()); + } + + auto runOptions = args.benchmark_cli.runOptions; + runOptions.constraintUnitScale = kConstraintUnitScale; + + std::cout << "Using num_steps=" << args.benchmark_cli.numSteps << "\n"; + dynamic_planning(args.benchmark_cli.numSteps, runOptions); + return 0; + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + PrintUsage(argv[0]); + return 1; + } +} diff --git a/examples/example_constraint_manifold/main_connected_poses.cpp b/examples/example_constraint_manifold/main_connected_poses.cpp new file mode 100644 index 000000000..d59a9741a --- /dev/null +++ b/examples/example_constraint_manifold/main_connected_poses.cpp @@ -0,0 +1,312 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file main_connected_poses.cpp + * @brief Two connected Pose2 trajectories benchmarked with constrained + * optimization methods. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtdynamics; +using gtsam::symbol_shorthand::A, gtsam::symbol_shorthand::B; + +namespace { +constexpr size_t kNumSteps = 100; +constexpr double kConstraintUnitScale = 1.0; +const auto kConstraintNoise = noiseModel::Isotropic::Sigma(3, 1e0); +const auto kPriorNoise = noiseModel::Isotropic::Sigma(3, 1e-1); +const auto kOdoNoise = noiseModel::Isotropic::Sigma(3, 1e-1); +const Vector kOdoSigma = (Vector(3) << 0.1, 0.1, 0.1).finished(); + +Sampler& OdoSampler() { + static Sampler sampler(noiseModel::Diagonal::Sigmas(kOdoSigma)); + return sampler; +} + +struct ConnectedPosesArgs { + ConstrainedOptBenchmark::ParsedCli benchmark_cli; + bool debug_cm_components = false; +}; + +void PrintUsage(const char* program_name) { + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "connected_poses"; + ConstrainedOptBenchmark::PrintUsage(std::cout, program_name, defaults); + std::cout + << "Example-specific options:\n" + << " --debug-cm-components Print CM component/manifold diagnostics.\n"; +} + +ConnectedPosesArgs ParseArgs(int argc, char** argv) { + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "connected_poses"; + + ConnectedPosesArgs args{ + ConstrainedOptBenchmark::ParseCli(argc, argv, defaults)}; + std::vector remaining; + for (const auto& arg : args.benchmark_cli.unknownArgs) { + if (arg == "--debug-cm-components") { + args.debug_cm_components = true; + } else { + remaining.push_back(arg); + } + } + args.benchmark_cli.unknownArgs = remaining; + return args; +} +} // namespace + +Pose2 add_noise(const Pose2& pose, Sampler& sampler) { + auto xi = sampler.sample(); + return pose.expmap(xi); +} + +/** Build the connected-pose constraints between trajectories A and B. */ +NonlinearFactorGraph get_constraints_graph(const Values& gt) { + NonlinearFactorGraph constraints_graph; + + for (size_t k = 0; k <= kNumSteps; k++) { + Pose2 pose_1 = gt.at(A(k)); + Pose2 pose_2 = gt.at(B(k)); + Pose2 rel_pose = pose_1.inverse() * pose_2; + constraints_graph.emplace_shared>(A(k), B(k), rel_pose, + kConstraintNoise); + } + + return constraints_graph; +} + +double EvaluatePoseError(const Values& gt, const Values& result) { + double error1 = 0; + double error2 = 0; + for (size_t k = 1; k <= kNumSteps; k++) { + { + Pose2 gt_pose = gt.at(A(k)); + Pose2 est_pose = result.at(A(k)); + Pose2 rel_pose = est_pose.inverse().compose(gt_pose); + Matrix3 diff = rel_pose.matrix() - I_3x3; + error1 += pow(diff.norm(), 2); + } + { + Pose2 gt_pose = gt.at(B(k)); + Pose2 est_pose = result.at(B(k)); + Pose2 rel_pose = est_pose.inverse().compose(gt_pose); + Matrix3 diff = rel_pose.matrix() - I_3x3; + error2 += pow(diff.norm(), 2); + } + } + std::cout << sqrt(error1 / kNumSteps) << "\t" << sqrt(error2 / kNumSteps) + << "\n"; + return sqrt(error1 / kNumSteps) + sqrt(error2 / kNumSteps); +} + +void ExportTrajectoryCsv(const Values& values, const std::string& file_path) { + std::ofstream out(file_path); + if (!out.is_open()) { + throw std::runtime_error("Failed to open trajectory file: " + file_path); + } + out << "t,ax,ay,ath,bx,by,bth\n"; + for (size_t k = 0; k <= kNumSteps; ++k) { + const Pose2 a = values.at(A(k)); + const Pose2 b = values.at(B(k)); + out << k << "," << a.x() << "," << a.y() << "," << a.theta() << "," << b.x() + << "," << b.y() << "," << b.theta() << "\n"; + } +} + +std::vector> GetOdoMeasurements(const Values& gt) { + std::vector> odometryMeasurements; + for (size_t k = 0; k < kNumSteps; k++) { + Pose2 pose1_curr = gt.at(A(k)); + Pose2 pose1_next = gt.at(A(k + 1)); + Pose2 rel_pose_1 = pose1_curr.inverse() * pose1_next; + Pose2 pose2_curr = gt.at(B(k)); + Pose2 pose2_next = gt.at(B(k + 1)); + Pose2 rel_pose_2 = pose2_curr.inverse() * pose2_next; + rel_pose_1 = add_noise(rel_pose_1, OdoSampler()); + rel_pose_2 = add_noise(rel_pose_2, OdoSampler()); + odometryMeasurements.push_back(std::vector{rel_pose_1, rel_pose_2}); + } + return odometryMeasurements; +} + +/** Build priors and odometry costs for both connected trajectories. */ +NonlinearFactorGraph get_costs( + const Values& gt, + const std::vector>& odometryMeasurements) { + NonlinearFactorGraph costs; + + costs.emplace_shared>(A(0), gt.at(A(0)), + kPriorNoise); + costs.emplace_shared>(B(0), gt.at(B(0)), + kPriorNoise); + + for (size_t k = 0; k < kNumSteps; k++) { + costs.emplace_shared>( + A(k), A(k + 1), odometryMeasurements[k][0], kOdoNoise); + costs.emplace_shared>( + B(k), B(k + 1), odometryMeasurements[k][1], kOdoNoise); + } + return costs; +} + +Values get_gt_values() { + Values gt; + for (size_t k = 0; k <= kNumSteps; k++) { + gt.insert(A(k), Pose2(k * 0.1, 5.0, k * 0.1)); + gt.insert(B(k), Pose2(k * 0.1, -5.0, -k * 0.1)); + } + return gt; +} + +/** Build initial values by integrating the noisy odometry from the initial + * pose. */ +Values get_init_values( + const Values& gt, + const std::vector>& odometryMeasurements) { + Values init_values; + init_values.insert(A(0), gt.at(A(0))); + init_values.insert(B(0), gt.at(B(0))); + for (size_t k = 1; k <= kNumSteps; k++) { + Pose2 rel_pose1 = odometryMeasurements[k - 1][0]; + Pose2 rel_pose2 = odometryMeasurements[k - 1][1]; + Pose2 pose_1 = init_values.at(A(k - 1)).compose(rel_pose1); + Pose2 pose_2 = init_values.at(B(k - 1)).compose(rel_pose2); + init_values.insert(A(k), pose_1); + init_values.insert(B(k), pose_2); + } + return init_values; +} + +void PrintCMComponentDebug(const EConsOptProblem& problem, + const ManifoldOptimizerParameters& mopt_params, + const LevenbergMarquardtParams& lm_params, + bool debug_enabled) { + if (!debug_enabled) return; + + std::cout << "[CM DEBUG] ===== start component/manifold dump =====\n"; + std::cout << "[CM DEBUG] constraints dim: " << problem.constraintsDimension() + << ", costs dim: " << problem.costsDimension() + << ", values dim: " << problem.valuesDimension() << "\n"; + + NonlinearMOptimizer debug_optimizer(mopt_params, lm_params); + auto mopt_problem = debug_optimizer.initializeMoptProblem( + problem.costs(), problem.constraints(), problem.initValues()); + + mopt_problem.print("[CM DEBUG] "); + + std::map key_component_map; + for (const Key& cm_key : mopt_problem.manifold_keys_) { + const auto& cm = mopt_problem.values_.at(cm_key).cast(); + for (const Key& base_key : cm.values().keys()) { + key_component_map[base_key] = cm_key; + } + } + for (const Key& cm_key : mopt_problem.fixed_manifolds_.keys()) { + const auto& cm = + mopt_problem.fixed_manifolds_.at(cm_key).cast(); + for (const Key& base_key : cm.values().keys()) { + key_component_map[base_key] = cm_key; + } + } + + std::cout << "[CM DEBUG] base_key -> manifold_key mapping:\n"; + for (const auto& it : key_component_map) { + std::cout << "[CM DEBUG] " << DefaultKeyFormatter(it.first) << " -> " + << DefaultKeyFormatter(it.second) << "\n"; + } + std::cout << "[CM DEBUG] ===== end component/manifold dump =====\n"; +} + +/** Benchmark constrained optimizers on the connected-pose estimation problem. + */ +void kinematic_planning(const ConnectedPosesArgs& args) { + // Create constrained optimization problem. + auto gt = get_gt_values(); + auto constraintsGraph = get_constraints_graph(gt); + std::vector> odoMeasurements = GetOdoMeasurements(gt); + auto costs = get_costs(gt, odoMeasurements); + auto initValues = get_init_values(gt, odoMeasurements); + auto constraints = + gtsam::NonlinearEqualityConstraints::FromCostGraph(constraintsGraph); + auto problem = EConsOptProblem(costs, constraints, initValues); + + LevenbergMarquardtParams lmParams; + lmParams.setlambdaUpperBound(1e10); + + std::cout << "pose error: " << EvaluatePoseError(gt, initValues) << "\n"; + + auto runOptions = args.benchmark_cli.runOptions; + runOptions.constraintUnitScale = kConstraintUnitScale; + runOptions.softMu = 1e4; + + auto moptFactory = [](ConstrainedOptBenchmark::Method) { + auto moptParams = ConstrainedOptBenchmark::DefaultMoptParams(); + moptParams.cc_params->retractor_creator->params() + ->lm_params.linearSolverType = + gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY; + return moptParams; + }; + + if (args.debug_cm_components && + (runOptions.methods.count(ConstrainedOptBenchmark::Method::CM_F) > 0 || + runOptions.methods.count(ConstrainedOptBenchmark::Method::CM_I) > 0)) { + auto debugMopt = moptFactory(ConstrainedOptBenchmark::Method::CM_F); + PrintCMComponentDebug(problem, debugMopt, lmParams, true); + } + + ConstrainedOptBenchmark runner(runOptions); + runner.setProblemFactory( + [=]() { return EConsOptProblem(costs, constraints, initValues); }); + runner.setOuterLmBaseParams(lmParams); + runner.setMoptFactory(moptFactory); + runner.setResultCallback( + [&](ConstrainedOptBenchmark::Method method, const Values& result) { + std::cout << "pose error: " << EvaluatePoseError(gt, result) << "\n"; + ExportTrajectoryCsv(result, ConstrainedOptBenchmark::MethodDataPath( + runOptions, method, "_traj.csv")); + }); + + std::ostringstream latexOs; + runner.run(latexOs); + std::cout << latexOs.str(); +} + +int main(int argc, char** argv) { + try { + const ConnectedPosesArgs args = ParseArgs(argc, argv); + if (!args.benchmark_cli.unknownArgs.empty()) { + throw std::invalid_argument("Unknown option: " + + args.benchmark_cli.unknownArgs.front()); + } + kinematic_planning(args); + return 0; + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + PrintUsage(argv[0]); + return 1; + } +} diff --git a/examples/example_constraint_manifold/main_quadruped.cpp b/examples/example_constraint_manifold/main_quadruped.cpp new file mode 100644 index 000000000..c3da8d831 --- /dev/null +++ b/examples/example_constraint_manifold/main_quadruped.cpp @@ -0,0 +1,201 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file main_quadruped.cpp + * @brief Trajectory optimization for a legged robot with contacts. + * @author Alejandro Escontrela + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "QuadrupedUtils.h" +#include "gtdynamics/cmopt/ConstraintManifold.h" +#include "gtdynamics/cmopt/TspaceBasis.h" +#include "gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h" +#include "gtdynamics/constrained_optimizer/ConstrainedOptimizer.h" +#include "gtdynamics/factors/ContactPointFactor.h" +#include "gtdynamics/universal_robot/Link.h" +#include "gtdynamics/utils/DynamicsSymbol.h" +#include "gtdynamics/utils/values.h" + +using namespace gtdynamics; +using namespace gtsam; + +namespace { +constexpr double kConstraintUnitScale = 1e-3; +} // namespace + +void TrajectoryOptimization( + size_t numSteps, const ConstrainedOptBenchmark::Options& runOptions) { + /// Initialize vision60 robot + Vision60Robot vision60; + vision60.express_redundancy = false; + + /// Scenario + double dt = 0.1; + Pose3 basePoseInit(Rot3::Identity(), Point3(0, 0, vision60.nominal_height)); + Vector6 baseTwistInit = Vector6::Zero(); + + // std::vector des_poses {Pose3(Rot3::Identity(), Point3(0, 0, + // vision60.nominal_height + 0.1)), Pose3(Rot3::Rz(0.1), Point3(0, 0, + // vision60.nominal_height + 0.2)), Pose3(Rot3::Rz(0.1), Point3(0, 0, + // vision60.nominal_height + 0.1))}; + + // std::vector des_poses_t {1.0, 2.0, 3.0}; + + std::vector desPoses{ + Pose3(Rot3::Ry(-0.2), Point3(0.2, 0, vision60.nominal_height + 0.2))}; + const double horizonTime = numSteps * dt; + std::vector desPosesT{std::min(3.0, horizonTime)}; + + /// Get constraints, costs, and initial values. + auto constraintsGraph = vision60.getConstraintsGraphTrajectory(numSteps); + auto constraints = + gtsam::NonlinearEqualityConstraints::FromCostGraph(constraintsGraph); + auto initValues = vision60.getInitValuesTrajectory(numSteps, dt, basePoseInit, + desPoses, desPosesT); + NonlinearFactorGraph collocationCosts = + vision60.collocationCosts(numSteps, dt); + NonlinearFactorGraph boundaryCosts = vision60.boundaryCosts( + basePoseInit, baseTwistInit, desPoses, desPosesT, dt); + NonlinearFactorGraph minTorqueCosts = vision60.minTorqueCosts(numSteps); + NonlinearFactorGraph frictionConeCosts = vision60.frictionConeCosts(numSteps); + NonlinearFactorGraph costs; + costs.add(collocationCosts); + costs.add(boundaryCosts); + costs.add(minTorqueCosts); + costs.add(frictionConeCosts); + auto evaluateCosts = [=](const Values& values) { + std::cout << "collocation costs:\t" << collocationCosts.error(values) + << "\n"; + std::cout << "boundary costs:\t" << boundaryCosts.error(values) << "\n"; + std::cout << "min torque costs:\t" << minTorqueCosts.error(values) << "\n"; + std::cout << "friction cone costs:\t" << frictionConeCosts.error(values) + << "\n"; + }; + + /// Export initial trajectory + evaluateCosts(initValues); + vision60.exportTrajectory( + initValues, numSteps, + std::string(kDataPath) + runOptions.id + "_init_traj.csv"); + + auto createProblem = [=]() { + return EConsOptProblem(costs, constraints, initValues); + }; + + LevenbergMarquardtParams lmParams; + lmParams.minModelFidelity = 0.3; + lmParams.linearSolverType = + gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY; + lmParams.setMaxIterations(30); + lmParams.relativeErrorTol = 1e-3; + lmParams.setlambdaUpperBound(1e2); + + ConstrainedOptBenchmark runner(runOptions); + runner.setProblemFactory(createProblem); + runner.setOuterLmBaseParams(lmParams); + runner.setOuterLmConfig([](ConstrainedOptBenchmark::Method method, + LevenbergMarquardtParams* params) { + if (method == ConstrainedOptBenchmark::Method::CM_F || + method == ConstrainedOptBenchmark::Method::CM_I) { + params->setlambdaInitial(1e1); + } + }); + runner.setMoptFactory([&](ConstrainedOptBenchmark::Method) { + auto moptParams = ConstrainedOptBenchmark::DefaultMoptParamsSV( + vision60.getBasisKeyFunc()); + moptParams.cc_params->retractor_creator->params()->use_basis_keys = true; + moptParams.cc_params->retractor_creator->params()->sigma = 1.0; + moptParams.cc_params->retractor_creator->params()->apply_base_retraction = + true; + moptParams.cc_params->retractor_creator->params()->check_feasible = true; + moptParams.cc_params->retractor_creator->params() + ->lm_params.linearSolverType = + gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY; + moptParams.cc_params->retractor_creator->params() + ->lm_params.setlambdaUpperBound(1e2); + return moptParams; + }); + runner.setResultCallback( + [&](ConstrainedOptBenchmark::Method method, const Values& result) { + evaluateCosts(result); + vision60.exportTrajectory(result, numSteps, + ConstrainedOptBenchmark::MethodDataPath( + runOptions, method, "_traj.csv")); + }); + + std::ostringstream latexOs; + runner.run(latexOs); + std::cout << latexOs.str(); +} + +int main(int argc, char** argv) { + try { + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "quadruped"; + defaults.enableNumSteps = true; + defaults.defaultNumSteps = 30; + defaults.defaultMethods = {ConstrainedOptBenchmark::Method::SOFT, + ConstrainedOptBenchmark::Method::CM_I}; + + auto parsed = ConstrainedOptBenchmark::ParseCli(argc, argv, defaults); + if (!parsed.unknownArgs.empty()) { + throw std::invalid_argument("Unknown option: " + + parsed.unknownArgs.front()); + } + + parsed.runOptions.softMu = 1e2; + parsed.runOptions.constraintUnitScale = kConstraintUnitScale; + + std::cout << "Using num_steps=" << parsed.numSteps << "\n"; + std::cout << "Methods: " << "\n"; + for (ConstrainedOptBenchmark::Method method : parsed.runOptions.methods) { + std::cout << " - " << ConstrainedOptBenchmark::MethodToken(method) + << "\n"; + } + TrajectoryOptimization(parsed.numSteps, parsed.runOptions); + return 0; + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "quadruped"; + defaults.enableNumSteps = true; + defaults.defaultNumSteps = 30; + defaults.defaultMethods = {ConstrainedOptBenchmark::Method::SOFT, + ConstrainedOptBenchmark::Method::CM_I}; + ConstrainedOptBenchmark::PrintUsage(std::cerr, argv[0], defaults); + return 1; + } +} diff --git a/examples/example_constraint_manifold/main_range_constraint.cpp b/examples/example_constraint_manifold/main_range_constraint.cpp new file mode 100644 index 000000000..fba5f2c1d --- /dev/null +++ b/examples/example_constraint_manifold/main_range_constraint.cpp @@ -0,0 +1,252 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file main_range_constraint.cpp + * @brief Two-vehicle state estimation with range constraint. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using gtsam::symbol_shorthand::A, gtsam::symbol_shorthand::B; +using namespace gtdynamics; + +namespace { +constexpr size_t kNumSteps = 100; +constexpr double kConstraintUnitScale = 1.0; +const auto kRangeNoise = noiseModel::Isotropic::Sigma(1, 1e0); +const auto kPriorNoise = noiseModel::Isotropic::Sigma(3, 1e-1); +const auto kOdoNoise = noiseModel::Isotropic::Sigma(3, 1e-1); +const Vector kInitValueSigma = (Vector(3) << 0.5, 0.5, 0.5).finished(); +const Vector kOdoSigma = (Vector(3) << 0.1, 0.1, 0.1).finished(); + +Sampler& OdoSampler() { + static Sampler sampler(noiseModel::Diagonal::Sigmas(kOdoSigma)); + return sampler; +} + +Sampler& InitValueSampler() { + static Sampler sampler(noiseModel::Diagonal::Sigmas(kInitValueSigma)); + return sampler; +} + +struct RangeConstraintArgs { + ConstrainedOptBenchmark::ParsedCli benchmark_cli; +}; + +void PrintUsage(const char* program_name) { + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "range_constraint"; + ConstrainedOptBenchmark::PrintUsage(std::cout, program_name, defaults); +} + +RangeConstraintArgs ParseArgs(int argc, char** argv) { + ConstrainedOptBenchmark::CliDefaults defaults; + defaults.id = "range_constraint"; + return RangeConstraintArgs{ + ConstrainedOptBenchmark::ParseCli(argc, argv, defaults)}; +} +} // namespace + +Pose2 add_noise(const Pose2& pose, Sampler& sampler) { + auto xi = sampler.sample(); + return pose.expmap(xi); +} + +/** Build range constraints between trajectories A and B at each step. */ +NonlinearFactorGraph get_constraints_graph(const Values& gt) { + NonlinearFactorGraph constraints_graph; + + for (size_t k = 0; k <= kNumSteps; k++) { + Pose2 pose_1 = gt.at(A(k)); + Pose2 pose_2 = gt.at(B(k)); + double range = pose_1.range(pose_2); + constraints_graph.emplace_shared>( + A(k), B(k), range, kRangeNoise); + } + + return constraints_graph; +} + +/** Build priors and noisy odometry costs for both trajectories. */ +NonlinearFactorGraph get_costs(const Values& gt) { + NonlinearFactorGraph costs; + + costs.emplace_shared>(A(0), gt.at(A(0)), + kPriorNoise); + costs.emplace_shared>(B(0), gt.at(B(0)), + kPriorNoise); + + for (size_t k = 0; k < kNumSteps; k++) { + Pose2 pose1_curr = gt.at(A(k)); + Pose2 pose1_next = gt.at(A(k + 1)); + Pose2 rel_pose_1 = pose1_curr.inverse() * pose1_next; + Pose2 pose2_curr = gt.at(B(k)); + Pose2 pose2_next = gt.at(B(k + 1)); + Pose2 rel_pose_2 = pose2_curr.inverse() * pose2_next; + + rel_pose_1 = add_noise(rel_pose_1, OdoSampler()); + rel_pose_2 = add_noise(rel_pose_2, OdoSampler()); + + costs.emplace_shared>(A(k), A(k + 1), rel_pose_1, + kOdoNoise); + costs.emplace_shared>(B(k), B(k + 1), rel_pose_2, + kOdoNoise); + } + return costs; +} + +Values get_gt_values() { + Values gt; + for (size_t k = 0; k <= kNumSteps; k++) { + gt.insert(A(k), Pose2(k * 0.1, 5.0, k * 0.1)); + gt.insert(B(k), Pose2(k * 0.1, -5.0, -k * 0.1)); + } + return gt; +} + +/** Build initial values by adding noise to the ground-truth trajectories. */ +Values get_init_values(const Values& gt) { + Values init_values; + for (size_t k = 0; k <= kNumSteps; k++) { + Pose2 pose_1 = gt.at(A(k)); + Pose2 pose_2 = gt.at(B(k)); + pose_1 = add_noise(pose_1, InitValueSampler()); + pose_2 = add_noise(pose_2, InitValueSampler()); + init_values.insert(A(k), pose_1); + init_values.insert(B(k), pose_2); + } + return init_values; +} + +/** Function to manually define the basis keys for each constraint manifold. */ +KeyVector FindBasisKeys(const KeyVector& keys) { + KeyVector basis_keys; + for (const Key& key : keys) { + auto symbol = Symbol(key); + if (symbol.chr() == 'a') { + basis_keys.push_back(key); + } + } + return basis_keys; +} + +double EvaluatePoseError(const Values& gt, const Values& result) { + double error1 = 0; + double error2 = 0; + for (size_t k = 1; k <= kNumSteps; k++) { + { + Pose2 gt_pose = gt.at(A(k)); + Pose2 est_pose = result.at(A(k)); + Pose2 rel_pose = est_pose.inverse().compose(gt_pose); + Matrix3 diff = rel_pose.matrix() - I_3x3; + // std::cout << diff << "\n"; + // std::cout << diff.norm() << "\n"; + error1 += pow(diff.norm(), 2); + } + { + Pose2 gt_pose = gt.at(B(k)); + Pose2 est_pose = result.at(B(k)); + Pose2 rel_pose = est_pose.inverse().compose(gt_pose); + Matrix3 diff = rel_pose.matrix() - I_3x3; + error2 += pow(diff.norm(), 2); + } + } + std::cout << sqrt(error1 / kNumSteps) << "\t" << sqrt(error2 / kNumSteps) + << "\n"; + return sqrt(error1 / kNumSteps) + sqrt(error2 / kNumSteps); +} + +void ExportTrajectoryCsv(const Values& values, const std::string& file_path) { + std::ofstream out(file_path); + if (!out.is_open()) { + throw std::runtime_error("Failed to open trajectory file: " + file_path); + } + out << "t,ax,ay,ath,bx,by,bth\n"; + for (size_t k = 0; k <= kNumSteps; ++k) { + const Pose2 a = values.at(A(k)); + const Pose2 b = values.at(B(k)); + out << k << "," << a.x() << "," << a.y() << "," << a.theta() << "," << b.x() + << "," << b.y() << "," << b.theta() << "\n"; + } +} + +/** Benchmark constrained optimizers on range-constrained trajectory estimation. + */ +void kinematic_planning(const RangeConstraintArgs& args) { + // problem + auto gt = get_gt_values(); + auto constraintsGraph = get_constraints_graph(gt); + auto costs = get_costs(gt); + auto initValues = get_init_values(gt); + auto constraints = + gtsam::NonlinearEqualityConstraints::FromCostGraph(constraintsGraph); + auto runOptions = args.benchmark_cli.runOptions; + runOptions.constraintUnitScale = kConstraintUnitScale; + runOptions.softMu = 1e4; + runOptions.cmFRetractorMaxIterations = 10; + runOptions.cmIRetractorMaxIterations = 1; + + std::cout << "pose error: " << EvaluatePoseError(gt, initValues) << "\n"; + + ConstrainedOptBenchmark runner(runOptions); + runner.setProblemFactory( + [=]() { return EConsOptProblem(costs, constraints, initValues); }); + runner.setOuterLmBaseParams(LevenbergMarquardtParams()); + runner.setMoptFactory([](ConstrainedOptBenchmark::Method) { + auto moptParams = ConstrainedOptBenchmark::DefaultMoptParams(); + moptParams.cc_params->retractor_creator->params() + ->lm_params.linearSolverType = + gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY; + return moptParams; + }); + runner.setResultCallback( + [&](ConstrainedOptBenchmark::Method method, const Values& result) { + std::cout << "pose error: " << EvaluatePoseError(gt, result) << "\n"; + ExportTrajectoryCsv(result, ConstrainedOptBenchmark::MethodDataPath( + runOptions, method, "_traj.csv")); + }); + + ExportTrajectoryCsv( + initValues, std::string(kDataPath) + runOptions.id + "_init_traj.csv"); + + std::ostringstream latexOs; + runner.run(latexOs); + std::cout << latexOs.str(); +} + +int main(int argc, char** argv) { + try { + const RangeConstraintArgs args = ParseArgs(argc, argv); + if (!args.benchmark_cli.unknownArgs.empty()) { + throw std::invalid_argument("Unknown option: " + + args.benchmark_cli.unknownArgs.front()); + } + kinematic_planning(args); + return 0; + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + PrintUsage(argv[0]); + return 1; + } +} diff --git a/examples/example_constraint_manifold/quadruped.ipynb b/examples/example_constraint_manifold/quadruped.ipynb new file mode 100644 index 000000000..8de11ae68 --- /dev/null +++ b/examples/example_constraint_manifold/quadruped.ipynb @@ -0,0 +1,3365 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "5b4e1182", + "metadata": {}, + "source": [ + "# Quadruped Timing (Soft + I)\n", + "\n", + "Builds and runs the quadruped benchmark with command-line options, then plots timing for the selected methods." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "fa22bc8b", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "(PosixPath('/Users/dellaert/git/GTDynamics'),\n", + " PosixPath('/Users/dellaert/git/GTDynamics/build'),\n", + " PosixPath('/Users/dellaert/git/GTDynamics/data'),\n", + " 15,\n", + " 'soft,i')" + ] + }, + "execution_count": 1, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "from pathlib import Path\n", + "import re\n", + "import subprocess\n", + "\n", + "import pandas as pd\n", + "import plotly.graph_objects as go\n", + "import gtdynamics as gtd\n", + "\n", + "\n", + "def find_repo_root(start: Path) -> Path:\n", + " for candidate in [start, *start.parents]:\n", + " if (candidate / \"CMakeLists.txt\").exists() and (candidate / \"examples\" / \"example_constraint_manifold\").exists():\n", + " return candidate\n", + " raise RuntimeError(\"Could not find GTDynamics repo root.\")\n", + "\n", + "\n", + "repo_root = find_repo_root(Path.cwd().resolve())\n", + "build_dir = repo_root / \"build\"\n", + "example_bin = build_dir / \"examples\" / \"example_constraint_manifold\" / \"example_constraint_manifold_quadruped_mp\"\n", + "data_dir = Path(gtd.DATA_PATH)\n", + "\n", + "num_steps = 15\n", + "methods = \"soft,i\"\n", + "\n", + "repo_root, build_dir, data_dir, num_steps, methods\n" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "4f542ef1", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[ 95%] Built target gtdynamics\n", + "[100%] Built target example_constraint_manifold_quadruped_mp\n", + "Using num_steps=15\n", + "Methods: soft=1, f=0, i=1\n", + "collocation costs:\t134.298\n", + "boundary costs:\t1.62008e-24\n", + "min torque costs:\t60.7941\n", + "friction cone costs:\t0\n", + "soft constraints:\n", + "[BENCH] Soft Constraint: f_dim=7366, v_dim=7200, time_s=0.099, iters=7, constraint_l2=3.729e-11, cost=195.092\n", + "collocation costs:\t134.298\n", + "boundary costs:\t1.62008e-24\n", + "min torque costs:\t60.7941\n", + "friction cone costs:\t0\n", + "constraint manifold basis variables feasible: skipped\n", + "constraint manifold basis variables infeasible:\n", + "[BENCH] \\textbf{Constraint Manifold (I)}: f_dim=454, v_dim=288, time_s=1.054, iters=0, constraint_l2=1.430e-09, cost=3517.25\n", + "collocation costs:\t3488.49\n", + "boundary costs:\t0.334668\n", + "min torque costs:\t28.4309\n", + "friction cone costs:\t0\n", + "& Soft Constraint & $7366 \\times 7200$ & 0.099 & 7 & 3.73e-11 & 195.09\\\\\n", + "& \\textbf{Constraint Manifold (I)} & $454 \\times 288$ & 1.054 & 0 & 1.43e-09 & 3517.25\\\\\n", + "\n" + ] + } + ], + "source": [ + "subprocess.run([\"make\", \"-j6\", \"example_constraint_manifold_quadruped_mp\"], cwd=build_dir, check=True)\n", + "run_cmd = [str(example_bin), \"--num-steps\", str(num_steps), \"--methods\", methods]\n", + "run = subprocess.run(run_cmd, cwd=build_dir, check=True, text=True, capture_output=True)\n", + "print(run.stdout)\n", + "run_stdout = run.stdout\n" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "cc56b284", + "metadata": {}, + "outputs": [ + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "text": [ + "0.099s", + "1.054s" + ], + "textposition": "outside", + "type": "bar", + "x": [ + "Soft", + "I" + ], + "y": { + "bdata": "8tJNYhBYuT93vp8aL93wPw==", + "dtype": "f8" + } + } + ], + "layout": { + "height": 450, + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermap": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermap" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "#E5ECF6", + "showlakes": true, + "showland": true, + "subunitcolor": "white" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "#E5ECF6", + "polar": { + "angularaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "radialaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "yaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "zaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "baxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "caxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + } + } + }, + "title": { + "text": "Quadruped Timing (soft,i, num_steps=15)" + }, + "width": 700, + "xaxis": { + "title": { + "text": "Method" + } + }, + "yaxis": { + "title": { + "text": "Time [s]" + } + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "data": { + "text/html": [ + "

\n", + "\n", + "\n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + "
methodtime_s
0Soft0.099
1I1.054
\n", + "
" + ], + "text/plain": [ + " method time_s\n", + "0 Soft 0.099\n", + "1 I 1.054" + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "pattern = re.compile(r\"\\[BENCH\\]\\s*(?:\\\\textbf\\{)?([^:}]+)(?:\\})?:.*time_s=([0-9.]+)\")\n", + "timings = {}\n", + "for line in run_stdout.splitlines():\n", + " m = pattern.search(line)\n", + " if not m:\n", + " continue\n", + " name = m.group(1).strip()\n", + " time_s = float(m.group(2))\n", + " if name == \"Soft Constraint\":\n", + " timings[\"Soft\"] = time_s\n", + " elif \"Constraint Manifold (I)\" in name:\n", + " timings[\"I\"] = time_s\n", + "\n", + "timing_df = pd.DataFrame({\"method\": list(timings.keys()), \"time_s\": list(timings.values())})\n", + "if timing_df.empty:\n", + " raise RuntimeError(\"No timing rows found in benchmark output.\")\n", + "\n", + "order = [\"Soft\", \"I\"]\n", + "timing_df[\"method\"] = pd.Categorical(timing_df[\"method\"], categories=order, ordered=True)\n", + "timing_df = timing_df.sort_values(\"method\")\n", + "\n", + "fig = go.Figure(go.Bar(\n", + " x=timing_df[\"method\"],\n", + " y=timing_df[\"time_s\"],\n", + " text=timing_df[\"time_s\"].map(lambda v: f\"{v:.3f}s\"),\n", + " textposition=\"outside\",\n", + "))\n", + "fig.update_layout(\n", + " title=f\"Quadruped Timing ({methods}, num_steps={num_steps})\",\n", + " xaxis_title=\"Method\",\n", + " yaxis_title=\"Time [s]\",\n", + " width=700,\n", + " height=450,\n", + ")\n", + "fig.show()\n", + "timing_df\n" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "9a479431", + "metadata": {}, + "outputs": [ + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "legendgroup": "init", + "line": { + "color": "#1f77b4" + }, + "mode": "lines", + "name": "init", + "showlegend": true, + "type": "scatter", + "xaxis": "x", + "y": { + "bdata": "AAAAAAAAAAC/u5UlOsuwv4yeW+hKBL6/bvdynxwFxL9S8BRypZ7Hv1ga+FEN+8m/nyCx3T1Ay7/uIkxRLo3Lv6pE2VvK+cq/h01k5gKXyb/36uOh727Hv5eMYyR7hMS/AtaqXRPSwL9BKO/jaI64vxJNoIhFDKu/oDcVqTC2ML8=", + "dtype": "f8" + }, + "yaxis": "y" + }, + { + "legendgroup": "init", + "line": { + "color": "#1f77b4" + }, + "mode": "lines", + "name": "init", + "showlegend": false, + "type": "scatter", + "xaxis": "x2", + "y": { + "bdata": "AAAAAAAAAAAGoFG69K/ivxi0kIDR5dy/48KBkCxg1b/2QgHbwYjNv9UgzO1e7sG/FXR7SWO0rr8FxCRcyCOIP8rBbAIMy7M/MZdUbTfBwT82ct2U8lrJPwEYz6Chf9A/hLuzdtuF1D9wfO2ZJQHZP2FPO/w1Wd4/hnR4COOn4j8=", + "dtype": "f8" + }, + "yaxis": "y2" + }, + { + "legendgroup": "init", + "line": { + "color": "#1f77b4" + }, + "mode": "lines", + "name": "init", + "showlegend": false, + "type": "scatter", + "xaxis": "x3", + "y": { + "bdata": "AAAAAAAAAIBGlzeHa8URwC3Q7pBiAPQ/fEj43t+g8T9SEDy+vevuP6cC7nn+NOs/w/NSsTEv6D/zrQ/rjdrlP0BpqFFIMuQ/CFbVy+804z+5/l2fOeviP+iE0EGXcOM/rI2xE14C5T+/DMaIRCHoP5ROJJhq5u0/02achqiC9D8=", + "dtype": "f8" + }, + "yaxis": "y3" + }, + { + "legendgroup": "init", + "line": { + "color": "#1f77b4" + }, + "mode": "lines", + "name": "init", + "showlegend": false, + "type": "scatter", + "xaxis": "x4", + "y": { + "bdata": "c2cmGM5lEcBy3CkdrM8WwGMraFpiRQTALei9MQSA+L/VPbK5ap7dvx75g4HnXuU/wQEtXcG2/T+wdD48S9AIQAPQKF36lxFA3Zcz2xX6FkAktOVcioscQANC6+HLJCFAukp319kYJEBcjexKyyAnQHzRHi+kOypAlkG1wYloLUA=", + "dtype": "f8" + }, + "yaxis": "y4" + }, + { + "legendgroup": "manifold (I)", + "line": { + "color": "#d62728" + }, + "mode": "lines", + "name": "manifold (I)", + "showlegend": true, + "type": "scatter", + "xaxis": "x", + "y": { + "bdata": "yxKdZRahoL/vxoLCoEzLv78PBwlRvtq/hv90AwXe17+M9Q1MbhTBv1YRbjKqDMs/IQa69gX04j/Lgok/irruP7jOv1326/A/0gDeAgnK8T8rieyDLAvwP1oQyvs4mvA/z0nvG1977D/QDyOER1vwPyAldm1vt+M/ldi1vd2SHD8=", + "dtype": "f8" + }, + "yaxis": "y" + }, + { + "legendgroup": "manifold (I)", + "line": { + "color": "#d62728" + }, + "mode": "lines", + "name": "manifold (I)", + "showlegend": false, + "type": "scatter", + "xaxis": "x2", + "y": { + "bdata": "e7slOWBXc7++9zdorz4GwBFtx9Rd2eu/ISOgwhHk+T/JkGPrGaIIQPD49q5Bnw1AGQPrOH6oDkC9woL7Ae8AQEgVxausbdQ/CHWRQlnYBsBwl/26093wP0DZlCu8C/0/vhHds65REcB9rrZif9n1Px+6oL5lnhHAroODvYnRHMA=", + "dtype": "f8" + }, + "yaxis": "y2" + }, + { + "legendgroup": "manifold (I)", + "line": { + "color": "#d62728" + }, + "mode": "lines", + "name": "manifold (I)", + "showlegend": false, + "type": "scatter", + "xaxis": "x3", + "y": { + "bdata": "/gqZK4OcR8BUck7sod0MwBxEa0Wb4UBAahMn9zvsMUDhXwSNmVQlQISDvYkhuek/RzzZzYx+C0AJceXsncZawIuMDkhCUGPAcLTjhl9UYcDEeqNWmIJPQDL/6Js060fAhVypZ0HOUEDAPGTKhyhvwBg/jXvzElHAzEQRUrcLNcA=", + "dtype": "f8" + }, + "yaxis": "y3" + }, + { + "legendgroup": "manifold (I)", + "line": { + "color": "#d62728" + }, + "mode": "lines", + "name": "manifold (I)", + "showlegend": false, + "type": "scatter", + "xaxis": "x4", + "y": { + "bdata": "1GUxsfnAIcCyne+nxpsYwGTKh6BqlBFACCC1iZOLE0Av4dBbPIwUQFTIlXoWFBBAPNujN9xHBkBaETXR56PWP7sPQGoTJ4c/DfrS259L8r+ZnrDEA8reP9YApaFGIak/egCL/Pohlj+tMeiE0EEBwERtG0ZB8LA/SvHxCdl5e78=", + "dtype": "f8" + }, + "yaxis": "y4" + }, + { + "legendgroup": "soft", + "line": { + "color": "#2ca02c" + }, + "mode": "lines", + "name": "soft", + "showlegend": true, + "type": "scatter", + "xaxis": "x", + "y": { + "bdata": "AAAAAAAAAAC/u5UlOsuwv4yeW+hKBL6/bvdynxwFxL9S8BRypZ7Hv1ga+FEN+8m/nyCx3T1Ay7/uIkxRLo3Lv6pE2VvK+cq/h01k5gKXyb/36uOh727Hv5eMYyR7hMS/AtaqXRPSwL9BKO/jaI64vxJNoIhFDKu/oDcVqTC2ML8=", + "dtype": "f8" + }, + "yaxis": "y" + }, + { + "legendgroup": "soft", + "line": { + "color": "#2ca02c" + }, + "mode": "lines", + "name": "soft", + "showlegend": false, + "type": "scatter", + "xaxis": "x2", + "y": { + "bdata": "AAAAAAAAAAAGoFG69K/ivxi0kIDR5dy/48KBkCxg1b/2QgHbwYjNv9UgzO1e7sG/FXR7SWO0rr8FxCRcyCOIP8rBbAIMy7M/MZdUbTfBwT82ct2U8lrJPwEYz6Chf9A/hLuzdtuF1D9wfO2ZJQHZP2FPO/w1Wd4/hnR4COOn4j8=", + "dtype": "f8" + }, + "yaxis": "y2" + }, + { + "legendgroup": "soft", + "line": { + "color": "#2ca02c" + }, + "mode": "lines", + "name": "soft", + "showlegend": false, + "type": "scatter", + "xaxis": "x3", + "y": { + "bdata": "AAAAAAAAAIBGlzeHa8URwC3Q7pBiAPQ/fEj43t+g8T9SEDy+vevuP6cC7nn+NOs/w/NSsTEv6D/zrQ/rjdrlP0BpqFFIMuQ/CFbVy+804z+5/l2fOeviP+iE0EGXcOM/rI2xE14C5T+/DMaIRCHoP5ROJJhq5u0/02achqiC9D8=", + "dtype": "f8" + }, + "yaxis": "y3" + }, + { + "legendgroup": "soft", + "line": { + "color": "#2ca02c" + }, + "mode": "lines", + "name": "soft", + "showlegend": false, + "type": "scatter", + "xaxis": "x4", + "y": { + "bdata": "c2cmGM5lEcBy3CkdrM8WwGMraFpiRQTALei9MQSA+L/VPbK5ap7dvx75g4HnXuU/wQEtXcG2/T+wdD48S9AIQAPQKF36lxFA3Zcz2xX6FkAktOVcioscQANC6+HLJCFAukp319kYJEBcjexKyyAnQHzRHi+kOypAlkG1wYloLUA=", + "dtype": "f8" + }, + "yaxis": "y4" + } + ], + "layout": { + "annotations": [ + { + "font": { + "size": 16 + }, + "showarrow": false, + "text": "q", + "x": 0.225, + "xanchor": "center", + "xref": "paper", + "y": 1, + "yanchor": "bottom", + "yref": "paper" + }, + { + "font": { + "size": 16 + }, + "showarrow": false, + "text": "v", + "x": 0.775, + "xanchor": "center", + "xref": "paper", + "y": 1, + "yanchor": "bottom", + "yref": "paper" + }, + { + "font": { + "size": 16 + }, + "showarrow": false, + "text": "a", + "x": 0.225, + "xanchor": "center", + "xref": "paper", + "y": 0.43, + "yanchor": "bottom", + "yref": "paper" + }, + { + "font": { + "size": 16 + }, + "showarrow": false, + "text": "T", + "x": 0.775, + "xanchor": "center", + "xref": "paper", + "y": 0.43, + "yanchor": "bottom", + "yref": "paper" + } + ], + "height": 620, + "legend": { + "title": { + "text": "trajectory" + } + }, + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermap": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermap" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "#E5ECF6", + "showlakes": true, + "showland": true, + "subunitcolor": "white" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "#E5ECF6", + "polar": { + "angularaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "radialaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "yaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "zaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "baxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "caxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + } + } + }, + "title": { + "text": "hip joint trajectories" + }, + "width": 950, + "xaxis": { + "anchor": "y", + "domain": [ + 0, + 0.45 + ], + "matches": "x3", + "showticklabels": false + }, + "xaxis2": { + "anchor": "y2", + "domain": [ + 0.55, + 1 + ], + "matches": "x4", + "showticklabels": false + }, + "xaxis3": { + "anchor": "y3", + "domain": [ + 0, + 0.45 + ], + "title": { + "text": "step" + } + }, + "xaxis4": { + "anchor": "y4", + "domain": [ + 0.55, + 1 + ], + "title": { + "text": "step" + } + }, + "yaxis": { + "anchor": "x", + "domain": [ + 0.5700000000000001, + 1 + ] + }, + "yaxis2": { + "anchor": "x2", + "domain": [ + 0.5700000000000001, + 1 + ] + }, + "yaxis3": { + "anchor": "x3", + "domain": [ + 0, + 0.43 + ] + }, + "yaxis4": { + "anchor": "x4", + "domain": [ + 0, + 0.43 + ] + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "legendgroup": "init", + "line": { + "color": "#1f77b4" + }, + "mode": "lines", + "name": "init", + "showlegend": true, + "type": "scatter", + "xaxis": "x", + "y": { + "bdata": "AAAAAAAAAABV+Z6RCI2oP2iVmdL6W7o/ev1JfO4ExT8dke9S6pLNP6lPcodNZNM/aRmp91RO2D8HCVG+oIXdPySAm8WLheE/GsBbIEFx5D8i+yDLgonnP3Ai+rX10+o/Z9Km6h5Z7j+rzJTW3xLxP9cS8kHPJvM/aLCp86h49T8=", + "dtype": "f8" + }, + "yaxis": "y" + }, + { + "legendgroup": "init", + "line": { + "color": "#1f77b4" + }, + "mode": "lines", + "name": "init", + "showlegend": false, + "type": "scatter", + "xaxis": "x2", + "y": { + "bdata": "AAAAAAAAAABnRGlv8IXgP9FALJs5pOI/XFZhM8CF5D93ZRcMrjnmP8+/Xfbrzuc/OEw0SMFT6T+mgLT/AdbqP3GRe7q6Y+w/bR0c7E0M7j9/944aE+LvP4Yhcvp6/vA/+yDLgok/8j9u+N10y87zP7vvGB772fU/ccrcfCO6+D8=", + "dtype": "f8" + }, + "yaxis": "y2" + }, + { + "legendgroup": "init", + "line": { + "color": "#1f77b4" + }, + "mode": "lines", + "name": "init", + "showlegend": false, + "type": "scatter", + "xaxis": "x3", + "y": { + "bdata": "AAAAAAAAAICPccXFUXkXQOqzA64r5uM/f6Dctu/R4T86V5QSglXgP6vrUE1J1t4/OnZQiesY3j9ckZighm/eP1J95xcl6N8/S8gHPZtV4T9NFYxK6oTjP3iXi/hOzOY/VP1K58Oz6z9sByP2CaDxP06zQLtDyvc/a9YZ3xd3AUA=", + "dtype": "f8" + }, + "yaxis": "y3" + }, + { + "legendgroup": "init", + "line": { + "color": "#1f77b4" + }, + "mode": "lines", + "name": "init", + "showlegend": false, + "type": "scatter", + "xaxis": "x4", + "y": { + "bdata": "EvkupS5NMkC9qUiFsdE0QC/BqQ8kbzJAvfvjvWo9MkDdJAaBlecxQGtj7ISXcDFAd4cUAyTaMECyoDAo0yQwQDtxOV6BoC5AP8dHizO2LEC8y0V8J4YqQICbxYuFCShA1Xd+UYI2JUChLlIoC/8hQOp4zEBlnBxAmwEuyJYFFEA=", + "dtype": "f8" + }, + "yaxis": "y4" + }, + { + "legendgroup": "manifold (I)", + "line": { + "color": "#d62728" + }, + "mode": "lines", + "name": "manifold (I)", + "showlegend": true, + "type": "scatter", + "xaxis": "x", + "y": { + "bdata": "fSHkvP+Pgz+j6IGPwYqrP39QFymUhc8/6X5OQX424z+T/fM0YJDwP/j6Wpca4fc/vJaQD3r2/j8namluhRADQGbZk8Dm/AJAP3Jr0m2pA0DPoKF/gksCQCAnTBjNCgNAR8oWSbsxAUDwMy4cCEkEQCEjoMIRRABAgAuyZfl69T8=", + "dtype": "f8" + }, + "yaxis": "y" + }, + { + "legendgroup": "manifold (I)", + "line": { + "color": "#d62728" + }, + "mode": "lines", + "name": "manifold (I)", + "showlegend": false, + "type": "scatter", + "xaxis": "x2", + "y": { + "bdata": "fJ3Ul6Wdej9fJoqQuh3nP9An8iTpugZA4BCq1OyhD0CmQjwSLw8SQIzYJ4BidBFAgzEiUWgZEUAsRIfAkUD2P2wkCcIVUMg/r7DgfsDDFcA11CgkmRX+P/JDpREzuwpANIXOa+xyHcBhbCHIQQkUQANd+wJ6YRDARWKCGr5VH8A=", + "dtype": "f8" + }, + "yaxis": "y2" + }, + { + "legendgroup": "manifold (I)", + "line": { + "color": "#d62728" + }, + "mode": "lines", + "name": "manifold (I)", + "showlegend": false, + "type": "scatter", + "xaxis": "x3", + "y": { + "bdata": "KII4DyeQI8C78e7IWIU4QLR224XmSi5AFr1TAfc0IUDsoBLXMW75P8puZvSjkRzA3X2OjxZ3IUCgTnl0o0xowPXZAdfV+3DAgGPPnoshcMAtmWN5V4lbQE0R4PQuaVXAAyMva+KsYEAdOdIZmMl7wIApAwe0/1zAXVFKCFaRMMA=", + "dtype": "f8" + }, + "yaxis": "y3" + }, + { + "legendgroup": "manifold (I)", + "line": { + "color": "#d62728" + }, + "mode": "lines", + "name": "manifold (I)", + "showlegend": false, + "type": "scatter", + "xaxis": "x4", + "y": { + "bdata": "zZNrCmTeLEBhi90+q0A8QCtoWmJlHDhAXWqEfqYeMUB+4CpPIGQkQL9hokEKHhVAnOEGfH4YBUDUJ7nDJjLzv4sWoG016/O/Q3OdRlrq9L9L5ljeVc/1P7h4eM+B5du/wAevXdqwAUAldm1vt0QMwL3IBPwaSb4/zR39L9eirb8=", + "dtype": "f8" + }, + "yaxis": "y4" + }, + { + "legendgroup": "soft", + "line": { + "color": "#2ca02c" + }, + "mode": "lines", + "name": "soft", + "showlegend": true, + "type": "scatter", + "xaxis": "x", + "y": { + "bdata": "AAAAAAAAAABV+Z6RCI2oP2iVmdL6W7o/ev1JfO4ExT8dke9S6pLNP6lPcodNZNM/aRmp91RO2D8HCVG+oIXdPySAm8WLheE/GsBbIEFx5D8i+yDLgonnP3Ai+rX10+o/Z9Km6h5Z7j+rzJTW3xLxP9cS8kHPJvM/aLCp86h49T8=", + "dtype": "f8" + }, + "yaxis": "y" + }, + { + "legendgroup": "soft", + "line": { + "color": "#2ca02c" + }, + "mode": "lines", + "name": "soft", + "showlegend": false, + "type": "scatter", + "xaxis": "x2", + "y": { + "bdata": "AAAAAAAAAABnRGlv8IXgP9FALJs5pOI/XFZhM8CF5D93ZRcMrjnmP8+/Xfbrzuc/OEw0SMFT6T+mgLT/AdbqP3GRe7q6Y+w/bR0c7E0M7j9/944aE+LvP4Yhcvp6/vA/+yDLgok/8j9u+N10y87zP7vvGB772fU/ccrcfCO6+D8=", + "dtype": "f8" + }, + "yaxis": "y2" + }, + { + "legendgroup": "soft", + "line": { + "color": "#2ca02c" + }, + "mode": "lines", + "name": "soft", + "showlegend": false, + "type": "scatter", + "xaxis": "x3", + "y": { + "bdata": "AAAAAAAAAICPccXFUXkXQOqzA64r5uM/f6Dctu/R4T86V5QSglXgP6vrUE1J1t4/OnZQiesY3j9ckZighm/eP1J95xcl6N8/S8gHPZtV4T9NFYxK6oTjP3iXi/hOzOY/VP1K58Oz6z9sByP2CaDxP06zQLtDyvc/a9YZ3xd3AUA=", + "dtype": "f8" + }, + "yaxis": "y3" + }, + { + "legendgroup": "soft", + "line": { + "color": "#2ca02c" + }, + "mode": "lines", + "name": "soft", + "showlegend": false, + "type": "scatter", + "xaxis": "x4", + "y": { + "bdata": "EvkupS5NMkC9qUiFsdE0QC/BqQ8kbzJAvfvjvWo9MkDdJAaBlecxQGtj7ISXcDFAd4cUAyTaMECyoDAo0yQwQDtxOV6BoC5AP8dHizO2LEC8y0V8J4YqQICbxYuFCShA1Xd+UYI2JUChLlIoC/8hQOp4zEBlnBxAmwEuyJYFFEA=", + "dtype": "f8" + }, + "yaxis": "y4" + } + ], + "layout": { + "annotations": [ + { + "font": { + "size": 16 + }, + "showarrow": false, + "text": "q", + "x": 0.225, + "xanchor": "center", + "xref": "paper", + "y": 1, + "yanchor": "bottom", + "yref": "paper" + }, + { + "font": { + "size": 16 + }, + "showarrow": false, + "text": "v", + "x": 0.775, + "xanchor": "center", + "xref": "paper", + "y": 1, + "yanchor": "bottom", + "yref": "paper" + }, + { + "font": { + "size": 16 + }, + "showarrow": false, + "text": "a", + "x": 0.225, + "xanchor": "center", + "xref": "paper", + "y": 0.43, + "yanchor": "bottom", + "yref": "paper" + }, + { + "font": { + "size": 16 + }, + "showarrow": false, + "text": "T", + "x": 0.775, + "xanchor": "center", + "xref": "paper", + "y": 0.43, + "yanchor": "bottom", + "yref": "paper" + } + ], + "height": 620, + "legend": { + "title": { + "text": "trajectory" + } + }, + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermap": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermap" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "#E5ECF6", + "showlakes": true, + "showland": true, + "subunitcolor": "white" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "#E5ECF6", + "polar": { + "angularaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "radialaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "yaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "zaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "baxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "caxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + } + } + }, + "title": { + "text": "knee joint trajectories" + }, + "width": 950, + "xaxis": { + "anchor": "y", + "domain": [ + 0, + 0.45 + ], + "matches": "x3", + "showticklabels": false + }, + "xaxis2": { + "anchor": "y2", + "domain": [ + 0.55, + 1 + ], + "matches": "x4", + "showticklabels": false + }, + "xaxis3": { + "anchor": "y3", + "domain": [ + 0, + 0.45 + ], + "title": { + "text": "step" + } + }, + "xaxis4": { + "anchor": "y4", + "domain": [ + 0.55, + 1 + ], + "title": { + "text": "step" + } + }, + "yaxis": { + "anchor": "x", + "domain": [ + 0.5700000000000001, + 1 + ] + }, + "yaxis2": { + "anchor": "x2", + "domain": [ + 0.5700000000000001, + 1 + ] + }, + "yaxis3": { + "anchor": "x3", + "domain": [ + 0, + 0.43 + ] + }, + "yaxis4": { + "anchor": "x4", + "domain": [ + 0, + 0.43 + ] + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Used trajectory files: ['init_traj.csv', 'cm_infeas_traj.csv', 'soft_traj.csv']\n" + ] + } + ], + "source": [ + "from plotly.subplots import make_subplots\n", + "\n", + "\n", + "def load_traj_arrays(file_path: Path):\n", + " df = pd.read_csv(file_path)\n", + " jangles = df[[str(j) for j in range(12)]].to_numpy()\n", + " jvels = df[[f\"{j}.1\" for j in range(12)]].to_numpy()\n", + " jaccels = df[[f\"{j}.2\" for j in range(12)]].to_numpy()\n", + " jtorques = df[[f\"{j}.3\" for j in range(12)]].to_numpy()\n", + " return jangles, jvels, jaccels, jtorques\n", + "\n", + "\n", + "cm_file = \"cm_traj.csv\" if (data_dir / \"cm_traj.csv\").exists() else \"cm_infeas_traj.csv\"\n", + "cm_label = \"manifold\" if cm_file == \"cm_traj.csv\" else \"manifold (I)\"\n", + "traj_specs = [(\"init\", \"init_traj.csv\"), (cm_label, cm_file), (\"soft\", \"soft_traj.csv\")]\n", + "method_colors = {\n", + " \"init\": \"#1f77b4\",\n", + " \"manifold\": \"#d62728\",\n", + " \"manifold (I)\": \"#d62728\",\n", + " \"soft\": \"#2ca02c\",\n", + "}\n", + "\n", + "\n", + "def plot_joint_comparison(idx: int, joint_name: str):\n", + " fig = make_subplots(\n", + " rows=2,\n", + " cols=2,\n", + " subplot_titles=(\"q\", \"v\", \"a\", \"T\"),\n", + " shared_xaxes=True,\n", + " vertical_spacing=0.14,\n", + " )\n", + "\n", + " for label, filename in traj_specs:\n", + " jangles, jvels, jaccels, jtorques = load_traj_arrays(data_dir / filename)\n", + " series = [jangles[:, idx], jvels[:, idx], jaccels[:, idx], jtorques[:, idx]]\n", + " color = method_colors.get(label)\n", + "\n", + " for k, y in enumerate(series):\n", + " row = 1 + k // 2\n", + " col = 1 + k % 2\n", + " fig.add_trace(\n", + " go.Scatter(\n", + " y=y,\n", + " mode=\"lines\",\n", + " name=label,\n", + " legendgroup=label,\n", + " showlegend=(k == 0),\n", + " line={\"color\": color},\n", + " ),\n", + " row=row,\n", + " col=col,\n", + " )\n", + "\n", + " fig.update_xaxes(title_text=\"step\", row=2, col=1)\n", + " fig.update_xaxes(title_text=\"step\", row=2, col=2)\n", + " fig.update_layout(\n", + " title=f\"{joint_name} joint trajectories\",\n", + " width=950,\n", + " height=620,\n", + " legend_title=\"trajectory\",\n", + " )\n", + " fig.show()\n", + "\n", + "\n", + "plot_joint_comparison(0, \"hip\")\n", + "plot_joint_comparison(1, \"knee\")\n", + "print(\"Used trajectory files:\", [f for _, f in traj_specs])\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9d32da41", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/example_constraint_manifold/quadruped_plot.py b/examples/example_constraint_manifold/quadruped_plot.py new file mode 100644 index 000000000..7a06bd202 --- /dev/null +++ b/examples/example_constraint_manifold/quadruped_plot.py @@ -0,0 +1,72 @@ +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd +import gtdynamics as gtd + +# load date +def load_data(file_path): + df = pd.read_csv(file_path) + jangles_list = [] + jvels_list = [] + jaccels_list = [] + jtorques_list = [] + + for i in range(len(df)): + jangles = [df.loc[i][str(j)] for j in range(12)] + jvels = [df.loc[i][str(j) + '.1'] for j in range(12)] + jaccels = [df.loc[i][str(j) + '.2'] for j in range(12)] + jtorques = [df.loc[i][str(j) + '.3'] for j in range(12)] + + # print(jangles) + jangles_list.append(jangles) + jvels_list.append(jvels) + jaccels_list.append(jaccels) + jtorques_list.append(jtorques) + + jangles_array = np.array(jangles_list) + jvels_array = np.array(jvels_list) + jaccels_array = np.array(jaccels_list) + jtorques_array = np.array(jtorques_list) + + return jangles_array, jvels_array, jaccels_array, jtorques_array + +def make_plot(axs, jangles_array, jvels_array, jaccels_array, jtorques_array, idx, label): + axs[0, 0].plot(jangles_array[:, idx], label = label) + axs[0, 1].plot(jvels_array[:, idx], label = label) + axs[1, 0].plot(jaccels_array[:, idx], label = label) + axs[1, 1].plot(jtorques_array[:, idx], label = label) + axs[0, 0].set_title("q") + axs[0, 1].set_title("v") + axs[1, 0].set_title("a") + axs[1, 1].set_title("T") + +fig, axs = plt.subplots(2, 2) +idx = 0 +file_path = gtd.DATA_PATH + 'init_traj.csv' +jangles_array, jvels_array, jaccels_array, jtorques_array = load_data(file_path) +make_plot(axs, jangles_array, jvels_array, jaccels_array, jtorques_array, idx, "init") +file_path = gtd.DATA_PATH + 'cm_traj.csv' +jangles_array, jvels_array, jaccels_array, jtorques_array = load_data(file_path) +make_plot(axs, jangles_array, jvels_array, jaccels_array, jtorques_array, idx, "manifold") +file_path = gtd.DATA_PATH + 'soft_traj.csv' +jangles_array, jvels_array, jaccels_array, jtorques_array = load_data(file_path) +make_plot(axs, jangles_array, jvels_array, jaccels_array, jtorques_array, idx, "soft") +axs[0, 1].legend() +fig.suptitle("hip joint") + + +fig, axs = plt.subplots(2, 2) +idx = 1 +file_path = gtd.DATA_PATH + 'init_traj.csv' +jangles_array, jvels_array, jaccels_array, jtorques_array = load_data(file_path) +make_plot(axs, jangles_array, jvels_array, jaccels_array, jtorques_array, idx, "init") +file_path = gtd.DATA_PATH + 'cm_traj.csv' +jangles_array, jvels_array, jaccels_array, jtorques_array = load_data(file_path) +make_plot(axs, jangles_array, jvels_array, jaccels_array, jtorques_array, idx, "manifold") +file_path = gtd.DATA_PATH + 'soft_traj.csv' +jangles_array, jvels_array, jaccels_array, jtorques_array = load_data(file_path) +make_plot(axs, jangles_array, jvels_array, jaccels_array, jtorques_array, idx, "soft") +axs[0, 1].legend() +fig.suptitle("knee joint") + +plt.show() diff --git a/examples/example_constraint_manifold/quadruped_visualize.py b/examples/example_constraint_manifold/quadruped_visualize.py new file mode 100644 index 000000000..653ee9f4a --- /dev/null +++ b/examples/example_constraint_manifold/quadruped_visualize.py @@ -0,0 +1,140 @@ +"""Run kinematic motion planning using GTDynamics outputs.""" +import time + +import numpy as np +import pandas as pd +import pybullet as p +import pybullet_data + +import gtdynamics as gtd + +# pylint: disable=I1101, C0103 + +_ = p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) # To load plane SDF. +p.setGravity(0, 0, -9.8) +planeId = p.loadURDF("plane.urdf") +p.changeDynamics(planeId, -1, lateralFriction=2.0) +quad_id = p.loadURDF(gtd.URDF_PATH + "vision60.urdf", [0, 0, 0.21], + [0, 0, 0, 1], False, False) + +joint_to_jid_map = {} +for i in range(p.getNumJoints(quad_id)): + jinfo = p.getJointInfo(quad_id, i) + joint_to_jid_map[jinfo[1].decode("utf-8")] = jinfo[0] + + +#TODO(Varun) This should be passed as a cmdline argument +df = pd.read_csv(gtd.DATA_PATH + 'cm_traj.csv') +print(df.columns) + +input("Press ENTER to continue.") + +pos, orn = p.getBasePositionAndOrientation(quad_id) + +print("Init Base\n\tPos: {}\n\tOrn: {}".format(pos, + p.getEulerFromQuaternion(orn))) + +# Store positions and times for analysis. +t = 0 +ts = [] +all_pos_sim = [] +dt = 0.1 + +# To draw goal coordinate frames. +goal_pos = None +goal_orn = None +debug_line_x, debug_line_y, debug_line_z = None, None, None + +debug_iters = 20 +for i in range(len(df)): + jangles = df.loc[i][[str(i) for i in range(12)]] + jvels = df.loc[i][[str(i) + '.1' for i in range(12)]] + jaccels = df.loc[i][[str(i) + '.2' for i in range(12)]] + jtorques = df.loc[i][[str(i) + '.3' for i in range(12)]] + + base_pos = df.loc[i][['base_x', 'base_y', 'base_z']].tolist() + # base_pos[2] = base_pos[2] + 0.21 + base_orn = df.loc[i][['base_qx', 'base_qy', 'base_qz', 'base_qw']].tolist() + + p.resetBasePositionAndOrientation(quad_id, base_pos, base_orn) + + for joint, angle in jangles.items(): + target_velocity = jvels.get(joint + '.1', 0.0) + p.resetJointState(quad_id, + jointIndex=joint_to_jid_map[joint], + targetValue=angle, + targetVelocity=target_velocity) + + + gtd.sim.set_joint_angles(p, quad_id, joint_to_jid_map, jangles, jvels) + + # Update body CoM coordinate frame. + # new_pos, new_orn = p.getBasePositionAndOrientation(quad_id) + # new_pos = np.array(new_pos) + # new_R = np.array(p.getMatrixFromQuaternion(new_orn)).reshape(3, 3) + + + + # # Add goal pose coordinate frame. + # if (new_goal_pos != goal_pos) or (new_goal_orn != goal_orn): + + # # Remove old debug items. + # if debug_line_x is not None: + # p.removeUserDebugItem(debug_line_x) + # p.removeUserDebugItem(debug_line_y) + # p.removeUserDebugItem(debug_line_z) + + # t = np.array(new_goal_pos) + # R = np.array(p.getMatrixFromQuaternion(new_goal_orn)).reshape(3, 3) + + # new_debug_line_x = p.addUserDebugLine( + # np.array([0, 0, 0]) + t, np.matmul(R, np.array([0.1, 0, 0])) + t, + # lineColorRGB=[1, 0, 0], lineWidth=3) + # new_debug_line_y = p.addUserDebugLine( + # np.array([0, 0, 0]) + t, np.matmul(R, np.array([0, 0.1, 0])) + t, + # lineColorRGB=[0, 1, 0], lineWidth=3) + # new_debug_line_z = p.addUserDebugLine( + # np.array([0, 0, 0]) + t, np.matmul(R, np.array([0, 0, 0.1])) + t, + # lineColorRGB=[0, 0, 1], lineWidth=3) + + # goal_pos = new_goal_pos + # goal_orn = new_goal_orn + # debug_line_x = new_debug_line_x + # debug_line_y = new_debug_line_y + # debug_line_z = new_debug_line_z + + # if (i % debug_iters) == 0: + # print("\tIter {} Base\n\t\tPos: {}\n\t\tOrn: {}".format( + # i, new_pos, p.getEulerFromQuaternion(new_orn))) + # p.addUserDebugLine(pos, new_pos, lineColorRGB=[0, 1, 1], lineWidth=1) + # pos, orn = new_pos, new_orn + + # bod_debug_line_x = p.addUserDebugLine( + # np.array([0, 0, 0]) + new_pos, + # np.matmul(new_R, np.array([0.05, 0, 0])) + new_pos, + # lineColorRGB=[1, 0, 1], lineWidth=1, lifeTime=1.5) + # bod_debug_line_y = p.addUserDebugLine( + # np.array([0, 0, 0]) + new_pos, + # np.matmul(new_R, np.array([0, 0.05, 0])) + new_pos, + # lineColorRGB=[1, 0, 1], lineWidth=1, lifeTime=1.5) + # bod_debug_line_z = p.addUserDebugLine( + # np.array([0, 0, 0]) + new_pos, + # np.matmul(new_R, np.array([0, 0, 0.05])) + new_pos, + # lineColorRGB=[1, 0, 1], lineWidth=1, lifeTime=1.5) + + ts.append(t) + # all_pos_sim.append(new_pos) + + p.stepSimulation() + time.sleep(dt) + t += dt + +time.sleep(10) + +pos, orn = p.getBasePositionAndOrientation(quad_id) +print("Final Base\n\tPos: {}\n\tOrn: {}".format(pos, + p.getEulerFromQuaternion(orn))) + + +p.disconnect() diff --git a/examples/example_forward_dynamics/CMakeLists.txt b/examples/example_forward_dynamics/CMakeLists.txt index ea3847c2e..077704445 100644 --- a/examples/example_forward_dynamics/CMakeLists.txt +++ b/examples/example_forward_dynamics/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.9...3.29) project(example_forward_dynamics C CXX) # Build Executable diff --git a/examples/example_forward_dynamics/main.cpp b/examples/example_forward_dynamics/main.cpp index 61899bde8..8392527de 100644 --- a/examples/example_forward_dynamics/main.cpp +++ b/examples/example_forward_dynamics/main.cpp @@ -1,7 +1,7 @@ #include #include #include -#include +#include #include #include @@ -33,7 +33,8 @@ int main(int argc, char** argv) { kdfg.add(fd_priors); // Initialize solution. - auto init_values = ZeroValues(simple_rr, 0); + Initializer initializer; + auto init_values = initializer.ZeroValues(simple_rr, 0); // Compute the forward dynamics. gtsam::LevenbergMarquardtOptimizer optimizer(kdfg, init_values); diff --git a/examples/example_full_kinodynamic_balancing/CMakeLists.txt b/examples/example_full_kinodynamic_balancing/CMakeLists.txt index 4b868b87b..884a815bf 100644 --- a/examples/example_full_kinodynamic_balancing/CMakeLists.txt +++ b/examples/example_full_kinodynamic_balancing/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.9...3.29) project(example_full_kinodynamic_balancing C CXX) # Build Executable diff --git a/examples/example_full_kinodynamic_balancing/main.cpp b/examples/example_full_kinodynamic_balancing/main.cpp index 4cc3b10b8..6abfd74fd 100644 --- a/examples/example_full_kinodynamic_balancing/main.cpp +++ b/examples/example_full_kinodynamic_balancing/main.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include #include @@ -24,8 +24,6 @@ #include #include -#include -#include #include #include #include @@ -186,6 +184,7 @@ int main(int argc, char** argv) { // Initialize solution. gtsam::Values init_vals; + Initializer initializer; std::string initialization_technique = "inverse_kinematics"; if (initialization_technique == "interp") // TODO(aescontrela): Figure out why the linearly interpolated initial @@ -193,13 +192,14 @@ int main(int argc, char** argv) { // a difficult time optimizing the trajectory when the initial solution lies // in the infeasible region. This would make sense if I were using an IPM to // solve this problem... - init_vals = InitializeSolutionInterpolationMultiPhase( + init_vals = initializer.InitializeSolutionInterpolationMultiPhase( vision60, "body", base_pose_init, des_poses, des_poses_t, dt, 0.0, contact_points); else if (initialization_technique == "zeros") - init_vals = ZeroValuesTrajectory(vision60, t_steps, 0, 0.0, contact_points); + init_vals = initializer.ZeroValuesTrajectory(vision60, t_steps, 0, 0.0, + contact_points); else if (initialization_technique == "inverse_kinematics") - init_vals = InitializeSolutionInverseKinematics( + init_vals = initializer.InitializeSolutionInverseKinematics( vision60, "body", base_pose_init, des_poses, des_poses_t, dt, 0.0, contact_points); @@ -235,7 +235,10 @@ int main(int argc, char** argv) { // Log the joint angles, velocities, accels, torques, and current goal pose. std::vector jnames; for (auto&& joint : vision60.joints()) jnames.push_back(joint->name()); - std::string jnames_str = boost::algorithm::join(jnames, ","); + std::string jnames_str = ""; + for (size_t j = 0; j < jnames.size(); j++) { + jnames_str += jnames[j] + (j != jnames.size() - 1 ? "," : ""); + } std::ofstream traj_file; traj_file.open("traj.csv"); // angles, vels, accels, torques. @@ -273,7 +276,10 @@ int main(int argc, char** argv) { } } - std::string vals_str = boost::algorithm::join(vals, ","); + std::string vals_str = ""; + for (size_t j = 0; j < vals.size(); j++) { + vals_str += vals[j] + (j != vals.size() - 1 ? "," : ""); + } traj_file << vals_str << "\n"; } traj_file.close(); diff --git a/examples/example_full_kinodynamic_balancing/sim.py b/examples/example_full_kinodynamic_balancing/sim.py index 3a31fb688..1169e5b66 100644 --- a/examples/example_full_kinodynamic_balancing/sim.py +++ b/examples/example_full_kinodynamic_balancing/sim.py @@ -13,9 +13,9 @@ _ = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) # To load plane SDF. p.setGravity(0, 0, -9.8) -planeId = p.loadURDF("plane.urdf") +planeId = p.loadURDF(gtd.URDF_PATH + "plane.urdf") p.changeDynamics(planeId, -1, lateralFriction=2.0) -quad_id = p.loadURDF("vision60.urdf", [0, 0, 0.21], [0, 0, 0, 1], False, +quad_id = p.loadURDF(gtd.URDF_PATH + "vision60.urdf", [0, 0, 0.21], [0, 0, 0, 1], False, False) joint_to_jid_map = {} diff --git a/examples/example_full_kinodynamic_walking/CMakeLists.txt b/examples/example_full_kinodynamic_walking/CMakeLists.txt index ac8a6f011..4693cd053 100644 --- a/examples/example_full_kinodynamic_walking/CMakeLists.txt +++ b/examples/example_full_kinodynamic_walking/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.9...3.29) project(example_full_kinodynamic_walking C CXX) # Build Executables diff --git a/examples/example_full_kinodynamic_walking/main_rotate.cpp b/examples/example_full_kinodynamic_walking/main_rotate.cpp index ead877bd9..bba0aae47 100644 --- a/examples/example_full_kinodynamic_walking/main_rotate.cpp +++ b/examples/example_full_kinodynamic_walking/main_rotate.cpp @@ -17,15 +17,13 @@ #include #include #include -#include +#include #include #include #include #include #include -#include -#include #include #include #include @@ -151,10 +149,11 @@ int main(int argc, char** argv) { vector transition_graphs; vector transition_graph_init; double gaussian_noise = 1e-5; // Add gaussian noise to initial values. + Initializer initializer; for (int p = 1; p < phase_cps.size(); p++) { transition_graphs.push_back(graph_builder.dynamicsFactorGraph( robot, cum_phase_steps[p - 1], trans_cps[p - 1], mu)); - transition_graph_init.push_back(ZeroValues( + transition_graph_init.push_back(initializer.ZeroValues( robot, cum_phase_steps[p - 1], gaussian_noise, trans_cps[p - 1])); } @@ -300,7 +299,7 @@ int main(int argc, char** argv) { // Initialize solution. gtsam::Values init_vals; - init_vals = gtdynamics::MultiPhaseZeroValuesTrajectory( + init_vals = initializer.MultiPhaseZeroValuesTrajectory( robot, phase_steps, transition_graph_init, dt_des, gaussian_noise, phase_cps); @@ -316,7 +315,10 @@ int main(int argc, char** argv) { // Log the joint angles, velocities, accels, torques, and current goal pose. vector jnames; for (auto&& joint : robot.joints()) jnames.push_back(joint->name()); - string jnames_str = boost::algorithm::join(jnames, ","); + std::string jnames_str = ""; + for (size_t j = 0; j < jnames.size(); j++) { + jnames_str += jnames[j] + (j != jnames.size() - 1 ? "," : ""); + } std::ofstream traj_file; traj_file.open("traj.csv"); // angles, vels, accels, torques, time. @@ -339,7 +341,10 @@ int main(int argc, char** argv) { vals.push_back(std::to_string(results.atDouble(PhaseKey(phase)))); t++; - string vals_str = boost::algorithm::join(vals, ","); + std::string vals_str = ""; + for (size_t j = 0; j < vals.size(); j++) { + vals_str += vals[j] + (j != vals.size() - 1 ? "," : ""); + } traj_file << vals_str << "\n"; } } diff --git a/examples/example_full_kinodynamic_walking/main_walk_forward.cpp b/examples/example_full_kinodynamic_walking/main_walk_forward.cpp index 5b6556c0f..903abac39 100644 --- a/examples/example_full_kinodynamic_walking/main_walk_forward.cpp +++ b/examples/example_full_kinodynamic_walking/main_walk_forward.cpp @@ -17,15 +17,13 @@ #include #include #include -#include +#include #include #include #include #include #include -#include -#include #include #include #include @@ -144,6 +142,9 @@ int main(int argc, char** argv) { // Collocation scheme. auto collocation = CollocationScheme::Euler; + // Set initializer. + gtdynamics::Initializer initializer; + // Graphs for transition between phases + their initial values. vector transition_graphs; vector transition_graph_init; @@ -151,7 +152,7 @@ int main(int argc, char** argv) { for (int p = 1; p < phase_cps.size(); p++) { transition_graphs.push_back(graph_builder.dynamicsFactorGraph( robot, cum_phase_steps[p - 1], trans_cps[p - 1], mu)); - transition_graph_init.push_back(ZeroValues( + transition_graph_init.push_back(initializer.ZeroValues( robot, cum_phase_steps[p - 1], gaussian_noise, trans_cps[p - 1])); } @@ -278,7 +279,7 @@ int main(int argc, char** argv) { // Initialize solution. gtsam::Values init_vals; - init_vals = gtdynamics::MultiPhaseZeroValuesTrajectory( + init_vals = initializer.MultiPhaseZeroValuesTrajectory( robot, phase_steps, transition_graph_init, dt_des, gaussian_noise, phase_cps); @@ -296,7 +297,10 @@ int main(int argc, char** argv) { for (auto&& joint : robot.joints()) { joint_names.push_back(joint->name()); } - string joint_names_str = boost::algorithm::join(joint_names, ","); + std::string joint_names_str = ""; + for (size_t j = 0; j < joint_names.size(); j++) { + joint_names_str += joint_names[j] + (j != joint_names.size() - 1 ? "," : ""); + } std::ofstream traj_file; traj_file.open("traj.csv"); // angles, vels, accels, torques, time. @@ -323,7 +327,10 @@ int main(int argc, char** argv) { vals.push_back(std::to_string(results.atDouble(PhaseKey(phase)))); t++; - string vals_str = boost::algorithm::join(vals, ","); + std::string vals_str = ""; + for (size_t j = 0; j < vals.size(); j++) { + vals_str += vals[j] + (j != vals.size() - 1 ? "," : ""); + } traj_file << vals_str << "\n"; } } diff --git a/examples/example_full_kinodynamic_walking/sim.py b/examples/example_full_kinodynamic_walking/sim.py index 7a5ee90f0..4a743bdb5 100644 --- a/examples/example_full_kinodynamic_walking/sim.py +++ b/examples/example_full_kinodynamic_walking/sim.py @@ -14,9 +14,9 @@ _ = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) # To load plane SDF. p.setGravity(0, 0, -9.8) -planeId = p.loadURDF("plane.urdf") +planeId = p.loadURDF(gtd.URDF_PATH + "plane.urdf") p.changeDynamics(planeId, -1, lateralFriction=1) -quad_id = p.loadURDF("vision60.urdf", [0, 0, 0.21], [0, 0, 0, 1], False, +quad_id = p.loadURDF(gtd.URDF_PATH + "vision60.urdf", [0, 0, 0.21], [0, 0, 0, 1], False, False) joint_to_jid_map = {} diff --git a/examples/example_full_kinodynamic_walking/vision60.urdf b/examples/example_full_kinodynamic_walking/vision60.urdf deleted file mode 100644 index e5effde7b..000000000 --- a/examples/example_full_kinodynamic_walking/vision60.urdf +++ /dev/null @@ -1,480 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/examples/example_inverted_pendulum_trajectory_optimization/CMakeLists.txt b/examples/example_inverted_pendulum_trajectory_optimization/CMakeLists.txt index 1878629bc..892560cb8 100644 --- a/examples/example_inverted_pendulum_trajectory_optimization/CMakeLists.txt +++ b/examples/example_inverted_pendulum_trajectory_optimization/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.9...3.29) project(example_inverted_pendulum_trajectory_optimization C CXX) # Build Executable diff --git a/examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py b/examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py index 51cc6871a..c46e65ecc 100644 --- a/examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py +++ b/examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py @@ -10,7 +10,6 @@ """ import argparse -import math import os.path as osp import gtdynamics as gtd @@ -20,27 +19,6 @@ Isotropic = gtsam.noiseModel.Isotropic - -def jointAngleKey(id_, t=0): - """Get joint angle key.""" - return gtd.JointAngleKey(id_, t).key() - - -def jointVelKey(id_, t=0): - """Get joint velocity key.""" - return gtd.JointVelKey(id_, t).key() - - -def jointAccelKey(id_, t=0): - """Get joint acceleration key.""" - return gtd.JointAccelKey(id_, t).key() - - -def torqueKey(id_, t=0): - """Get torque key.""" - return gtd.TorqueKey(id_, t).key() - - URDF_PATH = osp.join(osp.dirname(osp.realpath(__file__)), "..", "..", "models", "urdfs") @@ -55,7 +33,7 @@ def run(args): T = 3.0 # seconds dt = 1. / 100 # Time horizon (s) and timestep duration (s). - t_steps = math.ceil(T / dt) # Timesteps. + t_steps = np.ceil(T / dt) # Timesteps. # Noise models: dynamics_model = Isotropic.Sigma(1, 1e-5) # Dynamics constraints. @@ -71,29 +49,29 @@ def run(args): # Add initial conditions to trajectory factor graph. theta_i = 0 dtheta_i = 0 - graph.addPriorDouble(jointAngleKey(j1_id, 0), theta_i, dynamics_model) - graph.addPriorDouble(jointVelKey(j1_id, 0), dtheta_i, dynamics_model) + graph.addPriorDouble(gtd.JointAngleKey(j1_id, 0), theta_i, dynamics_model) + graph.addPriorDouble(gtd.JointVelKey(j1_id, 0), dtheta_i, dynamics_model) # Add state and min torque objectives to trajectory factor graph. - theta_T = math.pi + theta_T = np.pi dtheta_T = 0 ddtheta_T = 0 - graph.addPriorDouble(jointAngleKey(j1_id, t_steps), theta_T, + graph.addPriorDouble(gtd.JointAngleKey(j1_id, t_steps), theta_T, objectives_model) - graph.addPriorDouble(jointVelKey(j1_id, t_steps), dtheta_T, + graph.addPriorDouble(gtd.JointVelKey(j1_id, t_steps), dtheta_T, objectives_model) - graph.addPriorDouble(jointAccelKey(j1_id, t_steps), ddtheta_T, + graph.addPriorDouble(gtd.JointAccelKey(j1_id, t_steps), ddtheta_T, objectives_model) # Apply state costs along the way if asked. if args.state_costs: for t in range(t_steps + 1): - graph.addPriorDouble(jointAngleKey(j1_id, t), theta_T, + graph.addPriorDouble(gtd.JointAngleKey(j1_id, t), theta_T, objectives_model) # Do apply control costs at all steps. for t in range(t_steps + 1): - graph.add(gtd.MinTorqueFactor(torqueKey(j1_id, t), control_model)) + graph.add(gtd.MinTorqueFactor(gtd.TorqueKey(j1_id, t), control_model)) # Initialize solution. init_vals = gtd.ZeroValuesTrajectory(ip, t_steps, 0, 0.0, None) diff --git a/examples/example_inverted_pendulum_trajectory_optimization/main.cpp b/examples/example_inverted_pendulum_trajectory_optimization/main.cpp index affee65fa..418e03a69 100644 --- a/examples/example_inverted_pendulum_trajectory_optimization/main.cpp +++ b/examples/example_inverted_pendulum_trajectory_optimization/main.cpp @@ -15,14 +15,12 @@ #include #include #include -#include +#include #include #include #include #include -#include -#include #include #include #include @@ -74,7 +72,8 @@ int main(int argc, char** argv) { graph.emplace_shared(TorqueKey(j1_id, t), control_model); // Initialize solution. - auto init_vals = ZeroValuesTrajectory(ip, t_steps, 0, 0.0); + Initializer initializer; + auto init_vals = initializer.ZeroValuesTrajectory(ip, t_steps, 0, 0.0); gtsam::LevenbergMarquardtParams params; params.setVerbosityLM("SUMMARY"); gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params); @@ -92,7 +91,11 @@ int main(int argc, char** argv) { TorqueKey(j1_id, t)}; std::vector vals = {std::to_string(t_elapsed)}; for (auto&& k : keys) vals.push_back(std::to_string(results.atDouble(k))); - traj_file << boost::algorithm::join(vals, ",") << "\n"; + std::string vals_str = ""; + for (size_t j = 0; j < vals.size(); j++) { + vals_str += vals[j] + (j != vals.size() - 1 ? "," : ""); + } + traj_file << vals_str << "\n"; } traj_file.close(); diff --git a/examples/example_inverted_pendulum_trajectory_optimization/sim.py b/examples/example_inverted_pendulum_trajectory_optimization/sim.py index 734c69be0..e5e9d5bc3 100644 --- a/examples/example_inverted_pendulum_trajectory_optimization/sim.py +++ b/examples/example_inverted_pendulum_trajectory_optimization/sim.py @@ -9,12 +9,14 @@ import pandas as pd import numpy as np +import gtdynamics as gtd + # pylint: disable=I1101, C0103 _ = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) # To load plane SDF. p.setGravity(0, 0, -9.8) -quad_id = p.loadURDF("inverted_pendulum.urdf", [0, 0, 0], [0, 0, 0, 1], False, +quad_id = p.loadURDF(gtd.URDF_PATH + "inverted_pendulum.urdf", [0, 0, 0], [0, 0, 0, 1], False, True) while True: diff --git a/examples/example_quadruped_mp/CMakeLists.txt b/examples/example_quadruped_mp/CMakeLists.txt index 6c1fcd178..fa93199c9 100644 --- a/examples/example_quadruped_mp/CMakeLists.txt +++ b/examples/example_quadruped_mp/CMakeLists.txt @@ -1,9 +1,9 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.9...3.29) project(example_quadruped_mp C CXX) # Build Executable add_executable(${PROJECT_NAME} main.cpp) -target_link_libraries(${PROJECT_NAME} PUBLIC gtdynamics) +target_link_libraries(${PROJECT_NAME} PRIVATE gtdynamics) target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_PREFIX_PATH}/include) add_custom_target( diff --git a/examples/example_quadruped_mp/kinematic_mp_sim.py b/examples/example_quadruped_mp/sim.py similarity index 94% rename from examples/example_quadruped_mp/kinematic_mp_sim.py rename to examples/example_quadruped_mp/sim.py index bb73e668f..8f30d9fce 100644 --- a/examples/example_quadruped_mp/kinematic_mp_sim.py +++ b/examples/example_quadruped_mp/sim.py @@ -17,9 +17,9 @@ _ = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) # To load plane SDF. p.setGravity(0, 0, -9.8) -planeId = p.loadURDF("plane.urdf") +planeId = p.loadURDF(gtd.URDF_PATH + "plane.urdf") p.changeDynamics(planeId, -1, lateralFriction=1) -quad_id = p.loadURDF("vision60.urdf", [0, 0, 0.21], [0, 0, 0, 1], False, +quad_id = p.loadURDF(gtd.URDF_PATH + "vision60.urdf", [0, 0, 0.21], [0, 0, 0, 1], False, False) joint_to_jid_map = {} diff --git a/examples/example_quadruped_mp/vision60.urdf b/examples/example_quadruped_mp/vision60.urdf deleted file mode 100644 index 0602273ba..000000000 --- a/examples/example_quadruped_mp/vision60.urdf +++ /dev/null @@ -1,480 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/examples/example_spider_walking/CMakeLists.txt b/examples/example_spider_walking/CMakeLists.txt index 9a22330fd..0761ef4e1 100644 --- a/examples/example_spider_walking/CMakeLists.txt +++ b/examples/example_spider_walking/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.9...3.29) project(example_spider_walking C CXX) # Build Executables diff --git a/examples/example_spider_walking/main.cpp b/examples/example_spider_walking/main.cpp index d6961ac81..fb0053ec8 100644 --- a/examples/example_spider_walking/main.cpp +++ b/examples/example_spider_walking/main.cpp @@ -19,8 +19,6 @@ #include #include -#include -#include #include #include #include @@ -49,9 +47,9 @@ Trajectory getTrajectory(const Robot& robot, size_t repeat) { links.insert(links.end(), even_links.begin(), even_links.end()); const Point3 contact_in_com(0, 0.19, 0); - auto stationary = boost::make_shared(links, contact_in_com); - auto odd = boost::make_shared(odd_links, contact_in_com); - auto even = boost::make_shared(even_links, contact_in_com); + auto stationary = std::make_shared(links, contact_in_com); + auto odd = std::make_shared(odd_links, contact_in_com); + auto even = std::make_shared(even_links, contact_in_com); FootContactVector states = {stationary, even, stationary, odd}; std::vector phase_lengths = {1,2,1,2}; @@ -135,8 +133,9 @@ int main(int argc, char** argv) { // Initialize solution. double gaussian_noise = 1e-5; + Initializer initializer; gtsam::Values init_vals = - trajectory.multiPhaseInitialValues(robot, gaussian_noise, desired_dt); + trajectory.multiPhaseInitialValues(robot, initializer, gaussian_noise, desired_dt); // Optimize! gtsam::LevenbergMarquardtParams params; diff --git a/examples/example_spider_walking/main_rotate.cpp b/examples/example_spider_walking/main_rotate.cpp index fb5eac02e..c67c3f833 100644 --- a/examples/example_spider_walking/main_rotate.cpp +++ b/examples/example_spider_walking/main_rotate.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include #include @@ -29,8 +29,6 @@ #include #include -#include -#include #include #include #include @@ -177,13 +175,14 @@ int main(int argc, char** argv) { auto collocation = gtdynamics::CollocationScheme::Euler; // Graphs for transition between phases + their initial values. + Initializer initializer; vector transition_graphs; for (int p = 1; p < phase_cps.size(); p++) { std::cout << "Creating transition graph" << std::endl; transition_graphs.push_back(graph_builder.dynamicsFactorGraph( robot, cum_phase_steps[p - 1], trans_cps[p - 1], mu)); std::cout << "Creating initial values" << std::endl; - transition_graph_init.push_back(ZeroValues( + transition_graph_init.push_back(initializer.ZeroValues( robot, cum_phase_steps[p - 1], gaussian_noise, trans_cps[p - 1])); } @@ -339,7 +338,7 @@ int main(int argc, char** argv) { // Initialize solution. gtsam::Values init_vals; - init_vals = gtdynamics::MultiPhaseZeroValuesTrajectory( + init_vals = initializer.MultiPhaseZeroValuesTrajectory( robot, phase_steps, transition_graph_init, dt_des, gaussian_noise, phase_cps); @@ -354,7 +353,10 @@ int main(int argc, char** argv) { vector joint_names; for (auto&& joint : robot.joints()) joint_names.push_back(joint->name()); - string joint_names_str = boost::algorithm::join(joint_names, ","); + std::string joint_names_str = ""; + for (size_t j = 0; j < joint_names.size(); j++) { + joint_names_str += joint_names[j] + (j != joint_names.size() - 1 ? "," : ""); + } std::ofstream traj_file; traj_file.open("rotation_traj.csv"); @@ -376,7 +378,10 @@ int main(int argc, char** argv) { vals.push_back(std::to_string(Torque(results, joint->id(), t))); vals.push_back(std::to_string(results.atDouble(PhaseKey(phase)))); t++; - string vals_str = boost::algorithm::join(vals, ","); + std::string vals_str = ""; + for (size_t j = 0; j < vals.size(); j++) { + vals_str += vals[j] + (j != vals.size() - 1 ? "," : ""); + } traj_file << vals_str << "\n"; } } diff --git a/examples/example_spider_walking/sim.py b/examples/example_spider_walking/sim.py index 3ae04fae1..11269f92d 100644 --- a/examples/example_spider_walking/sim.py +++ b/examples/example_spider_walking/sim.py @@ -16,7 +16,7 @@ p.setAdditionalSearchPath(pybullet_data.getDataPath()) # To load plane SDF. p.setGravity(0, 0, -9.8) p.setRealTimeSimulation(0) -planeId = p.loadURDF("walls.urdf") +planeId = p.loadURDF(gtd.URDF_PATH + "walls.urdf") p.changeDynamics(planeId, -1, lateralFriction=1) robot = p.loadSDF(gtd.SDF_PATH + "/spider_alt.sdf") # loadSDF returns a list of objects; the first is the integer ID. @@ -32,10 +32,10 @@ jinfo = p.getJointInfo(robot_id, i) joint_to_jid_map[jinfo[1].decode("utf-8")] = jinfo[0] -#Read walk forward trajectory file +# Read trajectory file +# Use 'forward_traj.csv' for forward +# Use 'rotation_traj.csv' for rotation df = pd.read_csv('forward_traj.csv') -# Read rotation trajectory file -# df = pd.read_csv('rotation_traj.csv') print(df.columns) input("Press ENTER to continue.") diff --git a/examples/scripts/CMakeLists.txt b/examples/scripts/CMakeLists.txt new file mode 100644 index 000000000..b7b255dfd --- /dev/null +++ b/examples/scripts/CMakeLists.txt @@ -0,0 +1,2 @@ +gtsamAddExamplesGlob("*.cpp" "" "gtdynamics" ON) + diff --git a/scripts/alejandro_yetong_01_id_four_bar.cpp b/examples/scripts/alejandro_yetong_01_id_four_bar.cpp similarity index 86% rename from scripts/alejandro_yetong_01_id_four_bar.cpp rename to examples/scripts/alejandro_yetong_01_id_four_bar.cpp index 59e46e62e..226536375 100644 --- a/scripts/alejandro_yetong_01_id_four_bar.cpp +++ b/examples/scripts/alejandro_yetong_01_id_four_bar.cpp @@ -11,24 +11,23 @@ * @author Alejandro Escontrela and Yetong Zhang */ +#include +#include +#include +#include #include #include #include #include -#include "gtdynamics/dynamics/DynamicsGraph.h" -#include "gtdynamics/factors/MinTorqueFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/initialize_solution_utils.h" - using namespace gtdynamics; int main(int argc, char** argv) { using four_bar_linkage_pure::planar_axis; auto robot = four_bar_linkage_pure::getRobot(); - gtsam::Values joint_angles_vels_accels; + Initializer initializer; gtsam::Vector3 gravity(0, -10, 0); robot = robot.fixLink("l1"); @@ -43,7 +42,7 @@ int main(int argc, char** argv) { graph_builder.dynamicsFactorGraph(robot, 0); // Inverse dynamics priors. We care about the torques. - gtsam::Vector joint_accels = gtsam::Vector::Zero(robot.numJoints()); + gtsam::Values joint_angles_vels_accels = initializer.ZeroValues(robot, 0); gtsam::NonlinearFactorGraph prior_factors = graph_builder.inverseDynamicsPriors(robot, 0, joint_angles_vels_accels); @@ -53,8 +52,7 @@ int main(int argc, char** argv) { prior_factors.addPrior(PoseKey(i, 0), link->bMcom(), gtsam::noiseModel::Constrained::All(6)); prior_factors.addPrior( - TwistKey(i, 0), gtsam::Z_6x1, - gtsam::noiseModel::Constrained::All(6)); + TwistKey(i, 0), gtsam::Z_6x1, gtsam::noiseModel::Constrained::All(6)); } graph.add(prior_factors); @@ -65,7 +63,7 @@ int main(int argc, char** argv) { gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1))); // Initialize solution. - gtsam::Values init_values = ZeroValues(robot, 0); + gtsam::Values init_values = initializer.ZeroValues(robot, 0); std::cout << "\033[1;32;7mFactor Graph Optimization:\033[0m" << std::endl; graph_builder.printGraph(graph); diff --git a/scripts/constrainedExample.h b/examples/scripts/constrainedExample.h similarity index 89% rename from scripts/constrainedExample.h rename to examples/scripts/constrainedExample.h index 4ff924e1e..e9e42b07b 100644 --- a/scripts/constrainedExample.h +++ b/examples/scripts/constrainedExample.h @@ -18,7 +18,6 @@ #include #include #include -#include #include namespace gtdynamics { @@ -33,8 +32,7 @@ using gtsam::Vector2; using gtsam::Vector2_; /// Exponential function e^x. -double exp_func(const double& x, - gtsam::OptionalJacobian<1, 1> H1 = boost::none) { +double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { double result = exp(x); if (H1) H1->setConstant(result); return result; @@ -52,7 +50,7 @@ class PowFunctor { PowFunctor(const double& c) : c_(c) {} double operator()(const double& x, - gtsam::OptionalJacobian<1, 1> H1 = boost::none) const { + gtsam::OptionalJacobian<1, 1> H1 = {}) const { if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); return pow(x, c_); } diff --git a/scripts/forward_dynamics_simple.cpp b/examples/scripts/forward_dynamics_simple.cpp similarity index 85% rename from scripts/forward_dynamics_simple.cpp rename to examples/scripts/forward_dynamics_simple.cpp index 8f3a5ede3..4d73ab699 100644 --- a/scripts/forward_dynamics_simple.cpp +++ b/examples/scripts/forward_dynamics_simple.cpp @@ -12,16 +12,15 @@ * @author Alejandro Escontrela, Stephanie McCormick, and Yetong Zhang */ +#include +#include +#include #include #include #include #include -#include "gtdynamics/dynamics/DynamicsGraph.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/initialize_solution_utils.h" - using namespace gtdynamics; int main(int argc, char** argv) { @@ -33,8 +32,7 @@ int main(int argc, char** argv) { // Build a factor graph with all the kinodynamics constraints. DynamicsGraph dg_builder = DynamicsGraph(gravity, planar_axis); - gtsam::NonlinearFactorGraph dfg = - dg_builder.dynamicsFactorGraph(robot, 0); + gtsam::NonlinearFactorGraph dfg = dg_builder.dynamicsFactorGraph(robot, 0); // Specify the priors and add them to the factor graph. gtsam::Values known_values; @@ -49,7 +47,8 @@ int main(int argc, char** argv) { dfg.add(fd_priors); // Obtain solution initialization. - gtsam::Values init_values = ZeroValues(robot, 0); + Initializer initializer; + gtsam::Values init_values = initializer.ZeroValues(robot, 0); // Compute the forward dynamics. gtsam::LevenbergMarquardtOptimizer optimizer(dfg, init_values); diff --git a/scripts/gerry00_ForwardDynamicsPrismatic.cpp b/examples/scripts/gerry00_ForwardDynamicsPrismatic.cpp similarity index 90% rename from scripts/gerry00_ForwardDynamicsPrismatic.cpp rename to examples/scripts/gerry00_ForwardDynamicsPrismatic.cpp index 849ff0c73..7a6eec7e8 100644 --- a/scripts/gerry00_ForwardDynamicsPrismatic.cpp +++ b/examples/scripts/gerry00_ForwardDynamicsPrismatic.cpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include #include @@ -54,16 +54,19 @@ int main(int argc, char** argv) { InsertJointVel(&theta_and_theta_dot, j2, 0, 0.1); InsertJointVel(&theta_and_theta_dot, j3, 0, 0.0); - std::vector taus; + // Insert torque values for all timesteps for (int t = 0; t <= T; t++) { - taus.push_back(gtsam::Vector::Zero(3)); + InsertTorque(&theta_and_theta_dot, j1, t, 0.0); + InsertTorque(&theta_and_theta_dot, j2, t, 0.0); + InsertTorque(&theta_and_theta_dot, j3, t, 0.0); } auto fd_priors = graph_builder.trajectoryFDPriors(simple_rpr, T, theta_and_theta_dot); kdfg.add(fd_priors); // Initialize solution. - auto init_values = ZeroValuesTrajectory(simple_rpr, T, 0); + Initializer initializer; + auto init_values = initializer.ZeroValuesTrajectory(simple_rpr, T, 0); // Compute the forward dynamics. gtsam::LevenbergMarquardtOptimizer optimizer(kdfg, init_values); diff --git a/scripts/gerry00_visualize.m b/examples/scripts/gerry00_visualize.m similarity index 100% rename from scripts/gerry00_visualize.m rename to examples/scripts/gerry00_visualize.m diff --git a/examples/scripts/nithya00_constrained_opt_benchmark.cpp b/examples/scripts/nithya00_constrained_opt_benchmark.cpp new file mode 100644 index 000000000..237f63052 --- /dev/null +++ b/examples/scripts/nithya00_constrained_opt_benchmark.cpp @@ -0,0 +1,127 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file nithya_yetong00_constrainedopt_benchmark.cpp + * @brief Benchmark penalty method optimizer and augmented lagrangian optimizer + * on a toy example, and output intermediate results to file. + * @author: Nithya Jayakumar + * @author: Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace gtsam; + +namespace { + +/// Exponential function e^x. +double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { + double result = exp(x); + if (H1) H1->setConstant(result); + return result; +} + +/// Exponential expression e^x. +Double_ exp(const Double_& x) { return Double_(exp_func, x); } + +/// Pow functor used for pow function. +class PowFunctor { + private: + double c_; + + public: + PowFunctor(const double& c) : c_(c) {} + + double operator()(const double& x, + gtsam::OptionalJacobian<1, 1> H1 = {}) const { + if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); + return pow(x, c_); + } +}; + +/// Pow function. +Double_ pow(const Double_& x, const double& c) { + PowFunctor pow_functor(c); + return Double_(pow_functor, x); +} + +/// Plus between Double expression and double. +Double_ operator+(const Double_& x, const double& d) { + return x + Double_(d); +} + +/// Negative sign operator. +Double_ operator-(const Double_& x) { return Double_(0.0) - x; } + +Symbol x1_key('x', 1); +Symbol x2_key('x', 2); +Double_ x1(x1_key), x2(x2_key); + +} // namespace + +int main(int argc, char** argv) { + /// Create a constrained optimization problem with 2 cost factors and 1 + /// constraint. + NonlinearFactorGraph graph; + gtsam::Double_ f1 = x1 + exp(-x2); + gtsam::Double_ f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; + auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); + graph.add(ExpressionFactor(cost_noise, 0., f1)); + graph.add(ExpressionFactor(cost_noise, 0., f2)); + + NonlinearEqualityConstraints constraints; + double sigma = 1.0; + gtsam::Double_ g1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); + constraints.emplace_shared>( + g1, 0.0, Vector1(sigma)); + + /// Create initial values. + Values init_values; + init_values.insert(x1_key, -0.2); + init_values.insert(x2_key, -0.2); + + auto problem = ConstrainedOptProblem::EqConstrainedOptProblem(graph, constraints); + + /// Solve the constraint problem with Penalty Method optimizer. + auto penalty_params = std::make_shared(); + gtsam::PenaltyOptimizer penalty_optimizer(problem, init_values, penalty_params); + Values penalty_results = penalty_optimizer.optimize(); + + /// Solve the constraint problem with Augmented Lagrangian optimizer. + auto almParams = std::make_shared(); + gtsam::AugmentedLagrangianOptimizer augl_optimizer(problem, init_values, + almParams); + Values augl_results = augl_optimizer.optimize(); + + /// Function to evaluate constraint violation. + auto evaluate_constraint = [&constraints](const gtsam::Values& values) { + return constraints.violationNorm(values); + }; + + /// Function to evaluate cost. + auto evaluate_cost = [&graph](const gtsam::Values& values) { + double cost = graph.error(values); + return cost; + }; + + /// Write results to files for plotting. + std::cout << "Penalty result: cost=" << evaluate_cost(penalty_results) + << " constraint=" << evaluate_constraint(penalty_results) << "\n"; + std::cout << "Augmented Lagrangian result: cost=" + << evaluate_cost(augl_results) + << " constraint=" << evaluate_constraint(augl_results) << "\n"; + return 0; +} diff --git a/scripts/nithya00_constrained_opt_benchmark.py b/examples/scripts/nithya00_constrained_opt_benchmark.py similarity index 100% rename from scripts/nithya00_constrained_opt_benchmark.py rename to examples/scripts/nithya00_constrained_opt_benchmark.py diff --git a/scripts/stephanie01_ForwardDynamicsThreeLink.cpp b/examples/scripts/stephanie01_ForwardDynamicsThreeLink.cpp similarity index 85% rename from scripts/stephanie01_ForwardDynamicsThreeLink.cpp rename to examples/scripts/stephanie01_ForwardDynamicsThreeLink.cpp index fbb1f6ec7..870fe1622 100644 --- a/scripts/stephanie01_ForwardDynamicsThreeLink.cpp +++ b/examples/scripts/stephanie01_ForwardDynamicsThreeLink.cpp @@ -12,18 +12,19 @@ * @author: Stephanie McCormick */ +#include +#include +#include #include -#include "gtdynamics/dynamics/DynamicsGraph.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/initialize_solution_utils.h" - using namespace gtdynamics; int main(int argc, char **argv) { // Load the three-link robot using the relevant namespace from RobotModels. auto robot = simple_rr::getRobot(); + Initializer initializer; + // Build the factor graph for the robot. robot = robot.fixLink("link_0"); gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished(); @@ -33,13 +34,12 @@ int main(int argc, char **argv) { auto graph = graph_builder.dynamicsFactorGraph(robot, 0); // Add forward dynamics priors to factor graph. - gtsam::Values values; - + gtsam::Values values = initializer.ZeroValues(robot, 0); auto priorFactors = graph_builder.forwardDynamicsPriors(robot, 0, values); graph.add(priorFactors); // Generate initial values to be passed in to the optimization function. - auto init_values = ZeroValues(robot, 0); + auto init_values = initializer.ZeroValues(robot, 0); // Compute forward dynamics. gtsam::GaussNewtonOptimizer optimizer(graph, init_values); diff --git a/scripts/stephanie02_spider_sdf.cpp b/examples/scripts/stephanie02_spider_sdf.cpp similarity index 88% rename from scripts/stephanie02_spider_sdf.cpp rename to examples/scripts/stephanie02_spider_sdf.cpp index bde2de549..b965e764a 100644 --- a/scripts/stephanie02_spider_sdf.cpp +++ b/examples/scripts/stephanie02_spider_sdf.cpp @@ -11,10 +11,10 @@ * @author Stephanie McCormick */ -#include +#include +#include -#include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/universal_robot/sdf.h" +#include using namespace gtdynamics; diff --git a/gtdynamics.i b/gtdynamics.i index 1b3c6df59..ec79ad8e7 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -10,13 +10,14 @@ namespace gtdynamics { #include const string URDF_PATH = kUrdfPath; const string SDF_PATH = kSdfPath; +const string DATA_PATH = kDataPath; // Global variable for key formatting const gtsam::KeyFormatter GTDKeyFormatter; /********************** factors **********************/ #include -class JointMeasurementFactor : gtsam::NonlinearFactor { +class JointMeasurementFactor : gtsam::NoiseModelFactor { JointMeasurementFactor(gtsam::Key wTp_key, gtsam::Key wTc_key, const gtsam::noiseModel::Base *cost_model, const gtdynamics::Joint *joint, @@ -25,12 +26,12 @@ class JointMeasurementFactor : gtsam::NonlinearFactor { const gtdynamics::Joint *joint, double joint_coordinate, size_t k); + double measured() const; void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter = gtdynamics::GTDKeyFormatter); }; #include - #include class ForwardKinematicsFactor : gtsam::NoiseModelFactor { ForwardKinematicsFactor(gtsam::Key bTl1_key, gtsam::Key bTl2_key, @@ -77,7 +78,7 @@ class ContactPointFactor : gtsam::NoiseModelFactor { }; #include -class MinTorqueFactor : gtsam::NonlinearFactor { +class MinTorqueFactor : gtsam::NoiseModelFactor { MinTorqueFactor(gtsam::Key torque_key, const gtsam::noiseModel::Base *cost_model); @@ -85,22 +86,22 @@ class MinTorqueFactor : gtsam::NonlinearFactor { const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter); }; -/// TODO(yetong): remove the wrapper for WrenchFactor once EqualityConstraint is -/// wrapped (Issue #319). +/// TODO(yetong): remove the wrapper for WrenchFactor once equality constraints +/// are wrapped (Issue #319). #include -gtsam::NonlinearFactor* WrenchFactor( +gtsam::NoiseModelFactor* WrenchFactor( const gtsam::noiseModel::Base *cost_model, const gtdynamics::Link *link, - const std::vector wrench_keys, int t = 0, - const boost::optional &gravity); + const std::vector wrench_keys, int t = 0, + const std::optional &gravity); #include -class EulerPoseCollocationFactor : gtsam::NonlinearFactor { +class EulerPoseCollocationFactor : gtsam::NoiseModelFactor { EulerPoseCollocationFactor(gtsam::Key pose_t0_key, gtsam::Key pose_t1_key, gtsam::Key twist_key, gtsam::Key dt_key, const gtsam::noiseModel::Base *cost_model); }; -class TrapezoidalPoseCollocationFactor : gtsam::NonlinearFactor { +class TrapezoidalPoseCollocationFactor : gtsam::NoiseModelFactor { TrapezoidalPoseCollocationFactor(gtsam::Key pose_t0_key, gtsam::Key pose_t1_key, gtsam::Key twist_t0_key, @@ -108,13 +109,13 @@ class TrapezoidalPoseCollocationFactor : gtsam::NonlinearFactor { const gtsam::noiseModel::Base *cost_model); }; -class EulerTwistCollocationFactor : gtsam::NonlinearFactor { +class EulerTwistCollocationFactor : gtsam::NoiseModelFactor { EulerTwistCollocationFactor(gtsam::Key twist_t0_key, gtsam::Key twist_t1_key, gtsam::Key accel_key, gtsam::Key dt_key, const gtsam::noiseModel::Base *cost_model); }; -class TrapezoidalTwistCollocationFactor : gtsam::NonlinearFactor { +class TrapezoidalTwistCollocationFactor : gtsam::NoiseModelFactor { TrapezoidalTwistCollocationFactor(gtsam::Key twist_t0_key, gtsam::Key twist_t1_key, gtsam::Key accel_t0_key, @@ -123,7 +124,7 @@ class TrapezoidalTwistCollocationFactor : gtsam::NonlinearFactor { }; #include -class ContactHeightFactor : gtsam::NonlinearFactor { +class ContactHeightFactor : gtsam::NoiseModelFactor { ContactHeightFactor(gtsam::Key pose_key, gtsam::noiseModel::Base *cost_model, const gtsam::Point3 &cTcom, const gtsam::Vector3 &gravity, double ground_plane_height = 0.0); @@ -137,7 +138,8 @@ class ContactHeightFactor : gtsam::NonlinearFactor { class Link { Link(); Link(int id, const string &name, const double mass, - const Matrix &inertia, const gtsam::Pose3 &bMcom, const gtsam::Pose3 &bMlink, bool is_fixed = false) ; + const gtsam::Matrix &inertia, const gtsam::Pose3 &bMcom, + const gtsam::Pose3 &bMlink, bool is_fixed = false); gtdynamics::Link *shared(); int id() const; @@ -151,7 +153,7 @@ class Link { string name() const; double mass() const; const gtsam::Pose3 ¢erOfMass(); - const Matrix &inertia(); + const gtsam::Matrix &inertia(); gtsam::Matrix6 inertiaMatrix(); void print(const std::string &s = "") const; @@ -159,7 +161,6 @@ class Link { static gtdynamics::Link fix(const gtdynamics::Link& link); static gtdynamics::Link fix(const gtdynamics::Link& link, gtsam::Pose3 &fixed_pose); static gtdynamics::Link unfix(const gtdynamics::Link& link); - }; /********************** joint **********************/ @@ -167,6 +168,7 @@ class Link { #include #include #include +#include class JointParams { JointParams(); @@ -181,27 +183,30 @@ class JointParams { }; virtual class Joint { + enum Type { Revolute, Prismatic, Screw, Fixed }; + uint8_t id() const; const gtsam::Pose3 &jMp() const; const gtsam::Pose3 &jMc() const; gtsam::Pose3 pMc() const; string name() const; - const Vector& pScrewAxis() const; - const Vector& cScrewAxis() const; - Vector screwAxis(const gtdynamics::Link *link) const; + gtdynamics::Type type() const; + const gtsam::Vector &pScrewAxis() const; + const gtsam::Vector &cScrewAxis() const; + gtsam::Vector screwAxis(const gtdynamics::Link *link) const; gtsam::Key key() const; string name() const; - gtdynamics::Link* otherLink(const gtdynamics::Link* link); - std::vector links() const; - gtdynamics::Link* parent() const; - gtdynamics::Link* child() const; + gtdynamics::Link *otherLink(const gtdynamics::Link *link); + std::vector links() const; + gtdynamics::Link *parent() const; + gtdynamics::Link *child() const; }; virtual class RevoluteJoint : gtdynamics::Joint { RevoluteJoint( int id, const string &name, const gtsam::Pose3 &wTj, const gtdynamics::Link *parent_link, const gtdynamics::Link *child_link, - const Vector &axis, + const gtsam::Vector &axis, const gtdynamics::JointParams ¶meters = gtdynamics::JointParams()); void print(const string &s = "") const; }; @@ -210,7 +215,7 @@ virtual class PrismaticJoint : gtdynamics::Joint { PrismaticJoint( int id, const string &name, const gtsam::Pose3 &wTj, const gtdynamics::Link *parent_link, const gtdynamics::Link *child_link, - const Vector &axis, + const gtsam::Vector &axis, const gtdynamics::JointParams ¶meters = gtdynamics::JointParams()); void print(const string &s = "") const; }; @@ -219,11 +224,18 @@ virtual class HelicalJoint : gtdynamics::Joint { HelicalJoint( int id, const string &name, const gtsam::Pose3 &wTj, const gtdynamics::Link *parent_link, const gtdynamics::Link *child_link, - const Vector &axis, double thread_pitch, + const gtsam::Vector &axis, double thread_pitch, const gtdynamics::JointParams ¶meters = gtdynamics::JointParams()); void print(const string &s = "") const; }; +virtual class FixedJoint : gtdynamics::Joint { + FixedJoint(int id, const string &name, const gtsam::Pose3 &wTj, + const gtdynamics::Link *parent_link, + const gtdynamics::Link *child_link); + void print(const string &s = "") const; +}; + /********************** robot **********************/ #include @@ -261,18 +273,17 @@ class Robot { gtsam::Values forwardKinematics( const gtsam::Values &known_values, size_t t, - const boost::optional &prior_link_name) const; + const std::optional &prior_link_name) const; // enabling serialization functionality void serialize() const; }; #include -// This version is only for URDF files. -gtdynamics::Robot CreateRobotFromFile(const string& urdf_file_path); -gtdynamics::Robot CreateRobotFromFile(const string& file_path, - const string& model_name); - +// Only SDF files require the model_name specified.. +gtdynamics::Robot CreateRobotFromFile(const string &urdf_file_path, + const string &model_name = "", + bool preserve_fixed_joint = false); /********************** utilities **********************/ #include @@ -303,6 +314,8 @@ class OptimizationParameters { class ContactGoal { ContactGoal(const gtdynamics::PointOnLink &point_on_link, const gtsam::Point3 &goal_point); + gtdynamics::PointOnLink point_on_link; + gtsam::Point3 goal_point; gtdynamics::Link *link() const; gtsam::Point3 &contactInCoM() const; bool satisfied(const gtsam::Values &values, size_t k = 0, @@ -377,12 +390,12 @@ enum CollocationScheme { Euler, RungeKutta, Trapezoidal, HermiteSimpson }; class DynamicsGraph { DynamicsGraph(); - DynamicsGraph(const boost::optional &gravity, - const boost::optional &planar_axis); + DynamicsGraph(const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, 9.81), + const std::optional &planar_axis); DynamicsGraph(const gtdynamics::OptimizerSetting &opt); DynamicsGraph(const gtdynamics::OptimizerSetting &opt, - const boost::optional &gravity, - const boost::optional &planar_axis); + const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, 9.81), + const std::optional &planar_axis); gtsam::GaussianFactorGraph linearDynamicsGraph( const gtdynamics::Robot &robot, const int t, @@ -404,28 +417,28 @@ class DynamicsGraph { gtsam::NonlinearFactorGraph qFactors( const gtdynamics::Robot &robot, const int t, - const boost::optional &contact_points) const; + const std::optional &contact_points) const; /* return v-level nonlinear factor graph (twist related factors) */ gtsam::NonlinearFactorGraph vFactors( const gtdynamics::Robot &robot, const int t, - const boost::optional &contact_points) const; + const std::optional &contact_points) const; /* return a-level nonlinear factor graph (acceleration related factors) */ gtsam::NonlinearFactorGraph aFactors( const gtdynamics::Robot &robot, const int t, - const boost::optional &contact_points) const; + const std::optional &contact_points) const; /* return dynamics-level nonlinear factor graph (wrench related factors) */ gtsam::NonlinearFactorGraph dynamicsFactors( const gtdynamics::Robot &robot, const int t, - const boost::optional &contact_points, - const boost::optional &mu) const; + const std::optional &contact_points, + const std::optional &mu) const; gtsam::NonlinearFactorGraph dynamicsFactorGraph( const gtdynamics::Robot &robot, const int t, - const boost::optional &contact_points, - const boost::optional &mu) const; + const std::optional &contact_points, + const std::optional &mu) const; gtsam::NonlinearFactorGraph inverseDynamicsPriors( const gtdynamics::Robot &robot, const int t, @@ -445,8 +458,8 @@ class DynamicsGraph { gtsam::NonlinearFactorGraph trajectoryFG( const gtdynamics::Robot &robot, const int num_steps, const double dt, const gtdynamics::CollocationScheme collocation, - const boost::optional &contact_points, - const boost::optional &mu) const; + const std::optional &contact_points, + const std::optional &mu) const; gtsam::NonlinearFactorGraph multiPhaseTrajectoryFG( const gtdynamics::Robot &robot, @@ -499,19 +512,19 @@ class DynamicsGraph { const gtdynamics::Robot &robot, const int t, const string &link_name, const gtsam::Pose3 &target_pose) const; - static Vector jointAccels(const gtdynamics::Robot &robot, + static gtsam::Vector jointAccels(const gtdynamics::Robot &robot, const gtsam::Values &result, const int t); /* return joint velocities. */ - static Vector jointVels(const gtdynamics::Robot &robot, + static gtsam::Vector jointVels(const gtdynamics::Robot &robot, const gtsam::Values &result, const int t); /* return joint angles. */ - static Vector jointAngles(const gtdynamics::Robot &robot, + static gtsam::Vector jointAngles(const gtdynamics::Robot &robot, const gtsam::Values &result, const int t); /* return joint torques. */ - static Vector jointTorques(const gtdynamics::Robot &robot, + static gtsam::Vector jointTorques(const gtdynamics::Robot &robot, const gtsam::Values &result, const int t); static gtdynamics::JointValueMap jointAccelsMap(const gtdynamics::Robot &robot, @@ -590,6 +603,15 @@ gtsam::NonlinearFactorGraph JointsAtRestObjectives( const gtsam::SharedNoiseModel &joint_velocity_model, const gtsam::SharedNoiseModel &joint_acceleration_model, int k = 0); +class PointGoalFactor : gtsam::NoiseModelFactor { + PointGoalFactor(gtsam::Key pose_key, // + gtsam::SharedNoiseModel cost_model, // + gtsam::Point3 point_com, // + gtsam::Point3 goal_point); + + gtsam::Point3 goalPoint(); +}; + gtsam::NonlinearFactorGraph PointGoalFactors( const gtsam::SharedNoiseModel &cost_model, const gtsam::Point3 &point_com, const std::vector &goal_trajectory, uint8_t i, @@ -603,14 +625,18 @@ std::vector SimpleSwingTrajectory(const gtsam::Point3 &start, size_t num_steps); /********************** Value Initialization **********************/ -#include -gtsam::Values ZeroValues( - const gtdynamics::Robot& robot, const int t, double gaussian_noise); +#include +class Initializer { + Initializer(); -gtsam::Values ZeroValuesTrajectory( - const gtdynamics::Robot& robot, const int num_steps, const int num_phases, - double gaussian_noise, - const boost::optional& contact_points); + gtsam::Values ZeroValues( + const gtdynamics::Robot& robot, const int t, double gaussian_noise); + + gtsam::Values ZeroValuesTrajectory( + const gtdynamics::Robot& robot, const int num_steps, const int num_phases, + double gaussian_noise, + const std::optional& contact_points); +}; /********************** symbols **********************/ @@ -640,19 +666,19 @@ class DynamicsSymbol { bool equals(const gtdynamics::DynamicsSymbol& expected, double tol); }; -gtdynamics::DynamicsSymbol JointAngleKey(int j, int t=0); -gtdynamics::DynamicsSymbol JointVelKey(int j, int t=0); -gtdynamics::DynamicsSymbol JointAccelKey(int j, int t=0); -gtdynamics::DynamicsSymbol TorqueKey(int j, int t=0); -gtdynamics::DynamicsSymbol TwistKey(int i, int t=0); -gtdynamics::DynamicsSymbol TwistAccelKey(int i, int t=0); -gtdynamics::DynamicsSymbol WrenchKey(int i, int j, int t=0); -gtdynamics::DynamicsSymbol PoseKey(int i, int t=0); -gtdynamics::DynamicsSymbol ContactWrenchKey(int i, int k, int t=0); -gtdynamics::DynamicsSymbol PhaseKey(int k); -gtdynamics::DynamicsSymbol TimeKey(int t); +gtsam::Key JointAngleKey(int j, int t=0); +gtsam::Key JointVelKey(int j, int t=0); +gtsam::Key JointAccelKey(int j, int t=0); +gtsam::Key TorqueKey(int j, int t=0); +gtsam::Key TwistKey(int i, int t=0); +gtsam::Key TwistAccelKey(int i, int t=0); +gtsam::Key WrenchKey(int i, int j, int t=0); +gtsam::Key PoseKey(int i, int t=0); +gtsam::Key ContactWrenchKey(int i, int k, int t=0); +gtsam::Key PhaseKey(int k); +gtsam::Key TimeKey(int t); -///////////////////// Key Methods ///////////////////// +/******************** Key Methods ********************/ void InsertJointAngle(gtsam::Values@ values, int j, int t, double value); void InsertJointAngle(gtsam::Values @values, int j, double value); @@ -719,12 +745,9 @@ gtsam::Vector6 Wrench(const gtsam::Values &values, int i, int j, int t=0); #include class Simulator { - Simulator(const gtdynamics::Robot &robot, const gtsam::Values &initial_values); - Simulator(const gtdynamics::Robot &robot, const gtsam::Values &initial_values, - const boost::optional &gravity); Simulator(const gtdynamics::Robot &robot, const gtsam::Values &initial_values, - const gtsam::Vector3 &gravity, - const gtsam::Vector3 &planar_axis); + const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, 9.81), + const std::optional &planar_axis); void reset(const double t); void forwardDynamics(const gtsam::Values &torques); @@ -757,7 +780,7 @@ class Interval { class Phase { Phase(size_t k_start, size_t k_end, - const boost::shared_ptr &constraints); + const std::shared_ptr &constraints); int numTimeSteps() const; void print(const string &s = ""); gtsam::Matrix jointMatrix(const gtdynamics::Robot &robot, @@ -803,15 +826,15 @@ class Trajectory { const gtdynamics::CollocationScheme collocation, double mu) const; std::vector - transitionPhaseInitialValues(const gtdynamics::Robot& robot, + transitionPhaseInitialValues(const gtdynamics::Robot& robot, const gtdynamics::Initializer &initializer, double gaussian_noise) const; - gtsam::Values multiPhaseInitialValues(const gtdynamics::Robot& robot, + gtsam::Values multiPhaseInitialValues(const gtdynamics::Robot& robot, const gtdynamics::Initializer &initializer, double gaussian_noise, double dt) const; std::vector finalTimeSteps() const; const Phase &phase(size_t p) const; size_t getStartTimeStep(size_t p) const; size_t getEndTimeStep(size_t p) const; - gtsam::NonlinearFactor pointGoalFactor(const gtdynamics::Robot &robot, + gtsam::NoiseModelFactor pointGoalFactor(const gtdynamics::Robot &robot, const string &link_name, const gtdynamics::PointOnLink &cp, size_t k, const gtsam::SharedNoiseModel &cost_model, diff --git a/gtdynamics/CMakeLists.txt b/gtdynamics/CMakeLists.txt index a70446d3a..6cadf2d2f 100644 --- a/gtdynamics/CMakeLists.txt +++ b/gtdynamics/CMakeLists.txt @@ -1,5 +1,5 @@ # All subdirectories that contain source code relevant to this library. -set(SOURCE_SUBDIRS universal_robot utils factors optimizer kinematics statics dynamics) +set(SOURCE_SUBDIRS universal_robot utils factors constraints optimizer constrained_optimizer kinematics statics dynamics cmopt) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -34,14 +34,22 @@ file(GLOB sources "*.cpp") include(GNUInstallDirs) foreach(SOURCE_SUBDIR ${SOURCE_SUBDIRS}) - file(GLOB GLOB_RESULT + file(GLOB GLOB_RESULT_CPP ${CMAKE_CURRENT_SOURCE_DIR}/${SOURCE_SUBDIR}/*.cpp) - list(APPEND sources ${GLOB_RESULT}) - file(GLOB GLOB_RESULT + if(SOURCE_SUBDIR STREQUAL "constrained_optimizer" AND NOT GTDYNAMICS_WITH_SUITESPARSE) + list(REMOVE_ITEM GLOB_RESULT_CPP + "${CMAKE_CURRENT_SOURCE_DIR}/constrained_optimizer/IPOptOptimizer.cpp") + endif() + list(APPEND sources ${GLOB_RESULT_CPP}) + file(GLOB GLOB_RESULT_H ${CMAKE_CURRENT_SOURCE_DIR}/${SOURCE_SUBDIR}/*.h) - list(APPEND headers ${GLOB_RESULT}) - # Install header file to INSTALL_INCUDEDIR/gtdynamics/subdirectory. - install(FILES ${GLOB_RESULT} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME}/${SOURCE_SUBDIR}) + if(SOURCE_SUBDIR STREQUAL "constrained_optimizer" AND NOT GTDYNAMICS_WITH_SUITESPARSE) + list(REMOVE_ITEM GLOB_RESULT_H + "${CMAKE_CURRENT_SOURCE_DIR}/constrained_optimizer/IPOptOptimizer.h") + endif() + list(APPEND headers ${GLOB_RESULT_H}) + # Install header file to INSTALL_INCLUDEDIR/gtdynamics/subdirectory. + install(FILES ${GLOB_RESULT_H} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME}/${SOURCE_SUBDIR}) # Process subdirectory. message(STATUS "Building ${SOURCE_SUBDIR}") @@ -57,11 +65,27 @@ install(FILES "${PROJECT_BINARY_DIR}/${PROJECT_NAME}/config.h" DESTINATION ${CMA # Needs to be a SHARED library so that the wrapper can compile correctly add_library(gtdynamics SHARED ${sources} ${headers}) -set_target_properties(gtdynamics PROPERTIES LINKER_LANGUAGE CXX) +if(GTDYNAMICS_WITH_SUITESPARSE) + find_package(SuiteSparse_config REQUIRED) + find_package(AMD REQUIRED) + find_package(COLAMD REQUIRED) + find_package(CHOLMOD REQUIRED) + find_package(CAMD REQUIRED) + find_package(CCOLAMD REQUIRED) + find_package(SPQR REQUIRED) + find_package(ifopt REQUIRED) +endif() -## Link all dependencies -target_link_libraries(gtdynamics ${GTSAM_LIBS} ${SDFormat_LIBRARIES}) +set_target_properties(gtdynamics PROPERTIES LINKER_LANGUAGE CXX) +target_link_libraries(gtdynamics PUBLIC ${SDFormat_LIBRARIES}) +target_link_libraries(gtdynamics PUBLIC ${GTDYNAMICS_ADDITIONAL_LIBRARIES}) +target_link_libraries(gtdynamics PUBLIC ${GTSAM_LIBS}) +if(GTDYNAMICS_WITH_SUITESPARSE) + target_link_libraries (gtdynamics PUBLIC SuiteSparse::SPQR SuiteSparse::CHOLMOD) + target_link_libraries(gtdynamics PUBLIC ifopt::ifopt_ipopt) + target_compile_definitions(gtdynamics PUBLIC GTDYNAMICS_WITH_SUITESPARSE) +endif() ## Include headers needed @@ -77,7 +101,12 @@ target_include_directories(gtdynamics BEFORE PUBLIC ) target_include_directories(gtdynamics PUBLIC ${SDFormat_INCLUDE_DIRS}) +# target_include_directories(gtdynamics PUBLIC ${SuiteSparse_INCLUDE_DIRS}) +# message(STATUS "SPQR_INCLUDE_DIRS: ${SPQR_INCLUDE_DIRS}") +# message(STATUS "SUITESPARSEQR_INCLUDE_DIRS: ${SUITESPARSEQR_INCLUDE_DIRS}") +# message(STATUS "SPQR_LIBRARY_DIRS: ${SPQR_LIBRARY_DIRS}") +# message(STATUS "SUITESPARSEQR_LIBRARY_DIRS: ${SUITESPARSEQR_LIBRARY_DIRS}") ## Install library and headers. install( diff --git a/gtdynamics/cablerobot/factors/CableLengthFactor.h b/gtdynamics/cablerobot/factors/CableLengthFactor.h index 634ca0024..b6fc0a375 100644 --- a/gtdynamics/cablerobot/factors/CableLengthFactor.h +++ b/gtdynamics/cablerobot/factors/CableLengthFactor.h @@ -8,13 +8,10 @@ #pragma once #include - #include #include #include -#include - -#include +#include #include #include @@ -25,12 +22,12 @@ namespace gtdynamics { * end effector pose and cable length */ class CableLengthFactor - : public gtsam::NoiseModelFactor2 { + : public gtsam::NoiseModelFactorN { private: using Pose3 = gtsam::Pose3; using Point3 = gtsam::Point3; using This = CableLengthFactor; - using Base = gtsam::NoiseModelFactor2; + using Base = gtsam::NoiseModelFactorN; Point3 wPa_, xPb_; @@ -57,8 +54,8 @@ class CableLengthFactor */ gtsam::Vector evaluateError( const double &l, const gtsam::Pose3 &wTx, - boost::optional H_l = boost::none, - boost::optional H_wTx = boost::none) const override { + gtsam::OptionalMatrixType H_l = nullptr, + gtsam::OptionalMatrixType H_wTx = nullptr) const override { gtsam::Matrix36 wPb_H_wTx; gtsam::Matrix13 H_wPb; auto wPb = wTx.transformFrom(xPb_, H_wTx ? &wPb_H_wTx : 0); @@ -70,7 +67,7 @@ class CableLengthFactor // @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -83,13 +80,15 @@ class CableLengthFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor2", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/cablerobot/factors/CableTensionFactor.cpp b/gtdynamics/cablerobot/factors/CableTensionFactor.cpp index 2ea2d1ef9..569de1a7b 100644 --- a/gtdynamics/cablerobot/factors/CableTensionFactor.cpp +++ b/gtdynamics/cablerobot/factors/CableTensionFactor.cpp @@ -9,13 +9,10 @@ #include "CableTensionFactor.h" #include - #include #include #include -#include -#include #include #include @@ -25,8 +22,8 @@ namespace gtdynamics { /******************************************************************************/ Vector6 CableTensionFactor::computeWrench( - double t, const Pose3 &wTx, boost::optional H_t, - boost::optional H_wTx) const { + double t, const Pose3 &wTx, gtsam::OptionalMatrixType H_t, + gtsam::OptionalMatrixType H_wTx) const { // Jacobians: cable direction Matrix33 dir_H_wPb; Matrix36 wPb_H_wTx; @@ -48,8 +45,8 @@ Vector6 CableTensionFactor::computeWrench( Vector3 xf = wTx.rotation().unrotate(wf, // force in the EE frame H_wTx ? &xf_H_wRx : 0, (H_t || H_wTx) ? &xf_H_wf : 0); - Vector3 xm = cross(xPb_, xf, // moment in the EE frame - boost::none, // + Vector3 xm = cross(xPb_, xf, // moment in the EE frame + nullptr, // (H_t || H_wTx) ? &xm_H_xf : 0); Vector6 F = (Vector6() << xm, xf).finished(); @@ -64,8 +61,8 @@ Vector6 CableTensionFactor::computeWrench( /******************************************************************************/ Vector6 CableTensionFactor::computeWrenchUsingAdjoint( - double t, const Pose3 &wTx, boost::optional H_t, - boost::optional H_wTx) const { + double t, const Pose3 &wTx, gtsam::OptionalMatrixType H_t, + gtsam::OptionalMatrixType H_wTx) const { // Jacobians: cable direction Matrix33 dir_H_wPb; Matrix36 wPb_H_wTx; diff --git a/gtdynamics/cablerobot/factors/CableTensionFactor.h b/gtdynamics/cablerobot/factors/CableTensionFactor.h index 79eca5aa9..102d68eaa 100644 --- a/gtdynamics/cablerobot/factors/CableTensionFactor.h +++ b/gtdynamics/cablerobot/factors/CableTensionFactor.h @@ -9,13 +9,10 @@ #pragma once #include - #include #include #include -#include - -#include +#include #include #include @@ -26,14 +23,14 @@ namespace gtdynamics { * amongst cable tension, end effector pose, and wrench felt by the end effector */ class CableTensionFactor - : public gtsam::NoiseModelFactor3 { + : public gtsam::NoiseModelFactorN { private: using Pose3 = gtsam::Pose3; using Point3 = gtsam::Point3; using Vector6 = gtsam::Vector6; using Vector3 = gtsam::Vector3; using This = CableTensionFactor; - using Base = gtsam::NoiseModelFactor3; + using Base = gtsam::NoiseModelFactorN; Point3 wPa_, xPb_; @@ -64,18 +61,16 @@ class CableTensionFactor * @param wTx the pose of the end effector * @return Vector6: calculated wrench */ - Vector6 computeWrench( - double tension, const Pose3 &wTx, - boost::optional H_t = boost::none, - boost::optional H_wTx = boost::none) const; + Vector6 computeWrench(double tension, const Pose3 &wTx, + gtsam::OptionalMatrixType H_t = nullptr, + gtsam::OptionalMatrixType H_wTx = nullptr) const; // an alternate version of the above function that uses adjoint; will upgrade // to this version once I figure out how to get the jacobian from an Adjoint // operation Vector6 computeWrenchUsingAdjoint( - double tension, const Pose3 &wTx, - boost::optional H_t = boost::none, - boost::optional H_wTx = boost::none) const; + double tension, const Pose3 &wTx, gtsam::OptionalMatrixType H_t = nullptr, + gtsam::OptionalMatrixType H_wTx = nullptr) const; public: /** Cable wrench factor @@ -86,9 +81,9 @@ class CableTensionFactor */ gtsam::Vector evaluateError( const double &t, const Pose3 &wTx, const Vector6 &Fx, - boost::optional H_t = boost::none, - boost::optional H_wTx = boost::none, - boost::optional H_Fx = boost::none) const override { + gtsam::OptionalMatrixType H_t = nullptr, + gtsam::OptionalMatrixType H_wTx = nullptr, + gtsam::OptionalMatrixType H_Fx = nullptr) const override { Vector6 error = (Vector6() << Fx - computeWrench(t, wTx, H_t, H_wTx)).finished(); if (H_t) *H_t = -(*H_t); @@ -99,7 +94,7 @@ class CableTensionFactor // @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -112,13 +107,15 @@ class CableTensionFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor3", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp b/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp index 6376958aa..80f1a331f 100644 --- a/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp +++ b/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp @@ -12,7 +12,6 @@ #include #include -#include #include #include @@ -21,8 +20,8 @@ using namespace gtsam; namespace gtdynamics { double CableVelocityFactor::computeLdot(const Pose3 &wTx, const Vector6 &Vx, - boost::optional H_wTx, - boost::optional H_Vx) const { + gtsam::OptionalMatrixType H_wTx, + gtsam::OptionalMatrixType H_Vx) const { // Jacobians: cable direction Matrix13 H_dir; Matrix33 dir_H_wPb; diff --git a/gtdynamics/cablerobot/factors/CableVelocityFactor.h b/gtdynamics/cablerobot/factors/CableVelocityFactor.h index 8370e182a..871c730ea 100644 --- a/gtdynamics/cablerobot/factors/CableVelocityFactor.h +++ b/gtdynamics/cablerobot/factors/CableVelocityFactor.h @@ -9,13 +9,10 @@ #pragma once #include - #include #include #include -#include - -#include +#include #include #include @@ -26,14 +23,14 @@ namespace gtdynamics { * amongst cable velocity, end-effector pose, and end-effector twist */ class CableVelocityFactor - : public gtsam::NoiseModelFactor3 { + : public gtsam::NoiseModelFactorN { private: using Point3 = gtsam::Point3; using Vector3 = gtsam::Vector3; using Pose3 = gtsam::Pose3; using Vector6 = gtsam::Vector6; using This = CableVelocityFactor; - using Base = gtsam::NoiseModelFactor3; + using Base = gtsam::NoiseModelFactorN; Point3 wPa_, xPb_; @@ -61,8 +58,8 @@ class CableVelocityFactor * @return Vector6: calculated wrench */ double computeLdot(const Pose3 &wTx, const Vector6 &Vx, - boost::optional H_wTx = boost::none, - boost::optional H_Vx = boost::none) const; + gtsam::OptionalMatrixType H_wTx = nullptr, + gtsam::OptionalMatrixType H_Vx = nullptr) const; public: /** Cable factor @@ -73,9 +70,9 @@ class CableVelocityFactor */ gtsam::Vector evaluateError( const double &ldot, const Pose3 &wTx, const Vector6 &Vx, - boost::optional H_ldot = boost::none, - boost::optional H_wTx = boost::none, - boost::optional H_Vx = boost::none) const override { + gtsam::OptionalMatrixType H_ldot = nullptr, + gtsam::OptionalMatrixType H_wTx = nullptr, + gtsam::OptionalMatrixType H_Vx = nullptr) const override { double expected_ldot = computeLdot(wTx, Vx, H_wTx, H_Vx); if (H_ldot) *H_ldot = gtsam::I_1x1; if (H_wTx) *H_wTx = -(*H_wTx); @@ -85,7 +82,7 @@ class CableVelocityFactor // @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -98,13 +95,15 @@ class CableVelocityFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor3", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/cablerobot/factors/PriorFactor.h b/gtdynamics/cablerobot/factors/PriorFactor.h index 3295bef9c..b8aac64c0 100644 --- a/gtdynamics/cablerobot/factors/PriorFactor.h +++ b/gtdynamics/cablerobot/factors/PriorFactor.h @@ -8,10 +8,8 @@ #pragma once -#include - -#include #include +#include #include diff --git a/gtdynamics/cablerobot/src/cdpr_planar.py b/gtdynamics/cablerobot/src/cdpr_planar.py index a3c8fe930..560319c5b 100644 --- a/gtdynamics/cablerobot/src/cdpr_planar.py +++ b/gtdynamics/cablerobot/src/cdpr_planar.py @@ -110,16 +110,16 @@ def kinematics_factors(self, ks=[]): for ji in range(4): kfg.push_back( gtd.CableLengthFactor( - gtd.JointAngleKey(ji, k).key(), - gtd.PoseKey(self.ee_id(), k).key(), # + gtd.JointAngleKey(ji, k), + gtd.PoseKey(self.ee_id(), k), # self.costmodel_l, self.params.a_locs[ji], self.params.b_locs[ji])) kfg.push_back( gtd.CableVelocityFactor( - gtd.JointVelKey(ji, k).key(), - gtd.PoseKey(self.ee_id(), k).key(), - gtd.TwistKey(self.ee_id(), k).key(), # + gtd.JointVelKey(ji, k), + gtd.PoseKey(self.ee_id(), k), + gtd.TwistKey(self.ee_id(), k), # self.costmodel_ldot, self.params.a_locs[ji], self.params.b_locs[ji])) @@ -129,7 +129,7 @@ def kinematics_factors(self, ks=[]): kfg.push_back( gtsam.LinearContainerFactor( gtsam.JacobianFactor( - gtd.PoseKey(self.ee_id(), k).key(), + gtd.PoseKey(self.ee_id(), k), np.array([[1, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 1, 0.]]), np.zeros(3), self.costmodel_planar_pose), zeroT)) @@ -138,7 +138,7 @@ def kinematics_factors(self, ks=[]): kfg.push_back( gtsam.LinearContainerFactor( gtsam.JacobianFactor( - gtd.TwistKey(self.ee_id(), k).key(), + gtd.TwistKey(self.ee_id(), k), np.array([[1, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 1, 0.]]), np.zeros(3), self.costmodel_planar_twist), zeroV)) @@ -160,7 +160,7 @@ def dynamics_factors(self, ks=[]): """ dfg = gtsam.NonlinearFactorGraph() for k in ks: - # TODO(yetong): Use EqualityConstraint.createFactor when wrapped. + # TODO(yetong): Use gtsam equality constraints when wrapped. dfg.add( gtd.WrenchFactor(self.costmodel_wrench, self.eelink(), [ gtd.WrenchKey(self.ee_id(), 0, k), @@ -170,12 +170,12 @@ def dynamics_factors(self, ks=[]): ], k, self.params.gravity)) for ji in range(4): dfg.push_back( - gtd.CableTensionFactor( - gtd.TorqueKey(ji, k).key(), - gtd.PoseKey(self.ee_id(), k).key(), - gtd.WrenchKey(self.ee_id(), ji, - k).key(), self.costmodel_torque, - self.params.a_locs[ji], self.params.b_locs[ji])) + gtd.CableTensionFactor(gtd.TorqueKey(ji, k), + gtd.PoseKey(self.ee_id(), k), + gtd.WrenchKey(self.ee_id(), ji, k), + self.costmodel_torque, + self.params.a_locs[ji], + self.params.b_locs[ji])) return dfg def collocation_factors(self, ks=[], dt=0.01): @@ -195,15 +195,15 @@ def collocation_factors(self, ks=[], dt=0.01): for k in ks: dfg.push_back( gtd.EulerPoseCollocationFactor( - gtd.PoseKey(self.ee_id(), k).key(), - gtd.PoseKey(self.ee_id(), k + 1).key(), - gtd.TwistKey(self.ee_id(), k).key(), 0, + gtd.PoseKey(self.ee_id(), k), + gtd.PoseKey(self.ee_id(), k + 1), + gtd.TwistKey(self.ee_id(), k), 0, self.costmodel_posecollo)) dfg.push_back( gtd.EulerTwistCollocationFactor( - gtd.TwistKey(self.ee_id(), k).key(), - gtd.TwistKey(self.ee_id(), k + 1).key(), - gtd.TwistAccelKey(self.ee_id(), k).key(), 0, + gtd.TwistKey(self.ee_id(), k), + gtd.TwistKey(self.ee_id(), k + 1), + gtd.TwistAccelKey(self.ee_id(), k), 0, self.costmodel_twistcollo)) dfg.push_back(gtd.PriorFactorDouble(0, dt, self.costmodel_dt)) return dfg @@ -226,13 +226,11 @@ def priors_fk(self, ks=[], ls=[[]], ldots=[[]]): for k, l, ldot in zip(ks, ls, ldots): for ji, (lval, ldotval) in enumerate(zip(l, ldot)): graph.push_back( - gtd.PriorFactorDouble( - gtd.JointAngleKey(ji, k).key(), lval, - self.costmodel_prior_l)) + gtd.PriorFactorDouble(gtd.JointAngleKey(ji, k), lval, + self.costmodel_prior_l)) graph.push_back( - gtd.PriorFactorDouble( - gtd.JointVelKey(ji, k).key(), ldotval, - self.costmodel_prior_ldot)) + gtd.PriorFactorDouble(gtd.JointVelKey(ji, k), ldotval, + self.costmodel_prior_ldot)) return graph def priors_ik(self, ks=[], Ts=[], Vs=[]): @@ -251,13 +249,11 @@ def priors_ik(self, ks=[], Ts=[], Vs=[]): graph = gtsam.NonlinearFactorGraph() for k, T, V in zip(ks, Ts, Vs): graph.push_back( - gtsam.PriorFactorPose3( - gtd.PoseKey(self.ee_id(), k).key(), T, - self.costmodel_prior_pose)) + gtsam.PriorFactorPose3(gtd.PoseKey(self.ee_id(), k), T, + self.costmodel_prior_pose)) graph.push_back( - gtd.PriorFactorVector6( - gtd.TwistKey(self.ee_id(), k).key(), V, - self.costmodel_prior_twist)) + gtd.PriorFactorVector6(gtd.TwistKey(self.ee_id(), k), V, + self.costmodel_prior_twist)) return graph # note: I am not using the strict definitions for forward/inverse dynamics. @@ -281,9 +277,8 @@ def priors_id(self, ks=[], torquess=[[]]): for k, torques in zip(ks, torquess): for ji, torque in enumerate(torques): graph.push_back( - gtd.PriorFactorDouble( - gtd.TorqueKey(ji, k).key(), torque, - self.costmodel_prior_tau)) + gtd.PriorFactorDouble(gtd.TorqueKey(ji, k), torque, + self.costmodel_prior_tau)) return graph def priors_fd(self, ks=[], VAs=[]): @@ -302,7 +297,6 @@ def priors_fd(self, ks=[], VAs=[]): graph = gtsam.NonlinearFactorGraph() for k, VA in zip(ks, VAs): graph.push_back( - gtd.PriorFactorVector6( - gtd.TwistAccelKey(self.ee_id(), k).key(), VA, - self.costmodel_prior_twistaccel)) + gtd.PriorFactorVector6(gtd.TwistAccelKey(self.ee_id(), k), VA, + self.costmodel_prior_twistaccel)) return graph diff --git a/gtdynamics/cablerobot/src/cdpr_planar_controller.py b/gtdynamics/cablerobot/src/cdpr_planar_controller.py index 78e07a44a..7119af239 100644 --- a/gtdynamics/cablerobot/src/cdpr_planar_controller.py +++ b/gtdynamics/cablerobot/src/cdpr_planar_controller.py @@ -104,13 +104,13 @@ def create_ilqr_fg(cdpr, x0, pdes, dt, Q, R): for ji in range(4): fg.push_back( gtd.PriorFactorDouble( - gtd.TorqueKey(ji, k).key(), 0.0, + gtd.TorqueKey(ji, k), 0.0, gtsam.noiseModel.Diagonal.Precisions(R))) # state objective costs cost_x = gtsam.noiseModel.Isotropic.Sigma(6, 0.001) if Q is None else \ gtsam.noiseModel.Diagonal.Precisions(Q) for k in range(N): fg.push_back( - gtsam.PriorFactorPose3( - gtd.PoseKey(cdpr.ee_id(), k).key(), pdes[k], cost_x)) + gtsam.PriorFactorPose3(gtd.PoseKey(cdpr.ee_id(), k), pdes[k], + cost_x)) return fg diff --git a/gtdynamics/cablerobot/src/test_cdpr_planar.py b/gtdynamics/cablerobot/src/test_cdpr_planar.py index f15a97ba9..b0ea57f25 100644 --- a/gtdynamics/cablerobot/src/test_cdpr_planar.py +++ b/gtdynamics/cablerobot/src/test_cdpr_planar.py @@ -108,17 +108,15 @@ def testDynamicsInstantaneous(self): cdpr.priors_fd([0], [gtd.TwistAccel(values, cdpr.ee_id(), 0)])) # redundancy resolution dfg.push_back( - gtd.PriorFactorDouble( - gtd.TorqueKey(1, 0).key(), 0.0, - gtsam.noiseModel.Unit.Create(1))) + gtd.PriorFactorDouble(gtd.TorqueKey(1, 0), 0.0, + gtsam.noiseModel.Unit.Create(1))) dfg.push_back( - gtd.PriorFactorDouble( - gtd.TorqueKey(2, 0).key(), 0.0, - gtsam.noiseModel.Unit.Create(1))) + gtd.PriorFactorDouble(gtd.TorqueKey(2, 0), 0.0, + gtsam.noiseModel.Unit.Create(1))) # initialize and solve init = gtsam.Values(values) for ji in range(4): - init.erase(gtd.TorqueKey(ji, 0).key()) + init.erase(gtd.TorqueKey(ji, 0)) gtd.InsertTorque(init, ji, 0, -1) results = gtsam.LevenbergMarquardtOptimizer(dfg, init).optimize() self.gtsamAssertEquals(results, values) diff --git a/gtdynamics/cmopt/CMakeLists.txt b/gtdynamics/cmopt/CMakeLists.txt new file mode 100644 index 000000000..e69de29bb diff --git a/gtdynamics/cmopt/ConstraintManifold.cpp b/gtdynamics/cmopt/ConstraintManifold.cpp new file mode 100644 index 000000000..bf0b20a4e --- /dev/null +++ b/gtdynamics/cmopt/ConstraintManifold.cpp @@ -0,0 +1,149 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ConstraintManifold.cpp + * @brief Constraint manifold implementations. + * @author: Yetong Zhang + */ + +#include +#include +#include +#include +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +Values ConstraintManifold::constructValues( + const gtsam::Values &values, + const Retractor::shared_ptr &retractor, bool retract_init) { + if (retract_init) { + return retractor->retractConstraints(std::move(values)); + } else { + return values; + } +} + +/* ************************************************************************* */ +const gtsam::Value &ConstraintManifold::recover(const gtsam::Key key, + ChartJacobian H) const { + if (H) { + // choose the corresponding rows in basis + makeSureBasisConstructed(); + *H = basis_->recoverJacobian(key); + } + return values_.at(key); +} + +/* ************************************************************************* */ +ConstraintManifold ConstraintManifold::retract(const gtsam::Vector &xi, + ChartJacobian H1, + ChartJacobian H2) const { + // Compute delta for each variable and perform update. + makeSureBasisConstructed(); + // std::cout << "xi: " << xi.transpose() << "\n"; + VectorValues delta = basis_->computeTangentVector(xi); + Values new_values = retractor_->retract(values_, delta); + + // Set jacobian as 0 since they are not used for optimization. + if (H1) + throw std::runtime_error( + "ConstraintManifold retract jacobian not implemented."); + if (H2) + throw std::runtime_error( + "ConstraintManifold retract jacobian not implemented."); + + // Satisfy the constraints in the connected component. + return createWithNewValues(new_values); +} + +/* ************************************************************************* */ +gtsam::Vector ConstraintManifold::localCoordinates(const ConstraintManifold &g, + ChartJacobian H1, + ChartJacobian H2) const { + makeSureBasisConstructed(); + Vector xi = basis_->localCoordinates(values_, g.values_); + + // Set jacobian as 0 since they are not used for optimization. + if (H1) + throw std::runtime_error( + "ConstraintManifold localCoordinates jacobian not implemented."); + if (H2) + throw std::runtime_error( + "ConstraintManifold localCoordinates jacobian not implemented."); + + return xi; +} + +/* ************************************************************************* */ +void ConstraintManifold::print(const std::string &s) const { + std::cout << (s.empty() ? s : s + " ") << "ConstraintManifold" << std::endl; + values_.print(); +} + +/* ************************************************************************* */ +bool ConstraintManifold::equals(const ConstraintManifold &other, + double tol) const { + return values_.equals(other.values_, tol); +} + +/* ************************************************************************* */ +const Values ConstraintManifold::feasibleValues() const { + gtsam::LevenbergMarquardtOptimizer optimizer(constraints_->penaltyGraph(), values_); + return optimizer.optimize(); +} + +/* ************************************************************************* */ +Values EManifoldValues::baseValues() const { + Values base_values; + for (const auto &it : *this) { + base_values.insert(it.second.values()); + } + return base_values; +} + +/* ************************************************************************* */ +KeyVector EManifoldValues::keys() const { + KeyVector key_vector; + for (const auto &it : *this) { + key_vector.push_back(it.first); + } + return key_vector; +} + +/* ************************************************************************* */ +VectorValues +EManifoldValues::computeTangentVector(const VectorValues &delta) const { + VectorValues tangent_vector; + for (const auto &it : *this) { + tangent_vector.insert( + it.second.basis()->computeTangentVector(delta.at(it.first))); + } + return tangent_vector; +} + +/* ************************************************************************* */ +EManifoldValues EManifoldValues::retract(const VectorValues &delta) const { + EManifoldValues new_values; + for (const auto &it : *this) { + new_values.insert({it.first, it.second.retract(delta.at(it.first))}); + } + return new_values; +} + +/* ************************************************************************* */ +std::map EManifoldValues::dims() const { + std::map dims_map; + for (const auto &it : *this) { + dims_map.insert({it.first, it.second.dim()}); + } + return dims_map; +} + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/ConstraintManifold.h b/gtdynamics/cmopt/ConstraintManifold.h new file mode 100644 index 000000000..c948632a7 --- /dev/null +++ b/gtdynamics/cmopt/ConstraintManifold.h @@ -0,0 +1,296 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ConstraintManifold.h + * @brief Manifold representing variables satisfying constraints. + * @author: Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::Key; +using gtsam::KeyVector; +using gtsam::OptionalJacobian; +using gtsam::Value; +using gtsam::Values; +using gtsam::Vector; +using gtsam::VectorValues; + +using EqualityConstraints = gtsam::NonlinearEqualityConstraints; + +/** + * Manifold for one constraint-connected component (CCC). + * + * This type stores the original variables that belong to one CCC and exposes + * manifold operations (`retract`, `localCoordinates`, and `recover`) so the + * CCC can be optimized as a single manifold-valued variable in transformed + * optimization problems. + * + * The manifold dimension is `embedding_dim - constraint_dim`, and tangent + * space mapping is delegated to `TspaceBasis` while feasibility projection is + * delegated to `Retractor`. + * + * @see README.md#constraint-manifold + * @see README.md#tangent-basis + * @see README.md#retraction + */ +class ConstraintManifold { + public: + /** + * Parameters that define basis construction and retraction behavior. + * + * `basis_creator` controls tangent-space parameterization and + * `retractor_creator` controls how feasibility is enforced after updates. + * + * @see README.md#tangent-basis + * @see README.md#retraction + */ + struct Params { + using shared_ptr = std::shared_ptr; + TspaceBasisCreator::shared_ptr basis_creator; + RetractorCreator::shared_ptr retractor_creator; + + /** Default constructor. */ + Params() + : basis_creator(std::make_shared()), + retractor_creator(std::make_shared()) {} + }; + + protected: + Params::shared_ptr params_; + EqualityConstraints::shared_ptr constraints_; + Retractor::shared_ptr retractor_; // retraction operation + gtsam::Values values_; // values of variables in CCC + size_t embedding_dim_; // dimension of embedding space + size_t constraint_dim_; // dimension of constraints + size_t dim_; // dimension of constraint manifold + TspaceBasis::shared_ptr basis_; // tangent space basis + + public: + enum { dimension = Eigen::Dynamic }; + + typedef OptionalJacobian ChartJacobian; + + /** + * Constructor from a constraint-connected component. + * @param constraints Equality constraints defining the component. + * @param values Initial values of variables in the connected component. + * @param params Parameters controlling basis and retraction behavior. + * @param retract_init If true, retract values to satisfy constraints at + * construction. + * @param basis Optional pre-built tangent basis. + */ + ConstraintManifold( + const EqualityConstraints::shared_ptr constraints, + const gtsam::Values &values, + const Params::shared_ptr ¶ms = std::make_shared(), + bool retract_init = true, + std::optional basis = {}) + : params_(params), + constraints_(constraints), + retractor_(params->retractor_creator->create(constraints_)), + values_(constructValues(values, retractor_, retract_init)), + embedding_dim_(values_.dim()), + constraint_dim_(constraints_->dim()), + dim_(embedding_dim_ > constraint_dim_ ? embedding_dim_ - constraint_dim_ + : 0), + basis_(basis ? *basis + : (dim_ == 0 ? createFixedBasis(constraints_, values_) + : params->basis_creator->create(constraints_, + values_))) {} + + /** + * Construct from another manifold instance with updated values. + * @param other Source manifold providing structural data. + * @param values New values for variables in the component. + */ + ConstraintManifold(const ConstraintManifold &other, const Values &values) + : params_(other.params_), + constraints_(other.constraints_), + retractor_(other.retractor_), + values_(values), + embedding_dim_(other.embedding_dim_), + constraint_dim_(other.constraint_dim_), + dim_(other.dim_), + basis_(other.basis_->createWithNewValues(values_)) {} + + /** + * Construct a new manifold with updated values. + * @param values New values for variables in the component. + * @return A new manifold instance with updated basis/retraction state. + * @note This indirectly calls `retractConstraints`. + */ + ConstraintManifold createWithNewValues(const gtsam::Values &values) const { + return ConstraintManifold(*this, values); + } + + /// Dimension of the constraint manifold. + inline size_t dim() const { return dim_; } + + /// Base values of the CCC. + inline const Values &values() const { return values_; } + + /** + * Recover the value of a base variable. + * @param key Variable key in the connected component. + * @param H1 Optional Jacobian of recover operation w.r.t. manifold + * coordinates. + * @return Reference to the recovered base value. + */ + const gtsam::Value &recover(const gtsam::Key key, + ChartJacobian H1 = {}) const; + + /** + * Recover the typed value of a base variable. + * @tparam ValueType Value type to cast to. + * @param key Variable key in the connected component. + * @param H1 Optional Jacobian of recover operation w.r.t. manifold + * coordinates. + * @return Reference to the recovered typed value. + */ + template + inline const ValueType &recover(const gtsam::Key key, + ChartJacobian H1 = {}) const { + return recover(key, H1).cast(); + } + + /** + * Retract this manifold element with a tangent update. + * @param xi Tangent-space coordinates. + * @param H1 Optional Jacobian w.r.t. current state (not implemented). + * @param H2 Optional Jacobian w.r.t. tangent update (not implemented). + * @return Retracted manifold element. + * @note Jacobians are not implemented and throw if requested. + */ + ConstraintManifold retract(const gtsam::Vector &xi, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const; + + /** + * Compute local coordinates from this manifold element to another. + * @param g Target manifold element. + * @param H1 Optional Jacobian w.r.t. this manifold element (not implemented). + * @param H2 Optional Jacobian w.r.t. target manifold element + * (not implemented). + * @return Tangent-space coordinates from `*this` to `g`. + */ + gtsam::Vector localCoordinates(const ConstraintManifold &g, + ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const; + + /// print + void print(const std::string &s = "") const; + + /** + * Check manifold equality by comparing base values. + * @param other Manifold to compare with. + * @param tol Absolute comparison tolerance. + * @return True if values are equal within tolerance. + */ + bool equals(const ConstraintManifold &other, double tol = 1e-8) const; + + /// Return the basis of the tangent space. + const TspaceBasis::shared_ptr &basis() const { return basis_; } + + /// Return the retractor. + const Retractor::shared_ptr &retractor() const { return retractor_; } + + /// Return values projected onto the feasible set by LM on penalty graph. + const Values feasibleValues() const; + + protected: + static TspaceBasis::shared_ptr createFixedBasis( + const EqualityConstraints::shared_ptr &constraints, + const gtsam::Values &values) { + auto basis_params = std::make_shared(); + basis_params->always_construct_basis = false; + return std::make_shared(constraints, values, + basis_params); + } + + /** + * Initialize values for the manifold state. + * @param values Candidate values of variables in the connected component. + * @param retractor Retraction object used for feasibility projection. + * @param retract_init If true, perform constraint retraction. + * @return Initialized values, optionally retracted to feasibility. + */ + static Values constructValues(const gtsam::Values &values, + const Retractor::shared_ptr &retractor, + bool retract_init); + + /// Make sure the tangent space basis is constructed. + /// NOTE: The static mutex creates a global synchronization point across all + /// ConstraintManifold instances. This is a known limitation that could cause + /// unnecessary contention. A better design would move the mutex to the + /// TspaceBasis class itself. + void makeSureBasisConstructed() const { + if (!basis_->isConstructed()) { + static std::mutex basis_mutex; + std::lock_guard lock(basis_mutex); + if (!basis_->isConstructed()) { + basis_->construct(values_); + } + } + } +}; + +/** + * Container of manifold values keyed by manifold variable keys. + * + * This utility wraps a map of `ConstraintManifold` objects and provides helper + * operations used during optimization, including flattening to base values and + * lifting deltas to base tangent vectors. + * + * @see README.md#constraint-manifold + * @see README.md#retraction + */ +class EManifoldValues : public std::map { + public: + using base = std::map; + using base::base; + + /// Collect all base variables from manifold values. + Values baseValues() const; + + /// Return keys of manifold variables. + KeyVector keys() const; + + /// Lift per-manifold delta vectors to base tangent vectors. + VectorValues computeTangentVector(const VectorValues &delta) const; + + /// Retract each manifold value by the corresponding update in `delta`. + EManifoldValues retract(const VectorValues &delta) const; + + /// Return manifold dimensions keyed by manifold variable. + std::map dims() const; +}; + +} // namespace gtdynamics + +namespace gtsam { +template <> +struct traits + : gtsam::internal::Manifold {}; +} // namespace gtsam diff --git a/gtdynamics/cmopt/LMManifoldOptimizer.cpp b/gtdynamics/cmopt/LMManifoldOptimizer.cpp new file mode 100644 index 000000000..3c83b192d --- /dev/null +++ b/gtdynamics/cmopt/LMManifoldOptimizer.cpp @@ -0,0 +1,156 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LMManifoldOptimizer.cpp + * @brief A nonlinear optimizer that uses the Levenberg-Marquardt trust-region + * scheme + * @author Yetong Zhang + * @date Oct 26, 2023 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtdynamics { + +/* ************************************************************************* */ +Values LMManifoldOptimizer::optimize( + const NonlinearFactorGraph &costs, + const EqualityConstraints &constraints, + const Values &init_values) const { + auto mopt_problem = initializeMoptProblem(costs, constraints, init_values); + return optimize(costs, mopt_problem); +} + +/* ************************************************************************* */ +Values LMManifoldOptimizer::optimize( + const NonlinearFactorGraph &graph, const ManifoldOptProblem &mopt_problem) const { + // Construct initial state + LMState state(graph, mopt_problem, params_.lambdaInitial, + params_.lambdaFactor, 0); + + // check if we're already close enough + if (state.error <= params_.errorTol) { + details_->emplace_back(state); + return state.baseValues(); + } + + // Iterative loop + if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { + std::cout << "Initial error: " << state.error << "\n"; + LMTrial::PrintTitle(); + } + + LMState prev_state; + do { + prev_state = state; + LMIterDetails iter_details = iterate(graph, mopt_problem.graph_, state); + state = LMState::FromLastIteration(iter_details, graph, params_); + details_->push_back(iter_details); + } while (state.iterations < params_.maxIterations && + !checkConvergence(prev_state, state) && + checkLambdaWithinLimits(state.lambda) && std::isfinite(state.error)); + details_->emplace_back(state); + return state.baseValues(); +} + +/* ************************************************************************* */ +LMIterDetails +LMManifoldOptimizer::iterate(const NonlinearFactorGraph &graph, + const NonlinearFactorGraph &manifold_graph, + const LMState &state) const { + LMIterDetails iter_details(state); + + // Set lambda for first trial. + double lambda = state.lambda; + double lambda_factor = state.lambda_factor; + + // Perform trials until any of follwing conditions is met + // * 1) trial is successful + // * 2) update is too small + // * 3) lambda goes beyond limits + while (true) { + // Perform the trial. + LMTrial trial(state, graph, manifold_graph, lambda, params_); + if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { + trial.print(state); + } + iter_details.trials.emplace_back(trial); + + // Check condition 1. + if (trial.step_is_successful) { + break; + } + + // Check condition 2. + if (trial.linear_update.solve_successful) { + double abs_change_tol = std::max(params_.absoluteErrorTol, + params_.relativeErrorTol * state.error); + if (trial.linear_update.cost_change < abs_change_tol && + trial.nonlinear_update.cost_change < abs_change_tol) { + break; + } + } + + // Set lambda for next trial. + trial.setNextLambda(lambda, lambda_factor, params_); + + // Check condition 3. + if (!checkLambdaWithinLimits(lambda)) { + break; + } + } + return iter_details; +} + +/* ************************************************************************* */ +bool LMManifoldOptimizer::checkLambdaWithinLimits(const double &lambda) const { + return lambda <= params_.lambdaUpperBound && + lambda >= params_.lambdaLowerBound; +} + +/* ************************************************************************* */ +bool LMManifoldOptimizer::checkConvergence(const LMState &prev_state, + const LMState &state) const { + + if (state.error <= params_.errorTol) + return true; + + // check if diverges + double absoluteDecrease = prev_state.error - state.error; + + // calculate relative error decrease and update currentError + double relativeDecrease = absoluteDecrease / prev_state.error; + bool converged = (params_.relativeErrorTol && + (relativeDecrease <= params_.relativeErrorTol)) || + (absoluteDecrease <= params_.absoluteErrorTol); + return converged; +} + +} /* namespace gtdynamics */ diff --git a/gtdynamics/cmopt/LMManifoldOptimizer.h b/gtdynamics/cmopt/LMManifoldOptimizer.h new file mode 100644 index 000000000..f347f3d5d --- /dev/null +++ b/gtdynamics/cmopt/LMManifoldOptimizer.h @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LMManifoldOptimizer.h + * @brief A nonlinear manifold optimizer that uses the Levenberg-Marquardt + * trust-region scheme + * @author Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::LevenbergMarquardtParams; +using gtsam::NonlinearFactorGraph; +using gtsam::Values; + +/** + * Levenberg-Marquardt optimizer over transformed manifold variables. + * + * This optimizer runs LM on the manifold optimization problem produced by + * `ManifoldOptimizer` and keeps detailed trial/state bookkeeping in + * `LMManifoldOptimizerState`. + * + * It is the explicit LM implementation path in `cmopt`, complementary to the + * generic `NonlinearMOptimizer` wrapper around standard GTSAM optimizers. + * + * @see README.md#solvers + * @see ManifoldOptimizer + * @see LMState + * @see LMTrial + */ +class LMManifoldOptimizer : public ManifoldOptimizer { + protected: + const LevenbergMarquardtParams params_; ///< LM parameters + std::shared_ptr details_; + + public: + typedef std::shared_ptr shared_ptr; + + const LMItersDetails &details() const { return *details_; } + + /** + * Constructor. + * @param mopt_params Parameters for manifold construction/transformation. + * @param params Levenberg-Marquardt parameters for manifold optimization. + */ + LMManifoldOptimizer( + const ManifoldOptimizerParameters &mopt_params, + const LevenbergMarquardtParams ¶ms = LevenbergMarquardtParams()) + : ManifoldOptimizer(mopt_params), + params_(params), + details_(std::make_shared()) {} + + /** Virtual destructor */ + ~LMManifoldOptimizer() {} + + /** Read-only access the parameters */ + const LevenbergMarquardtParams ¶ms() const { return params_; } + + /** + * Run manifold optimization from a constrained problem definition. + * @param graph Cost factor graph. + * @param constraints Equality constraints. + * @param initial_values Initial values for all variables. + * @return Optimized base-variable values. + */ + virtual gtsam::Values optimize( + const gtsam::NonlinearFactorGraph &graph, + const EqualityConstraints &constraints, + const gtsam::Values &initial_values) const override; + + /** + * Optimize an already transformed manifold optimization problem. + * @param graph Original base cost graph, used for error evaluation. + * @param mopt_problem Transformed manifold optimization problem. + * @return Optimized base-variable values. + */ + gtsam::Values optimize(const NonlinearFactorGraph &graph, + const ManifoldOptProblem &mopt_problem) const; + + /** + * Perform one outer LM iteration, potentially with multiple lambda trials. + * @param graph Original base cost graph. + * @param manifold_graph Transformed manifold graph. + * @param state Current optimizer state. + * @return Iteration details including all trials. + */ + LMIterDetails iterate(const NonlinearFactorGraph &graph, + const NonlinearFactorGraph &manifold_graph, + const LMState &state) const; + + /** + * Check convergence between two consecutive states. + * @param prev_state Previous state. + * @param state Current state. + * @return True if convergence criteria are met. + */ + bool checkConvergence(const LMState &prev_state, const LMState &state) const; + + /// Check whether lambda is within configured bounds. + bool checkLambdaWithinLimits(const double &lambda) const; +}; + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/LMManifoldOptimizerState.cpp b/gtdynamics/cmopt/LMManifoldOptimizerState.cpp new file mode 100644 index 000000000..d5eb2d2ba --- /dev/null +++ b/gtdynamics/cmopt/LMManifoldOptimizerState.cpp @@ -0,0 +1,361 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using std::cout, std::setw, std::setprecision, std::endl; + +namespace gtdynamics { + +/* ************************************************************************* */ +/* <============================= LMState =================================> */ +/* ************************************************************************* */ + +LMState::LMState(const NonlinearFactorGraph &graph, + const ManifoldOptProblem &problem, const double &_lambda, + const double &_lambda_factor, size_t _iterations) + : manifolds(problem.manifolds()), + unconstrained_values(problem.unconstrainedValues()), + const_manifolds(problem.constManifolds()), + error(EvaluateGraphError(graph, manifolds, unconstrained_values, + const_manifolds)), + lambda(_lambda), lambda_factor(_lambda_factor), iterations(_iterations) {} + +/* ************************************************************************* */ +LMState LMState::FromLastIteration(const LMIterDetails &iter_details, + const NonlinearFactorGraph &graph, + const LevenbergMarquardtParams ¶ms) { + double lambda; + const auto &last_trial = iter_details.trials.back(); + const auto &prev_state = iter_details.state; + LMState state; + if (last_trial.step_is_successful) { + state.manifolds = last_trial.nonlinear_update.new_manifolds; + state.unconstrained_values = + last_trial.nonlinear_update.new_unconstrained_values; + state.const_manifolds = prev_state.const_manifolds; + state.error = last_trial.nonlinear_update.new_error; + state.lambda = last_trial.linear_update.lambda; + state.lambda_factor = prev_state.lambda_factor; + last_trial.setNextLambda(state.lambda, state.lambda_factor, params); + } else { + // pick the trials with smallest error + throw std::runtime_error("not implemented"); + } + state.iterations = prev_state.iterations + 1; + state.totalNumberInnerIterations = + prev_state.totalNumberInnerIterations + iter_details.trials.size(); + return state; +} + +/* ************************************************************************* */ +double LMState::EvaluateGraphError(const NonlinearFactorGraph &graph, + const EManifoldValues &_manifolds, + const Values &_unconstrained_values, + const EManifoldValues &_const_manifolds) { + Values base_values = _manifolds.baseValues(); + base_values.insert(_unconstrained_values); + base_values.insert(_const_manifolds.baseValues()); + return graph.error(base_values); +} + +/* ************************************************************************* */ +Values LMState::baseValues() const { + Values base_values = manifolds.baseValues(); + base_values.insert(unconstrained_values); + base_values.insert(const_manifolds.baseValues()); + return base_values; +} + +/* ************************************************************************* */ +/* <============================= LMTrial =================================> */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +LMTrial::LMTrial(const LMState &state, const NonlinearFactorGraph &graph, + const NonlinearFactorGraph &manifold_graph, + const double &lambda, const LevenbergMarquardtParams ¶ms) { + // std::cout << "========= " << state.iterations << " ======= \n"; + auto start = std::chrono::high_resolution_clock::now(); + step_is_successful = false; + + // Compute linear update and linear cost change + linear_update = LinearUpdate(lambda, manifold_graph, state, params); + if (!linear_update.solve_successful) { + return; + } + + // Compute nonlinear update and nonlinear cost change + nonlinear_update = NonlinearUpdate(state, linear_update, graph); + + // Decide if accept or reject trial + model_fidelity = nonlinear_update.cost_change / linear_update.cost_change; + if (linear_update.cost_change > + std::numeric_limits::epsilon() * linear_update.old_error && + model_fidelity > params.minModelFidelity) { + step_is_successful = true; + } + + auto end = std::chrono::high_resolution_clock::now(); + trial_time = + std::chrono::duration_cast(end - start) + .count() / + 1e6; +} + +/* ************************************************************************* */ +void LMTrial::setNextLambda(double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const { + if (step_is_successful) { + setDecreasedNextLambda(new_lambda, new_lambda_factor, params); + } else { + setIncreasedNextLambda(new_lambda, new_lambda_factor, params); + } +} + +/* ************************************************************************* */ +void LMTrial::setIncreasedNextLambda( + double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const { + new_lambda *= new_lambda_factor; + if (!params.useFixedLambdaFactor) { + new_lambda_factor *= 2.0; + } +} + +/* ************************************************************************* */ +void LMTrial::setDecreasedNextLambda( + double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const { + if (params.useFixedLambdaFactor) { + new_lambda /= new_lambda_factor; + } else { + new_lambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * model_fidelity - 1.0, 3)); + new_lambda_factor *= 2.0; + } + new_lambda = std::max(params.lambdaLowerBound, new_lambda); +} + +/* ************************************************************************* */ +void LMTrial::PrintTitle() { + cout << setw(10) << "iter" + << " " << setw(12) << "error " + << " " << setw(12) << "nonlinear " + << " " << setw(12) << "linear " + << " " << setw(10) << "lambda " + << " " << setw(10) << "solve_succ " + << " " << setw(10) << "time " + << " " << setw(10) << "delta_norm" << endl; +} + +/* ************************************************************************* */ +void LMTrial::print(const LMState &state) const { + cout << setw(10) << state.iterations << " " << setw(12) << setprecision(4) + << nonlinear_update.new_error << " " << setw(12) << setprecision(4) + << nonlinear_update.cost_change << " " << setw(10) << setprecision(4) + << linear_update.cost_change << " " << setw(10) << setprecision(2) + << linear_update.lambda << " " << setw(10) + << (linear_update.solve_successful ? "T " : "F ") << " " << setw(10) + << setprecision(2) << trial_time << setw(10) << setprecision(4) + << linear_update.delta.norm() << endl; +} + +/* ************************************************************************* */ +/* <===================== IELMTrial::LinearUpdate =========================> */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +LMTrial::LinearUpdate::LinearUpdate(const double &_lambda, + const NonlinearFactorGraph &manifold_graph, + const LMState &state, + const LevenbergMarquardtParams ¶ms) + : lambda(_lambda) { + // linearize and build damped system + Values state_values = state.unconstrained_values; + for (const auto &it : state.manifolds) { + state_values.insert(it.first, it.second); + } + GaussianFactorGraph::shared_ptr linear = + manifold_graph.linearize(state_values); + VectorValues sqrt_hessian_diagonal = SqrtHessianDiagonal(*linear, params); + auto damped_system = + buildDampedSystem(*linear, sqrt_hessian_diagonal, state, params); + + // solve delta + try { + delta = SolveLinear(damped_system, params); + solve_successful = true; + } catch (const gtsam::IndeterminantLinearSystemException &) { + solve_successful = false; + return; + } + tangent_vector = computeTangentVector(delta, state); + old_error = linear->error(VectorValues::Zero(delta)); + new_error = linear->error(delta); + cost_change = old_error - new_error; +} + +/* ************************************************************************* */ +LMCachedModel *LMTrial::LinearUpdate::getCachedModel(size_t dim) const { + if (dim >= noiseModelCache.size()) + noiseModelCache.resize(dim + 1); + LMCachedModel *item = &noiseModelCache[dim]; + if (!item->model) + *item = LMCachedModel(dim, 1.0 / std::sqrt(lambda)); + return item; +} + +/* ************************************************************************* */ +GaussianFactorGraph +LMTrial::LinearUpdate::buildDampedSystem(GaussianFactorGraph damped, + const LMState &state) const { + noiseModelCache.resize(0); + // for each of the variables, add a prior + damped.reserve(damped.size() + state.unconstrained_values.size()); + std::map dims = state.manifolds.dims(); + std::map dims_unconstrained = state.unconstrained_values.dims(); + dims.insert(dims_unconstrained.begin(), dims_unconstrained.end()); + for (const auto &key_dim : dims) { + const Key &key = key_dim.first; + const size_t &dim = key_dim.second; + const LMCachedModel *item = getCachedModel(dim); + damped.emplace_shared(key, item->A, item->b, item->model); + } + return damped; +} + +/* ************************************************************************* */ +GaussianFactorGraph LMTrial::LinearUpdate::buildDampedSystem( + GaussianFactorGraph damped, // gets copied + const VectorValues &sqrtHessianDiagonal) const { + noiseModelCache.resize(0); + damped.reserve(damped.size() + sqrtHessianDiagonal.size()); + for (const auto &key_vector : sqrtHessianDiagonal) { + try { + const Key key = key_vector.first; + const size_t dim = key_vector.second.size(); + LMCachedModel *item = getCachedModel(dim); + item->A.diagonal() = sqrtHessianDiagonal.at(key); // use diag(hessian) + damped.emplace_shared(key, item->A, item->b, item->model); + } catch (const std::out_of_range &) { + continue; // Don't attempt any damping if no key found in diagonal + } + } + return damped; +} + +/* ************************************************************************* */ +GaussianFactorGraph LMTrial::LinearUpdate::buildDampedSystem( + const GaussianFactorGraph &linear, const VectorValues &sqrtHessianDiagonal, + const LMState &state, const LevenbergMarquardtParams ¶ms) const { + if (params.getDiagonalDamping()) + return buildDampedSystem(linear, sqrtHessianDiagonal); + else + return buildDampedSystem(linear, state); +} + +/* ************************************************************************* */ +VectorValues +LMTrial::LinearUpdate::computeTangentVector(const VectorValues &delta, + const LMState &state) const { + VectorValues tangent_vector; + for (const auto &it : state.manifolds) { + const Vector &xi = delta.at(it.first); + tangent_vector.insert(it.second.basis()->computeTangentVector(xi)); + } + for (const auto &it : state.const_manifolds) { + tangent_vector.insert(it.second.values().zeroVectors()); + } + for (const Key &key : state.unconstrained_values.keys()) { + tangent_vector.insert(key, delta.at(key)); + } + return tangent_vector; +} + +/* ************************************************************************* */ +/* <==================== IELMTrial::NonlinearUpdate =======================> */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +LMTrial::NonlinearUpdate::NonlinearUpdate(const LMState &state, + const LinearUpdate &linear_update, + const NonlinearFactorGraph &graph) { + + // retract for manifolds + VectorValues tangent_vector_manifold = + SubValues(linear_update.delta, state.manifolds.keys()); + new_manifolds = state.manifolds.retract(tangent_vector_manifold); + + // retract for unconstrained variables + VectorValues tangent_vector_unconstrained = + SubValues(linear_update.delta, state.unconstrained_values.keys()); + new_unconstrained_values = + state.unconstrained_values.retract(tangent_vector_unconstrained); + + // compute error + new_error = LMState::EvaluateGraphError( + graph, new_manifolds, new_unconstrained_values, state.const_manifolds); + cost_change = state.error - new_error; +} + +/* ************************************************************************* */ +/* <========================= IELMItersDetails ============================> */ +/* ************************************************************************* */ +void LMItersDetails::exportFile(const std::string &state_file_path, + const std::string &trial_file_path) const { + std::ofstream state_file, trial_file; + state_file.open(state_file_path); + trial_file.open(trial_file_path); + + state_file << "iterations" + << "," + << "lambda" + << "," + << "error" + << "\n"; + trial_file << "iterations" + << "," + << "lambda" + << "," + << "error" + << "," + << "step_is_successful" + << "," + << "linear_cost_change" + << "," + << "nonlinear_cost_change" + << "," + << "model_fidelity" + << "\n"; + + for (const auto &iter_details : *this) { + const auto &state = iter_details.state; + state_file << state.iterations << "," << state.lambda << "," << state.error + << "\n"; + for (const auto &trial : iter_details.trials) { + trial_file << state.iterations << "," << trial.linear_update.lambda << "," + << trial.nonlinear_update.new_error << "," + << trial.step_is_successful << "," + << trial.linear_update.cost_change << "," + << trial.nonlinear_update.cost_change << "," + << trial.model_fidelity << "\n"; + } + } + state_file.close(); + trial_file.close(); +} + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/LMManifoldOptimizerState.h b/gtdynamics/cmopt/LMManifoldOptimizerState.h new file mode 100644 index 000000000..2b60b4a50 --- /dev/null +++ b/gtdynamics/cmopt/LMManifoldOptimizerState.h @@ -0,0 +1,317 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LMManifoldOptimizerState.h + * @brief State and Trial for LMManifoldOptimizer + * @author Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::GaussianFactorGraph; +using gtsam::LevenbergMarquardtParams; +using gtsam::NonlinearFactorGraph; +using gtsam::Values; +using gtsam::VectorValues; + +struct LMState; +struct LMTrial; +struct LMIterDetails; + +/** + * State for one outer iteration of manifold LM optimization. + * + * This structure stores manifold variables, unconstrained variables, fixed + * manifold variables, and LM damping/error metadata used by trial generation + * and convergence checks. + * + * @see README.md#solvers + */ +struct LMState { + public: + EManifoldValues manifolds; + Values unconstrained_values; + EManifoldValues const_manifolds; + double error = 0; + double lambda = 0; + double lambda_factor = 0; + size_t iterations = 0; + size_t totalNumberInnerIterations = 0; + + LMState() {} + + /** + * Constructor. + * @param graph Base cost graph. + * @param problem Transformed manifold optimization problem. + * @param _lambda Initial damping value. + * @param _lambda_factor Lambda scaling factor. + * @param _iterations Current outer iteration index. + */ + LMState(const NonlinearFactorGraph &graph, const ManifoldOptProblem &problem, + const double &_lambda, const double &_lambda_factor, + size_t _iterations = 0); + + /** + * Initialize the next state from the previous iteration details. + * @param iter_details Completed iteration details. + * @param graph Base cost graph. + * @param params LM parameters. + * @return New LM state for the next outer iteration. + */ + static LMState FromLastIteration(const LMIterDetails &iter_details, + const NonlinearFactorGraph &graph, + const LevenbergMarquardtParams ¶ms); + + /** + * Evaluate base graph error from manifold and unconstrained values. + * @param graph Base cost graph. + * @param _manifolds Current manifold values. + * @param _unconstrained_values Current unconstrained values. + * @param _const_manifolds Fixed manifold values. + * @return Total nonlinear graph error. + */ + static double EvaluateGraphError(const NonlinearFactorGraph &graph, + const EManifoldValues &_manifolds, + const Values &_unconstrained_values, + const EManifoldValues &_const_manifolds); + + /// Reconstruct base variable values from state partitions. + Values baseValues() const; +}; + +/** + * One inner LM trial at a specific lambda. + * + * Each trial computes a linearized update and a nonlinear update, evaluates + * model fidelity, and decides whether the trial is accepted. + * + * @see README.md#solvers + */ +struct LMTrial { + struct LinearUpdate; + struct NonlinearUpdate; + + /** + * Perform a single LM trial at a specified lambda. + * @param state Current LM state. + * @param graph Base cost graph. + * @param manifold_graph Transformed manifold graph. + * @param lambda Trial lambda. + * @param params LM parameters. + */ + LMTrial(const LMState &state, const NonlinearFactorGraph &graph, + const NonlinearFactorGraph &manifold_graph, const double &lambda, + const LevenbergMarquardtParams ¶ms); + + /** + * Linearized LM update for a single trial. + * + * Stores the solved delta, lifted tangent vector, linearized error change, + * and solve success flag. + * + * @see README.md#solvers + */ + struct LinearUpdate { + double lambda; + VectorValues delta; + VectorValues tangent_vector; + double old_error; + double new_error; + double cost_change; + bool solve_successful; + + /** Default constructor. */ + LinearUpdate() {} + + /** + * Compute the linearized LM update for one trial. + * @param _lambda Trial lambda. + * @param manifold_graph Transformed manifold graph. + * @param state Current LM state. + * @param params LM parameters. + */ + LinearUpdate(const double &_lambda, + const NonlinearFactorGraph &manifold_graph, + const LMState &state, const LevenbergMarquardtParams ¶ms); + + protected: + /** Following functions are adapted from LMOptimizerState. */ + + // Small cache of A|b|model indexed by dimension. Can save many mallocs. + mutable std::vector noiseModelCache; + + LMCachedModel *getCachedModel(size_t dim) const; + + /** + * Build a damped linear system with isotropic damping. + * @param damped Linearized graph copy. + * @param state Current LM state. + * @return Damped Gaussian factor graph. + */ + GaussianFactorGraph buildDampedSystem( + GaussianFactorGraph damped /* gets copied */, + const LMState &state) const; + + /** + * Build a damped linear system using Hessian-diagonal damping. + * @param damped Linearized graph copy. + * @param sqrtHessianDiagonal Square roots of Hessian diagonal blocks. + * @return Damped Gaussian factor graph. + */ + GaussianFactorGraph buildDampedSystem( + GaussianFactorGraph damped, // gets copied + const VectorValues &sqrtHessianDiagonal) const; + + /** + * Dispatch damped-system construction based on LM damping configuration. + * @param linear Linearized graph. + * @param sqrtHessianDiagonal Square roots of Hessian diagonal blocks. + * @param state Current LM state. + * @param params LM parameters. + * @return Damped Gaussian factor graph. + */ + GaussianFactorGraph buildDampedSystem( + const GaussianFactorGraph &linear, + const VectorValues &sqrtHessianDiagonal, const LMState &state, + const LevenbergMarquardtParams ¶ms) const; + + /** + * Lift manifold-space delta to base-space tangent vectors. + * @param delta Delta in transformed variable space. + * @param state Current LM state. + * @return Tangent vectors for base variables. + */ + VectorValues computeTangentVector(const VectorValues &delta, + const LMState &state) const; + }; + + /** + * Nonlinear update obtained by retracting a linear trial update. + * + * Stores updated manifold/unconstrained values and nonlinear error change. + * + * @see README.md#solvers + */ + struct NonlinearUpdate { + EManifoldValues new_manifolds; + Values new_unconstrained_values; + double new_error; + double cost_change; + + /** Default constructor. */ + NonlinearUpdate() {} + + /** + * Compute nonlinear trial update from a linear update. + * @param state Current LM state. + * @param linear_update Linear trial update. + * @param graph Base cost graph. + */ + NonlinearUpdate(const LMState &state, const LinearUpdate &linear_update, + const NonlinearFactorGraph &graph); + }; + + public: + // linear update + LinearUpdate linear_update; + + // nonlinear update + NonlinearUpdate nonlinear_update; + + // decision making + double model_fidelity; + bool step_is_successful; + bool stop_searching_lambda; + double trial_time; + + /** + * Update lambda for the next trial/state. + * @param new_lambda Output lambda value. + * @param new_lambda_factor Output lambda scaling factor. + * @param params LM parameters. + */ + void setNextLambda(double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const; + + /** + * Increase lambda for the next trial/state. + * @param new_lambda Output lambda value. + * @param new_lambda_factor Output lambda scaling factor. + * @param params LM parameters. + */ + void setIncreasedNextLambda(double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const; + + /** + * Decrease lambda for the next trial/state. + * @param new_lambda Output lambda value. + * @param new_lambda_factor Output lambda scaling factor. + * @param params LM parameters. + */ + void setDecreasedNextLambda(double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const; + + /** + * Print summary information of this trial. + * @param state State corresponding to the trial. + */ + void print(const LMState &state) const; + + /// Print table header for LM trial summaries. + static void PrintTitle(); +}; + +/** + * Aggregated details for one outer iteration, including all inner trials. + * + * @see README.md#solvers + */ +struct LMIterDetails { + LMState state; + std::vector trials; + + LMIterDetails(const LMState &_state) : state(_state), trials() {} +}; + +/** + * Sequence of iteration details for an entire optimization run. + * + * @see README.md#solvers + */ +class LMItersDetails : public std::vector { + public: + using base = std::vector; + using base::base; + + /** + * Export iteration and trial details to CSV files. + * @param state_file_path Output path for per-state CSV. + * @param trial_file_path Output path for per-trial CSV. + */ + void exportFile(const std::string &state_file_path, + const std::string &trial_file_path) const; +}; + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/ManifoldOptimizer.cpp b/gtdynamics/cmopt/ManifoldOptimizer.cpp new file mode 100644 index 000000000..bf4264b76 --- /dev/null +++ b/gtdynamics/cmopt/ManifoldOptimizer.cpp @@ -0,0 +1,322 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ManifoldOptimizer.cpp + * @brief Implementation of manifold optimizer. + * @author: Yetong Zhang + */ + +#include +#include +#include + +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +Values ManifoldOptProblem::unconstrainedValues() const { + return SubValues(values_, unconstrained_keys_); +} + +/* ************************************************************************* */ +EManifoldValues ManifoldOptProblem::manifolds() const { + EManifoldValues e_manifolds; + for (const Key& key : manifold_keys_) { + e_manifolds.insert({key, values_.at(key)}); + } + return e_manifolds; +} + +/* ************************************************************************* */ +EManifoldValues ManifoldOptProblem::constManifolds() const { + EManifoldValues e_manifolds; + for (const Key& key : fixed_manifolds_.keys()) { + e_manifolds.insert({key, fixed_manifolds_.at(key)}); + } + return e_manifolds; +} + +/* ************************************************************************* */ +std::pair ManifoldOptProblem::problemDimension() const { + size_t values_dim = values_.dim(); + size_t graph_dim = 0; + for (const auto& factor : graph_) { + graph_dim += factor->dim(); + } + return std::make_pair(graph_dim, values_dim); +} + +/* ************************************************************************* */ +void ManifoldOptProblem::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s; + std::cout << "found " << components_.size() + << " components: " << manifold_keys_.size() << " free, " + << fixed_manifolds_.size() << " fixed\n"; + for (const Key& cm_key : manifold_keys_) { + const auto& cm = values_.at(cm_key).cast(); + std::cout << "component " << keyFormatter(cm_key) << ":\tdimension " + << cm.dim() << "\n"; + for (const auto& key : cm.values().keys()) { + std::cout << "\t" << keyFormatter(key); + } + std::cout << "\n"; + } + std::cout << "fully constrained manifolds:\n"; + for (const auto& cm_key : fixed_manifolds_.keys()) { + const auto& cm = fixed_manifolds_.at(cm_key).cast(); + for (const auto& key : cm.values().keys()) { + std::cout << "\t" << keyFormatter(key); + } + std::cout << "\n"; + } +} + +/* ************************************************************************* */ +ManifoldOptimizerParameters::ManifoldOptimizerParameters() + : Base(), + cc_params(std::make_shared()), + retract_init(true) {} + +/* ************************************************************************* */ +EqualityConstraints::shared_ptr ManifoldOptimizer::IdentifyConnectedComponent( + const EqualityConstraints& constraints, const gtsam::Key start_key, + gtsam::KeySet& keys, const gtsam::VariableIndex& var_index) { + std::set constraint_indices; + + std::stack key_stack; + key_stack.push(start_key); + while (!key_stack.empty()) { + gtsam::Key key = key_stack.top(); + key_stack.pop(); + // find all constraints connected to key + for (const auto& constraint_index : var_index[key]) { + constraint_indices.insert(constraint_index); + for (const auto& neighbor_key : + constraints.at(constraint_index)->keys()) { + if (keys.find(neighbor_key) != keys.end()) { + keys.erase(neighbor_key); + key_stack.push(neighbor_key); + } + } + } + } + + auto cc_constraints = std::make_shared(); + for (const auto& constraint_index : constraint_indices) { + cc_constraints->push_back(constraints.at(constraint_index)); + } + return cc_constraints; +} + +/* ************************************************************************* */ +std::vector +ManifoldOptimizer::IdentifyConnectedComponents( + const EqualityConstraints& constraints) { + // Get all the keys in constraints. + gtsam::VariableIndex constraint_var_index(constraints); + gtsam::KeySet constraint_keys = constraints.keys(); + + // Find connected component using DFS algorithm. + std::vector components; + while (!constraint_keys.empty()) { + Key key = *constraint_keys.begin(); + constraint_keys.erase(key); + components.emplace_back(IdentifyConnectedComponent( + constraints, key, constraint_keys, constraint_var_index)); + } + return components; +} + +/* ************************************************************************* */ +NonlinearFactorGraph ManifoldOptimizer::ManifoldGraph( + const NonlinearFactorGraph& graph, const std::map& var2man_keymap, + const Values& fc_manifolds) { + NonlinearFactorGraph manifold_graph; + for (const auto& factor : graph) { + std::map replacement_map; + for (const Key& key : factor->keys()) { + if (var2man_keymap.find(key) != var2man_keymap.end()) { + replacement_map[key] = var2man_keymap.at(key); + } + } + if (replacement_map.size() > 0) { + NoiseModelFactor::shared_ptr noise_factor = + std::dynamic_pointer_cast(factor); + auto subs_factor = std::make_shared( + noise_factor, replacement_map, fc_manifolds); + if (subs_factor->checkActive()) { + manifold_graph.add(subs_factor); + } + } else { + manifold_graph.add(factor); + } + } + return manifold_graph; +} + +/* ************************************************************************* */ +ManifoldOptProblem ManifoldOptimizer::problemTransform( + const EConsOptProblem& equalityConstrainedProblem) const { + ManifoldOptProblem mopt_problem; + mopt_problem.components_ = + IdentifyConnectedComponents(equalityConstrainedProblem.e_constraints_); + constructMoptValues(equalityConstrainedProblem, mopt_problem); + constructMoptGraph(equalityConstrainedProblem, mopt_problem); + return mopt_problem; +} + +/* ************************************************************************* */ +void ManifoldOptimizer::constructMoptValues( + const EConsOptProblem& equalityConstrainedProblem, + ManifoldOptProblem& mopt_problem) const { + constructManifoldValues(equalityConstrainedProblem, mopt_problem); + constructUnconstrainedValues(equalityConstrainedProblem, mopt_problem); +} + +/* ************************************************************************* */ +void ManifoldOptimizer::constructManifoldValues( + const EConsOptProblem& equalityConstrainedProblem, + ManifoldOptProblem& mopt_problem) const { + for (size_t i = 0; i < mopt_problem.components_.size(); i++) { + // Find the values of variables in the component. + const auto& component = mopt_problem.components_.at(i); + const KeySet component_keys = component->keys(); + if (component_keys.empty()) continue; + const Key component_key = *component_keys.begin(); + Values component_values; + for (const Key& key : component_keys) { + component_values.insert(key, equalityConstrainedProblem.values_.at(key)); + } + // Construct manifold value + auto constraint_manifold = ConstraintManifold( + component, component_values, p_.cc_params, p_.retract_init); + // check if the manifold is fully constrained + if (constraint_manifold.dim() > 0) { + mopt_problem.values_.insert(component_key, constraint_manifold); + mopt_problem.manifold_keys_.insert(component_key); + } else { + mopt_problem.fixed_manifolds_.insert(component_key, constraint_manifold); + } + } +} + +/* ************************************************************************* */ +void ManifoldOptimizer::constructUnconstrainedValues( + const EConsOptProblem& equalityConstrainedProblem, + ManifoldOptProblem& mopt_problem) { + // Find out which variables are unconstrained + mopt_problem.unconstrained_keys_ = equalityConstrainedProblem.costs_.keys(); + KeySet& unconstrained_keys = mopt_problem.unconstrained_keys_; + for (const auto& component : mopt_problem.components_) { + for (const Key& key : component->keys()) { + if (unconstrained_keys.find(key) != unconstrained_keys.end()) { + unconstrained_keys.erase(key); + } + } + } + // Copy the values of unconstrained variables + for (const Key& key : unconstrained_keys) { + mopt_problem.values_.insert(key, + equalityConstrainedProblem.values_.at(key)); + } +} + +/* ************************************************************************* */ +void ManifoldOptimizer::constructMoptGraph( + const EConsOptProblem& equalityConstrainedProblem, + ManifoldOptProblem& mopt_problem) { + // Construct base key to component map. + std::map key_component_map; + for (const Key& cm_key : mopt_problem.manifold_keys_) { + const auto& cm = mopt_problem.values_.at(cm_key).cast(); + for (const Key& base_key : cm.values().keys()) { + key_component_map[base_key] = cm_key; + } + } + for (const Key& cm_key : mopt_problem.fixed_manifolds_.keys()) { + const auto& cm = + mopt_problem.fixed_manifolds_.at(cm_key).cast(); + for (const Key& base_key : cm.values().keys()) { + key_component_map[base_key] = cm_key; + } + } + + // Turn factors involved with constraint variables into SubstituteFactor. + for (const auto& factor : equalityConstrainedProblem.costs_) { + std::map replacement_map; + for (const Key& key : factor->keys()) { + if (key_component_map.find(key) != key_component_map.end()) { + replacement_map[key] = key_component_map.at(key); + } + } + if (replacement_map.size() > 0) { + NoiseModelFactor::shared_ptr noise_factor = + std::dynamic_pointer_cast(factor); + auto subs_factor = std::make_shared( + noise_factor, replacement_map, mopt_problem.fixed_manifolds_); + if (subs_factor->checkActive()) { + mopt_problem.graph_.add(subs_factor); + } + } else { + mopt_problem.graph_.add(factor); + } + } +} + +/* ************************************************************************* */ +Values ManifoldOptimizer::baseValues(const ManifoldOptProblem& mopt_problem, + const Values& nopt_values) const { + Values base_values; + for (const Key& key : mopt_problem.fixed_manifolds_.keys()) { + base_values.insert(mopt_problem.fixed_manifolds_.at(key) + .cast() + .values()); + } + for (const Key& key : mopt_problem.unconstrained_keys_) { + base_values.insert(key, nopt_values.at(key)); + } + for (const Key& key : mopt_problem.manifold_keys_) { + if (p_.retract_final) { + base_values.insert( + nopt_values.at(key).cast().feasibleValues()); + } else { + base_values.insert( + nopt_values.at(key).cast().values()); + } + } + return base_values; +} + +/* ************************************************************************* */ +VectorValues ManifoldOptimizer::baseTangentVector( + const ManifoldOptProblem& mopt_problem, const Values& values, + const VectorValues& delta) const { + VectorValues tangent_vector; + for (const Key& key : mopt_problem.unconstrained_keys_) { + tangent_vector.insert(key, delta.at(key)); + } + for (const Key& key : mopt_problem.manifold_keys_) { + tangent_vector.insert( + values.at(key).cast().basis()->computeTangentVector( + delta.at(key))); + } + return tangent_vector; +} + +/* ************************************************************************* */ +ManifoldOptProblem ManifoldOptimizer::initializeMoptProblem( + const gtsam::NonlinearFactorGraph& costs, + const EqualityConstraints& constraints, + const gtsam::Values& init_values) const { + EConsOptProblem equalityConstrainedProblem(costs, constraints, init_values); + return problemTransform(equalityConstrainedProblem); +} + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/ManifoldOptimizer.h b/gtdynamics/cmopt/ManifoldOptimizer.h new file mode 100644 index 000000000..b2394928d --- /dev/null +++ b/gtdynamics/cmopt/ManifoldOptimizer.h @@ -0,0 +1,230 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ManifoldOptimizer.h + * @brief Manifold Optimzier with manually written LM algorithm. + * @author: Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::DefaultKeyFormatter; +using gtsam::Key; +using gtsam::KeyFormatter; +using gtsam::KeySet; +using gtsam::NonlinearFactorGraph; +using gtsam::Values; +using gtsam::VariableIndex; +using gtsam::VectorValues; + +using EqualityConstraints = gtsam::NonlinearEqualityConstraints; + +/** + * Transformed manifold optimization problem data. + * + * This holds the transformed factor graph, manifold/unconstrained values, and + * bookkeeping sets needed to move between transformed and base variable spaces. + * + * @see README.md#ccc-transformation + * @see README.md#equivalent-factors + */ +struct ManifoldOptProblem { + NonlinearFactorGraph graph_; // cost function on constraint manifolds + std::vector + components_; // All the constraint-connected components + Values + values_; // values for constraint manifolds and unconstrained variables + Values fixed_manifolds_; // fully constrained variables + KeySet unconstrained_keys_; // keys for unconstrained variables + KeySet manifold_keys_; // keys for constraint manifold variables + + /** Dimension of the manifold optimization problem, as factor dimension x + * variable dimension. + * @return Pair of `(total_factor_dimension, total_variable_dimension)`. + */ + std::pair problemDimension() const; + + /// Return values of unconstrained variables only. + Values unconstrainedValues() const; + + /// Return free (non-fixed) constraint manifold values. + EManifoldValues manifolds() const; + + /// Return fixed (zero-dimensional) manifold values. + EManifoldValues constManifolds() const; + + /** + * Print a summary of manifold optimization problem contents. + * @param s Prefix string. + * @param keyFormatter Key formatter. + */ + void print(const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter) const; +}; + +/** + * Parameters controlling manifold problem construction and post-processing. + * + * @see README.md#ccc-transformation + * @see README.md#retraction + */ +struct ManifoldOptimizerParameters : public ConstrainedOptimizationParameters { + using Base = ConstrainedOptimizationParameters; + ConstraintManifold::Params::shared_ptr + cc_params; // Parameter for constraint-connected components + bool retract_init = true; // Perform retraction on constructing values for + // connected component. + bool retract_final = false; // Perform retraction on manifolds after + // optimization, used for infeasible methods. + /// Default Constructor. + ManifoldOptimizerParameters(); +}; + +/** + * Base class that transforms equality-constrained problems into manifold form. + * + * This class identifies constraint-connected components, builds + * `ConstraintManifold` values, and replaces base factors with equivalent + * factors over manifold variables. + * + * @see README.md#ccc-transformation + * @see README.md#equivalent-factors + */ +class ManifoldOptimizer : public ConstrainedOptimizer { + public: + using shared_ptr = std::shared_ptr; + + protected: + const ManifoldOptimizerParameters p_; + + public: + /// Default constructor. + ManifoldOptimizer() : p_(ManifoldOptimizerParameters()) {} + + /// Construct from parameters. + ManifoldOptimizer(const ManifoldOptimizerParameters ¶meters) + : p_(parameters) {} + + public: + /** + * Find a connected component containing `start_key` using DFS. + * @param constraints Equality constraints. + * @param start_key Starting key for DFS. + * @param keys In/out set of unexplored keys; keys in the found component are + * removed. + * @param var_index Variable index over constraints. + * @return Constraint subset corresponding to the connected component. + */ + static EqualityConstraints::shared_ptr IdentifyConnectedComponent( + const EqualityConstraints &constraints, const Key start_key, KeySet &keys, + const VariableIndex &var_index); + + /// Identify the connected components by constraints. + static std::vector + IdentifyConnectedComponents(const EqualityConstraints &constraints); + + /** + * Create equivalent cost graph on manifold variables. + * @param graph Original factor graph. + * @param var2man_keymap Mapping from base variable key to manifold key. + * @param fc_manifolds Values of fully constrained manifolds. + * @return Transformed manifold graph. + */ + static NonlinearFactorGraph ManifoldGraph( + const NonlinearFactorGraph &graph, + const std::map &var2man_keymap, + const Values &fc_manifolds = Values()); + + /** Create values for the manifold optimization problem by (1) create + * constraint manifolds for constraint-connected components; (2) identify if + * the constraint manifold is fully constrained; (3) collect unconstrained + * variables. + * @param equalityConstrainedProblem Input constrained problem. + * @param mopt_problem Output manifold optimization problem. + */ + void constructMoptValues(const EConsOptProblem &equalityConstrainedProblem, + ManifoldOptProblem &mopt_problem) const; + + /** + * Create initial values for constraint manifold variables. + * @param equalityConstrainedProblem Input constrained problem. + * @param mopt_problem Output manifold optimization problem. + */ + void constructManifoldValues( + const EConsOptProblem &equalityConstrainedProblem, + ManifoldOptProblem &mopt_problem) const; + + /** + * Collect initial values for unconstrained variables. + * @param equalityConstrainedProblem Input constrained problem. + * @param mopt_problem Output manifold optimization problem. + */ + static void constructUnconstrainedValues( + const EConsOptProblem &equalityConstrainedProblem, + ManifoldOptProblem &mopt_problem); + + /** Create a factor graph of cost function with the constraint manifold + * variables. + * @param equalityConstrainedProblem Input constrained problem. + * @param mopt_problem Output manifold optimization problem. + */ + static void constructMoptGraph( + const EConsOptProblem &equalityConstrainedProblem, + ManifoldOptProblem &mopt_problem); + + /** Transform an equality-constrained optimization problem into a manifold + * optimization problem by creating constraint manifolds. + * @param equalityConstrainedProblem Input constrained problem. + * @return Transformed manifold optimization problem. + */ + ManifoldOptProblem problemTransform( + const EConsOptProblem &equalityConstrainedProblem) const; + + /** + * Initialize and transform an equality-constrained problem. + * @param costs Cost factor graph. + * @param constraints Equality constraints. + * @param init_values Initial values. + * @return Initialized manifold optimization problem. + */ + ManifoldOptProblem initializeMoptProblem( + const NonlinearFactorGraph &costs, const EqualityConstraints &constraints, + const Values &init_values) const; + + /** + * Reconstruct base variable values from optimized manifold values. + * @param mopt_problem Transformed manifold optimization problem. + * @param nopt_values Optimized values in transformed variable space. + * @return Values in original base variable space. + */ + Values baseValues(const ManifoldOptProblem &mopt_problem, + const Values &nopt_values) const; + + /** + * Lift transformed-space delta to base-space tangent vectors. + * @param mopt_problem Transformed manifold optimization problem. + * @param values Current transformed-space values. + * @param delta Delta in transformed variable space. + * @return Tangent vectors in base variable space. + */ + VectorValues baseTangentVector(const ManifoldOptProblem &mopt_problem, + const Values &values, + const VectorValues &delta) const; +}; + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/MultiJacobian.cpp b/gtdynamics/cmopt/MultiJacobian.cpp new file mode 100644 index 000000000..fb31e40c3 --- /dev/null +++ b/gtdynamics/cmopt/MultiJacobian.cpp @@ -0,0 +1,231 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file MultiJacobian.cpp + * @brief multi-jacobian implementations. + * @author: Yetong Zhang + */ + +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +MultiJacobian::MultiJacobian(const Key &key, const Matrix &matrix) + : Base(), dim_(matrix.rows()) { + emplace(key, matrix); +} + +/* ************************************************************************* */ +MultiJacobian MultiJacobian::Identity(const Key &key, const size_t &dim) { + MultiJacobian jacobian; + jacobian[key] = Matrix::Identity(dim, dim); + jacobian.dim_ = dim; + return jacobian; +} + +/* ************************************************************************* */ +MultiJacobian MultiJacobian::VerticalStack(const MultiJacobian &jac1, + const MultiJacobian &jac2) { + size_t dim1 = jac1.numRows(); + size_t dim2 = jac2.numRows(); + size_t dim = jac1.numRows() + jac2.numRows(); + + MultiJacobian jac; + for (const auto &[key, matrix] : jac1) { + Matrix expanded_mat(dim, matrix.cols()); + expanded_mat << matrix, Matrix::Zero(dim2, matrix.cols()); + jac.addJacobian(key, expanded_mat); + } + for (const auto &[key, matrix] : jac2) { + Matrix expanded_mat(dim, matrix.cols()); + expanded_mat << Matrix::Zero(dim1, matrix.cols()), matrix; + jac.addJacobian(key, expanded_mat); + } + return jac; +} + +/* ************************************************************************* */ +size_t MultiJacobian::numRows() const { + if (dim_ == -1) { + throw std::runtime_error("no data yet"); + } + return dim_; +} + +/* ************************************************************************* */ +void MultiJacobian::addJacobian(const Key &key, const Matrix &matrix) { + if (dim_ == -1) { + dim_ = matrix.rows(); + } + if (find(key) == end()) { + if (!matrix.isZero(1e-8)) { + emplace(key, matrix); + } + } else { + at(key) = at(key) + matrix; + if (at(key).isZero(1e-8)) { + erase(key); + } + } +} + +/* ************************************************************************* */ +void MultiJacobian::addJacobianChainRule(const Matrix &H_relative, + const MultiJacobian parent_jacobian) { + for (const auto &it : parent_jacobian) { + addJacobian(it.first, H_relative * it.second); + } +} + +/* ************************************************************************* */ +VectorValues MultiJacobian::row(const size_t i) const { + VectorValues jac_i; + for (const auto &[key, mat] : *this) { + jac_i.insert(key, mat.row(i).transpose()); + } + return jac_i; +} + +/* ************************************************************************* */ +void MultiJacobian::print(const std::string &s, + const KeyFormatter &keyFormatter) const { + std::cout << s; + for (const auto &it : (*this)) { + std::cout << keyFormatter(it.first) << ":\n"; + std::cout << it.second << "\n"; + } +} + +/* ************************************************************************* */ +bool MultiJacobian::equals(const MultiJacobian &other, double tol) const { + for (const auto &it : *this) { + const Key &key = it.first; + if (other.find(key) == other.end()) { + if (!gtsam::assert_equal(it.second, + Matrix::Zero(it.second.rows(), it.second.cols()), + tol)) { + return false; + } + } else if (!gtsam::assert_equal(it.second, other.at(key), tol)) { + return false; + } + } + for (const auto &it : other) { + if (find(it.first) == end()) { + if (!gtsam::assert_equal(it.second, + Matrix::Zero(it.second.rows(), it.second.cols()), + tol)) { + return false; + } + } + } + return true; +} + +KeySet MultiJacobian::keys() const { + KeySet jac_keys; + for (const auto &it : *this) { + jac_keys.insert(it.first); + } + return jac_keys; +} + +/* ************************************************************************* */ +void ComputeBayesNetJacobian(const GaussianBayesNet &bn, + const KeyVector &basis_keys, + const std::map &var_dim, + MultiJacobians &jacobians) { + // set jacobian of basis variables to identity + for (const Key &key : basis_keys) { + jacobians.emplace(key, MultiJacobian::Identity(key, var_dim.at(key))); + } + + GaussianBayesNet reversed_bn = bn; + std::reverse(reversed_bn.begin(), reversed_bn.end()); + + // iteratively set jacobian of other variables + for (auto cg : reversed_bn) { + const Matrix S_mat = -cg->R().triangularView().solve(cg->S()); + Eigen::DenseIndex frontal_position = 0; + for (auto frontal = cg->beginFrontals(); frontal != cg->endFrontals(); + ++frontal) { + const Key frontal_key = *frontal; + const auto frontal_dim = cg->getDim(frontal); + // initialize jacobian for frontal variable + jacobians[frontal_key] = MultiJacobian(); + // add the jacobian component from each parent + Eigen::DenseIndex parent_position = 0; + for (auto parent = cg->beginParents(); parent != cg->endParents(); + ++parent) { + const Key parent_key = *parent; + const auto parent_dim = cg->getDim(parent); + if (frontal_position + frontal_dim > S_mat.rows() || + parent_position + parent_dim > S_mat.cols()) { + throw std::runtime_error( + "frontal: " + std::to_string(frontal_position) + ", " + + std::to_string(frontal_dim) + + "\tparent: " + std::to_string(parent_position) + ", " + + std::to_string(parent_dim) + + "\tS_mat: " + std::to_string(S_mat.rows()) + ", " + + std::to_string(S_mat.cols()) + + "\tR: " + std::to_string(cg->R().cols()) + ", " + + std::to_string(cg->R().rows()) + + "\tS: " + std::to_string(cg->S().cols()) + ", " + + std::to_string(cg->S().rows())); + } + Matrix S_parent = S_mat.block(frontal_position, parent_position, + frontal_dim, parent_dim); + jacobians[frontal_key].addJacobianChainRule(S_parent, + jacobians[parent_key]); + parent_position += parent_dim; + } + frontal_position += frontal_dim; + } + } +} + +void MultiJacobian::operator+=(const MultiJacobian &other) { + for (const auto &it : other) { + addJacobian(it.first, it.second); + } +} + +MultiJacobian operator*(const Matrix &m, const MultiJacobian &jac) { + MultiJacobian new_jac; + for (const auto &it : jac) { + new_jac.insert({it.first, m * it.second}); + } + return new_jac; +} + +MultiJacobian operator+(const MultiJacobian &jac1, const MultiJacobian &jac2) { + MultiJacobian sum_jac = jac1; + for (const auto &it : jac2) { + sum_jac.addJacobian(it.first, it.second); + } + return sum_jac; +} + +MultiJacobians JacobiansMultiply(const MultiJacobians &jacs1, + const MultiJacobians &jacs2) { + MultiJacobians result_jacs; + for (const auto &it : jacs1) { + MultiJacobian jac; + const MultiJacobian &jac1 = it.second; + for (const auto &it1 : jac1) { + const Key &key = it1.first; + const Matrix &m = it1.second; + jac += m * jacs2.at(key); + } + result_jacs.insert({it.first, jac}); + } + return result_jacs; +} + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/MultiJacobian.h b/gtdynamics/cmopt/MultiJacobian.h new file mode 100644 index 000000000..36ee2f2d2 --- /dev/null +++ b/gtdynamics/cmopt/MultiJacobian.h @@ -0,0 +1,160 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file MultiJacobian.h + * @brief Jacobian w.r.t. multiple variables. + * @author: Yetong Zhang + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace gtdynamics { + +using gtsam::DefaultKeyFormatter; +using gtsam::GaussianBayesNet; +using gtsam::Key; +using gtsam::KeyFormatter; +using gtsam::KeySet; +using gtsam::KeyVector; +using gtsam::Matrix; +using gtsam::VectorValues; + +/** + * Jacobian map with respect to multiple variables. + * + * For an output variable block `x = f(m, n, o)`, this stores partial Jacobians + * `{dx/dm, dx/dn, dx/do}` keyed by variable key and supports chain-rule + * composition used by elimination-based tangent basis construction. + * + * @see README.md#tangent-basis + */ +class MultiJacobian : public std::unordered_map { + protected: + int dim_ = -1; + + public: + using Base = std::unordered_map; + + /// Default constructor. + MultiJacobian() : Base() {} + + /** + * Constructor from a Jacobian to a single variable. + * @param key Variable key. + * @param matrix Jacobian matrix for the variable. + */ + MultiJacobian(const Key& key, const Matrix& matrix); + + /** + * Construct identity Jacobian for a single variable. + * @param key Variable key. + * @param dim Variable dimension. + * @return Identity multi-variable Jacobian. + */ + static MultiJacobian Identity(const Key& key, const size_t& dim); + + /** + * Vertically stack two multi-variable Jacobians. + * @param jac1 First Jacobian block. + * @param jac2 Second Jacobian block. + * @return Vertically stacked Jacobian. + */ + static MultiJacobian VerticalStack(const MultiJacobian& jac1, + const MultiJacobian& jac2); + + /** + * Add Jacobian for a variable. + * @param key Variable key. + * @param matrix Jacobian block to add. + * @note If an entry already exists for `key`, matrices are summed. + */ + void addJacobian(const Key& key, const Matrix& matrix); + + /** + * Add Jacobian terms by chain rule. + * @param H_relative Relative Jacobian to the parent variable. + * @param parent_jacobian Parent Jacobian map. + */ + void addJacobianChainRule(const Matrix& H_relative, + const MultiJacobian parent_jacobian); + + /** + * Print Jacobian entries. + * @param s Prefix string. + * @param keyFormatter Key formatter. + */ + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** + * Compare two multi-variable Jacobians. + * @param other Jacobian to compare with. + * @param tol Absolute comparison tolerance. + * @return True if equal within tolerance. + */ + bool equals(const MultiJacobian& other, double tol = 1e-8) const; + + /// Return the i-th row of the jacobian as vector values. + VectorValues row(const size_t i) const; + + KeySet keys() const; + + size_t numRows() const; + + void operator+=(const MultiJacobian& other); +}; + +/** + * Left-multiply each Jacobian block by a matrix. + * @param m Left multiplication matrix. + * @param jac Input Jacobian. + * @return Resulting Jacobian. + */ +MultiJacobian operator*(const Matrix& m, const MultiJacobian& jac); + +/** + * Add two multi-variable Jacobians. + * @param jac1 First Jacobian. + * @param jac2 Second Jacobian. + * @return Sum Jacobian. + */ +MultiJacobian operator+(const MultiJacobian& jac1, const MultiJacobian& jac2); + +/** jacobian of multiple variables w.r.t. multiple variables. */ +typedef std::unordered_map MultiJacobians; + +/** + * Compose two Jacobian maps. + * @param multi_jac1 Outer Jacobian map. + * @param multi_jac2 Inner Jacobian map. + * @return Composed Jacobian map. + */ +MultiJacobians JacobiansMultiply(const MultiJacobians& multi_jac1, + const MultiJacobians& multi_jac2); + +/** Given a bayes net, compute the jacobians of all variables w.r.t. basis + * variables. + * @param bn bayes net + * @param basis_keys basis variables + * @param var_dim dimension of variables + * @param jacobians output, jacobians of all variables + */ +void ComputeBayesNetJacobian(const GaussianBayesNet& bn, + const KeyVector& basis_keys, + const std::map& var_dim, + MultiJacobians& jacobians); + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/NonlinearMOptimizer.cpp b/gtdynamics/cmopt/NonlinearMOptimizer.cpp new file mode 100644 index 000000000..141f91d6d --- /dev/null +++ b/gtdynamics/cmopt/NonlinearMOptimizer.cpp @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearMOptimizer.cpp + * @brief Manifold optimizer implementations. + * @author: Yetong Zhang + */ + +#include +#include +#include +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +Values NonlinearMOptimizer::optimize(const NonlinearFactorGraph& costs, + const EqualityConstraints& constraints, + const Values& init_values) const { + auto mopt_problem = initializeMoptProblem(costs, constraints, init_values); + return optimizeMOpt(mopt_problem); +} + +/* ************************************************************************* */ +Values NonlinearMOptimizer::optimizeMOpt( + const ManifoldOptProblem& mopt_problem) const { + auto nonlinear_optimizer = constructNonlinearOptimizer(mopt_problem); + auto nopt_values = nonlinear_optimizer->optimize(); + // if (intermediate_result) { + // intermediate_result->num_iters.push_back( + // std::dynamic_pointer_cast( + // nonlinear_optimizer) + // ->getInnerIterations()); + // } + return baseValues(mopt_problem, nopt_values); +} + +/* ************************************************************************* */ +std::shared_ptr +NonlinearMOptimizer::constructNonlinearOptimizer( + const ManifoldOptProblem& mopt_problem) const { + if (std::holds_alternative(nopt_params_)) { + return std::make_shared( + mopt_problem.graph_, mopt_problem.values_, + std::get(nopt_params_)); + } else if (std::holds_alternative(nopt_params_)) { + return std::make_shared( + mopt_problem.graph_, mopt_problem.values_, + std::get(nopt_params_)); + } else if (std::holds_alternative(nopt_params_)) { + return std::make_shared( + mopt_problem.graph_, mopt_problem.values_, + std::get(nopt_params_)); + } else { + return std::make_shared( + mopt_problem.graph_, mopt_problem.values_); + } +} + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/NonlinearMOptimizer.h b/gtdynamics/cmopt/NonlinearMOptimizer.h new file mode 100644 index 000000000..4c39c5e96 --- /dev/null +++ b/gtdynamics/cmopt/NonlinearMOptimizer.h @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearMOptimizer.h + * @brief Manifold optimizer that internally calls nonlinear optimizer. + * @author: Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +namespace gtdynamics { + +using gtsam::DoglegParams; +using gtsam::GaussNewtonParams; +using gtsam::LevenbergMarquardtParams; +using gtsam::NonlinearFactorGraph; +using gtsam::NonlinearOptimizer; +using gtsam::Values; + +/** + * Generic optimizer wrapper for transformed manifold problems. + * + * This optimizer relies on `ManifoldOptimizer` for transformation and then + * delegates optimization to a selected GTSAM nonlinear optimizer + * (Gauss-Newton, LM, or Dogleg) on manifold-valued variables. + * + * @see README.md#solvers + * @see README.md#ccc-transformation + */ +class NonlinearMOptimizer : public ManifoldOptimizer { + public: + using shared_ptr = std::shared_ptr; + typedef std::variant + NonlinearOptParamsVariant; + + protected: + NonlinearOptParamsVariant nopt_params_; + + public: + /** + * Construct from manifold and nonlinear optimizer parameters. + * @param mopt_params Parameters for manifold construction/transformation. + * @param nopt_params Parameters for the underlying nonlinear optimizer. + * @param basis_key_func Optional basis-key selector. + */ + NonlinearMOptimizer(const ManifoldOptimizerParameters& mopt_params, + const NonlinearOptParamsVariant& nopt_params, + std::optional basis_key_func = {}) + : ManifoldOptimizer(mopt_params), nopt_params_(nopt_params) {} + + /// Virtual destructor. + virtual ~NonlinearMOptimizer() {} + + /** + * Run manifold optimization from a constrained problem definition. + * @param graph Cost factor graph. + * @param constraints Equality constraints. + * @param initial_values Initial values for all variables. + * @return Optimized base-variable values. + */ + virtual Values optimize(const NonlinearFactorGraph& graph, + const EqualityConstraints& constraints, + const Values& initial_values) const override; + + /** + * Optimize an already transformed manifold optimization problem. + * @param mopt_problem Transformed manifold optimization problem. + * @return Optimized base-variable values. + */ + Values optimizeMOpt(const ManifoldOptProblem& mopt_problem) const; + + /** + * Create the underlying nonlinear optimizer instance. + * @param mopt_problem Transformed manifold optimization problem. + * @return Nonlinear optimizer configured for transformed variables. + */ + std::shared_ptr constructNonlinearOptimizer( + const ManifoldOptProblem& mopt_problem) const; + + protected: +}; + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/README.md b/gtdynamics/cmopt/README.md new file mode 100644 index 000000000..1ae6a75e3 --- /dev/null +++ b/gtdynamics/cmopt/README.md @@ -0,0 +1,149 @@ +# Constraint Manifold Optimization + + +## Overview +This note maps the math in *Constraint Manifolds for Robotic Inference and Planning* (ICRA 2023) to the GTDynamics implementation in `gtdynamics/cmopt`. + +> **Note:** This document was created from the paper and code using GitHub Copilot (codex 5.3) on Feb 8, 2026. + +The paper starts from a nonlinear equality-constrained problem: + +\[ +\min_{X \in \mathcal{M}} \sum_i f_i(X_i^f) +\quad \text{s.t.} \quad h_j(X_j^h)=0 +\] + +(paper Eq. 11), then transforms it into an unconstrained problem on new variables \(\Theta\): + +\[ +\min_{\Theta \in \bar{\mathcal{M}}} \sum_i \bar f_i(\Theta_i^f) +\] + +(paper Eq. 16), where each constraint-connected component (CCC) is replaced by a constraint manifold variable. + +In code, this transformation happens in three stages: +- Find CCCs from equality constraints: [`ManifoldOptimizer::IdentifyConnectedComponents`](ManifoldOptimizer.cpp#L119) +- Build one `ConstraintManifold` per CCC: [`ManifoldOptimizer::constructManifoldValues`](ManifoldOptimizer.cpp#L184) +- Build equivalent cost factors on manifold variables: [`ManifoldOptimizer::constructMoptGraph`](ManifoldOptimizer.cpp#L232), using [`SubstituteFactor`](../factors/SubstituteFactor.h#L39) +- Relevant headers/classes: [`ManifoldOptimizer`](ManifoldOptimizer.h), [`ConstraintManifold`](ConstraintManifold.h), [`SubstituteFactor`](../factors/SubstituteFactor.h) + +If you are looking for where the paper’s “equivalent factors” are created, start at: +- [`ManifoldOptimizer.cpp#L252`](ManifoldOptimizer.cpp#L252) +- [`ManifoldOptimizer.cpp#L262`](ManifoldOptimizer.cpp#L262) +- [`SubstituteFactor.cpp#L82`](../factors/SubstituteFactor.cpp#L82) + + +## 1) CCC Identification and Problem Transformation (Paper Sec. III-A) +Paper definition: +- CCC \(C_c = (X_c^C, H_c(X_c^C)=0)\) +- Replace each CCC by \(\theta_c \in \mathcal{M}_c\) +- Replace each affected cost factor \(f_i\) by \(\bar f_i\) through substitution (paper Eq. 15) + +Code mapping: +- DFS over constraint-variable bipartite structure: [`IdentifyConnectedComponent`](ManifoldOptimizer.cpp#L88) +- All components: [`IdentifyConnectedComponents`](ManifoldOptimizer.cpp#L119) +- Full transformation pipeline: [`problemTransform`](ManifoldOptimizer.cpp#L165) +- Build map from base keys to manifold keys: [`constructMoptGraph`](ManifoldOptimizer.cpp#L235) +- Relevant headers/classes: [`ManifoldOptimizer`](ManifoldOptimizer.h), [`ManifoldOptProblem`](ManifoldOptimizer.h), [`ConstraintManifold`](ConstraintManifold.h) + + +## 2) Constraint Manifold Object and Dimension (Paper Sec. IV) +Paper definition: + +\[ +\mathcal{M}_c = \{X_c^C \in \tilde{\mathcal{M}}_c \mid H_c(X_c^C)=0\} +\] + +(paper Eq. 13). + +Code mapping: +- Core type: [`ConstraintManifold`](ConstraintManifold.h#L46) +- Dimension set as `embedding_dim - constraint_dim`: [`ConstraintManifold.h#L90`](ConstraintManifold.h#L90) +- Optional initial feasibility projection on construction: [`ConstraintManifold::constructValues`](ConstraintManifold.cpp#L23) +- Recover base variable from manifold value (used by substituted factors): [`ConstraintManifold::recover`](ConstraintManifold.cpp#L34) +- Relevant headers/classes: [`ConstraintManifold`](ConstraintManifold.h), [`EManifoldValues`](ConstraintManifold.h) + + +## 3) Tangent Space Basis \(T_{\theta_c}\mathcal{M}_c=\ker D H_c\) (Paper Eqs. 19–20) +Paper definition: + +\[ +T_{\theta_c}\mathcal{M}_c = \ker D H_c(X_c^C), \quad +\delta X = B_{\theta_c}\,\xi +\] + +Code mapping (orthonormal/null-space basis): +- Build constraint Jacobian from linearized penalty graph: [`OrthonormalBasis::computeConstraintJacobian`](TspaceBasis.cpp#L57) +- Dense null-space basis \(N = \mathrm{kernel}(J_H)\): [`OrthonormalBasis::constructDense`](TspaceBasis.cpp#L108) +- Sparse basis construction: [`OrthonormalBasis::constructSparse`](TspaceBasis.cpp#L115) +- Map reduced coordinates \(\xi\) to ambient tangent vector \(\delta X\): [`OrthonormalBasis::computeTangentVector`](TspaceBasis.cpp#L223) + +Code mapping (elimination/basis-variable parameterization): +- Eliminate non-basis vars and recover Jacobian dependencies: [`EliminationBasis::construct`](TspaceBasis.cpp#L492) +- Bayes-net Jacobian propagation (chain rule): [`ComputeBayesNetJacobian`](MultiJacobian.cpp#L140) +- Reduced-to-full tangent lift: [`EliminationBasis::computeTangentVector`](TspaceBasis.cpp#L572) +- Relevant headers/classes: [`TspaceBasis`](TspaceBasis.h), [`OrthonormalBasis`](TspaceBasis.h), [`EliminationBasis`](TspaceBasis.h), [`MultiJacobian`](MultiJacobian.h) + + +## 4) Retraction on Constraint Manifold (Paper Sec. IV-B, Eqs. 22–24) +Paper idea: +- Retraction is done by moving in tangent space, then projecting/matching back to satisfy constraints. +- Paper gives metric projection (Eq. 22), approximate projection (Eq. 23), and basis-variable retraction (Eq. 24). + +Code mapping: +- Generic flow \( \xi \to \delta X \to R(\delta X)\): [`ConstraintManifold::retract`](ConstraintManifold.cpp#L45) +- Retraction base class, with “base retract then satisfy constraints”: [`Retractor::retract`](Retractor.h#L87) + +Implementations: +- Approximate projection via unconstrained solve on penalty graph (close to Eq. 23): [`UoptRetractor::retractConstraints`](Retractor.cpp#L56) +- Projection with extra priors before optional cleanup solve: [`ProjRetractor::retract`](Retractor.cpp#L88) +- Basis-variable retraction (close to Eq. 24): [`BasisRetractor::retractConstraints`](Retractor.cpp#L154) +- Relevant headers/classes: [`Retractor`](Retractor.h), [`UoptRetractor`](Retractor.h), [`ProjRetractor`](Retractor.h), [`BasisRetractor`](Retractor.h), [`DynamicsRetractor`](Retractor.h) + +Approximate retraction mode from the paper (Sec. V-B, Eq. 26) is used in examples by limiting inner retraction iterations: +- [`main_connected_poses.cpp#L323`](../../examples/example_constraint_manifold/main_connected_poses.cpp#L323) +- [`main_cablerobot.cpp#L223`](../../examples/example_constraint_manifold/main_cablerobot.cpp#L223) + + +## 5) Equivalent Cost Factors and Chain Rule Jacobians (Paper Eq. 25) +Paper equation: + +\[ +J_{\bar f}(\theta_c)=\sum_{k \in I_c^C \cap I_i^f} +J_{r_k}(\theta_c)\,J_f(x_k) +\] + +Code mapping: +- Equivalent factor type: [`SubstituteFactor`](../factors/SubstituteFactor.h#L39) +- Key substitution and de-duplication: [`computeNewKeys`](../factors/SubstituteFactor.cpp#L20) +- Handle fully constrained components (fixed manifolds): [`classifyKeys`](../factors/SubstituteFactor.cpp#L52) +- Build base values and evaluate base factor: [`unwhitenedError`](../factors/SubstituteFactor.cpp#L82) +- Chain-rule Jacobian accumulation with recover Jacobians: [`SubstituteFactor.cpp#L113`](../factors/SubstituteFactor.cpp#L113) +- Relevant headers/classes: [`ManifoldOptimizer`](ManifoldOptimizer.h), [`ConstraintManifold`](ConstraintManifold.h), [`SubstituteFactor`](../factors/SubstituteFactor.h) + +This is the concrete implementation of paper Eq. 15 (factor substitution) + Eq. 25 (Jacobian chain rule). + + +## 6) Solving the Transformed Problem +There are two optimizer paths in code: +- Generic “reuse GTSAM nonlinear optimizers on manifold-valued variables”: [`NonlinearMOptimizer`](NonlinearMOptimizer.cpp#L22) +- Custom LM implementation with explicit trial/state bookkeeping: [`LMManifoldOptimizer`](LMManifoldOptimizer.cpp#L43) + +In benchmark helpers, `ConstrainedOptBenchmark::OptimizeCmOpt` uses +`NonlinearMOptimizer`: +- [`ConstrainedOptBenchmark.cpp`](../constrained_optimizer/ConstrainedOptBenchmark.cpp) + +Default CM parameter builders: +- Dense/null-space + unconstrained retraction: + [`ConstrainedOptBenchmark::DefaultMoptParams`](../constrained_optimizer/ConstrainedOptBenchmark.cpp) +- Basis-variable elimination + basis retraction: + [`ConstrainedOptBenchmark::DefaultMoptParamsSV`](../constrained_optimizer/ConstrainedOptBenchmark.cpp) +- Relevant headers/classes: [`NonlinearMOptimizer`](NonlinearMOptimizer.h), [`LMManifoldOptimizer`](LMManifoldOptimizer.h), [`LMState/LMTrial`](LMManifoldOptimizerState.h), [`ManifoldOptimizerParameters`](ManifoldOptimizer.h) + + +## 7) Quick “Follow One Run” Pointers +For a concrete end-to-end run, `examples/example_constraint_manifold/main_connected_poses.cpp` is a good example: +- Build equality constraints from a constraint graph: [`main_connected_poses.cpp#L269`](../../examples/example_constraint_manifold/main_connected_poses.cpp#L269) +- Build default CM params: [`main_connected_poses.cpp#L307`](../../examples/example_constraint_manifold/main_connected_poses.cpp#L307) +- Run CM optimization: [`main_connected_poses.cpp#L317`](../../examples/example_constraint_manifold/main_connected_poses.cpp#L317) +- Relevant headers/classes: [`ManifoldOptimizer`](ManifoldOptimizer.h), [`ConstraintManifold`](ConstraintManifold.h), [`NonlinearMOptimizer`](NonlinearMOptimizer.h) diff --git a/gtdynamics/cmopt/Retractor.cpp b/gtdynamics/cmopt/Retractor.cpp new file mode 100644 index 000000000..8f110d33a --- /dev/null +++ b/gtdynamics/cmopt/Retractor.cpp @@ -0,0 +1,420 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file TspaceBasis.cpp + * @brief Tagent space basis implementations. + * @author: Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "factors/ConstVarFactor.h" +#include "utils/DynamicsSymbol.h" +#include "utils/values.h" + +namespace gtdynamics { + +/* ************************************************************************* */ +void Retractor::checkFeasible(const NonlinearFactorGraph &graph, + const Values &values) const { + if (params_->check_feasible) { + if (graph.error(values) > params_->feasible_threshold) { + std::cout << "fail: " << graph.error(values) << "\n"; + } + } +} + +/* ************************************************************************* */ +UoptRetractor::UoptRetractor(const EqualityConstraints::shared_ptr &constraints, + const RetractParams::shared_ptr ¶ms) + : Retractor(params), + optimizer_(constraints->penaltyGraph(), params->lm_params) {} + +/* ************************************************************************* */ +Values UoptRetractor::retractConstraints(const Values &values) { + optimizer_.setValues(values); + const Values &result = optimizer_.optimize(); + checkFeasible(optimizer_.mutableGraph(), result); + return result; +} + +/* ************************************************************************* */ +Values UoptRetractor::retractConstraints(Values &&values) { + optimizer_.setValues(values); + auto result = optimizer_.optimize(); + checkFeasible(optimizer_.mutableGraph(), result); + return std::move(result); +} + +/* ************************************************************************* */ +ProjRetractor::ProjRetractor(const EqualityConstraints::shared_ptr &constraints, + const RetractParams::shared_ptr ¶ms, + std::optional basis_keys) + : Retractor(params), merit_graph_(constraints->penaltyGraph()) { + if (params->use_basis_keys) { + basis_keys_ = *basis_keys; + } +} + +/* ************************************************************************* */ +Values ProjRetractor::retractConstraints(const Values &values) { + gtsam::LevenbergMarquardtOptimizer optimizer(merit_graph_, values); + return optimizer.optimize(); +} + +/* ************************************************************************* */ +Values ProjRetractor::retract(const Values &values, const VectorValues &delta) { + // optimize with priors + NonlinearFactorGraph graph = merit_graph_; + Values values_retract_base = retractBaseVariables(values, delta); + if (params_->use_basis_keys) { + AddGeneralPriors(values_retract_base, basis_keys_, params_->sigma, graph); + } else { + AddGeneralPriors(values_retract_base, params_->sigma, graph); + } + // const Values &init_values = + // params_->apply_base_retraction ? values_retract_base : values; + const Values &init_values = values_retract_base; + gtsam::LevenbergMarquardtOptimizer optimizer_with_priors(graph, init_values, + params_->lm_params); + const Values &result = optimizer_with_priors.optimize(); + if (params_->use_basis_keys && + optimizer_with_priors.error() < params_->feasible_threshold) { + return result; + } + + // optimize without priors + gtsam::LevenbergMarquardtOptimizer optimizer_without_priors( + merit_graph_, result, params_->lm_params); + const Values &final_result = optimizer_without_priors.optimize(); + checkFeasible(merit_graph_, final_result); + return final_result; +} + +/* ************************************************************************* */ +BasisRetractor::BasisRetractor( + const EqualityConstraints::shared_ptr &constraints, + const RetractParams::shared_ptr ¶ms, const KeyVector &basis_keys) + : Retractor(params), + merit_graph_(constraints->penaltyGraph()), + basis_keys_(basis_keys), + optimizer_(params->lm_params) { + NonlinearFactorGraph graph; + KeySet fixed_keys(basis_keys.begin(), basis_keys.end()); + for (const auto &factor : merit_graph_) { + // check if the factor contains fixed keys + bool contain_fixed_keys = false; + for (const Key &key : factor->keys()) { + if (fixed_keys.exists(key)) { + contain_fixed_keys = true; + break; + } + } + // update the factor if it constains fixed keys + if (contain_fixed_keys) { + NoiseModelFactor::shared_ptr noise_factor = + std::dynamic_pointer_cast(factor); + auto const_var_factor = + std::make_shared(noise_factor, fixed_keys); + if (const_var_factor->checkActive()) { + factors_with_fixed_vars_.push_back(const_var_factor); + graph.add(const_var_factor); + } + } else { + graph.add(factor); + } + } + // set the graph for optimizer + optimizer_.setGraph(graph); +} + +/* ************************************************************************* */ +Values BasisRetractor::retractConstraints(const Values &values) { + static const bool debug_retractor_stats = []() { + const char* env = std::getenv("GTDYN_DEBUG_RETRACTOR_STATS"); + return env && std::string(env) != "0"; + }(); + static size_t total_calls = 0; + static size_t fast_path_calls = 0; + static double total_ms = 0.0; + + const auto start = std::chrono::steady_clock::now(); + ++total_calls; + + // set fixed values for the const variable factors + for (auto &factor : factors_with_fixed_vars_) { + factor->setFixedValues(values); + } + // set initial values for optimizer + Values opt_values; + for (const Key &key : optimizer_.graph().keys()) { + opt_values.insert(key, values.at(key)); + } + + // Fast path: if constraints are already satisfied to threshold, avoid + // repeatedly running tiny LM solves. + const double initial_error = optimizer_.graph().error(opt_values); + if (initial_error <= params_->feasible_threshold) { + ++fast_path_calls; + Values result = opt_values; + for (const Key &key : basis_keys_) { + result.insert(key, values.at(key)); + } + const auto end = std::chrono::steady_clock::now(); + total_ms += + std::chrono::duration_cast(end - start) + .count() * + 1e-3; + if (debug_retractor_stats && total_calls % 200 == 0) { + std::cout << "[RETRACTOR] calls=" << total_calls + << ", fast_path=" << fast_path_calls + << ", avg_ms=" << (total_ms / static_cast(total_calls)) + << ", last_initial_error=" << initial_error << "\n"; + } + return result; + } + + optimizer_.setValues(std::move(opt_values)); + // optimize + Values result = optimizer_.optimize(); + + if (params_->recompute) { + if (optimizer_.graph().error(result) > 1e-5) { + std::cout << "recompute\n"; + for (const Key &key : basis_keys_) { + result.insert(key, values.at(key)); + } + auto optimizer = gtsam::LevenbergMarquardtOptimizer(merit_graph_, result); + result = optimizer.optimize(); + checkFeasible(merit_graph_, result); + return result; + } + } + + // add fixed varaibles to the result + for (const Key &key : basis_keys_) { + result.insert(key, values.at(key)); + } + const auto end = std::chrono::steady_clock::now(); + total_ms += + std::chrono::duration_cast(end - start) + .count() * + 1e-3; + if (debug_retractor_stats && total_calls % 200 == 0) { + std::cout << "[RETRACTOR] calls=" << total_calls + << ", fast_path=" << fast_path_calls + << ", avg_ms=" << (total_ms / static_cast(total_calls)) + << ", last_initial_error=" << initial_error << "\n"; + } + return result; +} + +bool isQLevel(const Key &key) { + DynamicsSymbol symbol(key); + return symbol.label() == "p" || symbol.label() == "q"; +} + +bool isVLevel(const Key &key) { + DynamicsSymbol symbol(key); + return symbol.label() == "V" || symbol.label() == "v"; +} + +/* ************************************************************************* */ +template +void AddLinearPriors(NonlinearFactorGraph &graph, const CONTAINER &keys, + const Values &values = Values()) { + for (const Key &key : keys) { + DynamicsSymbol symbol(key); + if (symbol.label() == "p") { + graph.addPrior( + key, + values.exists(key) ? values.at(key) : gtsam::Pose3(), + gtsam::noiseModel::Isotropic::Sigma(6, 1e-2)); + } else if (symbol.label() == "q" || symbol.label() == "v" || + symbol.label() == "a" || symbol.label() == "T") { + graph.addPrior(key, + values.exists(key) ? values.atDouble(key) : 0.0, + gtsam::noiseModel::Isotropic::Sigma(1, 1e-2)); + } else { + gtsam::Vector6 value = values.exists(key) ? values.at(key) + : gtsam::Vector6::Zero(); + graph.addPrior( + key, value, gtsam::noiseModel::Isotropic::Sigma(6, 1e-2)); + } + } +} + +/* ************************************************************************* */ +template +void DynamicsRetractor::classifyKeys(const CONTAINER &keys, KeySet &q_keys, + KeySet &v_keys, KeySet &ad_keys) { + for (const Key &key : keys) { + if (isQLevel(key)) { + q_keys.insert(key); + } else if (isVLevel(key)) { + v_keys.insert(key); + } else { + ad_keys.insert(key); + } + } +} + +/* ************************************************************************* */ +DynamicsRetractor::DynamicsRetractor( + const EqualityConstraints::shared_ptr &constraints, + const RetractParams::shared_ptr ¶ms, + std::optional basis_keys) + : Retractor(params), + merit_graph_(constraints->penaltyGraph()), + optimizer_wp_q_(params->lm_params), + optimizer_wp_v_(params->lm_params), + optimizer_wp_ad_(params->lm_params), + optimizer_np_q_(params->lm_params), + optimizer_np_v_(params->lm_params), + optimizer_np_ad_(params->lm_params) { + /// classify keys + KeySet q_keys, v_keys, ad_keys, qv_keys; + classifyKeys(merit_graph_.keys(), q_keys, v_keys, ad_keys); + qv_keys = q_keys; + qv_keys.merge(v_keys); + + /// classify basis keys + if (params_->use_basis_keys) { + if (!basis_keys) { + throw std::runtime_error("basis keys not provided for retractor."); + } + classifyKeys(*basis_keys, basis_q_keys_, basis_v_keys_, basis_ad_keys_); + } else { + basis_q_keys_ = q_keys; + basis_v_keys_ = v_keys; + basis_ad_keys_ = ad_keys; + } + basis_q_keys_ = q_keys; + + /// classify factors + NonlinearFactorGraph graph_q, graph_v, graph_ad; + for (const auto &factor : merit_graph_) { + int lvl = 0; + for (const auto &key : factor->keys()) { + if (isQLevel(key)) { + lvl = std::max(lvl, 0); + } else if (isVLevel(key)) { + lvl = std::max(lvl, 1); + } else { + lvl = std::max(lvl, 2); + } + } + if (lvl == 0) { + graph_q.add(factor); + } else if (lvl == 1) { + graph_v.add(factor); + } else { + graph_ad.add(factor); + } + } + + /// Update as ConstVarFactors + std::tie(graph_v, const_var_factors_v_) = ConstVarGraph(graph_v, q_keys); + std::tie(graph_ad, const_var_factors_ad_) = ConstVarGraph(graph_ad, qv_keys); + + // Set the graphs for optimizers + optimizer_np_q_.setGraph(graph_q); + optimizer_np_v_.setGraph(graph_v); + optimizer_np_ad_.setGraph(graph_ad); + + // Set the graphs for optimizers + AddLinearPriors(graph_q, basis_q_keys_); + AddLinearPriors(graph_v, basis_v_keys_); + AddLinearPriors(graph_ad, basis_ad_keys_); + optimizer_wp_q_.setGraph(graph_q); + optimizer_wp_v_.setGraph(graph_v); + optimizer_wp_ad_.setGraph(graph_ad); +} + +/* ************************************************************************* */ +void DynamicsRetractor::updatePriors(const Values &values, const KeySet &keys, + NonlinearFactorGraph &graph) { + graph.erase(graph.begin() + graph.size() - keys.size(), graph.end()); + AddGeneralPriors(values, keys, params_->sigma, graph); +} + +/* ************************************************************************* */ +Values DynamicsRetractor::retractConstraints(const Values &values) { + Values known_values; + + // solve q level + updatePriors(values, basis_q_keys_, optimizer_wp_q_.mutableGraph()); + optimizer_wp_q_.setValues(SubValues(values, optimizer_wp_q_.graph().keys())); + auto results_q = optimizer_wp_q_.optimize(); + + optimizer_np_q_.setValues(results_q); + results_q = optimizer_np_q_.optimize(); + known_values.insert(results_q); + + // solve v level + updatePriors(values, basis_v_keys_, optimizer_wp_v_.mutableGraph()); + for (auto &factor : const_var_factors_v_) { + factor->setFixedValues(known_values); + } + optimizer_wp_v_.setValues(SubValues(values, optimizer_wp_v_.graph().keys())); + auto results_v = optimizer_wp_v_.optimize(); + + optimizer_np_v_.setValues(results_v); + results_v = optimizer_np_v_.optimize(); + known_values.insert(results_v); + + // solve a and dynamics level + updatePriors(values, basis_ad_keys_, optimizer_wp_ad_.mutableGraph()); + for (auto &factor : const_var_factors_ad_) { + factor->setFixedValues(known_values); + } + optimizer_wp_ad_.setValues( + SubValues(values, optimizer_wp_ad_.graph().keys())); + auto results_ad = optimizer_wp_ad_.optimize(); + + optimizer_np_ad_.setValues(results_ad); + results_ad = optimizer_np_ad_.optimize(); + known_values.insert(results_ad); + checkFeasible(merit_graph_, known_values); + + // NonlinearFactorGraph graph_wp_all = cc_->merit_graph_; + // KeySet all_basis_keys = basis_q_keys_; + // all_basis_keys.merge(basis_v_keys_); + // all_basis_keys.merge(basis_ad_keys_); + // AddGeneralPriors(values, all_basis_keys, params_->sigma, graph_wp_all); + + // LevenbergMarquardtParams lm_params = params_->lm_params; + // lm_params.setMaxIterations(10); + // LevenbergMarquardtOptimizer optimizer_wp_all(graph_wp_all, known_values, + // params_->lm_params); auto result = optimizer_wp_all.optimize(); + + // LevenbergMarquardtOptimizer optimizer_np_all(cc_->merit_graph_, result, + // params_->lm_params); result = optimizer_np_all.optimize(); + // checkFeasible(cc_->merit_graph_, result); + + return known_values; +} + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/Retractor.h b/gtdynamics/cmopt/Retractor.h new file mode 100644 index 000000000..4e386e594 --- /dev/null +++ b/gtdynamics/cmopt/Retractor.h @@ -0,0 +1,428 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file TspaceBasis.h + * @brief Basis for tangent space of constraint manifold. Detailed definition of + * tangent space and basis are available at Boumal20book Sec.8.4. + * @author: Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace gtdynamics { + +using EqualityConstraints = gtsam::NonlinearEqualityConstraints; +using gtsam::Key; +using gtsam::KeySet; +using gtsam::KeyVector; +using gtsam::LevenbergMarquardtParams; +using gtsam::NonlinearFactorGraph; +using gtsam::Values; +using gtsam::VectorValues; + +/** + * Parameters for constraint-manifold retraction. + * + * These parameters control feasibility checks, solver behavior, optional basis + * key usage, and prior strengths used by different retractor implementations. + * + * @see README.md#retraction + */ +struct RetractParams { + public: + using shared_ptr = std::shared_ptr; + + // Member variables + bool check_feasible = false; + double feasible_threshold = 1e-5; + LevenbergMarquardtParams lm_params; + bool use_basis_keys = false; + double sigma = 1.0; + bool apply_base_retraction = false; + bool recompute = false; + + // Constructor + RetractParams() = default; +}; + +/** + * Base class for constraint-manifold retraction. + * + * Retraction is implemented as: + * 1. Retract base variables in ambient space. + * 2. Enforce equality constraints to return to the constraint manifold. + * + * Concrete subclasses define different feasibility-enforcement strategies. + * + * @see README.md#retraction + */ +class Retractor { + protected: + RetractParams::shared_ptr params_; + + public: + using shared_ptr = std::shared_ptr; + + /// Default constructor. + Retractor(const RetractParams::shared_ptr ¶ms = + std::make_shared()) + : params_(params) {} + + virtual ~Retractor() {} + + /** + * Retract base variables in ambient space before enforcing constraints. + * @param values Base values composing the constraint manifold. + * @param delta Tangent update in ambient coordinates. + * @return Updated base values prior to constraint projection. + */ + virtual Values retractBaseVariables(const Values &values, + const VectorValues &delta) const { + return values.retract(delta); + } + + /** Retraction operation + * @param values base values composing the constraint manifold + * @param delta tangent vector + * @return retracted point on the manifold + */ + virtual Values retract(Values &&values, const VectorValues &delta) { + return retractConstraints(retractBaseVariables(values, delta)); + } + + /** Retraction operation + * @param values base values composing the constraint manifold + * @param delta tangent vector + * @return retracted point on the manifold + */ + virtual Values retract(const Values &values, const VectorValues &delta) { + return retractConstraints(retractBaseVariables(values, delta)); + } + + /** + * Retract values to satisfy constraints. + * @param values Candidate values, potentially infeasible. + * @return Constraint-satisfying values. + */ + virtual Values retractConstraints(Values &&values) { + return retractConstraints((const Values &)values); + }; + + /** Given values of variables in CCC that may violate the constraints, compute + * the values that satisfy the constraints. */ + virtual Values retractConstraints(const Values &values) = 0; + + protected: + /** + * Optionally report infeasibility. + * @param graph Constraint graph used for feasibility check. + * @param values Values to verify. + */ + void checkFeasible(const NonlinearFactorGraph &graph, + const Values &values) const; +}; + +/** + * Retractor using unconstrained optimization on the penalty graph. + * + * This corresponds to approximate metric projection behavior used in the + * tutorial. + * + * @see README.md#retraction + */ +class UoptRetractor : public Retractor { + protected: + MutableLMOptimizer optimizer_; + + public: + /** + * Constructor. + * @param constraints Equality constraints for the component. + * @param params Retraction parameters. + */ + UoptRetractor(const EqualityConstraints::shared_ptr &constraints, + const RetractParams::shared_ptr ¶ms = + std::make_shared()); + + /// Retraction operation. + Values retractConstraints(const Values &values) override; + + /// Retraction Inplace + Values retractConstraints(Values &&values) override; +}; + +/** + * Retractor implementing projection-style updates with optional priors. + * + * @see README.md#retraction + */ +class ProjRetractor : public Retractor { + protected: + NonlinearFactorGraph merit_graph_; + KeyVector basis_keys_; + + public: + /** + * Constructor. + * @param constraints Equality constraints for the component. + * @param params Retraction parameters. + * @param basis_keys Optional basis keys used when `use_basis_keys` is true. + */ + ProjRetractor(const EqualityConstraints::shared_ptr &constraints, + const RetractParams::shared_ptr ¶ms, + std::optional basis_keys = {}); + + /** + * Retract with projection-style retraction. + * @param values Base values composing the constraint manifold. + * @param delta Tangent update. + * @return Retraction result projected toward constraint manifold. + */ + Values retract(const Values &values, const VectorValues &delta) override; + + Values retractConstraints(const Values &values) override; +}; + +/** + * Retractor that fixes basis variables and solves for the remaining variables. + * + * This matches the basis-variable retraction idea in the tutorial and paper. + * + * @see README.md#retraction + * @see README.md#tangent-basis + */ +class BasisRetractor : public Retractor { + protected: + NonlinearFactorGraph merit_graph_; + KeyVector basis_keys_; + MutableLMOptimizer optimizer_; + std::vector> factors_with_fixed_vars_; + + public: + /** + * Constructor. + * @param constraints Equality constraints for the component. + * @param params Retraction parameters. + * @param basis_keys Basis keys held fixed during inner solve. + */ + BasisRetractor(const EqualityConstraints::shared_ptr &constraints, + const RetractParams::shared_ptr ¶ms, + const KeyVector &basis_keys); + + virtual ~BasisRetractor() {} + + /// Retraction operation. + Values retractConstraints(const Values &values) override; + + protected: + /** + * Build reduced retraction graph with basis keys treated as fixed. + * @param constraints Equality constraints for the component. + * @param basis_keys Basis keys held fixed during inner solve. + */ + void constructGraph(const EqualityConstraints::shared_ptr &constraints, + const KeyVector &basis_keys); +}; + +/** + * Customized hierarchical retractor for kinodynamics components. + * + * Variables/factors are split into levels (q, v, and acceleration/dynamics) + * and solved in sequence with level-specific optimizers and priors. + * + * @see README.md#retraction + */ +class DynamicsRetractor : public Retractor { + protected: + NonlinearFactorGraph merit_graph_; + MutableLMOptimizer optimizer_wp_q_, optimizer_wp_v_, optimizer_wp_ad_; + MutableLMOptimizer optimizer_np_q_, optimizer_np_v_, optimizer_np_ad_; + KeySet basis_q_keys_, basis_v_keys_, basis_ad_keys_; + std::vector> const_var_factors_v_, + const_var_factors_ad_; + + public: + /** + * Constructor. + * @param constraints Equality constraints for the component. + * @param params Retraction parameters. + * @param basis_keys Optional basis keys. + */ + DynamicsRetractor(const EqualityConstraints::shared_ptr &constraints, + const RetractParams::shared_ptr ¶ms, + std::optional basis_keys = {}); + + /// Retraction operation. + Values retractConstraints(const Values &values) override; + + protected: + /** + * Classify keys by dynamics level. + * @param keys Input key container. + * @param q_keys Output position-level keys. + * @param v_keys Output velocity-level keys. + * @param ad_keys Output acceleration/dynamics-level keys. + */ + template + static void classifyKeys(const CONTAINER &keys, KeySet &q_keys, + KeySet &v_keys, KeySet &ad_keys); + + /** + * Refresh linear priors in a graph for selected keys. + * @param values Values used as prior means. + * @param keys Keys whose priors are updated. + * @param graph Graph whose trailing prior factors are replaced. + */ + void updatePriors(const Values &values, const KeySet &keys, + NonlinearFactorGraph &graph); +}; + +/** + * Factory interface for creating retractor instances. + * + * @see README.md#retraction + */ +class RetractorCreator { + protected: + RetractParams::shared_ptr params_; + + public: + using shared_ptr = std::shared_ptr; + RetractorCreator(RetractParams::shared_ptr params) : params_(params) {} + + RetractParams::shared_ptr params() { return params_; } + + virtual ~RetractorCreator() {} + + virtual Retractor::shared_ptr create( + const EqualityConstraints::shared_ptr constraints) const = 0; +}; + +/** + * Factory for `UoptRetractor`. + * + * @see README.md#retraction + */ +class UoptRetractorCreator : public RetractorCreator { + public: + UoptRetractorCreator( + RetractParams::shared_ptr params = std::make_shared()) + : RetractorCreator(params) {} + + virtual ~UoptRetractorCreator() {} + + Retractor::shared_ptr create( + const EqualityConstraints::shared_ptr constraints) const override { + return std::make_shared(constraints, params_); + } +}; + +/** + * Factory for `ProjRetractor`. + * + * @see README.md#retraction + */ +class ProjRetractorCreator : public RetractorCreator { + public: + ProjRetractorCreator( + RetractParams::shared_ptr params = std::make_shared()) + : RetractorCreator(params) {} + + virtual ~ProjRetractorCreator() {} + + Retractor::shared_ptr create( + const EqualityConstraints::shared_ptr constraints) const override { + return std::make_shared(constraints, params_); + } +}; + +/** + * Factory for `BasisRetractor`. + * + * @see README.md#retraction + * @see README.md#tangent-basis + */ +class BasisRetractorCreator : public RetractorCreator { + public: + BasisKeyFunc basis_key_func_; + + public: + BasisRetractorCreator( + RetractParams::shared_ptr params = std::make_shared()) + : RetractorCreator(params) {} + + /** + * Constructor with custom basis-key selector. + * @param basis_key_func Callback selecting basis keys. + * @param params Retraction parameters. + */ + BasisRetractorCreator( + BasisKeyFunc basis_key_func, + RetractParams::shared_ptr params = std::make_shared()) + : RetractorCreator(params), basis_key_func_(basis_key_func) {} + + virtual ~BasisRetractorCreator() {} + + Retractor::shared_ptr create( + const EqualityConstraints::shared_ptr constraints) const override { + KeyVector basis_keys = basis_key_func_(constraints->keyVector()); + return std::make_shared(constraints, params_, basis_keys); + } +}; + +/** + * Factory for `DynamicsRetractor`. + * + * @see README.md#retraction + */ +class DynamicsRetractorCreator : public RetractorCreator { + protected: + BasisKeyFunc basis_key_func_; + + public: + DynamicsRetractorCreator(RetractParams::shared_ptr params) + : RetractorCreator(params) {} + + /** + * Constructor with custom basis-key selector. + * @param params Retraction parameters. + * @param basis_key_func Callback selecting basis keys. + */ + DynamicsRetractorCreator(RetractParams::shared_ptr params, + BasisKeyFunc basis_key_func) + : RetractorCreator(params), basis_key_func_(basis_key_func) {} + + virtual ~DynamicsRetractorCreator() {} + + Retractor::shared_ptr create( + const EqualityConstraints::shared_ptr constraints) const override { + if (params_->use_basis_keys) { + KeyVector basis_keys = basis_key_func_(constraints->keyVector()); + return std::make_shared(constraints, params_, + basis_keys); + } else { + return std::make_shared(constraints, params_); + } + } +}; + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/TspaceBasis.cpp b/gtdynamics/cmopt/TspaceBasis.cpp new file mode 100644 index 000000000..8f9555a6b --- /dev/null +++ b/gtdynamics/cmopt/TspaceBasis.cpp @@ -0,0 +1,628 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file TspaceBasis.cpp + * @brief Tagent space basis implementations. + * @author: Yetong Zhang + */ + +#include +#if defined(GTDYNAMICS_WITH_SUITESPARSE) +#include +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +std::vector TspaceBasis::basisVectors() const { + std::vector basis_vectors; + basis_vectors.reserve(dim()); + for (size_t i = 0; i < dim(); i++) { + Vector xi = Vector::Zero(dim()); + xi(i) = 1; + basis_vectors.emplace_back(computeTangentVector(xi)); + } + return basis_vectors; +} + +/* ************************************************************************* */ +Matrix OrthonormalBasis::rearrangeMatrix(const Matrix &A, + const KeyVector &A_keys) const { + Matrix jac_mat = Matrix::Zero(A.rows(), attributes_->total_var_dim); + size_t A_col_idx = 0; + for (const Key &key : A_keys) { + const size_t &var_dim = attributes_->var_dim.at(key); + jac_mat.middleCols(attributes_->var_location.at(key), var_dim) = + A.middleCols(A_col_idx, var_dim); + A_col_idx += var_dim; + } + return jac_mat; +} + +/* ************************************************************************* */ +Matrix OrthonormalBasis::computeConstraintJacobian(const Values &values) const { + auto linear_graph = attributes_->merit_graph.linearize(values); + gtsam::JacobianFactor combined(*linear_graph); + if (combined.keys().size() == values.keys().size()) { + return combined.jacobian().first; + } + + // Treatment for unconstrained keys + Matrix A = combined.jacobian().first; + return rearrangeMatrix(A, combined.keys()); +} + +/* ************************************************************************* */ +OrthonormalBasis::OrthonormalBasis( + const EqualityConstraints::shared_ptr &constraints, const Values &values, + const TspaceBasisParams::shared_ptr ¶ms) + : TspaceBasis(params), attributes_(std::make_shared()) { + // set attributes + attributes_->total_constraint_dim = constraints->dim(); + attributes_->total_var_dim = values.dim(); + attributes_->total_basis_dim = + attributes_->total_var_dim - attributes_->total_constraint_dim; + attributes_->merit_graph = constraints->penaltyGraph(); + size_t position = 0; + for (const Key &key : values.keys()) { + size_t var_dim = values.at(key).dim(); + attributes_->var_dim[key] = var_dim; + attributes_->var_location[key] = position; + position += var_dim; + } + if (params_->always_construct_basis) { + construct(values); + } +} + +/* ************************************************************************* */ +void OrthonormalBasis::construct(const Values &values) { + if (attributes_->merit_graph.size() == 0) { + basis_ = Matrix::Identity(attributes_->total_basis_dim, + attributes_->total_basis_dim); + } else { + if (params_->use_sparse) { + constructSparse(values); + } else { + constructDense(values); + } + } + is_constructed_ = true; +} + +/* ************************************************************************* */ +void OrthonormalBasis::constructDense(const Values &values) { + Matrix A = computeConstraintJacobian(values); // m x n + Eigen::FullPivLU lu(A); + basis_ = lu.kernel(); // n x n-m +} + +/* ************************************************************************* */ +void OrthonormalBasis::constructSparse(const Values &values) { + auto linear_graph = attributes_->merit_graph.linearize(values); + Ordering ordering; + auto triplets = linear_graph->sparseJacobian(); + size_t nrows = attributes_->total_constraint_dim; + size_t ncols = attributes_->total_var_dim + 1; + +#if defined(GTDYNAMICS_WITH_SUITESPARSE) + cholmod_common common; + cholmod_common *cc = &common; + cholmod_l_start(cc); + + cholmod_sparse *A_t = SparseJacobianTranspose(nrows, ncols, triplets, cc); + SuiteSparseQR_factorization *QR = SuiteSparseQR_factorize( + SPQR_ORDERING_DEFAULT, SPQR_DEFAULT_TOL, A_t, cc); + + cholmod_sparse *selection_mat = LastColsSelectionMat( + attributes_->total_var_dim, attributes_->total_basis_dim, cc); + + cholmod_sparse *basis_cholmod = + SuiteSparseQR_qmult(SPQR_QX, QR, selection_mat, cc); + + basis_ = CholmodToEigen(basis_cholmod, cc); + + cholmod_l_free_sparse(&A_t, cc); + SuiteSparseQR_free(&QR, cc); + cholmod_l_free_sparse(&selection_mat, cc); + cholmod_l_free_sparse(&basis_cholmod, cc); + cholmod_l_finish(cc); +#else + SpMatrix A_t = SparseJacobianTranspose(nrows, ncols, triplets); + Eigen::SparseQR> qr; + qr.compute(A_t); + if (qr.info() != Eigen::Success) { + throw std::runtime_error("Eigen SparseQR failed for tangent basis."); + } + + SpMatrix selection_mat = LastColsSelectionMat(attributes_->total_var_dim, + attributes_->total_basis_dim); + basis_ = qr.matrixQ() * Matrix(selection_mat); +#endif +} + +/* ************************************************************************* */ +TspaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraints( + const EqualityConstraints &constraints, const Values &values, + bool create_from_scratch) const { + // attributes + auto new_merit_graph = constraints.penaltyGraph(); + + auto new_attributes = std::make_shared(); + new_attributes->merit_graph = attributes_->merit_graph; + new_attributes->merit_graph.add(new_merit_graph); + new_attributes->var_location = attributes_->var_location; + new_attributes->var_dim = attributes_->var_dim; + new_attributes->total_var_dim = attributes_->total_var_dim; + new_attributes->total_constraint_dim = + attributes_->total_constraint_dim + constraints.dim(); + new_attributes->total_basis_dim = + new_attributes->total_var_dim - new_attributes->total_constraint_dim; + + if (create_from_scratch) { + auto new_basis = + std::make_shared(params_, new_attributes); + new_basis->construct(values); + return new_basis; + } + if (!is_constructed_) { + throw std::runtime_error("basis not constructed yet."); + } + auto new_linear_graph = new_merit_graph.linearize(values); + + if (params_->use_sparse) { + return createWithAdditionalConstraintsSparse(*new_linear_graph, + new_attributes); + } else { + return createWithAdditionalConstraintsDense(*new_linear_graph, + new_attributes); + } +} + +/* ************************************************************************* */ +TspaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraintsDense( + const GaussianFactorGraph &graph, + const Attributes::shared_ptr &new_attributes) const { + gtsam::JacobianFactor combined(graph); + Matrix A_new = combined.jacobian().first; + A_new = rearrangeMatrix(A_new, combined.keys()); + Matrix AB_new = A_new * basis_; + Eigen::FullPivLU lu(AB_new); + Matrix M = lu.kernel(); + Matrix new_basis = basis_ * M; + return std::make_shared(params_, new_attributes, new_basis); +} + +/* ************************************************************************* */ +TspaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraintsSparse( + const GaussianFactorGraph &graph, + const Attributes::shared_ptr &new_attributes) const { + SpMatrix A_new = eigenSparseJacobian(graph); + Matrix AB_new = A_new * basis_; + Eigen::FullPivLU lu(AB_new); + Matrix M = lu.kernel(); + Matrix new_basis = basis_ * M; + return std::make_shared(params_, new_attributes, new_basis); +} + +/* ************************************************************************* */ +VectorValues OrthonormalBasis::computeTangentVector(const Vector &xi) const { + VectorValues delta; + Vector x_xi = basis_ * xi; + for (const auto &[key, location] : attributes_->var_location) { + delta.insert(key, x_xi.middleRows(location, attributes_->var_dim.at(key))); + } + return delta; +} + +/* ************************************************************************* */ +Vector OrthonormalBasis::computeXi(const VectorValues &delta) const { + Vector x_xi = Vector::Zero(basis_.rows()); + for (const auto &it : attributes_->var_location) { + const Key &key = it.first; + x_xi.middleRows(it.second, attributes_->var_dim.at(key)) = delta.at(key); + } + // Vector xi = basis_.colPivHouseholderQr().solve(x_xi); + return basis_.completeOrthogonalDecomposition().pseudoInverse() * x_xi; +} + +/* ************************************************************************* */ +Matrix OrthonormalBasis::recoverJacobian(const Key &key) const { + return basis_.middleRows(attributes_->var_location.at(key), + attributes_->var_dim.at(key)); +} + +/* ************************************************************************* */ +Vector OrthonormalBasis::localCoordinates(const Values &values, + const Values &values_other) const { + Eigen::MatrixXd basis_pinv = + basis_.completeOrthogonalDecomposition().pseudoInverse(); + VectorValues delta = values.localCoordinates(values_other); + Vector xi_base = Vector::Zero(values.dim()); + for (const auto &it : delta) { + const Key &key = it.first; + xi_base.middleRows(attributes_->var_location.at(key), + attributes_->var_dim.at(key)) = it.second; + } + Vector xi = basis_pinv * xi_base; + return xi; +} + +#if defined(GTDYNAMICS_WITH_SUITESPARSE) +void PrintDense(const cholmod_dense *A, const cholmod_common *cc) { + for (int i = 0; i < A->nrow; i++) { + for (int j = 0; j < A->ncol; j++) { + std::cout << ((double *)(A->x))[j * A->nrow + i] << "\t"; + } + std::cout << "\n"; + } +} + +void PrintSparse(cholmod_sparse *A, cholmod_common *cc) { + cholmod_dense *A_dense = cholmod_l_sparse_to_dense(A, cc); + PrintDense(A_dense, cc); + cholmod_l_free_dense(&A_dense, cc); +} +#endif + +/* ************************************************************************* */ +#if defined(GTDYNAMICS_WITH_SUITESPARSE) +cholmod_sparse *OrthonormalBasis::SparseJacobianTranspose( + const size_t nrows, const size_t ncols, + const std::vector> &triplets, + cholmod_common *cc) { + int A_stype = 0; + int A_xdtype = CHOLMOD_DOUBLE + CHOLMOD_REAL; + + // count entires not from last col + size_t nnz = 0; + size_t last_col = ncols - 1; + for (const auto &it : triplets) { + if (std::get<1>(it) < last_col) { + nnz += 1; + } + } + + cholmod_triplet *T = + cholmod_l_allocate_triplet(ncols - 1, nrows, nnz, A_stype, A_xdtype, cc); + T->nnz = nnz; + size_t idx = 0; + for (const auto &triplet : triplets) { + if (std::get<1>(triplet) < last_col) { + std::tie(((int64_t *)(T->j))[idx], ((int64_t *)(T->i))[idx], + ((double *)(T->x))[idx]) = triplet; + idx += 1; + } + } + + cholmod_sparse *A = (cholmod_sparse *)cholmod_l_triplet_to_sparse(T, nnz, cc); + cholmod_l_free_triplet(&T, cc); + return A; +} + +/* ************************************************************************* */ +cholmod_sparse *OrthonormalBasis::LastColsSelectionMat(const size_t nrows, + const size_t ncols, + cholmod_common *cc) { + int A_stype = 0; + int A_xdtype = CHOLMOD_DOUBLE + CHOLMOD_REAL; + size_t nnz = ncols; + cholmod_triplet *T = + cholmod_l_allocate_triplet(nrows, nnz, ncols, A_stype, A_xdtype, cc); + T->nnz = nnz; + size_t pad = nrows - ncols; + for (int idx = 0; idx < ncols; idx++) { + ((int64_t *)(T->i))[idx] = pad + idx; + ((int64_t *)(T->j))[idx] = idx; + ((double *)(T->x))[idx] = 1; + } + cholmod_sparse *A = (cholmod_sparse *)cholmod_l_triplet_to_sparse(T, nnz, cc); + cholmod_l_free_triplet(&T, cc); + // PrintSparse(A, cc); + return A; +} + +/* ************************************************************************* */ +OrthonormalBasis::SpMatrix +OrthonormalBasis::CholmodToEigen(cholmod_sparse *A, cholmod_common *cc) { + + cholmod_triplet *T = cholmod_l_sparse_to_triplet(A, cc); + std::vector> triplets; + triplets.reserve(T->nnz); + for (int idx = 0; idx < T->nnz; idx++) { + triplets.emplace_back(((int64_t *)(T->i))[idx], ((int64_t *)(T->j))[idx], + ((double *)(T->x))[idx]); + } + + SpMatrix A_eigen(A->nrow, A->ncol); + A_eigen.setFromTriplets(triplets.begin(), triplets.end()); + cholmod_l_free_triplet(&T, cc); + return A_eigen; +} +#else +OrthonormalBasis::SpMatrix OrthonormalBasis::SparseJacobianTranspose( + const size_t nrows, const size_t ncols, + const std::vector> &triplets) { + const size_t last_col = ncols - 1; + std::vector> entries; + entries.reserve(triplets.size()); + for (const auto &triplet : triplets) { + if (std::get<1>(triplet) < last_col) { + entries.emplace_back(std::get<1>(triplet), std::get<0>(triplet), + std::get<2>(triplet)); + } + } + + SpMatrix A_t(ncols - 1, nrows); + A_t.setFromTriplets(entries.begin(), entries.end()); + return A_t; +} + +OrthonormalBasis::SpMatrix +OrthonormalBasis::LastColsSelectionMat(const size_t nrows, const size_t ncols) { + std::vector> entries; + entries.reserve(ncols); + const size_t pad = nrows - ncols; + for (size_t idx = 0; idx < ncols; idx++) { + entries.emplace_back(pad + idx, idx, 1.0); + } + + SpMatrix A(nrows, ncols); + A.setFromTriplets(entries.begin(), entries.end()); + return A; +} +#endif + +/* ************************************************************************* */ +OrthonormalBasis::SpMatrix +OrthonormalBasis::EigenSparseJacobian(const GaussianFactorGraph &graph) { + Ordering ordering(graph.keys()); + size_t rows, cols; + auto triplets = graph.sparseJacobian(ordering, rows, cols); + size_t last_col = cols - 1; + std::vector> entries; + for (auto &v : triplets) { + if (std::get<1>(v) < last_col) { + entries.emplace_back(std::get<0>(v), std::get<1>(v), std::get<2>(v)); + } + } + OrthonormalBasis::SpMatrix A(rows, cols - 1); + A.setFromTriplets(entries.begin(), entries.end()); + return A; +} + +/* ************************************************************************* */ +OrthonormalBasis::SpMatrix +OrthonormalBasis::eigenSparseJacobian(const GaussianFactorGraph &graph) const { + + std::vector> entries; + + size_t nrows = 0; + for (const auto &factor : graph) { + Matrix f_jacobian = factor->jacobian().first; + size_t factor_dim = f_jacobian.rows(); + size_t start_col_f = 0; + for (const Key &key : *factor) { + const auto &start_col_jacobian = attributes_->var_location.at(key); + for (size_t i = 0; i < factor_dim; i++) + for (size_t j = 0; j < attributes_->var_dim.at(key); j++) { + const double &s = f_jacobian(i, j + start_col_f); + if (std::abs(s) > 1e-12) + entries.emplace_back(nrows + i, start_col_jacobian + j, s); + } + start_col_f += attributes_->var_dim.at(key); + } + nrows += factor_dim; + } + OrthonormalBasis::SpMatrix A(nrows, attributes_->total_var_dim); + A.setFromTriplets(entries.begin(), entries.end()); + return A; +} + +/* ************************************************************************* */ +EliminationBasis::EliminationBasis( + const EqualityConstraints::shared_ptr &constraints, const Values &values, + const TspaceBasisParams::shared_ptr ¶ms, + std::optional basis_keys) + : TspaceBasis(params), attributes_(std::make_shared()) { + // Set the location of each basis variable. + attributes_->total_basis_dim = values.dim() - constraints->dim(); + attributes_->var_dim = values.dims(); + attributes_->merit_graph = constraints->penaltyGraph(); + + if (basis_keys) { + attributes_->basis_keys = *basis_keys; + size_t location = 0; + for (const Key &key : *basis_keys) { + attributes_->basis_location.insert({key, location}); + location += values.at(key).dim(); + } + if (attributes_->total_basis_dim != location) { + throw std::runtime_error( + "specified basis keys has wrong dimensions. manifold dim: " + + std::to_string(attributes_->total_basis_dim) + + "\tbasis keys dim: " + std::to_string(location)); + } + } else { + throw std::runtime_error( + "elimination basis wihthout keys not implemented."); + } + + // Partially eliminate all other variables (except for the basis variables) in + // the merit graph of constraints, and form a bayes net. The bays net + // represents how other variables depends on the basis variables, e.g., + // X_other = B x X_basis. + auto full_ordering = Ordering::ColamdConstrainedLast(attributes_->merit_graph, + attributes_->basis_keys); + attributes_->ordering = full_ordering; + for (size_t i = 0; i < attributes_->basis_keys.size(); i++) { + attributes_->ordering.pop_back(); + } + + // Compute jacobians w.r.t. basis variables. + if (params_->always_construct_basis) { + construct(values); + } +} + +/* ************************************************************************* */ +EliminationBasis::EliminationBasis(const Values &values, + const EliminationBasis &other) + : TspaceBasis(other.params_), attributes_(other.attributes_) { + if (params_->always_construct_basis) { + construct(values); + } +} + +/* ************************************************************************* */ +void EliminationBasis::construct(const Values &values) { + // TODO: scenarios with unconstrained keys + auto linear_graph = attributes_->merit_graph.linearize(values); + auto elim_result = linear_graph->eliminatePartialSequential( + attributes_->ordering, gtsam::EliminateQR); + auto bayes_net = elim_result.first; + ComputeBayesNetJacobian(*bayes_net, attributes_->basis_keys, + attributes_->var_dim, jacobians_); + is_constructed_ = true; +} + +/* ************************************************************************* */ +TspaceBasis::shared_ptr EliminationBasis::createWithAdditionalConstraints( + const EqualityConstraints &constraints, const Values &values, + bool create_from_scratch) const { + + // Identify the keys to eliminate + NonlinearFactorGraph new_merit_graph = constraints.penaltyGraph(); + KeyVector new_basis_keys; + KeySet new_constraint_keys = constraints.keys(); + for (const Key &key : attributes_->basis_keys) { + if (!new_constraint_keys.exists(key)) { + new_basis_keys.push_back(key); + } + } + + // Identify basis keys locations in xi + std::map new_basis_location; + size_t location = 0; + for (const Key &key : new_basis_keys) { + new_basis_location[key] = location; + location += attributes_->var_dim.at(key); + } + size_t new_basis_dim = location; + + // eliminate on the new factors + // Ordering new_ordering = + // Ordering::ColamdConstrainedLast(linear_graph, basis_keys_); + // for (size_t i = 0; i < new_basis_keys.size(); i++) { + // new_ordering.pop_back(); + // } + // auto elim_result = + // linear_graph.eliminatePartialSequential(new_ordering, EliminateQR); + // auto bayes_net = elim_result.first; + // MultiJacobians new_jacobians; + // ComputeBayesNetJacobian(*bayes_net, new_basis_keys, var_dim_, + // new_jacobians); + Ordering new_ordering(new_constraint_keys.begin(), new_constraint_keys.end()); + + Ordering total_ordering = attributes_->ordering; + total_ordering.insert(total_ordering.end(), new_ordering.begin(), + new_ordering.end()); + auto new_attributes = std::make_shared(); + new_attributes->merit_graph = attributes_->merit_graph; + new_attributes->merit_graph.add(new_merit_graph); + new_attributes->basis_keys = new_basis_keys; + new_attributes->ordering = total_ordering; + new_attributes->total_basis_dim = new_basis_dim; + new_attributes->basis_location = new_basis_location; + new_attributes->var_dim = attributes_->var_dim; + auto new_basis = std::make_shared(params_, new_attributes); + if (create_from_scratch) { + new_basis->construct(values); + } else { + MultiJacobians new_jacobians; + for (const Key &key : new_basis_keys) { + size_t dim = attributes_->var_dim.at(key); + new_jacobians.insert( + {key, MultiJacobian(key, Matrix::Identity(dim, dim))}); + } + for (const Key &key : new_constraint_keys) { + new_jacobians.insert({key, MultiJacobian()}); + } + new_basis->jacobians_ = JacobiansMultiply(jacobians_, new_jacobians); + new_basis->is_constructed_ = true; + } + return new_basis; +} + +/* ************************************************************************* */ +VectorValues EliminationBasis::computeTangentVector(const Vector &xi) const { + VectorValues delta; + // Set tangent vector for basis variables to corresponding segment in xi + for (const Key &key : attributes_->basis_keys) { + delta.insert(key, xi.segment(attributes_->basis_location.at(key), + attributes_->var_dim.at(key))); + } + // Compute tangent vector of non-basis variables + for (const auto &it : jacobians_) { + const Key &var_key = it.first; + if (delta.exists(var_key)) + continue; + Vector v = Vector::Zero(attributes_->var_dim.at(var_key)); + for (const auto &jac_it : it.second) { + const Key &basis_key = jac_it.first; + v += jac_it.second * delta.at(basis_key); + } + delta.insert(var_key, v); + } + return delta; +} + +/* ************************************************************************* */ +Vector EliminationBasis::computeXi(const VectorValues &delta) const { + Vector xi = Vector::Zero(attributes_->total_basis_dim); + for (const Key &key : attributes_->basis_keys) { + xi.segment(attributes_->basis_location.at(key), + attributes_->var_dim.at(key)) = delta.at(key); + } + return xi; +} + +/* ************************************************************************* */ +Matrix EliminationBasis::recoverJacobian(const Key &key) const { + Matrix H = + Matrix::Zero(attributes_->var_dim.at(key), attributes_->total_basis_dim); + for (const auto &it : jacobians_.at(key)) { + const Key &basis_key = it.first; + H.middleCols(attributes_->basis_location.at(basis_key), + attributes_->var_dim.at(basis_key)) = it.second; + } + return H; +} + +/* ************************************************************************* */ +Vector EliminationBasis::localCoordinates(const Values &values, + const Values &values_other) const { + Vector xi = Vector::Zero(attributes_->total_basis_dim); + for (const Key &key : attributes_->basis_keys) { + Vector x_xi = values.at(key).localCoordinates_(values_other.at(key)); + xi.segment(attributes_->basis_location.at(key), + attributes_->var_dim.at(key)) = x_xi; + } + return xi; +} + +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/TspaceBasis.h b/gtdynamics/cmopt/TspaceBasis.h new file mode 100644 index 000000000..7f8259a08 --- /dev/null +++ b/gtdynamics/cmopt/TspaceBasis.h @@ -0,0 +1,569 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file TspaceBasis.h + * @brief Basis for tangent space of constraint manifold. Detailed definition of + * tangent space and basis are available at Boumal20book Sec.8.4. + * @author: Yetong Zhang + */ + +#pragma once + +#include +#if defined(GTDYNAMICS_WITH_SUITESPARSE) +#include + +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtdynamics { + +using EqualityConstraints = gtsam::NonlinearEqualityConstraints; +using gtsam::DefaultKeyFormatter; +using gtsam::GaussianFactorGraph; +using gtsam::Key; +using gtsam::KeyFormatter; +using gtsam::KeyVector; +using gtsam::Matrix; +using gtsam::NonlinearFactorGraph; +using gtsam::Ordering; +using gtsam::Values; +using gtsam::Vector; +using gtsam::VectorValues; + +typedef std::function BasisKeyFunc; + +/** + * Parameters for tangent-space basis construction. + * + * These options select basis style (orthonormal vs. basis-key driven), + * construction timing, and sparse/dense backend preferences. + * + * @see README.md#tangent-basis + */ +struct TspaceBasisParams { + public: + using shared_ptr = std::shared_ptr; + + /// Member variables. + bool always_construct_basis = true; + bool use_basis_keys = false; + bool use_sparse = false; + + /** + * Constructor. + * @param _use_basis_keys If true, basis keys are explicitly selected. + * @param _always_construct_basis If true, build basis immediately. + */ + TspaceBasisParams(bool _use_basis_keys = false, + bool _always_construct_basis = true) + : always_construct_basis(_always_construct_basis), + use_basis_keys(_use_basis_keys) {} +}; + +/** + * Abstract interface for constraint-manifold tangent-space bases. + * + * Implementations map reduced coordinates to ambient tangent vectors, recover + * Jacobians for substituted factors, and compute local coordinates. + * + * @see README.md#tangent-basis + */ +class TspaceBasis { + protected: + TspaceBasisParams::shared_ptr params_; + bool is_constructed_; + + public: + using shared_ptr = std::shared_ptr; + + /// Default constructor. + TspaceBasis(TspaceBasisParams::shared_ptr params = + std::make_shared()) + : params_(params), is_constructed_(false) {} + + /// Default destructor. + virtual ~TspaceBasis() {} + + /// Construct new basis by using new values + virtual shared_ptr createWithNewValues(const Values &values) const = 0; + + /** + * Construct a basis by incorporating additional constraints. + * @param constraints Additional equality constraints. + * @param values Current variable values. + * @param create_from_scratch If true, rebuild basis from scratch. + * @return New basis object. + */ + virtual shared_ptr createWithAdditionalConstraints( + const EqualityConstraints &constraints, const Values &values, + bool create_from_scratch = false) const = 0; + + /** Compute the tangent vector in the ambient space, given a vector xi + * representing the magnitude of each basis component. */ + virtual VectorValues computeTangentVector(const Vector &xi) const = 0; + + /** Given a tangent vector, compute a vector xi representing the magnitude of + * basis components. */ + virtual Vector computeXi(const VectorValues &tangent_vector) const = 0; + + /** Compute the jacobian of recover function. i.e., function that recovers an + * original variable from the constraint manifold. */ + virtual Matrix recoverJacobian(const Key &key) const = 0; + + /** + * Compute local coordinates between two value assignments. + * @param values Source values. + * @param values_other Target values. + * @return Local coordinates in basis parameterization. + */ + virtual Vector localCoordinates(const Values &values, + const Values &values_other) const = 0; + + /// Dimension of the basis. + virtual size_t dim() const = 0; + + /// Construct the actual basis, all the heavy computation goes here. + virtual void construct(const Values &values) = 0; + + /// Return if the basis is already constructed. + bool isConstructed() const { return is_constructed_; } + + virtual void print( + const gtsam::KeyFormatter &keyFormatter = DefaultKeyFormatter) = 0; + + virtual std::vector basisVectors() const; +}; + +/** + * Orthonormal null-space basis of the constraint Jacobian. + * + * This basis computes `ker(J_H)` either with dense or sparse routines and + * provides reduced-coordinate parameterization for manifold updates. + * + * @see README.md#tangent-basis + */ +class OrthonormalBasis : public TspaceBasis { + protected: + // Structural properties that do not change with values. + struct Attributes { + NonlinearFactorGraph merit_graph; + std::map var_location; // location of variables in Jacobian + std::map var_dim; // dimension of variables + size_t total_var_dim; + size_t total_constraint_dim; + size_t total_basis_dim; + using shared_ptr = std::shared_ptr; + }; + Attributes::shared_ptr attributes_; + gtsam::Matrix basis_; // basis matrix for the tangent space + + typedef Eigen::SparseMatrix SpMatrix; + + public: + /** + * Constructor. + * @param constraints Equality constraints for the component. + * @param values Current values for variables in the component. + * @param params Basis construction parameters. + */ + OrthonormalBasis(const EqualityConstraints::shared_ptr &constraints, + const Values &values, + const TspaceBasisParams::shared_ptr ¶ms); + + /** + * Constructor from precomputed attributes. + * @param params Basis construction parameters. + * @param attributes Precomputed structural attributes. + */ + OrthonormalBasis(const TspaceBasisParams::shared_ptr ¶ms, + const Attributes::shared_ptr &attributes) + : TspaceBasis(params), attributes_(attributes) {} + + /** + * Constructor from precomputed attributes and matrix. + * @param params Basis construction parameters. + * @param attributes Precomputed structural attributes. + * @param mat Basis matrix. + */ + OrthonormalBasis(const TspaceBasisParams::shared_ptr ¶ms, + const Attributes::shared_ptr &attributes, const Matrix &mat) + : TspaceBasis(params), attributes_(attributes), basis_(mat) {} + + /** + * Constructor from another basis, reusing structural attributes. + * @param values Current values. + * @param other Source basis. + */ + OrthonormalBasis(const Values &values, const OrthonormalBasis &other) + : TspaceBasis(other.params_), attributes_(other.attributes_) { + if (params_->always_construct_basis) { + construct(values); + } + } + + /// Create basis with new values. + TspaceBasis::shared_ptr createWithNewValues( + const Values &values) const override { + return std::make_shared(values, *this); + } + + /** + * Construct a basis by incorporating additional constraints. + * @param constraints Additional equality constraints. + * @param values Current variable values. + * @param create_from_scratch If true, rebuild basis from scratch. + * @return New basis object. + */ + TspaceBasis::shared_ptr createWithAdditionalConstraints( + const EqualityConstraints &constraints, const Values &values, + bool create_from_scratch = false) const override; + + /// Construct the actual basis, all the heavy computation goes here. + void construct(const Values &values) override; + + /// Compute the tangent vector in the ambient space. + VectorValues computeTangentVector(const Vector &xi) const override; + + /// Compute xi of basis components. + Vector computeXi(const VectorValues &tangent_vector) const override; + + /// Jacobian of recover function. + Matrix recoverJacobian(const Key &key) const override; + + /** + * Compute local coordinates between two value assignments. + * @param values Source values. + * @param values_other Target values. + * @return Local coordinates in basis parameterization. + */ + Vector localCoordinates(const Values &values, + const Values &values_other) const override; + + /// Dimension of the basis. + size_t dim() const override { return attributes_->total_basis_dim; } + + /// Basis matrix. + const Matrix &matrix() const { return basis_; } + + void print( + const gtsam::KeyFormatter &keyFormatter = DefaultKeyFormatter) override { + std::cout << "Orthonormal basis\n" << matrix() << "\n"; + } + + protected: + void constructDense(const Values &values); + + void constructSparse(const Values &values); + + /** + * Update basis with additional constraints in dense mode. + * @param graph Linearized graph of added constraints. + * @param new_attributes Updated structural attributes. + * @return New orthonormal basis. + */ + TspaceBasis::shared_ptr createWithAdditionalConstraintsDense( + const GaussianFactorGraph &graph, + const Attributes::shared_ptr &new_attributes) const; + + /** + * Update basis with additional constraints in sparse mode. + * @param graph Linearized graph of added constraints. + * @param new_attributes Updated structural attributes. + * @return New orthonormal basis. + */ + TspaceBasis::shared_ptr createWithAdditionalConstraintsSparse( + const GaussianFactorGraph &graph, + const Attributes::shared_ptr &new_attributes) const; + + /** + * Rearrange Jacobian columns to match internal variable ordering. + * @param A Input Jacobian matrix. + * @param A_keys Keys corresponding to blocks in `A`. + * @return Rearranged Jacobian matrix. + */ + Matrix rearrangeMatrix(const Matrix &A, const KeyVector &A_keys) const; + + Matrix computeConstraintJacobian(const Values &values) const; + + /** + * Construct transposed sparse Jacobian from sparse triplets. + * @param nrows Number of rows in original Jacobian. + * @param ncols Number of cols in original Jacobian (including RHS column). + * @param triplets Sparse Jacobian triplets. + * @param cc CHOLMOD context. + * @return Transposed sparse Jacobian matrix. + */ +#if defined(GTDYNAMICS_WITH_SUITESPARSE) + static cholmod_sparse *SparseJacobianTranspose( + const size_t nrows, const size_t ncols, + const std::vector> &triplets, + cholmod_common *cc); + + /** + * Create sparse selector matrix that picks the last columns. + * @param nrows Number of rows. + * @param ncols Number of selected columns. + * @param cc CHOLMOD context. + * @return Sparse selector matrix. + */ + static cholmod_sparse *LastColsSelectionMat(const size_t nrows, + const size_t ncols, + cholmod_common *cc); + + /** + * Convert CHOLMOD sparse matrix to Eigen sparse matrix. + * @param A CHOLMOD sparse matrix. + * @param cc CHOLMOD context. + * @return Eigen sparse matrix. + */ + static SpMatrix CholmodToEigen(cholmod_sparse *A, cholmod_common *cc); +#else + /** + * Construct transposed sparse Jacobian from sparse triplets. + * @param nrows Number of rows in original Jacobian. + * @param ncols Number of cols in original Jacobian (including RHS column). + * @param triplets Sparse Jacobian triplets. + * @return Transposed sparse Jacobian matrix. + */ + static SpMatrix SparseJacobianTranspose( + const size_t nrows, const size_t ncols, + const std::vector> &triplets); + + /** + * Create sparse selector matrix that picks the last columns. + * @param nrows Number of rows. + * @param ncols Number of selected columns. + * @return Sparse selector matrix. + */ + static SpMatrix LastColsSelectionMat(const size_t nrows, const size_t ncols); +#endif + + SpMatrix eigenSparseJacobian(const GaussianFactorGraph &graph) const; + + // cholmod_sparse* suiteSparseJacobian(const GaussianFactorGraph &graph); + + /** Return jacobian of a Gaussian factor graph represented as an Eigen sparse + * matrix. */ + static SpMatrix EigenSparseJacobian(const GaussianFactorGraph &graph); +}; + +/** + * Tangent basis parameterized by selected basis variables. + * + * Non-basis variable updates are computed by elimination/Jacobian propagation, + * yielding an implicit `[B; I]`-style basis structure. + * + * @see README.md#tangent-basis + * @see README.md#retraction + */ +class EliminationBasis : public TspaceBasis { + protected: + struct Attributes { + NonlinearFactorGraph merit_graph; + KeyVector basis_keys; + Ordering ordering; + size_t total_basis_dim; + std::map basis_location; // location of basis var in xi + std::map var_dim; + + using shared_ptr = std::shared_ptr; + }; + Attributes::shared_ptr attributes_; + MultiJacobians jacobians_; + + public: + /** + * Constructor. + * @param constraints Equality constraints for the component. + * @param values Values of variables in the connected component. + * @param params Basis construction parameters. + * @param basis_keys Optional variables selected as basis variables. + */ + EliminationBasis(const EqualityConstraints::shared_ptr &constraints, + const Values &values, + const TspaceBasisParams::shared_ptr ¶ms, + std::optional basis_keys = {}); + + /** + * Constructor from another basis, reusing structural attributes. + * @param values Current values. + * @param other Source basis. + */ + EliminationBasis(const Values &values, const EliminationBasis &other); + + EliminationBasis(const TspaceBasisParams::shared_ptr ¶ms, + const Attributes::shared_ptr &attributes) + : TspaceBasis(params), attributes_(attributes) {} + + /// Create basis with new values. + TspaceBasis::shared_ptr createWithNewValues( + const Values &values) const override { + return std::make_shared(values, *this); + } + + /** + * Construct a basis by incorporating additional constraints. + * @param constraints Additional equality constraints. + * @param values Current variable values. + * @param create_from_scratch If true, rebuild basis from scratch. + * @return New basis object. + */ + TspaceBasis::shared_ptr createWithAdditionalConstraints( + const EqualityConstraints &constraints, const Values &values, + bool create_from_scratch = false) const override; + + /// Compute the tangent vector in the ambient space. + VectorValues computeTangentVector(const Vector &xi) const override; + + /// Compute xi of basis components. + Vector computeXi(const VectorValues &tangent_vector) const override; + + /// Jacobian of recover function. + Matrix recoverJacobian(const Key &key) const override; + + /** + * Compute local coordinates between two value assignments. + * @param values Source values. + * @param values_other Target values. + * @return Local coordinates in basis parameterization. + */ + Vector localCoordinates(const Values &values, + const Values &values_other) const override; + + /// Dimension of the basis. + size_t dim() const override { return attributes_->total_basis_dim; } + + /// Construct the actual basis. + void construct(const Values &values) override; + + /// print + void print( + const gtsam::KeyFormatter &keyFormatter = DefaultKeyFormatter) override { + std::cout << "Elimination basis\n"; + for (const auto &it : jacobians_) { + std::cout << "\033[1;31mVariable: " << keyFormatter(it.first) + << "\033[0m\n"; + it.second.print("", keyFormatter); + } + } + + /// Return a const reference to jacobians. + const MultiJacobians &jacobians() const { return jacobians_; } +}; + +/** + * Factory interface for creating tangent-space basis instances. + * + * @see README.md#tangent-basis + */ +class TspaceBasisCreator { + protected: + TspaceBasisParams::shared_ptr params_; + + public: + using shared_ptr = std::shared_ptr; + TspaceBasisCreator(TspaceBasisParams::shared_ptr params) : params_(params) {} + + virtual ~TspaceBasisCreator() {} + + /** + * Create a basis object for a component. + * @param constraints Equality constraints for the component. + * @param values Current values for variables in the component. + * @return Created basis object. + */ + virtual TspaceBasis::shared_ptr create( + const EqualityConstraints::shared_ptr constraints, + const Values &values) const = 0; +}; + +/** + * Factory for `OrthonormalBasis`. + * + * @see README.md#tangent-basis + */ +class OrthonormalBasisCreator : public TspaceBasisCreator { + public: + OrthonormalBasisCreator(TspaceBasisParams::shared_ptr params = + std::make_shared()) + : TspaceBasisCreator(params) {} + + static TspaceBasisCreator::shared_ptr CreateSparse() { + auto params = std::make_shared(); + params->use_sparse = true; + return std::make_shared(params); + } + + virtual ~OrthonormalBasisCreator() {} + + TspaceBasis::shared_ptr create( + const EqualityConstraints::shared_ptr constraints, + const Values &values) const override { + return std::make_shared(constraints, values, params_); + } +}; + +/** + * Factory for `EliminationBasis`. + * + * @see README.md#tangent-basis + */ +class EliminationBasisCreator : public TspaceBasisCreator { + protected: + BasisKeyFunc basis_key_func_; + + public: + EliminationBasisCreator(TspaceBasisParams::shared_ptr params = + std::make_shared(true)) + : TspaceBasisCreator(params) {} + + /** + * Create an elimination-basis factory with custom key selector. + * @param basis_key_func Callback selecting basis keys. + * @param params Basis construction parameters. + */ + EliminationBasisCreator(BasisKeyFunc basis_key_func, + TspaceBasisParams::shared_ptr params = + std::make_shared(true)) + : TspaceBasisCreator(params), basis_key_func_(basis_key_func) {} + + virtual ~EliminationBasisCreator() {} + + /** + * Create basis for one constraint-connected component. + * @param constraints Equality constraints for the component. + * @param values Current values for variables in the component. + * @return Created elimination basis. + */ + TspaceBasis::shared_ptr create( + const EqualityConstraints::shared_ptr constraints, + const Values &values) const override { + if (params_->use_basis_keys) { + KeyVector basis_keys = basis_key_func_(values.keys()); + return std::make_shared(constraints, values, params_, + basis_keys); + } + return std::make_shared(constraints, values, params_); + } +}; + +} // namespace gtdynamics diff --git a/gtdynamics/config.h.in b/gtdynamics/config.h.in index f3bb9c401..9a6eb32fd 100644 --- a/gtdynamics/config.h.in +++ b/gtdynamics/config.h.in @@ -29,5 +29,6 @@ namespace gtdynamics { // Paths to SDF & URDF files. constexpr const char* kSdfPath = "@PROJECT_SOURCE_DIR@/models/sdfs/"; constexpr const char* kUrdfPath = "@PROJECT_SOURCE_DIR@/models/urdfs/"; +constexpr const char* kDataPath = "@PROJECT_SOURCE_DIR@/data/"; constexpr const char* kTestPath = "@PROJECT_SOURCE_DIR@/tests/"; } // namespace gtdynamics diff --git a/gtdynamics/constrained_optimizer/CMakeLists.txt b/gtdynamics/constrained_optimizer/CMakeLists.txt new file mode 100644 index 000000000..e69de29bb diff --git a/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.cpp b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.cpp new file mode 100644 index 000000000..0a3b28eaf --- /dev/null +++ b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.cpp @@ -0,0 +1,593 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/* Constrained optimization benchmark implementations. */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { +namespace { + +constexpr const char* kBenchmarkCsvEnv = "GTDYN_BENCHMARK_CSV"; +constexpr const char* kBenchmarkIdEnv = "GTDYN_BENCHMARK_ID"; + +std::string CsvEscape(const std::string& value) { + std::string escaped = "\""; + for (char ch : value) { + if (ch == '"') + escaped += "\"\""; + else + escaped += ch; + } + escaped += "\""; + return escaped; +} + +std::string ToLower(std::string text) { + std::transform(text.begin(), text.end(), text.begin(), [](unsigned char c) { + return static_cast(std::tolower(c)); + }); + return text; +} + +std::string Trim(const std::string& text) { + const auto first = text.find_first_not_of(" \t\n\r"); + if (first == std::string::npos) return ""; + const auto last = text.find_last_not_of(" \t\n\r"); + return text.substr(first, last - first + 1); +} + +std::vector Split(const std::string& text, char delim) { + std::vector parts; + std::stringstream ss(text); + std::string item; + while (std::getline(ss, item, delim)) parts.push_back(item); + return parts; +} + +std::vector OrderedMethods() { + return {ConstrainedOptBenchmark::Method::SOFT, + ConstrainedOptBenchmark::Method::PENALTY, + ConstrainedOptBenchmark::Method::AUGMENTED_LAGRANGIAN, + ConstrainedOptBenchmark::Method::CM_F, + ConstrainedOptBenchmark::Method::CM_I}; +} + +std::string MethodsListString( + const std::set& methods) { + std::vector tokens; + for (ConstrainedOptBenchmark::Method method : OrderedMethods()) { + if (methods.count(method) == 0) continue; + tokens.push_back(ConstrainedOptBenchmark::MethodToken(method)); + } + + std::ostringstream oss; + for (size_t i = 0; i < tokens.size(); ++i) { + if (i > 0) oss << ","; + oss << tokens[i]; + } + return oss.str(); +} + +std::set ParseMethods( + const std::string& methodsText) { + std::set methods; + for (const auto& tokenRaw : Split(methodsText, ',')) { + const std::string token = ToLower(Trim(tokenRaw)); + if (token.empty()) continue; + if (token == "all") { + const auto methods = OrderedMethods(); + return std::set(methods.begin(), + methods.end()); + } + if (token == "soft") { + methods.insert(ConstrainedOptBenchmark::Method::SOFT); + continue; + } + if (token == "penalty") { + methods.insert(ConstrainedOptBenchmark::Method::PENALTY); + continue; + } + if (token == "alm" || token == "augmented_lagrangian") { + methods.insert(ConstrainedOptBenchmark::Method::AUGMENTED_LAGRANGIAN); + continue; + } + if (token == "f" || token == "cmf") { + methods.insert(ConstrainedOptBenchmark::Method::CM_F); + continue; + } + if (token == "i" || token == "cmi") { + methods.insert(ConstrainedOptBenchmark::Method::CM_I); + continue; + } + throw std::invalid_argument("Unknown method token: " + tokenRaw); + } + + if (methods.empty()) { + throw std::invalid_argument("No valid methods selected."); + } + + return methods; +} + +std::filesystem::path EnsureDataPath() { + const std::filesystem::path path(kDataPath); + std::filesystem::create_directories(path); + return path; +} + +std::string DefaultBenchmarkCsvPath( + const ConstrainedOptBenchmark::Options& options) { + const std::filesystem::path path = + EnsureDataPath() / (options.id + "_benchmark.csv"); + return path.string(); +} + +class ScopedEnvVar { + public: + ScopedEnvVar(const char* key, std::string value) : key_(key) { + const char* existing = std::getenv(key_); + if (existing) { + hadOldValue_ = true; + oldValue_ = existing; + } + setenv(key_, value.c_str(), 1); + } + + ~ScopedEnvVar() { + if (hadOldValue_) { + setenv(key_, oldValue_.c_str(), 1); + } else { + unsetenv(key_); + } + } + + private: + const char* key_; + bool hadOldValue_ = false; + std::string oldValue_; +}; + +void AppendBenchmarkCsv(const std::string& method, size_t fDim, size_t vDim, + double timeS, size_t numIters, double constraintL2, + double cost) { + const char* csvPath = std::getenv(kBenchmarkCsvEnv); + if (!csvPath || std::string(csvPath).empty()) return; + + const char* benchNameEnv = std::getenv(kBenchmarkIdEnv); + std::string benchName = benchNameEnv ? benchNameEnv : ""; + + bool writeHeader = true; + { + std::ifstream in(csvPath); + writeHeader = !in.good() || in.peek() == std::ifstream::traits_type::eof(); + } + + std::ofstream out(csvPath, std::ios::app); + if (!out) return; + + if (writeHeader) { + out << "benchmark,method,f_dim,v_dim,time_s,iters,constraint_l2,cost\n"; + } + out << CsvEscape(benchName) << "," << CsvEscape(method) << "," << fDim << "," + << vDim << "," << std::setprecision(12) << timeS << "," << numIters << "," + << std::scientific << std::setprecision(12) << constraintL2 << "," + << std::defaultfloat << std::setprecision(12) << cost << "\n"; +} + +void PrintLatex(std::ostream& latexOs, const std::string& expName, size_t fDim, + size_t vDim, double time, size_t numIters, double constraintVio, + double cost) { + std::cout << "[BENCH] " << expName << ": f_dim=" << fDim << ", v_dim=" << vDim + << ", time_s=" << std::setprecision(6) << std::defaultfloat << time + << ", iters=" << numIters << ", constraint_l2=" << std::scientific + << std::setprecision(3) << constraintVio + << ", cost=" << std::defaultfloat << std::setprecision(6) << cost + << "\n"; + latexOs << "& " + expName + " & $" << fDim << " \\times " << vDim << "$ & " + << std::setprecision(4) << time << std::defaultfloat << " & " + << numIters << " & " << std::scientific << std::setprecision(2) + << constraintVio << std::defaultfloat << " & " << std::fixed + << std::setprecision(2) << cost << std::defaultfloat << "\\\\\n"; + AppendBenchmarkCsv(expName, fDim, vDim, time, numIters, constraintVio, cost); +} + +} // namespace + +void ConstrainedOptBenchmark::PrintUsage( + std::ostream& os, const char* programName, + const ConstrainedOptBenchmark::CliDefaults& defaults) { + os << "Usage: " << programName << " [benchmark options] [example options]\n" + << "Benchmark options:\n" + << " --methods LIST Comma list from " + "{soft,penalty,alm,f,i,all}.\n" + << " --benchmark-id NAME Prefix for generated benchmark " + "artifacts.\n" + << " --benchmark-csv PATH CSV path for benchmark rows (default: " + "kDataPath/NAME_benchmark.csv).\n" + << " --verbose-benchmark Enable outer LM summary output.\n" + << " --verbose-retractor Enable retraction LM summary output.\n"; + if (defaults.enableNumSteps) { + os << " --num-steps N Number of trajectory steps (default: " + << defaults.defaultNumSteps << ").\n"; + } + os << "Defaults: benchmark-id=" << defaults.id + << ", methods=" << MethodsListString(defaults.defaultMethods) << "\n"; +} + +ConstrainedOptBenchmark::ParsedCli ConstrainedOptBenchmark::ParseCli( + int argc, char** argv, + const ConstrainedOptBenchmark::CliDefaults& defaults) { + ConstrainedOptBenchmark::ParsedCli parsed; + parsed.runOptions.id = defaults.id; + parsed.runOptions.methods = defaults.defaultMethods; + parsed.numSteps = defaults.defaultNumSteps; + + for (int i = 1; i < argc; ++i) { + const std::string arg(argv[i]); + if (arg == "--help" || arg == "-h") { + ConstrainedOptBenchmark::PrintUsage(std::cout, argv[0], defaults); + std::exit(0); + } else if (arg == "--verbose-benchmark") { + parsed.runOptions.verbose = true; + } else if (arg == "--verbose-retractor") { + parsed.runOptions.verboseRetractor = true; + } else if (arg == "--methods") { + if (i + 1 >= argc) + throw std::invalid_argument("Missing value for --methods"); + parsed.runOptions.methods = ParseMethods(argv[++i]); + } else if (arg.rfind("--methods=", 0) == 0) { + parsed.runOptions.methods = + ParseMethods(arg.substr(std::string("--methods=").size())); + } else if (arg == "--benchmark-id") { + if (i + 1 >= argc) + throw std::invalid_argument("Missing value for --benchmark-id"); + parsed.runOptions.id = argv[++i]; + } else if (arg.rfind("--benchmark-id=", 0) == 0) { + parsed.runOptions.id = arg.substr(std::string("--benchmark-id=").size()); + } else if (arg == "--benchmark-csv") { + if (i + 1 >= argc) + throw std::invalid_argument("Missing value for --benchmark-csv"); + parsed.runOptions.csvPath = argv[++i]; + } else if (arg.rfind("--benchmark-csv=", 0) == 0) { + parsed.runOptions.csvPath = + arg.substr(std::string("--benchmark-csv=").size()); + } else if (defaults.enableNumSteps && + (arg == "--num-steps" || arg == "-n")) { + if (i + 1 >= argc) + throw std::invalid_argument("Missing value for --num-steps"); + parsed.numSteps = std::stoul(argv[++i]); + } else if (defaults.enableNumSteps && arg.rfind("--num-steps=", 0) == 0) { + parsed.numSteps = + std::stoul(arg.substr(std::string("--num-steps=").size())); + } else if (defaults.enableNumSteps && + std::all_of(arg.begin(), arg.end(), + [](unsigned char c) { return std::isdigit(c); })) { + parsed.numSteps = std::stoul(arg); + } else { + parsed.unknownArgs.push_back(arg); + } + } + + if (defaults.enableNumSteps && parsed.numSteps == 0) { + throw std::invalid_argument("--num-steps must be greater than 0"); + } + + return parsed; +} + +std::string ConstrainedOptBenchmark::MethodToken( + ConstrainedOptBenchmark::Method method) { + switch (method) { + case ConstrainedOptBenchmark::Method::SOFT: + return "soft"; + case ConstrainedOptBenchmark::Method::PENALTY: + return "penalty"; + case ConstrainedOptBenchmark::Method::AUGMENTED_LAGRANGIAN: + return "alm"; + case ConstrainedOptBenchmark::Method::CM_F: + return "cm_f"; + case ConstrainedOptBenchmark::Method::CM_I: + return "cm_i"; + } + return "unknown"; +} + +std::string ConstrainedOptBenchmark::MethodLabel( + ConstrainedOptBenchmark::Method method) { + switch (method) { + case ConstrainedOptBenchmark::Method::SOFT: + return "Soft Constraint"; + case ConstrainedOptBenchmark::Method::PENALTY: + return "Penalty Method"; + case ConstrainedOptBenchmark::Method::AUGMENTED_LAGRANGIAN: + return "Augmented Lagrangian"; + case ConstrainedOptBenchmark::Method::CM_F: + return "Constraint Manifold (F)"; + case ConstrainedOptBenchmark::Method::CM_I: + return "Constraint Manifold (I)"; + } + return "Unknown"; +} + +std::string ConstrainedOptBenchmark::MethodDataPath( + const ConstrainedOptBenchmark::Options& options, + ConstrainedOptBenchmark::Method method, const std::string& suffix) { + const std::filesystem::path path = + EnsureDataPath() / + (options.id + "_" + ConstrainedOptBenchmark::MethodToken(method) + + suffix); + return path.string(); +} + +ConstrainedOptBenchmark::ConstrainedOptBenchmark( + ConstrainedOptBenchmark::Options options) + : options_(std::move(options)) { + moptFactory_ = [](ConstrainedOptBenchmark::Method) { + return ConstrainedOptBenchmark::DefaultMoptParams(); + }; +} + +void ConstrainedOptBenchmark::setProblemFactory(ProblemFactory factory) { + problemFactory_ = std::move(factory); +} + +void ConstrainedOptBenchmark::setOuterLmBaseParams( + LevenbergMarquardtParams params) { + outerLmParams_ = std::move(params); +} + +void ConstrainedOptBenchmark::setOuterLmConfig(LmConfig callback) { + outerLmConfig_ = std::move(callback); +} + +void ConstrainedOptBenchmark::setMoptFactory(MoptFactory factory) { + moptFactory_ = std::move(factory); +} + +void ConstrainedOptBenchmark::setResultCallback(ResultCallback callback) { + resultCallback_ = std::move(callback); +} + +void ConstrainedOptBenchmark::run(std::ostream& latexOs) { + if (!problemFactory_) { + throw std::runtime_error( + "ConstrainedOptBenchmark requires a problem factory."); + } + + if (options_.methods.empty()) { + throw std::runtime_error("ConstrainedOptBenchmark methods set is empty."); + } + + const std::string csvPath = options_.csvPath.empty() + ? DefaultBenchmarkCsvPath(options_) + : options_.csvPath; + + ScopedEnvVar benchCsvEnv(kBenchmarkCsvEnv, csvPath); + ScopedEnvVar benchIdEnv(kBenchmarkIdEnv, options_.id); + + for (ConstrainedOptBenchmark::Method method : OrderedMethods()) { + if (options_.methods.count(method) == 0) continue; + + LevenbergMarquardtParams lmParams = outerLmParams_; + if (options_.verbose) { + lmParams.setVerbosityLM("SUMMARY"); + } + if (outerLmConfig_) { + outerLmConfig_(method, &lmParams); + } + + auto problem = problemFactory_(); + + Values result; + switch (method) { + case ConstrainedOptBenchmark::Method::SOFT: { + std::cout << "soft constraints:\n"; + result = + OptimizeSoftConstraints(problem, latexOs, lmParams, options_.softMu, + options_.constraintUnitScale); + break; + } + case ConstrainedOptBenchmark::Method::PENALTY: { + std::cout << "penalty method:\n"; + auto penaltyParams = std::make_shared(); + penaltyParams->lmParams = lmParams; + result = OptimizePenalty(problem, latexOs, penaltyParams, + options_.constraintUnitScale); + break; + } + case ConstrainedOptBenchmark::Method::AUGMENTED_LAGRANGIAN: { + std::cout << "augmented lagrangian:\n"; + auto almParams = std::make_shared(); + almParams->lmParams = lmParams; + result = OptimizeAugmentedLagrangian(problem, latexOs, almParams, + options_.constraintUnitScale); + break; + } + case ConstrainedOptBenchmark::Method::CM_F: + case ConstrainedOptBenchmark::Method::CM_I: { + const bool feasible = method == ConstrainedOptBenchmark::Method::CM_F; + std::cout << "constraint manifold basis variables (" + << (feasible ? "feasible" : "infeasible") << "):\n"; + + auto moptParams = moptFactory_(method); + auto* retractLm = + &moptParams.cc_params->retractor_creator->params()->lm_params; + if (options_.verboseRetractor) { + retractLm->setVerbosityLM("SUMMARY"); + } + + retractLm->setMaxIterations(feasible + ? options_.cmFRetractorMaxIterations + : options_.cmIRetractorMaxIterations); + + if (!feasible && options_.cmIRetractFinal) { + moptParams.retract_final = true; + } + + result = OptimizeCmOpt(problem, latexOs, moptParams, lmParams, + ConstrainedOptBenchmark::MethodLabel(method), + options_.constraintUnitScale); + break; + } + } + + if (resultCallback_) { + resultCallback_(method, result); + } + } +} + +Values ConstrainedOptBenchmark::OptimizeSoftConstraints( + const EConsOptProblem& problem, std::ostream& latexOs, + LevenbergMarquardtParams lmParams, double mu, double constraintUnitScale) { + NonlinearFactorGraph graph = problem.costs_; + graph.add(problem.constraints().penaltyGraph(mu)); + + LevenbergMarquardtOptimizer optimizer(graph, problem.initValues(), lmParams); + auto optimizationStart = std::chrono::system_clock::now(); + auto result = optimizer.optimize(); + auto optimizationEnd = std::chrono::system_clock::now(); + const auto optimizationTimeMs = + std::chrono::duration_cast(optimizationEnd - + optimizationStart); + const double optimizationTime = optimizationTimeMs.count() * 1e-3; + + PrintLatex( + latexOs, "Soft Constraint", + problem.costsDimension() + problem.constraintsDimension(), + problem.valuesDimension(), optimizationTime, + optimizer.getInnerIterations(), + problem.evaluateEConstraintViolationL2Norm(result) * constraintUnitScale, + problem.evaluateCost(result)); + return result; +} + +ManifoldOptimizerParameters ConstrainedOptBenchmark::DefaultMoptParams() { + ManifoldOptimizerParameters moptParams; + auto retractorParams = std::make_shared(); + moptParams.cc_params->retractor_creator = + std::make_shared(retractorParams); + auto basisParams = std::make_shared(); + basisParams->always_construct_basis = false; + moptParams.cc_params->basis_creator = + std::make_shared(basisParams); + return moptParams; +} + +ManifoldOptimizerParameters ConstrainedOptBenchmark::DefaultMoptParamsSV( + const BasisKeyFunc& basisKeyFunc) { + ManifoldOptimizerParameters moptParams; + auto retractorParams = std::make_shared(); + retractorParams->use_basis_keys = true; + moptParams.cc_params->retractor_creator = + std::make_shared(basisKeyFunc, retractorParams); + auto basisParams = std::make_shared(); + basisParams->use_basis_keys = true; + basisParams->always_construct_basis = false; + moptParams.cc_params->basis_creator = + std::make_shared(basisKeyFunc, basisParams); + return moptParams; +} + +Values ConstrainedOptBenchmark::OptimizeCmOpt( + const EConsOptProblem& problem, std::ostream& latexOs, + ManifoldOptimizerParameters moptParams, LevenbergMarquardtParams lmParams, + const std::string& expName, double constraintUnitScale) { + NonlinearMOptimizer optimizer(moptParams, lmParams); + auto moptProblem = optimizer.initializeMoptProblem( + problem.costs(), problem.constraints(), problem.initValues()); + + auto optimizationStart = std::chrono::system_clock::now(); + auto result = optimizer.optimizeMOpt(moptProblem); + auto optimizationEnd = std::chrono::system_clock::now(); + const auto optimizationTimeMs = + std::chrono::duration_cast(optimizationEnd - + optimizationStart); + const double optimizationTime = optimizationTimeMs.count() * 1e-3; + + const auto problemDim = moptProblem.problemDimension(); + PrintLatex( + latexOs, "\\textbf{" + expName + "}", problemDim.first, problemDim.second, + optimizationTime, 0, + problem.evaluateEConstraintViolationL2Norm(result) * constraintUnitScale, + problem.evaluateCost(result)); + + return result; +} + +Values ConstrainedOptBenchmark::OptimizePenalty( + const EConsOptProblem& problem, std::ostream& latexOs, + gtsam::PenaltyOptimizerParams::shared_ptr params, + double constraintUnitScale) { + gtsam::NonlinearFactorGraph graph = problem.costs(); + graph.add(problem.constraints()); + gtsam::PenaltyOptimizer optimizer(graph, problem.initValues(), params); + + auto optimizationStart = std::chrono::system_clock::now(); + auto result = optimizer.optimize(); + auto optimizationEnd = std::chrono::system_clock::now(); + const auto optimizationTimeUs = + std::chrono::duration_cast(optimizationEnd - + optimizationStart); + const double optimizationTime = optimizationTimeUs.count() * 1e-6; + + PrintLatex( + latexOs, "Penalty Method", + problem.costsDimension() + problem.constraintsDimension(), + problem.valuesDimension(), optimizationTime, 0, + problem.evaluateEConstraintViolationL2Norm(result) * constraintUnitScale, + problem.evaluateCost(result)); + + return result; +} + +Values ConstrainedOptBenchmark::OptimizeAugmentedLagrangian( + const EConsOptProblem& problem, std::ostream& latexOs, + gtsam::AugmentedLagrangianParams::shared_ptr params, + double constraintUnitScale) { + gtsam::NonlinearFactorGraph graph = problem.costs(); + graph.add(problem.constraints()); + gtsam::AugmentedLagrangianOptimizer optimizer(graph, problem.initValues(), + params); + + auto optimizationStart = std::chrono::system_clock::now(); + auto result = optimizer.optimize(); + auto optimizationEnd = std::chrono::system_clock::now(); + const auto optimizationTimeMs = + std::chrono::duration_cast(optimizationEnd - + optimizationStart); + const double optimizationTime = optimizationTimeMs.count() * 1e-3; + + PrintLatex( + latexOs, "Augmented Lagrangian", + problem.costsDimension() + problem.constraintsDimension(), + problem.valuesDimension(), optimizationTime, 0, + problem.evaluateEConstraintViolationL2Norm(result) * constraintUnitScale, + problem.evaluateCost(result)); + + return result; +} + +} // namespace gtdynamics diff --git a/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h new file mode 100644 index 000000000..e408453e5 --- /dev/null +++ b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h @@ -0,0 +1,178 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ConstrainedOptBenchmark.h + * @brief Shared benchmark runner and CLI utilities for constrained optimization + * examples. + * @author Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::LevenbergMarquardtParams; +using gtsam::Values; + +/// Common benchmark runner to remove duplicated orchestration in examples. +class ConstrainedOptBenchmark { + public: + /// Supported optimization methods in constrained benchmark runs. + enum class Method { SOFT, PENALTY, AUGMENTED_LAGRANGIAN, CM_F, CM_I }; + + /// Runtime options shared by constrained benchmark examples. + struct Options { + /// Identifier used as filename prefix and benchmark row label. + std::string id = "benchmark"; + /// Optional explicit CSV output path (defaults under kDataPath when empty). + std::string csvPath; + /// Set of optimization methods to execute for one benchmark run. + std::set methods = {Method::SOFT, Method::PENALTY, + Method::AUGMENTED_LAGRANGIAN, Method::CM_F, + Method::CM_I}; + /// If true, enable summary logging for outer optimizers. + bool verbose = false; + /// If true, enable summary logging for manifold retractor optimizers. + bool verboseRetractor = false; + /// Soft-constraint penalty weight. + double softMu = 1.0; + /// Scale factor applied to reported constraint violation norms. + double constraintUnitScale = 1.0; + /// Retractor max iterations for CM(F). + size_t cmFRetractorMaxIterations = 10; + /// Retractor max iterations for CM(I). + size_t cmIRetractorMaxIterations = 1; + /// Whether CM(I) should apply a final projection/retraction. + bool cmIRetractFinal = true; + }; + + /// Defaults for CLI parsing shared by benchmark examples. + struct CliDefaults { + /// Default identifier used when not provided from CLI. + std::string id = "benchmark"; + /// Default methods to run when --methods is not specified. + std::set defaultMethods = {Method::SOFT, Method::PENALTY, + Method::AUGMENTED_LAGRANGIAN, + Method::CM_F, Method::CM_I}; + /// Whether parser accepts --num-steps. + bool enableNumSteps = false; + /// Default --num-steps value when enabled. + size_t defaultNumSteps = 0; + }; + + /// Parsed CLI options shared by benchmark examples. + struct ParsedCli { + /// Effective runtime options. + Options runOptions; + /// Effective number of steps (meaningful only when enabled by defaults). + size_t numSteps = 0; + /// Unrecognized args preserved for example-specific parsing/validation. + std::vector unknownArgs; + }; + + using ProblemFactory = std::function; + using LmConfig = + std::function; + using MoptFactory = std::function; + using ResultCallback = + std::function; + + /// Create a benchmark runner with selected runtime options. + explicit ConstrainedOptBenchmark(Options options); + + /// Set factory used to create a fresh optimization problem per method run. + void setProblemFactory(ProblemFactory factory); + + /// Set baseline outer LM parameters before per-method customization. + void setOuterLmBaseParams(LevenbergMarquardtParams params); + + /// Set callback to customize outer LM parameters per method. + void setOuterLmConfig(LmConfig callback); + + /// Set callback to construct manifold optimizer parameters per CM method. + void setMoptFactory(MoptFactory factory); + + /// Set callback invoked after each method completes with resulting values. + void setResultCallback(ResultCallback callback); + + /// Run selected methods and append rows to CSV. + void run(std::ostream& latexOs); + + /// Create default CM parameters using unconstrained retraction + dense basis. + static ManifoldOptimizerParameters DefaultMoptParams(); + + /// Create default CM parameters using basis-key elimination + basis + /// retraction. + static ManifoldOptimizerParameters DefaultMoptParamsSV( + const BasisKeyFunc& basisKeyFunc); + + /// Print common CLI usage flags for benchmark examples. + static void PrintUsage(std::ostream& os, const char* programName, + const CliDefaults& defaults); + + /// Parse common CLI arguments and preserve unknown args for example parsing. + static ParsedCli ParseCli(int argc, char** argv, const CliDefaults& defaults); + + /// Return the canonical short token for a method. + static std::string MethodToken(Method method); + + /// Return the display label used in benchmark output. + static std::string MethodLabel(Method method); + + /// Build a deterministic output file name under kDataPath for a method. + static std::string MethodDataPath(const Options& options, Method method, + const std::string& suffix); + + private: + static Values OptimizeSoftConstraints(const EConsOptProblem& problem, + std::ostream& latexOs, + LevenbergMarquardtParams lmParams, + double mu, double constraintUnitScale); + + static Values OptimizeCmOpt(const EConsOptProblem& problem, + std::ostream& latexOs, + ManifoldOptimizerParameters moptParams, + LevenbergMarquardtParams lmParams, + const std::string& expName, + double constraintUnitScale); + + static Values OptimizePenalty( + const EConsOptProblem& problem, std::ostream& latexOs, + gtsam::PenaltyOptimizerParams::shared_ptr params, + double constraintUnitScale); + + static Values OptimizeAugmentedLagrangian( + const EConsOptProblem& problem, std::ostream& latexOs, + gtsam::AugmentedLagrangianParams::shared_ptr params, + double constraintUnitScale); + + Options options_; + ProblemFactory problemFactory_; + LevenbergMarquardtParams outerLmParams_; + LmConfig outerLmConfig_; + MoptFactory moptFactory_; + ResultCallback resultCallback_; +}; + +} // namespace gtdynamics diff --git a/gtdynamics/constrained_optimizer/ConstrainedOptProblem.cpp b/gtdynamics/constrained_optimizer/ConstrainedOptProblem.cpp new file mode 100644 index 000000000..055ad7d81 --- /dev/null +++ b/gtdynamics/constrained_optimizer/ConstrainedOptProblem.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ConstrainedOptProblem.h + * @brief Constrained optimization problems. + * @author Yetong Zhang + */ + +#include +#include + +namespace gtdynamics { + +using gtsam::Double_; +using gtsam::Key; +using gtsam::ScalarExpressionInequalityConstraint; +using gtsam::Values; + +/* ************************************************************************* */ +size_t EConsOptProblem::costsDimension() const { + size_t costs_dim = 0; + for (const auto &factor : costs_) { + costs_dim += factor->dim(); + } + return costs_dim; +} + +/* ************************************************************************* */ +Key AuxilaryKey(uint64_t k) { + return DynamicsSymbol::SimpleSymbol("ax", k); +} + +/* ************************************************************************* */ +double ComputeAuxiliaryValue( + const ScalarExpressionInequalityConstraint::shared_ptr &i_constraint, + const Values &values) { + double constraint_value = i_constraint->expression().value(values); + if (constraint_value > 0) { + return 0; + } + return sqrt(-constraint_value); +} + +/* ************************************************************************* */ +gtsam::NonlinearEqualityConstraint::shared_ptr CreateAuxiliaryConstraint( + const ScalarExpressionInequalityConstraint::shared_ptr &i_constraint, + Key aux_key) { + auto expr = i_constraint->expression(); + Double_ aux_expr(aux_key); + Double_ new_expr = expr + aux_expr * aux_expr; + double tolerance = i_constraint->sigmas()(0); + return std::make_shared>( + new_expr, 0.0, gtsam::Vector1(tolerance)); +} + +/* ************************************************************************* */ +EConsOptProblem IEConsOptProblem::auxiliaryProblem() const { + gtsam::NonlinearEqualityConstraints aux_constraints; + Values aux_values; + + uint64_t k = 0; + for (const auto &i_constraint : iConstraints()) { + auto scalar_constraint = + std::dynamic_pointer_cast( + i_constraint); + if (!scalar_constraint) { + throw std::runtime_error( + "auxiliaryProblem only supports ScalarExpressionInequalityConstraint"); + } + Key aux_key = AuxilaryKey(k++); + aux_constraints.push_back(CreateAuxiliaryConstraint(scalar_constraint, + aux_key)); + aux_values.insert(aux_key, + ComputeAuxiliaryValue(scalar_constraint, values_)); + } + + gtsam::NonlinearEqualityConstraints all_constraints = eConstraints(); + for (const auto& constraint : aux_constraints) { + all_constraints.push_back(constraint); + } + Values all_values = values_; + all_values.insert(aux_values); + + return EConsOptProblem(costs(), all_constraints, all_values); +} + +} // namespace gtdynamics diff --git a/gtdynamics/constrained_optimizer/ConstrainedOptProblem.h b/gtdynamics/constrained_optimizer/ConstrainedOptProblem.h new file mode 100644 index 000000000..57c4b5ad7 --- /dev/null +++ b/gtdynamics/constrained_optimizer/ConstrainedOptProblem.h @@ -0,0 +1,105 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ConstrainedOptProblem.h + * @brief Constrained optimization problems. + * @author Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::NonlinearFactorGraph; +using gtsam::NonlinearInequalityConstraints; +using gtsam::Values; + +/** Equality-constrained optimization problem, in the form of + * argmin_x 0.5||f(X)||^2 + * s.t. h(X) = 0 + * where X represents the variables, 0.5||f(X)||^2 represents the quadratic cost + * functions, h(X)=0 represents the constraints. + */ +struct EConsOptProblem { + typedef std::function EvalFunc; + + NonlinearFactorGraph costs_; // cost function, ||f(X)||^2 + gtsam::NonlinearEqualityConstraints e_constraints_; // equality constraints. h(X)=0 + Values values_; // values of all variables, X + EvalFunc eval_func; + /// Constructor. + EConsOptProblem(const NonlinearFactorGraph &costs, + const gtsam::NonlinearEqualityConstraints &constraints, + const Values &values) + : costs_(costs), e_constraints_(constraints), values_(values) {} + + const NonlinearFactorGraph &costs() const { return costs_; } + const gtsam::NonlinearEqualityConstraints &constraints() const { + return e_constraints_; + } + const Values &initValues() const { return values_; } + + double evaluateCost(const Values &values) const { + return costs_.error(values); + } + + double evaluateEConstraintViolationL2Norm(const Values &values) const { + return e_constraints_.violationNorm(values); + } + + /// Evaluate the dimension of costs. + size_t costsDimension() const; + + /// Evaluate the dimension of constraints. + size_t constraintsDimension() const { return e_constraints_.dim(); } + + /// Evaluate the dimension of variables. + size_t valuesDimension() const { return values_.dim(); } +}; + +/** Equality-Inequality-constrained optimization problem, in the form of + * argmin_x 0.5||f(X)||^2 + * s.t. h(X) = 0 + * g(X) <= 0 + * where X represents the variables, 0.5||f(X)||^2 represents the quadratic cost + * functions, h(X)=0 represents the constraints. + */ +struct IEConsOptProblem : public EConsOptProblem { + NonlinearInequalityConstraints i_constraints_; + + /// Constructor. + IEConsOptProblem(const NonlinearFactorGraph &costs, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const NonlinearInequalityConstraints &i_constraints, + const Values &values) + : EConsOptProblem(costs, e_constraints, values), + i_constraints_(i_constraints) {} + + const gtsam::NonlinearEqualityConstraints &eConstraints() const { + return constraints(); + } + const NonlinearInequalityConstraints &iConstraints() const { + return i_constraints_; + } + + double evaluateIConstraintViolationL2Norm(const Values &values) const { + return i_constraints_.violationNorm(values); + } + + /// Equivalent equality-constrained optimization problem with auxiliary + /// variables z. Inequality constraints g(x)<=0 are transformed into equality + /// constraints g(x)+z^2=0. + EConsOptProblem auxiliaryProblem() const; +}; + +} // namespace gtdynamics diff --git a/gtdynamics/constrained_optimizer/ConstrainedOptimizer.cpp b/gtdynamics/constrained_optimizer/ConstrainedOptimizer.cpp new file mode 100644 index 000000000..b5d806465 --- /dev/null +++ b/gtdynamics/constrained_optimizer/ConstrainedOptimizer.cpp @@ -0,0 +1,60 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ConstrainedOptimizer.cpp + * @brief Base class constrained optimization. + * @author Yetong Zhang + */ + +#include +#include + +namespace gtdynamics { + +using gtsam::LevenbergMarquardtOptimizer; +using gtsam::LevenbergMarquardtParams; +using gtsam::NonlinearFactorGraph; +using gtsam::Values; + +/* ************************************************************************* */ +std::shared_ptr +ConstrainedOptimizer::CreateLMOptimizer( + const NonlinearFactorGraph &graph, const Values &values, + const bool store_lm_details, + const LevenbergMarquardtParams &lm_params) const { + if (store_lm_details) { + return std::make_shared(graph, values, lm_params); + } else { + return std::make_shared(graph, values, + lm_params); + } +} + +/* ************************************************************************* */ +std::pair, std::vector> +ConstrainedOptimizer::RetrieveLMItersValues( + std::shared_ptr optimizer) const { + auto history_optimizer = + std::static_pointer_cast(optimizer); + std::vector lm_iters_values; + std::vector lm_inner_iters; + size_t prev_accum_inner_iters = 0; + + for (const auto &state : history_optimizer->states()) { + if (state.totalNumberInnerIterations == 0) { + continue; + } + lm_iters_values.push_back(state.values); + lm_inner_iters.push_back(state.totalNumberInnerIterations - + prev_accum_inner_iters); + prev_accum_inner_iters = state.totalNumberInnerIterations; + } + return std::make_pair(lm_iters_values, lm_inner_iters); +} + +} // namespace gtdynamics diff --git a/gtdynamics/constrained_optimizer/ConstrainedOptimizer.h b/gtdynamics/constrained_optimizer/ConstrainedOptimizer.h new file mode 100644 index 000000000..25c989eae --- /dev/null +++ b/gtdynamics/constrained_optimizer/ConstrainedOptimizer.h @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ConstrainedOptimizer.h + * @brief Base class constrained optimization. + * @author Yetong Zhang + */ + +#pragma once + +#include +#include +#include + +namespace gtdynamics { + +using gtsam::LevenbergMarquardtParams; +using gtsam::LevenbergMarquardtOptimizer; +using gtsam::NonlinearFactorGraph; +using gtsam::Values; + +/// Constrained optimization parameters shared between all solvers. +struct ConstrainedOptimizationParameters { + using shared_ptr = std::shared_ptr; + size_t num_iterations; // number of iterations + LevenbergMarquardtParams lm_params; // LM parameters + bool verbose = false; + bool store_iter_details = false; // Store detailed info of each iter + bool store_lm_details = false; // Store iterations by LM + + /// Constructor with LM parameters. + ConstrainedOptimizationParameters( + const size_t _num_iterations = 10, + const LevenbergMarquardtParams &_lm_params = LevenbergMarquardtParams()) + : num_iterations(_num_iterations), lm_params(_lm_params) {} +}; + +/// Results after each iteration during the constrained optimization progress. +struct ConstrainedOptIterDetails { + Values values; // values after each inner loop + int num_lm_iters; // number of LM iterations for each inner loop + int num_lm_inner_iters; +}; + +/// Results from constrained optimization runs. +struct ConstrainedOptResult { + std::vector num_iters; +}; + +/// Base class for constrained optimizer. +class ConstrainedOptimizer { +public: + /** + * @brief Constructor. + */ + ConstrainedOptimizer() {} + + /** + * @brief Solve constrained optimization problem with equality constraints + * only. + * + * @param graph A Nonlinear factor graph representing cost. + * @param constraints All the constraints. + * @param initial_values Initial values for all variables. + * @return Values The result of the constrained optimization. + */ + virtual Values optimize(const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &constraints, + const Values &initial_values) const { + throw std::runtime_error( + "Equality constrained optimization not implemented"); + } + + /** + * @brief Solve constrained optimization problem with both equality and + * inequality constraints. + * + * @param graph A Nonlinear factor graph representing cost. + * @param e_constraints All the nonlinear equality constraints. + * @param i_constraints All the nonlinear inequality constraints. + * @param initial_values Initial values for all variables. + * @return Values The result of the constrained optimization. + */ + virtual Values optimize(const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const Values &initial_values) const { + throw std::runtime_error( + "Inequality constrained optimization not implemented"); + } + + /// Solve a constrained optimization problem with equality constraints only. + Values optimizeE(const EConsOptProblem &problem) const { + return optimize(problem.costs(), problem.constraints(), + problem.initValues()); + } + + /// Solve a constrained optimization problem with i and e constraints. + Values optimizeIE(const IEConsOptProblem &problem) const { + return optimize(problem.costs(), problem.eConstraints(), + problem.iConstraints(), problem.initValues()); + } + + /// Run optimization with auxiliary variables that transforms inequality + /// constraints into equality constraints of the form g(x)-z^2=0. + Values optimizeIEAuxiliary(const IEConsOptProblem &problem) const { + return optimizeE(problem.auxiliaryProblem()); + } + +protected: + std::shared_ptr + CreateLMOptimizer(const NonlinearFactorGraph &graph, const Values &values, + const bool store_lm_details, + const LevenbergMarquardtParams &lm_params) const; + + /// For each LM iteration, retrieve the values and number of inner iterations. + std::pair, std::vector> RetrieveLMItersValues( + std::shared_ptr optimizer) const; +}; + +} // namespace gtdynamics diff --git a/gtdynamics/constrained_optimizer/IPOptOptimizer.cpp b/gtdynamics/constrained_optimizer/IPOptOptimizer.cpp new file mode 100644 index 000000000..61e1dab0d --- /dev/null +++ b/gtdynamics/constrained_optimizer/IPOptOptimizer.cpp @@ -0,0 +1,433 @@ +#include +#include +#include + +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +/* **************************** translator ******************************* */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +Pose3 IFOptTranslator::VecToPose(const Vector6 &vec, OptionalJacobian<6, 6> H) { + if (H) { + Vector3 euler_angles(vec(0), vec(1), vec(2)); + Vector3 trans(vec(3), vec(4), vec(5)); + Matrix36 H_angles_vec, H_trans_vec; + H_angles_vec << I_3x3, Z_3x3; + H_trans_vec << Z_3x3, I_3x3; + Matrix33 H_rot_angles; + Rot3 rot = Rot3::RzRyRx(euler_angles, H_rot_angles); + Matrix63 H_pose_rot, H_pose_trans; + Pose3 pose = Pose3::Create(rot, trans, H_pose_rot, H_pose_trans); + *H = H_pose_rot * H_rot_angles * H_angles_vec + H_pose_trans * H_trans_vec; + return pose; + } else { + Vector3 euler_angles(vec(0), vec(1), vec(2)); + Vector3 trans(vec(3), vec(4), vec(5)); + Rot3 rot = Rot3::RzRyRx(euler_angles); + Pose3 pose = Pose3::Create(rot, trans); + return pose; + } +} + +/* ************************************************************************* */ +Vector6 IFOptTranslator::PoseToVec(const Pose3 &pose) { + Vector3 trans = pose.translation(); + Rot3 rot = pose.rotation(); + Vector3 euler_angles = rot.rpy(); + Vector6 vec; + vec << euler_angles, trans; + return vec; +} + +/* ************************************************************************* */ +Matrix IFOptTranslator::PoseJacobian(const Vector &vec, const Matrix &H_pose) { + Matrix66 H_pose_vec; + VecToPose(vec, H_pose_vec); + return H_pose * H_pose_vec; +} + +/* ************************************************************************* */ +Vector IFOptTranslator::PoseGradient(const Vector& vec, const Vector& g_pose) { + Matrix66 H_pose_vec; + VecToPose(vec, H_pose_vec); + return H_pose_vec.transpose() * g_pose; +} + +/* ************************************************************************* */ +bool IFOptTranslator::IsPoseKey(const Key key) { + DynamicsSymbol symb(key); + if (symb.label() == "p") { + return true; + } + return false; +} + +/* ************************************************************************* */ +Vector IFOptTranslator::valueToVec(const gtsam::Values &values, + const Key &key) { + size_t dim = values.at(key).dim(); + if (dim == 1) { + double val = values.atDouble(key); + return Vector1(val); + } else if (dim == 3) { + Vector3 val = values.at(key); + return val; + } else if (dim == 6) { + if (IsPoseKey(key)) { + Pose3 val = values.at(key); + return PoseToVec(val); + } else { + Vector6 val = values.at(key); + return val; + } + } else { + throw std::runtime_error("undefined dimension"); + } +} + +/* ************************************************************************* */ +Values IFOptTranslator::vecToValue(const Vector &vec, const Key &key) { + Values values; + size_t dim = vec.size(); + if (dim == 1) { + double val = vec(0); + values.insert(key, val); + } else if (dim == 3) { + Vector3 val = vec; + values.insert(key, val); + } else if (dim == 6) { + if (IsPoseKey(key)) { + Pose3 pose = VecToPose(vec); + values.insert(key, pose); + } else { + Vector6 val = vec; + values.insert(key, val); + } + } else { + throw std::runtime_error("undefined dimension"); + } + + return values; +} + +/* ************************************************************************* */ +std::string IFOptTranslator::keyToName(const Key &key) { + // return GTDKeyFormatter(key); + return std::to_string(key); +} + +/* ************************************************************************* */ +Key IFOptTranslator::nameToKey(const std::string &name) { + return std::stoull(name); +} + +/* ************************************************************************* */ +/* ***************************** optimizer ******************************* */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +Values IPOptimizer::optimize(const NonlinearFactorGraph &cost, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const Values &initial_values) const { + + // Ensure that all i-constraints are 1-dimensional + for (const auto &constraint : i_constraints) { + if (constraint->dim() != 1) { + throw std::runtime_error( + "IPOptimizer only supports scalar inequality constraints"); + } + } + + // 0. translate cost, constraints, values to ipopt format. + auto translator = std::make_shared(); + + // 1. define the problem + ifopt::Problem nlp; + for (const Key key : initial_values.keys()) { + nlp.AddVariableSet( + std::make_shared(initial_values, key, translator)); + } + for (size_t i = 0; i < e_constraints.size(); i++) { + const auto &constraint = e_constraints.at(i); + std::string name = "eConstraint" + std::to_string(i); + nlp.AddConstraintSet( + std::make_shared(constraint, name, translator)); + } + for (size_t i = 0; i < i_constraints.size(); i++) { + const auto &constraint = i_constraints.at(i); + std::string name = "iConstraint" + std::to_string(i); + nlp.AddConstraintSet( + std::make_shared(constraint, name, translator)); + } + for (size_t i = 0; i < cost.size(); i++) { + const auto &factor = cost.at(i); + std::string name = "cost" + std::to_string(i); + nlp.AddCostSet(std::make_shared(factor, name, translator)); + } + // nlp.AddVariableSet(std::make_shared()); + // nlp.AddConstraintSet(std::make_shared()); + // nlp.AddCostSet(std::make_shared()); + nlp.PrintCurrent(); + + // 2. choose solver and options + ifopt::IpoptSolver ipopt; + ipopt.SetOption("linear_solver", "mumps"); + ipopt.SetOption("jacobian_approximation", "exact"); + // ipopt.SetOption("hessian_approximation", "limited-memory"); + + // 3 . solve + ipopt.Solve(nlp); + + // 4. translate to gtsam Values + Values values; + auto ifopt_result = nlp.GetOptVariables(); + for (const Key key : initial_values.keys()) { + std::string name = translator->keyToName(key); + Vector x = ifopt_result->GetComponent(name)->GetValues(); + values.insert(translator->vecToValue(x, key)); + } + return values; +} + +/* ************************************************************************* */ +Values IPOptimizer::optimize(const NonlinearFactorGraph &cost, + const gtsam::NonlinearEqualityConstraints &constraints, + const Values &initial_values) const { + gtsam::NonlinearInequalityConstraints i_constraints; + return optimize(cost, constraints, i_constraints, initial_values); +} + +/* ************************************************************************* */ +/* ****************************** utils ********************************** */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +KeySet ConstructKeySet(const KeyVector &kv) { + KeySet key_set; + for (const auto &key : kv) { + key_set.insert(key); + } + return key_set; +} + +/* ************************************************************************* */ +Matrix RetrieveVarJacobian(const GaussianFactor::shared_ptr &linear_factor, + const Key &key) { + size_t start_col = 0; + size_t var_dim = 0; + for (auto it = linear_factor->begin(); it != linear_factor->end(); it++) { + if (*it == key) { + var_dim = linear_factor->getDim(it); + break; + } + start_col += linear_factor->getDim(it); + } + + /// Compute jacobian + auto jacobian = linear_factor->jacobian().first; + + return jacobian.middleCols(start_col, var_dim); +} + +/* ************************************************************************* */ +void FillJacobianHelper( + const Matrix &jacobian, + Eigen::SparseMatrix &jac_block) { + /// Fill entries + for (size_t i = 0; i < jacobian.rows(); i++) { + for (size_t j = 0; j < jacobian.cols(); j++) { + jac_block.coeffRef(i, j) = jacobian(i, j); + } + } +} + +/* ************************************************************************* */ +/* ***************************** variables ******************************* */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +IFOptVariable::IFOptVariable(const Values &values, const Key key, + const IFOptTranslator::shared_ptr translator) + : VariableSet(values.at(key).dim(), translator->keyToName(key)), + vec_(translator->valueToVec(values, key)) {} + +/* ************************************************************************* */ +IFOptVariable::VecBound IFOptVariable::GetBounds() const { + VecBound bounds(GetRows(), ifopt::NoBound); + return bounds; +} + +/* ************************************************************************* */ +/* **************************** e-constraint ***************************** */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +IFOptEConstraint::IFOptEConstraint( + gtsam::NonlinearEqualityConstraint::shared_ptr constraint, + const std::string &name, + const IFOptTranslator::shared_ptr translator) + : ConstraintSet(constraint->dim(), name), constraint_(constraint), + factor_(constraint->penaltyFactor(1.0)), + keys_(ConstructKeySet(factor_->keys())), translator_(translator) {} + +/* ************************************************************************* */ +Values IFOptEConstraint::GetValuesGTSAM() const { + Values values; + for (const Key &key : constraint_->keys()) { + std::string name = translator_->keyToName(key); + auto x = GetVariables()->GetComponent(name)->GetValues(); + values.insert(translator_->vecToValue(x, key)); + } + return values; +} + +/* ************************************************************************* */ +IFOptEConstraint::VectorXd IFOptEConstraint::GetValues() const { + // std::cout << "get values e-constraint\n"; + Values values = GetValuesGTSAM(); + return constraint_->whitenedError(values); +} + +/* ************************************************************************* */ +IFOptEConstraint::VecBound IFOptEConstraint::GetBounds() const { + VecBound b(GetRows(), ifopt::Bounds(0.0, 0.0)); + return b; +} + +/* ************************************************************************* */ +void IFOptEConstraint::FillJacobianBlock(std::string var_set, + Jacobian &jac_block) const { + Key key = translator_->nameToKey(var_set); + if (!keys_.exists(key)) { + return; + } + Values values = GetValuesGTSAM(); + auto linear_factor = factor_->linearize(values); + auto jacobian = RetrieveVarJacobian(linear_factor, key); + if (values.at(key).dim() == 6 && translator_->IsPoseKey(key)) { + std::string name = translator_->keyToName(key); + auto x = GetVariables()->GetComponent(name)->GetValues(); + jacobian = translator_->PoseJacobian(x, jacobian); + } + FillJacobianHelper(jacobian, jac_block); +} + +/* ************************************************************************* */ +/* **************************** i-constraint ***************************** */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +IFOptIConstraint::IFOptIConstraint( + gtsam::NonlinearInequalityConstraint::shared_ptr constraint, + const std::string &name, + const IFOptTranslator::shared_ptr translator) + : ConstraintSet(constraint->dim(), name), constraint_(constraint), + factor_(constraint->createEqualityConstraint()), + keys_(ConstructKeySet(factor_->keys())), translator_(translator) {} + +/* ************************************************************************* */ +Values IFOptIConstraint::GetValuesGTSAM() const { + Values values; + for (const Key &key : constraint_->keys()) { + std::string name = translator_->keyToName(key); + auto x = GetVariables()->GetComponent(name)->GetValues(); + values.insert(translator_->vecToValue(x, key)); + } + return values; +} + +/* ************************************************************************* */ +IFOptIConstraint::VectorXd IFOptIConstraint::GetValues() const { + // std::cout << "get values i-constraint\n"; + Values values = GetValuesGTSAM(); + gtsam::Vector eval = constraint_->unwhitenedExpr(values); + gtsam::Vector sigmas = constraint_->sigmas(); + for (size_t i = 0; i < static_cast(eval.size()); i++) { + eval(i) /= sigmas(i); + } + return eval; +} + +/* ************************************************************************* */ +IFOptIConstraint::VecBound IFOptIConstraint::GetBounds() const { + // std::cout << "get bounds i-constraint\n"; + VecBound b(GetRows(), ifopt::Bounds(-ifopt::inf, 0.0)); + return b; +} + +/* ************************************************************************* */ +void IFOptIConstraint::FillJacobianBlock(std::string var_set, + Jacobian &jac_block) const { + // std::cout << "FillJacobianBlock i-constraint\n"; + Key key = translator_->nameToKey(var_set); + if (!keys_.exists(key)) { + return; + } + Values values = GetValuesGTSAM(); + auto linear_factor = factor_->linearize(values); + auto jacobian = RetrieveVarJacobian(linear_factor, key); + if (values.at(key).dim() == 6 && translator_->IsPoseKey(key)) { + std::string name = translator_->keyToName(key); + auto x = GetVariables()->GetComponent(name)->GetValues(); + jacobian = translator_->PoseJacobian(x, jacobian); + } + FillJacobianHelper(jacobian, jac_block); +} + +/* ************************************************************************* */ +/* ******************************** cost ********************************* */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +IFOptCost::IFOptCost(NonlinearFactor::shared_ptr factor, + const std::string &name, + const IFOptTranslator::shared_ptr translator) + : ifopt::CostTerm(name), factor_(factor), + keys_(ConstructKeySet(factor->keys())), translator_(translator) {} + +/* ************************************************************************* */ +Values IFOptCost::GetValuesGTSAM() const { + Values values; + for (const Key &key : factor_->keys()) { + std::string name = translator_->keyToName(key); + auto x = GetVariables()->GetComponent(name)->GetValues(); + values.insert(translator_->vecToValue(x, key)); + } + return values; +} + +/* ************************************************************************* */ +double IFOptCost::GetCost() const { + // std::cout << "get values cost\n"; + Values values = GetValuesGTSAM(); + return factor_->error(values); +} + +/* ************************************************************************* */ +void IFOptCost::FillJacobianBlock(std::string var_set, Jacobian &jac) const { + // std::cout << "FillJacobianBlock cost\n"; + Key key = translator_->nameToKey(var_set); + if (!keys_.exists(key)) { + return; + } + Values values = GetValuesGTSAM(); + auto linear_factor = factor_->linearize(values); + VectorValues gradient_all = linear_factor->gradientAtZero(); + Vector gradient = gradient_all.at(key); + if (values.at(key).dim() == 6 && translator_->IsPoseKey(key)) { + std::string name = translator_->keyToName(key); + auto x = GetVariables()->GetComponent(name)->GetValues(); + gradient = translator_->PoseGradient(x, gradient); + } + for (size_t j = 0; j < gradient.size(); j++) { + jac.coeffRef(0, j) = gradient(j); + } + // std::cout << "FillJacobianBlock cost finish\n"; +} + +} // namespace gtdynamics diff --git a/gtdynamics/constrained_optimizer/IPOptOptimizer.h b/gtdynamics/constrained_optimizer/IPOptOptimizer.h new file mode 100644 index 000000000..111a56dfa --- /dev/null +++ b/gtdynamics/constrained_optimizer/IPOptOptimizer.h @@ -0,0 +1,160 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file IPOptOptimizer.h + * @brief Primal-dual interior point optimizer calling IPOPT. + * @author: Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::Key; +using gtsam::Matrix; +using gtsam::NonlinearFactorGraph; +using gtsam::OptionalJacobian; +using gtsam::Pose3; +using gtsam::Values; +using gtsam::Vector; +using gtsam::Vector6; + +class IFOptTranslator { +public: + using shared_ptr = std::shared_ptr; + + IFOptTranslator() {} + /// Translate a gtsam value into a vector compatible for ifopt + static Vector valueToVec(const gtsam::Values &values, const Key &key); + + static Values vecToValue(const Vector &vec, const Key &key); + + static std::string keyToName(const Key &key); + + static Key nameToKey(const std::string &name); + + static Pose3 VecToPose(const Vector6& vec, OptionalJacobian<6,6> H=nullptr); + + static Vector6 PoseToVec(const Pose3& pose); + + /// Transform the Jacobian w.r.t. pose into Jacobian w.r.t. vector. + static Matrix PoseJacobian(const Vector& vec, const Matrix& H_pose); + + static Vector PoseGradient(const Vector& vec, const Vector& g_pose); + + static bool IsPoseKey(const Key key); +}; + +class IPIterDetails { + IPIterDetails() {} +}; +typedef std::vector IPItersDetails; + +/// IPOPT optimizer. +class IPOptimizer : public ConstrainedOptimizer { +protected: +public: + /// Default constructor. + IPOptimizer() {} + + /// Run optimization with equality constraints only. + Values optimize(const NonlinearFactorGraph &cost, + const gtsam::NonlinearEqualityConstraints &constraints, + const Values &initial_values) const override; + + /// Run optimization with equality and inequality constraints. + Values optimize(const NonlinearFactorGraph &cost, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const Values &initial_values) const override; +}; + +/// Variable Set compatible with ifopt +class IFOptVariable : public ifopt::VariableSet { +protected: + Vector vec_; + +public: + IFOptVariable(const Values &values, const Key key, + const IFOptTranslator::shared_ptr translator); + + void SetVariables(const VectorXd &x) override { vec_ = x; } + + VectorXd GetValues() const override { return vec_; } + + VecBound GetBounds() const override; +}; + +/// Equality constraint compatible with ifopt +class IFOptEConstraint : public ifopt::ConstraintSet { +protected: + gtsam::NonlinearEqualityConstraint::shared_ptr constraint_; + NonlinearFactor::shared_ptr factor_; + KeySet keys_; + IFOptTranslator::shared_ptr translator_; + +public: + IFOptEConstraint(gtsam::NonlinearEqualityConstraint::shared_ptr constraint, + const std::string &name, + const IFOptTranslator::shared_ptr translator); + + VectorXd GetValues() const override; + VecBound GetBounds() const override; + void FillJacobianBlock(std::string var_set, + Jacobian &jac_block) const override; + +protected: + Values GetValuesGTSAM() const; +}; + +/// Inequality constraint compatible with ifopt +class IFOptIConstraint : public ifopt::ConstraintSet { +protected: + gtsam::NonlinearInequalityConstraint::shared_ptr constraint_; + NonlinearFactor::shared_ptr factor_; + KeySet keys_; + IFOptTranslator::shared_ptr translator_; + +public: + IFOptIConstraint(gtsam::NonlinearInequalityConstraint::shared_ptr constraint, + const std::string &name, + const IFOptTranslator::shared_ptr translator); + + VectorXd GetValues() const override; + VecBound GetBounds() const override; + void FillJacobianBlock(std::string var_set, + Jacobian &jac_block) const override; + +protected: + Values GetValuesGTSAM() const; +}; + +/// Cost terms compatible with ifopt +class IFOptCost : public ifopt::CostTerm { +protected: + NonlinearFactor::shared_ptr factor_; + KeySet keys_; + IFOptTranslator::shared_ptr translator_; + +public: + IFOptCost(NonlinearFactor::shared_ptr factor, const std::string &name, + const IFOptTranslator::shared_ptr translator); + double GetCost() const override; + void FillJacobianBlock(std::string var_set, Jacobian &jac) const override; + +protected: + Values GetValuesGTSAM() const; +}; + +} // namespace gtdynamics diff --git a/gtdynamics/constrained_optimizer/SQPOptimizer.cpp b/gtdynamics/constrained_optimizer/SQPOptimizer.cpp new file mode 100644 index 000000000..6cd52f621 --- /dev/null +++ b/gtdynamics/constrained_optimizer/SQPOptimizer.cpp @@ -0,0 +1,550 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SQPOptimizer.cpp + * @brief SQP implementations. + * @author: Yetong Zhang + */ + +#include + +#include +#include + +using namespace gtdynamics; +using std::cout, std::setprecision, std::setw, std::endl; + +namespace gtdynamics { + +using gtsam::IndeterminantLinearSystemException; +using gtsam::JacobianFactor; +using gtsam::Key; +using gtsam::Matrix; + +/* ************************************************************************* */ +/* <============================= SQPState ================================> */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +SQPState::SQPState(const Values &_values, const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const SQPParams ¶ms, const double _lambda, + const double _lambda_factor, const size_t _iterations) + : lambda(_lambda), lambda_factor(_lambda_factor), values(_values), + eval(SQPOptimizer::MeritFunction(graph, e_constraints, i_constraints, + values, params)), + iterations(_iterations), totalNumberInnerIterations(0) { + linear_cost = *graph.linearize(values); + linear_e_merit = *e_constraints.penaltyGraph().linearize(values); + gtsam::NonlinearFactorGraph e_constraint_graph; + for (const auto& constraint : e_constraints) { + e_constraint_graph.add(constraint); + } + linear_e_constraints = *e_constraint_graph.linearize(values); + linear_i_constraints = GaussianFactorGraph(); + linear_i_merit = GaussianFactorGraph(); + for (const auto &i_constraint : i_constraints) { + if (!i_constraint->feasible(values)) { + auto e_constraint = i_constraint->createEqualityConstraint(); + linear_i_constraints.push_back( + e_constraint->linearize(values)); + linear_i_merit.push_back( + e_constraint->penaltyFactor(1.0)->linearize(values)); + } + } + computeMinVector(); +} + +/* ************************************************************************* */ +SQPState SQPState::FromLastIteration(const SQPIterDetails &iter_details, + const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const SQPParams ¶ms) { + double lambda; + const auto &last_trial = iter_details.trials.back(); + const auto &prev_state = iter_details.state; + SQPState state; + if (last_trial.step_is_successful) { + auto state = SQPState(last_trial.new_values, graph, e_constraints, + i_constraints, params, last_trial.lambda, + prev_state.lambda_factor, prev_state.iterations + 1); + state.totalNumberInnerIterations = + prev_state.totalNumberInnerIterations + iter_details.trials.size(); + last_trial.setNextLambda(state.lambda, state.lambda_factor, + params.lm_params); + return state; + } else { + throw std::runtime_error("SQPState: No successful trials found"); + } +} + +/* ************************************************************************* */ +void SQPState::computeMinVector() { + GaussianFactorGraph graph; + graph.push_back(linear_e_constraints); + graph.push_back(linear_i_constraints); + for (const auto &key : values.keys()) { + size_t dim = values.at(key).dim(); + graph.emplace_shared(key, Matrix::Identity(dim, dim), + Vector::Zero(dim), + gtsam::noiseModel::Unit::Create(dim)); + } + min_vector = graph.optimize(); +} + +/* ************************************************************************* */ +/* <============================= SQPTrial ================================> */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +SQPTrial::SQPTrial(const SQPState &state, const double _lambda, + const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const SQPParams ¶ms) + : lambda(_lambda) { + // build damped system + GaussianFactorGraph qp_problem = constructConstrainedSystem(state, params); + GaussianFactorGraph damped_system = + buildDampedSystem(qp_problem, state, params.lm_params); + // damped_system.print(); + + // solve linear update + try { + // delta = damped_system.optimize(); + // GaussianFactorGraph new_system; + // for (auto factor : damped_system) { + // JacobianFactor::shared_ptr jacobian_factor( + // std::dynamic_pointer_cast(factor)); + // // if (jacobian_factor) { + // auto noise_model = jacobian_factor->get_model(); + // if (noise_model && noise_model->isConstrained()) { + // new_system.push_back(ZerobFactor(jacobian_factor)); + // } else { + // new_system.push_back(factor); + // } + // } + // for (const auto& key: state.values.keys()) { + // size_t dim = state.values.at(key).dim(); + // new_system.emplace_shared(key, Matrix::Identity(dim, + // dim)*1e3, Vector::Ones(dim), noiseModel::Unit::Create(dim)); + // } + delta = SolveLinear(damped_system, params.lm_params); + // auto delta1 = SolveLinear(new_system, params.lm_params); + // // delta = damped_system.optimize(); + // // auto delta1 = new_system.optimize(); + // std::cout << "linear error zero: " << + // damped_system.error(VectorValues::Zero(delta)) << "\n"; std::cout << + // "linear error delta: " << damped_system.error(delta) << "\n"; std::cout + // << "linear error delta1: " << damped_system.error(delta1) << "\n"; + + // std::cout << "new linear error delta1: " << new_system.error(delta1) << + // "\n"; std::cout << "new linear error zero: " << + // new_system.error(VectorValues::Zero(delta)) << "\n"; std::cout << + // "delta1_norm: " << delta1.norm() << "\n"; + solve_successful = true; + } catch (const IndeterminantLinearSystemException &) { + return; + } + VectorValues zero_delta = VectorValues::Zero(delta); + auto linear_eval_zero = + SQPOptimizer::MeritFunctionApprox(state, zero_delta, params); + auto linear_eval_delta = + SQPOptimizer::MeritFunctionApprox(state, delta, params); + linear_merit_change = linear_eval_zero.merit - linear_eval_delta.merit; + bound_rate = delta.norm() / state.min_vector.norm(); + if (linear_merit_change < 0 && bound_rate < 1.1) { + resolveLinearUsingMeritSystem(state, params); + } + + // apply nonlinear update + new_values = state.values.retract(delta); + eval = SQPOptimizer::MeritFunction(graph, e_constraints, i_constraints, + new_values, params); + nonlinear_merit_change = state.eval.merit - eval.merit; + + // model fidelity + if (linear_merit_change > 0) { + model_fidelity = nonlinear_merit_change / linear_merit_change; + } else if (nonlinear_merit_change > 0) { + model_fidelity = 1; + } else { + model_fidelity = 0; + } + + if (model_fidelity > params.lm_params.minModelFidelity) { + step_is_successful = true; + } +} + +/* ************************************************************************* */ +GaussianFactorGraph +SQPTrial::constructConstrainedSystem(const SQPState &state, + const SQPParams ¶ms) const { + GaussianFactorGraph graph = state.linear_cost; + if (params.use_qp_constrained_mu) { + graph.push_back( + ScaledBiasedFactors(state.linear_e_merit, params.qp_constrained_mu, 1)); + graph.push_back( + ScaledBiasedFactors(state.linear_i_merit, params.qp_constrained_mu, 1)); + } else { + graph.push_back(state.linear_e_constraints); + graph.push_back(state.linear_i_constraints); + } + return graph; +} + +/* ************************************************************************* */ +GaussianFactorGraph +SQPTrial::constructMeritSystem(const SQPState &state, + const SQPParams ¶ms) const { + GaussianFactorGraph graph = state.linear_cost; + VectorValues zero_vec = state.values.zeroVectors(); + double e_b_norm = sqrt(2 * state.linear_e_merit.error(zero_vec)); + double e_b_scale = 1 + params.merit_e_l1_mu / params.merit_e_l2_mu / e_b_norm; + graph.push_back(ScaledBiasedFactors(state.linear_e_merit, + params.merit_e_l2_mu, e_b_scale)); + double i_b_norm = sqrt(2 * state.linear_i_merit.error(zero_vec)); + double i_b_scale = 1 + params.merit_i_l1_mu / params.merit_i_l2_mu / i_b_norm; + graph.push_back(ScaledBiasedFactors(state.linear_i_merit, + params.merit_i_l2_mu, i_b_scale)); + return graph; +} + +/* ************************************************************************* */ +void SQPTrial::resolveLinearUsingMeritSystem(const SQPState &state, + const SQPParams ¶ms) { + use_merit_system = true; + GaussianFactorGraph qp_problem = constructMeritSystem(state, params); + GaussianFactorGraph damped_system = + buildDampedSystem(qp_problem, state, params.lm_params); + + // solve linear update + try { + delta = SolveLinear(damped_system, params.lm_params); + solve_successful = true; + } catch (const IndeterminantLinearSystemException &) { + solve_successful = false; + return; + } + VectorValues zero_delta = VectorValues::Zero(delta); + auto linear_eval_zero = + SQPOptimizer::MeritFunctionApprox(state, zero_delta, params); + auto linear_eval_delta = + SQPOptimizer::MeritFunctionApprox(state, delta, params); + linear_eval_zero.merit = qp_problem.error(zero_delta); + linear_eval_delta.merit = qp_problem.error(delta); + linear_merit_change = linear_eval_zero.merit - linear_eval_delta.merit; +} + +/* ************************************************************************* */ +LMCachedModel *SQPTrial::getCachedModel(size_t dim) const { + if (dim >= noiseModelCache.size()) + noiseModelCache.resize(dim + 1); + LMCachedModel *item = &noiseModelCache[dim]; + if (!item->model) + *item = LMCachedModel(dim, 1.0 / std::sqrt(lambda)); + return item; +} + +/* ************************************************************************* */ +GaussianFactorGraph +SQPTrial::buildDampedSystemUniform(GaussianFactorGraph damped, + const SQPState &state) const { + noiseModelCache.resize(0); + // for each of the variables, add a prior + damped.reserve(damped.size() + state.values.size()); + std::map dims = state.values.dims(); + for (const auto &key_dim : dims) { + const Key &key = key_dim.first; + const size_t &dim = key_dim.second; + const LMCachedModel *item = getCachedModel(dim); + damped.emplace_shared(key, item->A, item->b, item->model); + } + return damped; +} + +/* ************************************************************************* */ +GaussianFactorGraph SQPTrial::buildDampedSystemDiagonal( + GaussianFactorGraph damped, // gets copied + const SQPState &state, const LevenbergMarquardtParams &lm_params) const { + VectorValues sqrt_hessian_diagonal = + SqrtHessianDiagonal(state.linear_cost, lm_params); + noiseModelCache.resize(0); + damped.reserve(damped.size() + sqrt_hessian_diagonal.size()); + for (const auto &key_vector : sqrt_hessian_diagonal) { + try { + const Key key = key_vector.first; + const size_t dim = key_vector.second.size(); + LMCachedModel *item = getCachedModel(dim); + item->A.diagonal() = sqrt_hessian_diagonal.at(key); // use diag(hessian) + damped.emplace_shared(key, item->A, item->b, item->model); + } catch (const std::out_of_range &) { + continue; // Don't attempt any damping if no key found in diagonal + } + } + return damped; +} + +/* ************************************************************************* */ +GaussianFactorGraph +SQPTrial::buildDampedSystem(const GaussianFactorGraph &linear, + const SQPState &state, + const LevenbergMarquardtParams &lm_params) const { + if (lm_params.getDiagonalDamping()) + return buildDampedSystemDiagonal(linear, state, lm_params); + else + return buildDampedSystemUniform(linear, state); +} + +/* ************************************************************************* */ +void SQPTrial::setNextLambda(double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const { + if (step_is_successful) { + setDecreasedNextLambda(new_lambda, new_lambda_factor, params); + } else { + setIncreasedNextLambda(new_lambda, new_lambda_factor, params); + } +} + +/* ************************************************************************* */ +void SQPTrial::setIncreasedNextLambda( + double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const { + new_lambda *= new_lambda_factor; + if (!params.useFixedLambdaFactor) { + new_lambda_factor *= 2.0; + } +} + +/* ************************************************************************* */ +void SQPTrial::setDecreasedNextLambda( + double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const { + if (params.useFixedLambdaFactor) { + new_lambda /= new_lambda_factor; + } else { + new_lambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * model_fidelity - 1.0, 3)); + new_lambda_factor *= 2.0; + } + new_lambda = std::max(params.lambdaLowerBound, new_lambda); +} + +/* ************************************************************************* */ +/* <============================ SQPLogging ===============================> */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +void PrintSQPTrialTitle() { + cout << setw(10) << "iter " + << "|" << setw(12) << "merit " + << "|" << setw(12) << "cost " + << "|" << setw(12) << "e_vio " + << "|" << setw(12) << "i_vio " + << "|" << setw(10) << "m_system " + << "|" << setw(12) << "merit_change" + << "|" << setw(12) << "apprx_change" + << "|" << setw(12) << "m_fidelity" + << "|" << setw(10) << "lambda " + << "|" << setw(10) << "time " + << "|" << setw(10) << "delta_norm" + << "|" << endl; +} + +/* ************************************************************************* */ +void PrintSQPTrial(const SQPState &state, const SQPTrial &trial, + const SQPParams ¶ms) { + if (trial.step_is_successful) { + cout << "\033[0m"; + } else { + cout << "\033[90m"; + } + cout << setw(10) << state.iterations << "|"; + if (!trial.solve_successful) { + cout << "linear solve not successful\n"; + return; + } + cout << setw(12) << setprecision(4) << trial.eval.merit << "|"; + cout << setw(12) << setprecision(4) << trial.eval.cost << "|"; + cout << setw(12) << setprecision(4) << trial.eval.e_violation << "|"; + cout << setw(12) << setprecision(4) << trial.eval.i_violation << "|"; + cout << setw(10) << trial.use_merit_system << "|"; + cout << setw(12) << setprecision(4) << trial.nonlinear_merit_change << "|"; + cout << setw(12) << setprecision(4) << trial.linear_merit_change << "|"; + cout << setw(12) << setprecision(4) << trial.model_fidelity << "|"; + cout << setw(10) << setprecision(2) << trial.lambda << "|"; + cout << setw(10) << setprecision(2) << trial.trial_time << "|"; + cout << setw(10) << setprecision(4) << trial.delta.norm() << "|"; + cout << setw(10) << setprecision(4) << trial.bound_rate << "|"; + cout << endl; + cout << "\033[0m"; +} + +/* ************************************************************************* */ +Eval SQPOptimizer::MeritFunction(const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const Values &values, + const SQPParams ¶ms) { + Eval eval; + eval.cost = graph.error(values); + eval.e_violation = e_constraints.violationNorm(values); + eval.i_violation = i_constraints.violationNorm(values); + double e_vio_l2 = 0.5 * pow(eval.e_violation, 2); + double i_vio_l2 = 0.5 * pow(eval.i_violation, 2); + eval.merit = eval.cost + params.merit_e_l1_mu * eval.e_violation + + params.merit_e_l2_mu * e_vio_l2 + + params.merit_i_l1_mu * eval.i_violation + + params.merit_i_l2_mu * i_vio_l2; + return eval; +} + +/* ************************************************************************* */ +Eval SQPOptimizer::MeritFunctionApprox(const SQPState &state, + const VectorValues &delta, + const SQPParams ¶ms) { + Eval eval; + eval.cost = state.linear_cost.error(delta); + double e_vio_l2 = state.linear_e_merit.error(delta); + double i_vio_l2 = state.linear_i_merit.error(delta); + eval.e_violation = sqrt(2 * e_vio_l2); + eval.i_violation = sqrt(2 * i_vio_l2); + eval.merit = eval.cost + params.merit_e_l1_mu * eval.e_violation + + params.merit_e_l2_mu * e_vio_l2 + + params.merit_i_l1_mu * eval.i_violation + + params.merit_i_l2_mu * i_vio_l2; + return eval; +} + +/* ************************************************************************* */ +/* <=========================== SQPOptimizer ==============================> */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +Values SQPOptimizer::optimize(const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const Values &init_values) { + SQPState state(init_values, graph, e_constraints, i_constraints, *p_, + p_->lm_params.lambdaInitial, p_->lm_params.lambdaFactor, 0); + + if (p_->lm_params.verbosityLM == LevenbergMarquardtParams::SUMMARY) { + std::cout << "Initial cost: " << state.eval.cost + << "\te_vio: " << state.eval.e_violation + << "\ti_vio: " << state.eval.i_violation + << "\tmerit: " << state.eval.merit << "\n"; + PrintSQPTrialTitle(); + } + + SQPState new_state = state; + do { + state = new_state; + SQPIterDetails iter_details = + iterate(graph, e_constraints, i_constraints, state); + details_->push_back(iter_details); + if (!checkSuccessfulTrial(iter_details)) { + return state.values; + } + new_state = SQPState::FromLastIteration(iter_details, graph, e_constraints, + i_constraints, *p_); + } while (state.iterations < p_->lm_params.maxIterations && + !checkConvergence(state, new_state) && + checkLambdaWithinLimits(state.lambda)); + details_->emplace_back(new_state); + return new_state.values; +} + +/* ************************************************************************* */ +SQPIterDetails SQPOptimizer::iterate(const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const SQPState &state) { + const LevenbergMarquardtParams &lm_params = p_->lm_params; + SQPIterDetails iter_details(state); + + // Set lambda for first trial. + double lambda = state.lambda; + double lambda_factor = state.lambda_factor; + + // Perform trials until any of follwing conditions is met + // * 1) trial is successful + // * 2) update is too small + // * 3) lambda goes beyond limits + while (true) { + // Perform the trial. + SQPTrial trial(state, lambda, graph, e_constraints, i_constraints, *p_); + if (lm_params.verbosityLM == LevenbergMarquardtParams::SUMMARY) { + PrintSQPTrial(state, trial, *p_); + } + iter_details.trials.emplace_back(trial); + + // Check condition 1. + if (trial.step_is_successful) { + break; + } + + // Check condition 2. + if (trial.solve_successful) { + double abs_change_tol = + std::max(lm_params.absoluteErrorTol, + lm_params.relativeErrorTol * state.eval.merit); + if (abs(trial.linear_merit_change) < abs_change_tol && + abs(trial.nonlinear_merit_change) < abs_change_tol) { + break; + } + } + + // Set lambda for next trial. + trial.setNextLambda(lambda, lambda_factor, lm_params); + + // Check condition 3. + if (!checkLambdaWithinLimits(lambda)) { + break; + } + } + return iter_details; +} + +/* ************************************************************************* */ +bool SQPOptimizer::checkLambdaWithinLimits(const double &lambda) const { + return lambda <= p_->lm_params.lambdaUpperBound && + lambda >= p_->lm_params.lambdaLowerBound; +} + +/* ************************************************************************* */ +bool SQPOptimizer::checkConvergence(const SQPState &prev_state, + const SQPState &state) const { + + if (state.eval.merit <= p_->lm_params.errorTol) + return true; + + // check if diverges + double absoluteDecrease = prev_state.eval.merit - state.eval.merit; + + // calculate relative error decrease and update currentError + double relativeDecrease = absoluteDecrease / prev_state.eval.merit; + bool converged = (p_->lm_params.relativeErrorTol && + (relativeDecrease <= p_->lm_params.relativeErrorTol)) || + (absoluteDecrease <= p_->lm_params.absoluteErrorTol); + return converged; +} + +/* ************************************************************************* */ +bool SQPOptimizer::checkSuccessfulTrial( + const SQPIterDetails &iter_details) const { + for (const auto &trial : iter_details.trials) { + if (trial.step_is_successful) { + return true; + } + } + return false; +} + +} // namespace gtdynamics diff --git a/gtdynamics/constrained_optimizer/SQPOptimizer.h b/gtdynamics/constrained_optimizer/SQPOptimizer.h new file mode 100644 index 000000000..710d1ca9d --- /dev/null +++ b/gtdynamics/constrained_optimizer/SQPOptimizer.h @@ -0,0 +1,211 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SQPOptimizer.h + * @brief Trust-region SQP Optimizer. + * @author: Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::LevenbergMarquardtParams; +using gtsam::GaussianFactorGraph; +using gtsam::NonlinearFactorGraph; +using gtsam::Values; +using gtsam::Vector; +using gtsam::VectorValues; + +struct SQPParams : public ConstrainedOptimizationParameters { + using Base = ConstrainedOptimizationParameters; + using shared_ptr = std::shared_ptr; + double merit_e_l1_mu = 1e0; + double merit_e_l2_mu = 1e0; + double merit_i_l1_mu = 1e0; + double merit_i_l2_mu = 1e0; + bool use_qp_constrained_mu = false; + double qp_constrained_mu = 1e8; +}; + +struct Eval { + double cost; + double e_violation; + double i_violation; + double merit; + + void print() const { + std::cout << "\tcost:\t" << cost << "\n"; + std::cout << "\te_vio:\t" << e_violation << "\n"; + std::cout << "\ti_vio:\t" << i_violation << "\n"; + std::cout << "\tmerit:\t" << merit << "\n"; + } +}; + +class SQPIterDetails; + +class SQPState { +public: + double lambda; + double lambda_factor; + Values values; + Eval eval; + size_t iterations; + size_t totalNumberInnerIterations; + GaussianFactorGraph linear_cost; + GaussianFactorGraph linear_e_merit; + GaussianFactorGraph linear_e_constraints; + GaussianFactorGraph linear_i_merit; + GaussianFactorGraph linear_i_constraints; + VectorValues min_vector; // minimum vector that satisfy linear constraints + + SQPState() {} + + SQPState(const Values &_values, const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const SQPParams ¶ms, + const double _lambda, const double _lambda_factor, + const size_t _iterations = 0); + + static SQPState FromLastIteration(const SQPIterDetails &iter_details, + const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const SQPParams ¶ms); + +protected: + void computeMinVector(); +}; + +class SQPTrial { +public: + double lambda; + VectorValues delta; + Values new_values; + Eval eval; + double nonlinear_merit_change = 0; + double linear_merit_change = 0; + double model_fidelity = 0; + bool solve_successful = false; + bool step_is_successful = false; + bool use_merit_system = false; + double bound_rate = 0; + double trial_time = 0; + + SQPTrial(const SQPState &state, const double _lambda, + const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const SQPParams ¶ms); + + /// Update lambda for the next trial/state. + void setNextLambda(double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const; + + /// Set lambda as increased values for next trial/state. + void setIncreasedNextLambda(double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const; + + /// Set lambda as decreased values for next trial/state. + void setDecreasedNextLambda(double &new_lambda, double &new_lambda_factor, + const LevenbergMarquardtParams ¶ms) const; + +protected: + /// Construct constrained QP problem. + GaussianFactorGraph constructConstrainedSystem(const SQPState &state, + const SQPParams ¶ms) const; + + /// Construct quadratic approximation of merit function. + GaussianFactorGraph constructMeritSystem(const SQPState &state, + const SQPParams ¶ms) const; + + /// Re-solve linear system, using QP approximation of merit function as + /// objective function. + void resolveLinearUsingMeritSystem(const SQPState &state, + const SQPParams ¶ms); + + mutable std::vector noiseModelCache; + LMCachedModel *getCachedModel(size_t dim) const; + + GaussianFactorGraph + buildDampedSystemDiagonal(GaussianFactorGraph damped, const SQPState &state, + const LevenbergMarquardtParams &lm_params) const; + + GaussianFactorGraph buildDampedSystemUniform(GaussianFactorGraph damped, + const SQPState &state) const; + + GaussianFactorGraph + buildDampedSystem(const GaussianFactorGraph &damped, const SQPState &state, + const LevenbergMarquardtParams &lm_params) const; +}; + +class SQPIterDetails { +public: + SQPState state; + std::vector trials; + + SQPIterDetails(const SQPState &_state) : state(_state), trials() {} +}; + +class SQPItersDetails : public std::vector { +public: + using base = std::vector; + using base::base; + + // void exportFile(const std::string &state_file_path, + // const std::string &trial_file_path) const; +}; + +class SQPOptimizer { +protected: + SQPParams::shared_ptr p_; + std::shared_ptr details_; + +public: + typedef std::shared_ptr shared_ptr; + + const SQPItersDetails &details() const { return *details_; } + + SQPOptimizer(const SQPParams::shared_ptr &p = std::make_shared()) + : p_(p), details_(std::make_shared()) {} + + static Eval MeritFunction(const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const Values &values, const SQPParams ¶ms); + + static Eval MeritFunctionApprox(const SQPState &state, + const VectorValues &delta, + const SQPParams ¶ms); + + Values optimize(const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const Values &init_values); + + SQPIterDetails iterate(const NonlinearFactorGraph &graph, + const gtsam::NonlinearEqualityConstraints &e_constraints, + const gtsam::NonlinearInequalityConstraints &i_constraints, + const SQPState &state); + + /** Check if lambda is within limits. */ + bool checkLambdaWithinLimits(const double &lambda) const; + + bool checkConvergence(const SQPState &prev_state, + const SQPState &state) const; + + bool checkSuccessfulTrial(const SQPIterDetails &iter_details) const; +}; + +} // namespace gtdynamics diff --git a/gtdynamics/constraints/CMakeLists.txt b/gtdynamics/constraints/CMakeLists.txt new file mode 100644 index 000000000..e69de29bb diff --git a/gtdynamics/constraints/LinearInequalityConstraint.cpp b/gtdynamics/constraints/LinearInequalityConstraint.cpp new file mode 100644 index 000000000..19663aa66 --- /dev/null +++ b/gtdynamics/constraints/LinearInequalityConstraint.cpp @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file LinearInequalityConstraint.cpp + * @brief Linear inequality constraints in constrained optimization. + * @author: Yetong Zhang, Frank Dellaert + */ + +#include + +namespace gtdynamics { + +/* <=======================================================================> */ +/* <===================== LinearInequalityConstraint ======================> */ +/* <=======================================================================> */ + +/* ************************************************************************* */ +bool LinearInequalityConstraint::feasible(const gtsam::VectorValues &x, + double threshold) const { + for (const double &entry : (*this)(x)) { + if (entry > threshold) { + return false; + } + } + return true; +} + +/* ************************************************************************* */ +void LinearInequalityConstraint::print( + const gtsam::KeyFormatter &key_formatter) const { + createL2Factor()->print("", key_formatter); +} + +/* <=======================================================================> */ +/* <================= JacobianLinearInequalityConstraint ==================> */ +/* <=======================================================================> */ + +/* ************************************************************************* */ +gtsam::JacobianFactor::shared_ptr +JacobianLinearInequalityConstraint::createConstrainedFactor() const { + auto factor = std::make_shared(*factor_); + auto sigmas = gtsam::Vector::Zero(dim()); + factor->setModel(true, sigmas); + return factor; +} + +/* ************************************************************************* */ +MultiJacobian JacobianLinearInequalityConstraint::jacobian() const { + MultiJacobian jac; + size_t start_col = 0; + gtsam::Matrix jac_mat = factor_->jacobian().first; + for (auto it = factor_->begin(); it != factor_->end(); it++) { + size_t dim = factor_->getDim(it); + jac.addJacobian(*it, jac_mat.middleCols(start_col, dim)); + start_col += dim; + } + return jac; +} + +/* <=======================================================================> */ +/* <==================== LinearInequalityConstraints ======================> */ +/* <=======================================================================> */ + +/* ************************************************************************* */ +gtsam::GaussianFactorGraph LinearInequalityConstraints::constraintGraph( + const IndexSet &active_indices) const { + gtsam::GaussianFactorGraph graph; + for (const auto &index : active_indices) { + graph.push_back(at(index)->createConstrainedFactor()); + } + return graph; +} + +/* ************************************************************************* */ +void LinearInequalityConstraints::print( + const gtsam::KeyFormatter &key_formatter) const { + for (const auto &constraint : *this) { + constraint->print(key_formatter); + } +} + +} // namespace gtdynamics diff --git a/gtdynamics/constraints/LinearInequalityConstraint.h b/gtdynamics/constraints/LinearInequalityConstraint.h new file mode 100644 index 000000000..6abe533d6 --- /dev/null +++ b/gtdynamics/constraints/LinearInequalityConstraint.h @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file LinearInequalityConstraint.h + * @brief Linear inequality constraints in constrained optimization. + * @author: Yetong Zhang, Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace gtdynamics { + +/** + * Linear inequality constraint base class. + */ +class LinearInequalityConstraint { + public: + typedef LinearInequalityConstraint This; + typedef std::shared_ptr shared_ptr; + + /** Default constructor. */ + LinearInequalityConstraint() {} + + /** Destructor. */ + virtual ~LinearInequalityConstraint() {} + + /** @brief return the dimension of the constraint. */ + virtual size_t dim() const = 0; + + /** @brief Evaluate the constraint function. */ + virtual gtsam::Vector operator()(const gtsam::VectorValues &x) const = 0; + + /** @brief Check if constraint violation is within tolerance. */ + virtual bool feasible(const gtsam::VectorValues &x, + double threshold = 0) const; + + virtual bool isActive(const gtsam::VectorValues &x, + double threshold = 1e-5) const { + return (*this)(x).norm() < threshold; + } + + virtual gtsam::JacobianFactor::shared_ptr createL2Factor() const = 0; + + virtual gtsam::JacobianFactor::shared_ptr createConstrainedFactor() const = 0; + + virtual MultiJacobian jacobian() const = 0; + + virtual void print( + const gtsam::KeyFormatter &key_formatter = GTDKeyFormatter) const; +}; + +/** + * Linear inequality constraint represented with a Jacobian factor. + */ +class JacobianLinearInequalityConstraint : public LinearInequalityConstraint { + protected: + using base = LinearInequalityConstraint; + gtsam::JacobianFactor::shared_ptr factor_; + + public: + JacobianLinearInequalityConstraint( + const gtsam::JacobianFactor::shared_ptr &factor) + : base(), factor_(factor) {} + + size_t dim() const override { return factor_->rows(); } + + gtsam::Vector operator()(const gtsam::VectorValues &x) const override { + return factor_->error_vector(x); + } + + gtsam::JacobianFactor::shared_ptr createL2Factor() const override { + return factor_; + } + + gtsam::JacobianFactor::shared_ptr createConstrainedFactor() const override; + + MultiJacobian jacobian() const override; +}; + +class LinearInequalityConstraints + : public std::vector { + private: + using Base = std::vector; + + template + using IsDerived = + typename std::enable_if::value>::type; + + public: + typedef LinearInequalityConstraints This; + typedef std::shared_ptr shared_ptr; + + LinearInequalityConstraints() : Base() {} + + /// Emplace a shared pointer to constraint of given type. + template + IsDerived emplace_shared(Args &&...args) { + push_back(std::allocate_shared( + Eigen::aligned_allocator(), + std::forward(args)...)); + } + + gtsam::GaussianFactorGraph constraintGraph( + const IndexSet &active_indices) const; + + void print(const gtsam::KeyFormatter &key_formatter = GTDKeyFormatter) const; +}; + +using LinearIConstraintMap = + std::map; + +} // namespace gtdynamics diff --git a/gtdynamics/dynamics/Chain.cpp b/gtdynamics/dynamics/Chain.cpp index db4bf96f5..4baf55e5d 100644 --- a/gtdynamics/dynamics/Chain.cpp +++ b/gtdynamics/dynamics/Chain.cpp @@ -13,6 +13,7 @@ #include + namespace gtdynamics { Chain operator*(const Chain &chainA, const Chain &chainB) { @@ -45,8 +46,8 @@ Chain Chain::compose(std::vector &chains) { return total_chain; } -Pose3 Chain::poe(const Vector &q, boost::optional fTe, - gtsam::OptionalJacobian<-1, -1> J) { +Pose3 Chain::poe(const Vector &q, std::optional fTe, + gtsam::OptionalJacobian<-1, -1> J) const { // Check that input has good size if (q.size() != length()) { throw std::runtime_error( @@ -85,4 +86,207 @@ Pose3 Chain::poe(const Vector &q, boost::optional fTe, } return poe; } + +/** + * Calculate AdjointMap jacobian w.r.t. joint coordinate q + * @param q joint angle + * @param jMi this COM frame, expressed in next link's COM frame at rest + * @param screw_axis screw axis expressed in kth link's COM frame + * + * TODO(Varun) Perhaps move part of this to gtsam::Pose3::AdjointMap()? + */ +gtsam::Matrix6 AdjointMapJacobianQ(double q, const gtsam::Pose3 &jMi, + const gtsam::Vector6 &screw_axis) { + // taking opposite value of screw_axis_ is because + // jTi = Pose3::Expmap(-screw_axis_ * q) * jMi; + gtsam::Vector3 w = -screw_axis.head<3>(); + gtsam::Vector3 v = -screw_axis.tail<3>(); + gtsam::Pose3 kTj = gtsam::Pose3::Expmap(-screw_axis * q) * jMi; + auto w_skew = gtsam::skewSymmetric(w); + gtsam::Matrix3 H_expo = w_skew * cosf(q) + w_skew * w_skew * sinf(q); + gtsam::Matrix3 H_R = H_expo * jMi.rotation().matrix(); + gtsam::Vector3 H_T = + H_expo * (jMi.translation() - w_skew * v) + w * w.transpose() * v; + gtsam::Matrix3 H_TR = gtsam::skewSymmetric(H_T) * kTj.rotation().matrix() + + gtsam::skewSymmetric(kTj.translation()) * H_R; + gtsam::Matrix6 H = gtsam::Z_6x6; + gtsam::insertSub(H, H_R, 0, 0); + gtsam::insertSub(H, H_TR, 3, 0); + gtsam::insertSub(H, H_R, 3, 3); + return H; +} + +gtsam::Vector3 Chain::DynamicalEquality3( + const gtsam::Vector6 &wrench, const gtsam::Vector3 &angles, + const gtsam::Vector3 &torques, gtsam::OptionalJacobian<3, 6> H_wrench, + gtsam::OptionalJacobian<3, 3> H_angles, + gtsam::OptionalJacobian<3, 3> H_torques) const { + Matrix J; + poe(angles, {}, J); + if (H_wrench) { + // derivative of difference with respect to wrench + *H_wrench = J.transpose(); + } + if (H_angles) { + // derivative of difference with respect to angles + + // The manipulator Jacobian is built such that in every step of chain + // composition we multiply the existing screw axes with the adjoint map of + // the inverse pose of the new chain, and then stack the screw axes of the + // new chain horizontally in the axes matrix. + // This means that the first angle actually doesn't get into the matrix at + // all. The last column of the jacobian actually doesn't depend on the + // angles at all, the second column depends only on the third angle, and the + // first column depends on the second and third angles. + // This means that the 3*3 jacobian has an upper triangular structure. + Matrix A = gtsam::Z_3x3; + + // Calculate the Adjoint and take its derivative in relation to angles + auto ad_J_angles1 = AdjointMapJacobianQ(angles(1), Pose3(), axes_.col(1)); + auto ad_J_angles2 = AdjointMapJacobianQ(angles(2), Pose3(), axes_.col(2)); + + // Calculate the invers adjoint maps of the Poses, as we do in * operator + Pose3 p1_inv = Pose3::Expmap(-axes_.col(1) * angles(1)); + Pose3 p2_inv = Pose3::Expmap(-axes_.col(2) * angles(2)); + auto ad_inv_p1 = p1_inv.AdjointMap(); + auto ad_inv_p2 = p2_inv.AdjointMap(); + + // calculate the non-zero terms + A(1, 2) = (ad_J_angles2 * axes_.col(1)).transpose() * wrench; + A(0, 2) = (ad_J_angles2 * ad_inv_p1 * axes_.col(0)).transpose() * wrench; + A(0, 1) = (ad_inv_p2 * ad_J_angles1 * axes_.col(0)).transpose() * wrench; + + *H_angles = A; + } + if (H_torques) { + // derivative of difference with respect to torques + *H_torques = -gtsam::I_3x3; + } + + return (J.transpose() * wrench - torques); +} + +gtsam::Vector3_ Chain::ChainConstraint3( + const std::vector &joints, const gtsam::Key wrench_key, + size_t k) const { + // Get Expression for wrench + gtsam::Vector6_ wrench_0_T(wrench_key); + + // The constraint is true for the wrench exerted on the end-effector frame, so + // we need to adjoint from base to end-effector + gtsam::Vector6_ wrench_0_H = + (-1) * joints[0]->childWrenchAdjoint(wrench_0_T, k); + gtsam::Vector6_ wrench_1_U = + (-1) * joints[1]->childWrenchAdjoint(wrench_0_H, k); + gtsam::Vector6_ wrench_2_L = + (-1) * joints[2]->childWrenchAdjoint(wrench_1_U, k); + + // Get expression for joint angles as a column vector of size 3. + gtsam::Double_ angle0(JointAngleKey(joints[0]->id(), k)), + angle1(JointAngleKey(joints[1]->id(), k)), + angle2(JointAngleKey(joints[2]->id(), k)); + gtsam::Vector3_ angles(MakeVector3, angle0, angle1, angle2); + + // Get expression for joint torques as a column vector of size 3. + gtsam::Double_ torque0(TorqueKey(joints[0]->id(), k)), + torque1(TorqueKey(joints[1]->id(), k)), + torque2(TorqueKey(joints[2]->id(), k)); + gtsam::Vector3_ torques(MakeVector3, torque0, torque1, torque2); + + // Get expression of the dynamical equality + gtsam::Vector3_ torque_diff( + std::bind(&Chain::DynamicalEquality3, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, + std::placeholders::_6), + wrench_2_L, angles, torques); + + return torque_diff; +} + +gtsam::Vector6_ Chain::AdjointWrenchConstraint3( + const std::vector &joints, const gtsam::Key body_wrench_key, + size_t k) const { + // Get Expression for wrench + gtsam::Vector6_ wrench_0_T(body_wrench_key); + + // Get expression for joint angles as a column vector of size 3. + gtsam::Double_ angle0(JointAngleKey(joints[0]->id(), k)), + angle1(JointAngleKey(joints[1]->id(), k)), + angle2(JointAngleKey(joints[2]->id(), k)); + gtsam::Vector3_ angles(MakeVector3, angle0, angle1, angle2); + + // Get expression of the dynamical equality + gtsam::Vector6_ wrench_end_effector( + std::bind(&Chain::AdjointWrenchEquality3, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4), + angles, wrench_0_T); + + return wrench_end_effector; +} + +gtsam::Vector6 Chain::AdjointWrenchEquality3( + const gtsam::Vector3 &angles, const gtsam::Vector6 &wrench_body, + gtsam::OptionalJacobian<6, 3> H_angles, + gtsam::OptionalJacobian<6, 6> H_wrench_body) const { + Matrix J_theta; + gtsam::Matrix6 H_T; + + // Get POE transformation from body to end-effector. + Pose3 T_theta = poe(angles, {}, H_angles ? &J_theta : nullptr); + + // Get end-effector wrench by Adjoint. This is true for a massless leg. + gtsam::Vector6 transformed_wrench = + T_theta.AdjointTranspose(wrench_body,H_angles ? &H_T : nullptr , H_wrench_body); + + if (H_angles) { + *H_angles = H_T * J_theta; + } + + return transformed_wrench; +} + +gtsam::Vector6_ Chain::Poe3Factor(const std::vector &joints, + const gtsam::Key wTb_key, + const gtsam::Key wTe_key, size_t k) const { + // Get Expression for poses + gtsam::Pose3_ wTb(wTb_key); + gtsam::Pose3_ wTe(wTe_key); + + // Get expression for joint angles as a column vector of size 3. + gtsam::Double_ angle0(JointAngleKey(joints[0]->id(), k)), + angle1(JointAngleKey(joints[1]->id(), k)), + angle2(JointAngleKey(joints[2]->id(), k)); + gtsam::Vector3_ angles(MakeVector3, angle0, angle1, angle2); + + // Get expression for forward kinematics + gtsam::Pose3_ end_effector_pose( + std::bind(&Chain::PoeEquality3, this, std::placeholders::_1, + std::placeholders::_2), + angles); + + // compose + gtsam::Pose3_ wTe_hat = wTb * end_effector_pose; + + // get error expression + gtsam::Vector6_ error_expression = gtsam::logmap(wTe_hat, wTe); + + return error_expression; +} + +gtsam::Pose3 Chain::PoeEquality3(const gtsam::Vector3 &angles, + gtsam::OptionalJacobian<6, 3> H_angles) const { + Matrix J_theta; + + // Get POE transformation from body to end-effector. + Pose3 T_theta = poe(angles, {}, H_angles ? &J_theta : nullptr); + + if (H_angles) { + *H_angles =J_theta; + } + + return T_theta; +} + } // namespace gtdynamics diff --git a/gtdynamics/dynamics/Chain.h b/gtdynamics/dynamics/Chain.h index d25ab6b04..dc580b054 100644 --- a/gtdynamics/dynamics/Chain.h +++ b/gtdynamics/dynamics/Chain.h @@ -17,8 +17,13 @@ #include #include #include +#include -#include +#include + +#include "gtdynamics/universal_robot/Joint.h" +#include "gtdynamics/utils/utils.h" +#include "gtdynamics/utils/values.h" using gtsam::Matrix; using gtsam::Pose3; @@ -78,7 +83,7 @@ class Chain { inline const Matrix &axes() const { return axes_; } // Return number of columns in axes_ matrix - const size_t length() const { return axes_.cols(); } + size_t length() const { return axes_.cols(); } /** * Perform forward kinematics given q, return Pose of end-effector and @@ -91,8 +96,123 @@ class Chain { * @return ............ Pose of the end-effector calculated using Product of * Exponentials */ - Pose3 poe(const Vector &q, boost::optional fTe = boost::none, - gtsam::OptionalJacobian<-1, -1> J = boost::none); -}; + Pose3 poe(const Vector &q, std::optional fTe = {}, + gtsam::OptionalJacobian<-1, -1> J = {}) const; + + /** + * This function implements the dynamic dependency between the + * joint torques and the wrench applied on the body FOR A 3-LINK CHAIN (tau = + * J*F) The input wrench is the wrench applied on the body by the joint + * closest to the body. This equation is true in the case of massless links in + * the chain. Detailed explanation in readme (chain.md) + * + * @param wrench .................. Wrench applied on the body by the joint + * closest to it in the chain. + * @param angles .................. Angles of the joints in the chain. + * @param torques ................. Torques applied by the joints. + * @return ........................ Vector of difference. + */ + gtsam::Vector3 DynamicalEquality3( + const gtsam::Vector6 &wrench, const gtsam::Vector3 &angles, + const gtsam::Vector3 &torques, + gtsam::OptionalJacobian<3, 6> H_wrench = {}, + gtsam::OptionalJacobian<3, 3> H_angles = {}, + gtsam::OptionalJacobian<3, 3> H_torques = {}) const; + + /** + * This function creates a gtsam expression of the Chain constraint FOR A + * 3-LINK CHAIN. + * + * @param joints ............... Vector of joints in the kinematic chain,FROM + * BODY TO END-EFFECTOR. + * @param wrench_key ........... Key of the wrench applied on the body by the + * joint closest to the body. + * @param k .................... Time slice. + * @return ..................... GTSAM expression of the chain constraint. + */ + gtsam::Vector3_ ChainConstraint3(const std::vector &joints, + const gtsam::Key wrench_key, size_t k) const; + + /** + * This function creates a gtsam expression of the End-Effector wrench using + * a Chain under massless leg assumption and Product of Exponentials. + * + * @param joints ............... Vector of joints in the kinematic chain, FROM + * BODY TO END-EFFECTOR. + * @param wrench_key ........... Key of the wrench applied on the body by the + * joint closest to the body. + * @param k .................... Time slice. + * @return ..................... GTSAM expression of the chain constraint. + */ + gtsam::Vector6_ AdjointWrenchConstraint3( + const std::vector &joints, + const gtsam::Key body_wrench_key, size_t k) const; + + /** + * This function calculates the end-effector wrench using POE and chain. + * + * + * + * @param angles .............. angles of the joints in the chain FROM + * BODY TO END-EFFECTOR. + * @param wrench_body ......... wrench applied by the first joint in the chain + * on the body link + * @return ................... gtsam expression of the end-effector wrench + */ + gtsam::Vector6 AdjointWrenchEquality3( + const gtsam::Vector3 &angles, const gtsam::Vector6 &wrench_body, + gtsam::OptionalJacobian<6, 3> H_angles = {}, + gtsam::OptionalJacobian<6, 6> H_wrench_body = {}) const; + + /** + * This function creates a gtsam expression factor of the End-Effector pose + * using Product of Exponentials and Chain (forward kinematics). + * + * @param joints ............... Vector of joints in the kinematic chain, FROM + * BODY TO END-EFFECTOR. + * @param wTb_key............... Key of body link pose + * @param wTe_key............... Key of end-effector pose + * @param cost_model............ cost model for factor + * @param k .................... Time slice. + * @return ..................... GTSAM expression of the chain constraint. + */ + gtsam::Vector6_ Poe3Factor(const std::vector &joints, + const gtsam::Key wTb_key, const gtsam::Key wTe_key, + size_t k) const; + + /** + * This function calculates the end-effector pose using POE and chain + * (forward kinematics). + * + * @param angles .............. angles of the joints in the chain FROM + * BODY TO END-EFFECTOR. + * @return ................... gtsam expression of the end-effector wrench + */ + gtsam::Pose3 PoeEquality3( + const gtsam::Vector3 &angles, + gtsam::OptionalJacobian<6, 3> H_angles = {}) const; + +}; // Chain class + +// Helper function to create expression with a vector, used in +// ChainConstraint3. +inline gtsam::Vector3 MakeVector3(const double &value0, const double &value1, + const double &value2, + gtsam::OptionalJacobian<3, 1> J0 = {}, + gtsam::OptionalJacobian<3, 1> J1 = {}, + gtsam::OptionalJacobian<3, 1> J2 = {}) { + gtsam::Vector3 q; + q << value0, value1, value2; + if (J0) { + *J0 << 1.0, 0.0, 0.0; + } + if (J1) { + *J1 << 0.0, 1.0, 0.0; + } + if (J2) { + *J2 << 0.0, 0.0, 1.0; + } + return q; +} } // namespace gtdynamics diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.cpp b/gtdynamics/dynamics/ChainDynamicsGraph.cpp new file mode 100644 index 000000000..d8b015ef3 --- /dev/null +++ b/gtdynamics/dynamics/ChainDynamicsGraph.cpp @@ -0,0 +1,221 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file DynamicsGraph.h + * @brief Builds a lean dynamics graph from a Robot object. + * @author Dan Barladeanu + */ + +#include "gtdynamics/dynamics/ChainDynamicsGraph.h" + +namespace gtdynamics { + +using gtsam::Key; +using gtsam::Point3; +using gtsam::Vector3_; +using gtsam::Vector6; +using gtsam::Vector6_; + +std::vector> ChainDynamicsGraph::getChainJoints( + const Robot &robot) { + std::vector FR(3), FL(3), RR(3), RL(3); + + int loc = 0; + // TODO(Varun): This seems very specific to the A1, which is sad. + for (auto &&joint : robot.joints()) { + if (joint->name().find("lower") != std::string::npos) { + loc = 2; + } + if (joint->name().find("upper") != std::string::npos) { + loc = 1; + } + if (joint->name().find("hip") != std::string::npos) { + loc = 0; + } + if (joint->name().find("FR") != std::string::npos) { + FR[loc] = joint; + } + if (joint->name().find("FL") != std::string::npos) { + FL[loc] = joint; + } + if (joint->name().find("RR") != std::string::npos) { + RR[loc] = joint; + } + if (joint->name().find("RL") != std::string::npos) { + RL[loc] = joint; + } + } + + std::vector> chain_joints{FL, FR, RL, RR}; + + return chain_joints; +} + +Chain BuildChain(std::vector &joints) { + auto j0 = joints[0]; + auto j1 = joints[1]; + auto j2 = joints[2]; + + // Calculate all relevant relative poses. + Pose3 M_T_H = j0->pMc(); + Pose3 M_H_T = M_T_H.inverse(); + Pose3 M_H_U = j1->pMc(); + Pose3 M_U_H = M_H_U.inverse(); + Pose3 M_U_L = j2->pMc(); + Pose3 M_L_U = M_U_L.inverse(); + Pose3 M_T_L = M_T_H * M_H_U * M_U_L; + Pose3 M_L_T = M_T_L.inverse(); + + // Create chains + Chain chain1(M_T_H, j0->cScrewAxis()); + Chain chain2(M_H_U, j1->cScrewAxis()); + Chain chain3(M_U_L, j2->cScrewAxis()); + + std::vector chains{chain1, chain2, chain3}; + + Chain composed = Chain::compose(chains); + + return composed; +} + +std::vector ChainDynamicsGraph::getComposedChains( + std::vector> &chain_joints) { + Chain composed_fr, composed_fl, composed_rr, composed_rl; + + composed_fl = BuildChain(chain_joints[0]); + composed_fr = BuildChain(chain_joints[1]); + composed_rl = BuildChain(chain_joints[2]); + composed_rr = BuildChain(chain_joints[3]); + + // TODO(Varun): make the code agnostic to quadrupeds, bipeds, hexapods, etc + std::vector composed_chains{composed_fl, composed_fr, composed_rl, + composed_rr}; + + return composed_chains; +} + +NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactors( + const Robot &robot, const int t, + const std::optional &contact_points, + const std::optional &mu) const { + // Initialize graph + NonlinearFactorGraph graph; + + // Set Gravity Wrench + Vector6 gravityMass; + auto graph_gravity = gravity(); + gravityMass << 0.0, 0.0, 0.0, gravity() * trunk_mass_; + Vector6_ gravity_wrench(gravityMass); + + // Create expression for wrench constraint on trunk + Vector6_ trunk_wrench_constraint = gravity_wrench; + + // Set tolerances + + // Get tolerance + Vector6 wrench_tolerance = opt().f_cost_model->sigmas(); + Vector3 contact_tolerance = opt().cm_cost_model->sigmas(); + + std::vector wrench_keys; + + for (int i = 0; i < 4; ++i) { + bool foot_in_contact = false; + // Get key for wrench at hip joint with id 0 + const Key wrench_key_3i_T = WrenchKey(0, 3 * i, t); + + // create expression for the wrench and initialize to zero + Vector6_ wrench_3i_T(Vector6::Zero()); + + // Set wrench expression on trunk by leg, according to contact + for (auto &&cp : *contact_points) { + if (cp.link->id() == 3 * (i + 1)) { + wrench_3i_T = Vector6_(wrench_key_3i_T); + foot_in_contact = true; + } + } + + // add wrench to trunk constraint + wrench_keys.push_back(wrench_key_3i_T); + + // Get expression for end effector wrench using chain + Vector6_ wrench_end_effector = composed_chains_[i].AdjointWrenchConstraint3( + chain_joints_[i], wrench_key_3i_T, t); + + // Create contact constraint + Point3 contact_in_com(0.0, 0.0, -0.07); + Vector3_ contact_constraint = ContactDynamicsMomentConstraint( + wrench_end_effector, + gtsam::Pose3(gtsam::Rot3(), (-1) * contact_in_com)); + gtsam::ExpressionEqualityConstraint contact_expression( + contact_constraint, gtsam::Vector3::Zero(), contact_tolerance); + if (foot_in_contact) { + graph.add(contact_expression.penaltyFactor(1.0)); + } else { + Vector6 wrench_zero = gtsam::Z_6x1; + graph.addPrior(wrench_key_3i_T, wrench_zero, opt().f_cost_model); + } + } + + // Add wrench factor for trunk link + graph.add(WrenchFactor(opt().f_cost_model, robot.link("trunk"), wrench_keys, + t, gravity())); + + return graph; +} + +gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors( + const Robot &robot, const int t, + const std::optional &contact_points) const { + NonlinearFactorGraph graph; + + // Get Pose key for base link + const Key base_key = PoseKey(0, t); + + // Get tolerance + Vector6 tolerance = opt().p_cost_model->sigmas(); + + for (int i = 0; i < 4; ++i) { + // Get end effector key + const Key end_effector_key = PoseKey(3 * (i + 1), t); + + // Get expression for end effector pose + gtsam::Vector6_ chain_expression = composed_chains_[i].Poe3Factor( + chain_joints_[i], base_key, end_effector_key, t); + + gtsam::ExpressionEqualityConstraint chain_constraint( + chain_expression, gtsam::Vector6::Zero(), tolerance); + + graph.add(chain_constraint.penaltyFactor(1.0)); + } + + // Add contact factors. + if (contact_points) { + for (auto &&cp : *contact_points) { + ContactHeightFactor contact_pose_factor( + PoseKey(cp.link->id(), t), opt().cp_cost_model, cp.point, gravity()); + graph.add(contact_pose_factor); + } + } + + return graph; +} + +gtsam::NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactorGraph( + const Robot &robot, const int t, + const std::optional &contact_points, + const std::optional &mu) const { + NonlinearFactorGraph graph; + graph.add(qFactors(robot, t, contact_points)); + // TODO(Varun): Why are these commented out? + // graph.add(vFactors(robot, t, contact_points)); + // graph.add(aFactors(robot, t, contact_points)); + graph.add(dynamicsFactors(robot, t, contact_points, mu)); + return graph; +} + +} // namespace gtdynamics diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.h b/gtdynamics/dynamics/ChainDynamicsGraph.h new file mode 100644 index 000000000..88ce9020a --- /dev/null +++ b/gtdynamics/dynamics/ChainDynamicsGraph.h @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ChainDynamicsGraph.h + * @brief Builds a chain dynamics graph from a Robot object. + * @author Dan Barladeanu + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::NonlinearFactorGraph; +using gtsam::Vector3; + +/** + * ChainDynamicsGraph organizes a robot into chains (e.g., legs) and builds + * nonlinear factor graphs for pose (q-level) and dynamics at a given time + * step. It partitions the robot's joints into chain structures, composes + * chain-level dynamics, and aggregates factors including optional contact + * points and friction. The resulting graphs enable constrained optimization + * with GTSAM for robots that can be modeled as a trunk with multiple chains. + */ +class ChainDynamicsGraph : public DynamicsGraph { + private: + std::vector> chain_joints_; + std::vector composed_chains_; + double trunk_mass_; + + public: + /** + * Constructor. + * @param robot the robot. + * @param opt settings for optimizer. + * @param gravity gravity in world frame. + * @param planar_axis axis of the plane, used only for planar robot. + */ + ChainDynamicsGraph(const Robot &robot, const OptimizerSetting &opt, + const Vector3 &gravity = Vector3(0, 0, -9.8), + const std::optional &planar_axis = {}) + : DynamicsGraph(opt, gravity, planar_axis), + chain_joints_(getChainJoints(robot)), + composed_chains_(getComposedChains(chain_joints_)), + trunk_mass_(robot.link("trunk")->mass()) {} + + // Destructor + ~ChainDynamicsGraph() {} + + /// Return q-level nonlinear factor graph (pose related factors) + NonlinearFactorGraph qFactors( + const Robot &robot, const int t, + const std::optional &contact_points = {}) const override; + + /** + * Return nonlinear factor graph of all dynamics factors. + * @param robot the robot. + * @param t time step. + * @param contact_points optional vector of contact points. + * @param mu optional coefficient of static friction. + */ + NonlinearFactorGraph dynamicsFactors( + const Robot &robot, const int t, + const std::optional &contact_points = {}, + const std::optional &mu = {}) const override; + + /** + * Return nonlinear factor graph of all dynamics factors. + * @param robot the robot. + * @param t time step. + * @param contact_points optional vector of contact points. + * @param mu optional coefficient of static friction. + */ + NonlinearFactorGraph dynamicsFactorGraph( + const Robot &robot, const int t, + const std::optional &contact_points = {}, + const std::optional &mu = {}) const override; + + // Get a vector of legs, each leg is a vector of its joints + static std::vector> getChainJoints( + const Robot &robot); + + // Get composed chains from chain joints + static std::vector getComposedChains( + std::vector> &chain_joints); +}; + +} // namespace gtdynamics diff --git a/gtdynamics/dynamics/Dynamics.h b/gtdynamics/dynamics/Dynamics.h index 8ed818eb7..4115bda75 100644 --- a/gtdynamics/dynamics/Dynamics.h +++ b/gtdynamics/dynamics/Dynamics.h @@ -22,7 +22,7 @@ namespace gtdynamics { /// calculate Coriolis term and jacobian w.r.t. joint coordinate twist gtsam::Vector6 Coriolis(const gtsam::Matrix6 &inertia, const gtsam::Vector6 &twist, - gtsam::OptionalJacobian<6, 6> H_twist = boost::none); + gtsam::OptionalJacobian<6, 6> H_twist = {}); /// Matrix vector multiplication. template diff --git a/gtdynamics/dynamics/DynamicsGraph.cpp b/gtdynamics/dynamics/DynamicsGraph.cpp index 94146ab6f..81737270d 100644 --- a/gtdynamics/dynamics/DynamicsGraph.cpp +++ b/gtdynamics/dynamics/DynamicsGraph.cpp @@ -11,8 +11,16 @@ * @author Yetong Zhang, Alejandro Escontrela */ -#include "gtdynamics/dynamics/DynamicsGraph.h" - +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -25,16 +33,6 @@ #include #include -#include "gtdynamics/factors/ContactDynamicsFrictionConeFactor.h" -#include "gtdynamics/factors/ContactDynamicsMomentFactor.h" -#include "gtdynamics/factors/ContactHeightFactor.h" -#include "gtdynamics/factors/ContactKinematicsAccelFactor.h" -#include "gtdynamics/factors/ContactKinematicsTwistFactor.h" -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/utils/JsonSaver.h" -#include "gtdynamics/utils/utils.h" -#include "gtdynamics/utils/values.h" - using gtsam::Double_; using gtsam::ExpressionFactor; using gtsam::GaussianFactorGraph; @@ -52,7 +50,7 @@ using gtsam::Z_6x1; namespace gtdynamics { GaussianFactorGraph DynamicsGraph::linearDynamicsGraph( - const Robot &robot, const int t, const gtsam::Values &known_values) { + const Robot &robot, const int t, const gtsam::Values &known_values) const { GaussianFactorGraph graph; auto all_constrained = gtsam::noiseModel::Constrained::All(6); for (auto &&link : robot.links()) { @@ -71,13 +69,13 @@ GaussianFactorGraph DynamicsGraph::linearDynamicsGraph( const Pose3 T_wi = Pose(known_values, i, t); const Vector6 V_i = Twist(known_values, i, t); Vector6 rhs = Pose3::adjointMap(V_i).transpose() * G_i * V_i; - if (gravity_) { - Vector gravitational_force = - T_wi.rotation().transpose() * (*gravity_) * m_i; - for (int i = 3; i < 6; ++i) { - rhs[i] += gravitational_force[i - 3]; - } + + // Compute gravitational forces. If gravity=(0, 0, 0), this will be zeros. + Vector gravitational_force = T_wi.rotation().transpose() * gravity_ * m_i; + for (int i = 3; i < 6; ++i) { + rhs[i] += gravitational_force[i - 3]; } + auto accel_key = TwistAccelKey(i, t); if (connected_joints.size() == 0) { graph.add(accel_key, G_i, rhs, all_constrained); @@ -94,8 +92,9 @@ GaussianFactorGraph DynamicsGraph::linearDynamicsGraph( OptimizerSetting opt_; for (auto &&joint : robot.joints()) { - graph += joint->linearAFactors(t, known_values, opt_, planar_axis_); - graph += joint->linearDynamicsFactors(t, known_values, opt_, planar_axis_); + graph.push_back(joint->linearAFactors(t, known_values, opt_, planar_axis_)); + graph.push_back( + joint->linearDynamicsFactors(t, known_values, opt_, planar_axis_)); } return graph; @@ -106,7 +105,7 @@ GaussianFactorGraph DynamicsGraph::linearFDPriors( OptimizerSetting opt_ = OptimizerSetting(); GaussianFactorGraph graph; for (auto &&joint : robot.joints()) - graph += joint->linearFDPriors(t, torques, opt_); + graph.push_back(joint->linearFDPriors(t, torques, opt_)); return graph; } @@ -124,11 +123,11 @@ GaussianFactorGraph DynamicsGraph::linearIDPriors( } Values DynamicsGraph::linearSolveFD(const Robot &robot, const int t, - const gtsam::Values &known_values) { + const gtsam::Values &known_values) const { // construct and solve linear graph GaussianFactorGraph graph = linearDynamicsGraph(robot, t, known_values); GaussianFactorGraph priors = linearFDPriors(robot, t, known_values); - graph += priors; + graph.push_back(priors); gtsam::VectorValues results = graph.optimize(); // arrange values @@ -161,7 +160,7 @@ Values DynamicsGraph::linearSolveID(const Robot &robot, const int t, // construct and solve linear graph GaussianFactorGraph graph = linearDynamicsGraph(robot, t, known_values); GaussianFactorGraph priors = linearIDPriors(robot, t, known_values); - graph += priors; + graph.push_back(priors); gtsam::VectorValues results = graph.optimize(); @@ -194,7 +193,7 @@ Values DynamicsGraph::linearSolveID(const Robot &robot, const int t, gtsam::NonlinearFactorGraph DynamicsGraph::qFactors( const Robot &robot, const int k, - const boost::optional &contact_points) const { + const std::optional &contact_points) const { NonlinearFactorGraph graph; for (auto &&link : robot.links()) if (link->isFixed()) @@ -208,18 +207,11 @@ gtsam::NonlinearFactorGraph DynamicsGraph::qFactors( JointAngleKey(joint->id(), k), opt_.p_cost_model, joint)); } - // TODO(frank): whoever write this should clean up this mess. - gtsam::Vector3 gravity; - if (gravity_) - gravity = *gravity_; - else - gravity = gtsam::Vector3(0, 0, -9.8); - // Add contact factors. if (contact_points) { for (auto &&cp : *contact_points) { ContactHeightFactor contact_pose_factor( - PoseKey(cp.link->id(), k), opt_.cp_cost_model, cp.point, gravity); + PoseKey(cp.link->id(), k), opt_.cp_cost_model, cp.point, gravity_); graph.add(contact_pose_factor); } } @@ -229,7 +221,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::qFactors( gtsam::NonlinearFactorGraph DynamicsGraph::vFactors( const Robot &robot, const int t, - const boost::optional &contact_points) const { + const std::optional &contact_points) const { NonlinearFactorGraph graph; for (auto &&link : robot.links()) if (link->isFixed()) @@ -254,7 +246,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::vFactors( gtsam::NonlinearFactorGraph DynamicsGraph::aFactors( const Robot &robot, const int t, - const boost::optional &contact_points) const { + const std::optional &contact_points) const { NonlinearFactorGraph graph; for (auto &&link : robot.links()) if (link->isFixed()) @@ -279,17 +271,10 @@ gtsam::NonlinearFactorGraph DynamicsGraph::aFactors( // TODO(frank): migrate to Dynamics::graph gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors( const Robot &robot, const int k, - const boost::optional &contact_points, - const boost::optional &mu) const { + const std::optional &contact_points, + const std::optional &mu) const { NonlinearFactorGraph graph; - // TODO(frank): whoever write this should clean up this mess. - gtsam::Vector3 gravity; - if (gravity_) - gravity = *gravity_; - else - gravity = gtsam::Vector3(0, 0, -9.8); - double mu_; // Static friction coefficient. if (mu) mu_ = *mu; @@ -300,7 +285,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors( int i = link->id(); if (!link->isFixed()) { const auto &connected_joints = link->joints(); - std::vector wrench_keys; + std::vector wrench_keys; // Add wrench keys for joints. for (auto &&joint : connected_joints) @@ -317,7 +302,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors( // Add contact dynamics constraints. graph.emplace_shared( PoseKey(i, k), wrench_key, opt_.cfriction_cost_model, mu_, - gravity); + gravity_); graph.emplace_shared( wrench_key, opt_.cm_cost_model, @@ -327,7 +312,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors( // add wrench factor for link graph.add( - WrenchFactor(opt_.fa_cost_model, link, wrench_keys, k, gravity)); + WrenchFactor(opt_.fa_cost_model, link, wrench_keys, k, gravity_)); } } @@ -338,17 +323,18 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors( auto const_joint = joint; graph.add(WrenchEquivalenceFactor(opt_.f_cost_model, const_joint, k)); graph.add(TorqueFactor(opt_.t_cost_model, const_joint, k)); - if (planar_axis_) + if (planar_axis_) { graph.add(WrenchPlanarFactor(opt_.planar_cost_model, *planar_axis_, const_joint, k)); + } } return graph; } gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactorGraph( const Robot &robot, const int t, - const boost::optional &contact_points, - const boost::optional &mu) const { + const std::optional &contact_points, + const std::optional &mu) const { NonlinearFactorGraph graph; graph.add(qFactors(robot, t, contact_points)); graph.add(vFactors(robot, t, contact_points)); @@ -360,8 +346,8 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactorGraph( gtsam::NonlinearFactorGraph DynamicsGraph::trajectoryFG( const Robot &robot, const int num_steps, const double dt, const CollocationScheme collocation, - const boost::optional &contact_points, - const boost::optional &mu) const { + const std::optional &contact_points, + const std::optional &mu) const { NonlinearFactorGraph graph; for (int t = 0; t < num_steps + 1; t++) { graph.add(dynamicsFactorGraph(robot, t, contact_points, mu)); @@ -376,16 +362,16 @@ gtsam::NonlinearFactorGraph DynamicsGraph::multiPhaseTrajectoryFG( const Robot &robot, const std::vector &phase_steps, const std::vector &transition_graphs, const CollocationScheme collocation, - const boost::optional> &phase_contact_points, - const boost::optional &mu) const { + const std::optional> &phase_contact_points, + const std::optional &mu) const { NonlinearFactorGraph graph; int num_phases = phase_steps.size(); // Return either PointOnLinks or None if none specified for phase p auto contact_points = - [&phase_contact_points](int p) -> boost::optional { + [&phase_contact_points](int p) -> std::optional { if (phase_contact_points) return (*phase_contact_points)[p]; - return boost::none; + return {}; }; // First slice, k==0 @@ -449,9 +435,10 @@ double multDouble(const double &d1, const double &d2, return d1 * d2; } -void DynamicsGraph::addMultiPhaseCollocationFactorDouble( - NonlinearFactorGraph *graph, const Key x0_key, const Key x1_key, - const Key v0_key, const Key v1_key, const Key phase_key, +std::shared_ptr> +DynamicsGraph::collocationFactorDouble( + const gtsam::Key x0_key, const gtsam::Key x1_key, const gtsam::Key v0_key, + const gtsam::Key v1_key, const gtsam::Key phase_key, const gtsam::noiseModel::Base::shared_ptr &cost_model, const CollocationScheme collocation) { Double_ phase_expr(phase_key); @@ -462,17 +449,47 @@ void DynamicsGraph::addMultiPhaseCollocationFactorDouble( Double_ v0dt(multDouble, phase_expr, v0_expr); if (collocation == CollocationScheme::Euler) { - graph->add(ExpressionFactor(cost_model, 0.0, x0_expr + v0dt - x1_expr)); + return std::make_shared>(cost_model, 0.0, + x0_expr + v0dt - x1_expr); } else if (collocation == CollocationScheme::Trapezoidal) { Double_ v1dt(multDouble, phase_expr, v1_expr); - graph->add(ExpressionFactor(cost_model, 0.0, - x0_expr + 0.5 * v0dt + 0.5 * v1dt - x1_expr)); + return std::make_shared>( + cost_model, 0.0, x0_expr + 0.5 * v0dt + 0.5 * v1dt - x1_expr); } else { throw std::runtime_error( "runge-kutta and hermite-simpson not implemented yet"); } } +std::shared_ptr> +DynamicsGraph::multiPhaseJointCollocationQFactor(const size_t j, const size_t k, + const gtsam::Key phase_key, + const gtsam::noiseModel::Base::shared_ptr &cost_model, + const CollocationScheme collocation) { + return collocationFactorDouble(JointAngleKey(j, k), JointAngleKey(j, k + 1), + JointVelKey(j, k), JointVelKey(j, k + 1), + phase_key, cost_model, collocation); +} + +std::shared_ptr> +DynamicsGraph::multiPhaseJointCollocationVFactor( + const size_t j, const size_t k, const gtsam::Key phase_key, + const gtsam::noiseModel::Base::shared_ptr &cost_model, + const CollocationScheme collocation) { + return collocationFactorDouble(JointVelKey(j, k), JointVelKey(j, k + 1), + JointAccelKey(j, k), JointAccelKey(j, k + 1), + phase_key, cost_model, collocation); +} + +void DynamicsGraph::addMultiPhaseCollocationFactorDouble( + NonlinearFactorGraph *graph, const Key x0_key, const Key x1_key, + const Key v0_key, const Key v1_key, const Key phase_key, + const gtsam::noiseModel::Base::shared_ptr &cost_model, + const CollocationScheme collocation) { + graph->add(collocationFactorDouble(x0_key, x1_key, v0_key, v1_key, phase_key, + cost_model, collocation)); +} + gtsam::NonlinearFactorGraph DynamicsGraph::jointCollocationFactors( const int j, const int t, const double dt, const CollocationScheme collocation) const { diff --git a/gtdynamics/dynamics/DynamicsGraph.h b/gtdynamics/dynamics/DynamicsGraph.h index 4ece11dc0..e7e2b07b1 100644 --- a/gtdynamics/dynamics/DynamicsGraph.h +++ b/gtdynamics/dynamics/DynamicsGraph.h @@ -13,39 +13,23 @@ #pragma once +#include +#include +#include #include #include #include -#include #include #include +#include #include #include -#include "gtdynamics/dynamics/OptimizerSetting.h" -#include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/utils/PointOnLink.h" - namespace gtdynamics { using JointValueMap = std::map; -/// Shorthand for C_i_c_k, for contact wrench c on i-th link at time step k. -inline DynamicsSymbol ContactWrenchKey(int i, int c, int k = 0) { - return DynamicsSymbol::LinkJointSymbol("C", i, c, k); -} - -/* Shorthand for dt_k, for duration for timestep dt_k during phase k. */ -inline DynamicsSymbol PhaseKey(int k) { - return DynamicsSymbol::SimpleSymbol("dt", k); -} - -/* Shorthand for t_k, time at time step k. */ -inline DynamicsSymbol TimeKey(int k) { - return DynamicsSymbol::SimpleSymbol("t", k); -} - /** Collocation methods. */ enum CollocationScheme { Euler, RungeKutta, Trapezoidal, HermiteSimpson }; @@ -56,7 +40,8 @@ enum CollocationScheme { Euler, RungeKutta, Trapezoidal, HermiteSimpson }; class DynamicsGraph { private: OptimizerSetting opt_; - boost::optional gravity_, planar_axis_; + const gtsam::Vector3 gravity_; + std::optional planar_axis_; public: /** @@ -64,9 +49,8 @@ class DynamicsGraph { * @param gravity gravity in world frame * @param planar_axis axis of the plane, used only for planar robot */ - DynamicsGraph( - const boost::optional &gravity = boost::none, - const boost::optional &planar_axis = boost::none) + DynamicsGraph(const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, -9.81), + const std::optional &planar_axis = {}) : opt_(OptimizerSetting()), gravity_(gravity), planar_axis_(planar_axis) {} @@ -77,10 +61,9 @@ class DynamicsGraph { * @param gravity gravity in world frame * @param planar_axis axis of the plane, used only for planar robot */ - DynamicsGraph( - const OptimizerSetting &opt, - const boost::optional &gravity = boost::none, - const boost::optional &planar_axis = boost::none) + DynamicsGraph(const OptimizerSetting &opt, + const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, -9.8), + const std::optional &planar_axis = {}) : opt_(opt), gravity_(gravity), planar_axis_(planar_axis) {} ~DynamicsGraph() {} @@ -92,7 +75,7 @@ class DynamicsGraph { * @param known_values Values with kinematics, must include poses and twists */ gtsam::GaussianFactorGraph linearDynamicsGraph( - const Robot &robot, const int t, const gtsam::Values &known_values); + const Robot &robot, const int t, const gtsam::Values &known_values) const; /// Return linear factor graph with priors on torques. static gtsam::GaussianFactorGraph linearFDPriors( @@ -113,7 +96,7 @@ class DynamicsGraph { * joint torques, and link twist accelerations */ gtsam::Values linearSolveFD(const Robot &robot, const int t, - const gtsam::Values &known_values); + const gtsam::Values &known_values) const; /** * Solve inverse kinodynamics using linear factor graph, Values version. @@ -127,25 +110,25 @@ class DynamicsGraph { const gtsam::Values &known_values); /// Return q-level nonlinear factor graph (pose related factors) - gtsam::NonlinearFactorGraph qFactors( + virtual gtsam::NonlinearFactorGraph qFactors( const Robot &robot, const int t, - const boost::optional &contact_points = boost::none) const; + const std::optional &contact_points = {}) const; /// Return v-level nonlinear factor graph (twist related factors) gtsam::NonlinearFactorGraph vFactors( const Robot &robot, const int t, - const boost::optional &contact_points = boost::none) const; + const std::optional &contact_points = {}) const; /// Return a-level nonlinear factor graph (acceleration related factors) gtsam::NonlinearFactorGraph aFactors( const Robot &robot, const int t, - const boost::optional &contact_points = boost::none) const; + const std::optional &contact_points = {}) const; /// Return dynamics-level nonlinear factor graph (wrench related factors) - gtsam::NonlinearFactorGraph dynamicsFactors( + virtual gtsam::NonlinearFactorGraph dynamicsFactors( const Robot &robot, const int t, - const boost::optional &contact_points = boost::none, - const boost::optional &mu = boost::none) const; + const std::optional &contact_points = {}, + const std::optional &mu = {}) const; /** * Return nonlinear factor graph of all dynamics factors @@ -155,10 +138,10 @@ class DynamicsGraph { * @param contact_points optional vector of contact points. * @param mu optional coefficient of static friction. */ - gtsam::NonlinearFactorGraph dynamicsFactorGraph( + virtual gtsam::NonlinearFactorGraph dynamicsFactorGraph( const Robot &robot, const int t, - const boost::optional &contact_points = boost::none, - const boost::optional &mu = boost::none) const; + const std::optional &contact_points = {}, + const std::optional &mu = {}) const; /** * Return prior factors of torque, angle, velocity @@ -198,8 +181,8 @@ class DynamicsGraph { gtsam::NonlinearFactorGraph trajectoryFG( const Robot &robot, const int num_steps, const double dt, const CollocationScheme collocation = Trapezoidal, - const boost::optional &contact_points = boost::none, - const boost::optional &mu = boost::none) const; + const std::optional &contact_points = {}, + const std::optional &mu = {}) const; /** * Return nonlinear factor graph of the entire trajectory for multi-phase @@ -214,9 +197,27 @@ class DynamicsGraph { const Robot &robot, const std::vector &phase_steps, const std::vector &transition_graphs, const CollocationScheme collocation = Trapezoidal, - const boost::optional> &phase_contact_points = - boost::none, - const boost::optional &mu = boost::none) const; + const std::optional> &phase_contact_points = {}, + const std::optional &mu = {}) const; + + static std::shared_ptr> + collocationFactorDouble(const gtsam::Key x0_key, const gtsam::Key x1_key, + const gtsam::Key v0_key, const gtsam::Key v1_key, + const gtsam::Key phase_key, + const gtsam::noiseModel::Base::shared_ptr &cost_model, + const CollocationScheme collocation = Trapezoidal); + + static std::shared_ptr> + multiPhaseJointCollocationQFactor(const size_t j, const size_t k, + const gtsam::Key phase_key, + const gtsam::noiseModel::Base::shared_ptr &cost_model, + const CollocationScheme collocation = Trapezoidal); + + static std::shared_ptr> + multiPhaseJointCollocationVFactor(const size_t j, const size_t k, + const gtsam::Key phase_key, + const gtsam::noiseModel::Base::shared_ptr &cost_model, + const CollocationScheme collocation = Trapezoidal); /** Add collocation factor for doubles. */ static void addCollocationFactorDouble( @@ -308,6 +309,8 @@ class DynamicsGraph { const Robot &robot, const int t, const std::string &link_name, const gtsam::Pose3 &target_pose) const; + const gtsam::Vector3& gravity() const { return gravity_; } + /** * Return the joint accelerations * @param robot the robot diff --git a/gtdynamics/dynamics/OptimizerSetting.cpp b/gtdynamics/dynamics/OptimizerSetting.cpp index f8c8e8d71..e1ce005ea 100644 --- a/gtdynamics/dynamics/OptimizerSetting.cpp +++ b/gtdynamics/dynamics/OptimizerSetting.cpp @@ -11,7 +11,7 @@ * @author Mandy Xie */ -#include "gtdynamics/dynamics/OptimizerSetting.h" +#include namespace gtdynamics { diff --git a/gtdynamics/dynamics/OptimizerSetting.h b/gtdynamics/dynamics/OptimizerSetting.h index 8e03e28d5..e146833bd 100644 --- a/gtdynamics/dynamics/OptimizerSetting.h +++ b/gtdynamics/dynamics/OptimizerSetting.h @@ -56,20 +56,20 @@ class OptimizerSetting { jl_cost_model; // joint limit factor /// optimization settings - IterationType opt_type; // optimizer type - VerbosityLevel opt_verbosity; // optimizer print out - double rel_thresh; // relative error decrease threshold for stopping - // optimization - int max_iter; // max iteration for stopping optimization + IterationType opt_type = GaussNewton; // optimizer type + VerbosityLevel opt_verbosity = None; // optimizer print out + double rel_thresh = 1e-2; // relative error decrease threshold for stopping + // optimization + int max_iter = 50; // max iteration for stopping optimization /// collision checking setting - double epsilon; // obstacle clearance - double obsSigma; // obstacle cost model covariance + double epsilon = 0.0; // obstacle clearance + double obsSigma = 0.001; // obstacle cost model covariance /// default constructor OptimizerSetting(); - //TODO: Make this accept an object like OptimizerParams. + // TODO: Make this accept an object like OptimizerParams. /** * Constructor which accepts various noise sigma values. * diff --git a/gtdynamics/dynamics/Simulator.h b/gtdynamics/dynamics/Simulator.h index 4fe5a8d1c..ae9b5c605 100644 --- a/gtdynamics/dynamics/Simulator.h +++ b/gtdynamics/dynamics/Simulator.h @@ -13,33 +13,30 @@ #pragma once +#include +#include +#include #include -#include +#include #include #include -#include "gtdynamics/dynamics/DynamicsGraph.h" -#include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/utils/values.h" - namespace gtdynamics { /** * Simulator is a class which simulate robot arm motion using forward * dynamics. */ class Simulator { -private: + private: Robot robot_; int t_; DynamicsGraph graph_builder_; gtsam::Values initial_values_; - boost::optional gravity_; - boost::optional planar_axis_; gtsam::Values current_values_; gtsam::Values new_kinematics_; -public: + public: /** * Constructor * @@ -50,9 +47,10 @@ class Simulator { * @param planar_axis planar axis vector */ Simulator(const Robot &robot, const gtsam::Values &initial_values, - const boost::optional &gravity = boost::none, - const boost::optional &planar_axis = boost::none) - : robot_(robot), t_(0), + const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, 9.81), + const std::optional &planar_axis = {}) + : robot_(robot), + t_(0), graph_builder_(DynamicsGraph(gravity, planar_axis)), initial_values_(initial_values) { reset(); @@ -129,4 +127,4 @@ class Simulator { const gtsam::Values &getValues() const { return current_values_; } }; -} // namespace gtdynamics +} // namespace gtdynamics diff --git a/gtdynamics/factors/BiasedFactor.h b/gtdynamics/factors/BiasedFactor.h new file mode 100644 index 000000000..ce6e7a6b0 --- /dev/null +++ b/gtdynamics/factors/BiasedFactor.h @@ -0,0 +1,92 @@ +#pragma once + +#include +#include +#include + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#endif + +namespace gtdynamics { + +using gtsam::NoiseModelFactor; +using gtsam::SharedNoiseModel; +using gtsam::Values; +using gtsam::Vector; + +/** A factor that adds a constant bias term to the original factor. + * This factor is used in augmented Lagrangian optimizer to create biased cost + * functions. + * Note that the noise model is stored twice (both in base factor and the + * noisemodel of substitute factor. The noisemodel in the base factor will be + * ignored. */ +class BiasedFactor : public NoiseModelFactor { + protected: + typedef NoiseModelFactor Base; + typedef BiasedFactor This; + + // original factor + Base::shared_ptr base_factor_; + Vector bias_; + + public: + typedef std::shared_ptr shared_ptr; + + /** Default constructor for I/O only */ + BiasedFactor() {} + + /** Destructor */ + ~BiasedFactor() override {} + + /** + * Constructor + * @param base_factor original factor on X + * @param bias the bias term + */ + BiasedFactor(const Base::shared_ptr &base_factor, const Vector &bias) + : Base(base_factor->noiseModel(), base_factor->keys()), + base_factor_(base_factor), + bias_(bias) {} + + BiasedFactor(const Base::shared_ptr &base_factor, const Vector &bias, + const SharedNoiseModel &noise_model) + : Base(noise_model, base_factor->keys()), + base_factor_(base_factor), + bias_(bias) {} + + /** + * Error function *without* the NoiseModel, \f$ z-h(x) \f$. + * Override this method to finish implementing an N-way factor. + * If the optional arguments is specified, it should compute + * both the function evaluation and its derivative(s) in H. + */ + virtual Vector unwhitenedError( + const Values &x, + gtsam::OptionalMatrixVecType H = nullptr) const override { + return base_factor_->unwhitenedError(x, H) + bias_; + } + + /** Return a deep copy of this factor. */ + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &boost::serialization::make_nvp( + "BiasedFactor", boost::serialization::base_object(*this)); + ar &BOOST_SERIALIZATION_NVP(base_factor_); + ar &BOOST_SERIALIZATION_NVP(bias_); + } +#endif + +}; // \class BiasedFactor + +} // namespace gtdynamics diff --git a/gtdynamics/factors/CollocationFactors.h b/gtdynamics/factors/CollocationFactors.h index a49c708c0..ab4553b9b 100644 --- a/gtdynamics/factors/CollocationFactors.h +++ b/gtdynamics/factors/CollocationFactors.h @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include @@ -30,10 +30,10 @@ namespace gtdynamics { * * @return pose_t1 link pose at next time step */ -gtsam::Pose3 predictPose( - const gtsam::Pose3 &pose_t0, const gtsam::Vector6 &twistdt, - gtsam::OptionalJacobian<6, 6> H_pose_t0 = boost::none, - gtsam::OptionalJacobian<6, 6> H_twistdt = boost::none) { +inline gtsam::Pose3 predictPose(const gtsam::Pose3 &pose_t0, + const gtsam::Vector6 &twistdt, + gtsam::OptionalJacobian<6, 6> H_pose_t0 = {}, + gtsam::OptionalJacobian<6, 6> H_twistdt = {}) { gtsam::Matrix6 Hexp; gtsam::Pose3 t1Tt0 = gtsam::Pose3::Expmap(twistdt, Hexp); @@ -47,21 +47,21 @@ gtsam::Pose3 predictPose( } /** - * EulerPoseCollocationFactor is a four-way nonlinear factor between link pose of - * current and next time steps + * EulerPoseCollocationFactor is a four-way nonlinear factor between link pose + * of current and next time steps */ class EulerPoseCollocationFactor - : public gtsam::NoiseModelFactor4 { private: using This = EulerPoseCollocationFactor; - using Base = gtsam::NoiseModelFactor4; public: - EulerPoseCollocationFactor(gtsam::Key pose_t0_key, gtsam::Key pose_t1_key, - gtsam::Key twist_key, gtsam::Key dt_key, - const gtsam::noiseModel::Base::shared_ptr &cost_model) + EulerPoseCollocationFactor( + gtsam::Key pose_t0_key, gtsam::Key pose_t1_key, gtsam::Key twist_key, + gtsam::Key dt_key, const gtsam::noiseModel::Base::shared_ptr &cost_model) : Base(cost_model, pose_t0_key, pose_t1_key, twist_key, dt_key) {} virtual ~EulerPoseCollocationFactor() {} @@ -76,10 +76,10 @@ class EulerPoseCollocationFactor gtsam::Vector evaluateError( const gtsam::Pose3 &pose_t0, const gtsam::Pose3 &pose_t1, const gtsam::Vector6 &twist, const double &dt, - boost::optional H_pose_t0 = boost::none, - boost::optional H_pose_t1 = boost::none, - boost::optional H_twist = boost::none, - boost::optional H_dt = boost::none) const override { + gtsam::OptionalMatrixType H_pose_t0 = nullptr, + gtsam::OptionalMatrixType H_pose_t1 = nullptr, + gtsam::OptionalMatrixType H_twist = nullptr, + gtsam::OptionalMatrixType H_dt = nullptr) const override { gtsam::Vector6 twistdt = twist * dt; gtsam::Matrix6 H_twistdt; auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); @@ -96,9 +96,9 @@ class EulerPoseCollocationFactor return error; } - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -111,25 +111,27 @@ class EulerPoseCollocationFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE const &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor4", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; /** - * TrapezoidalPoseCollocationFactor is a five-way nonlinear factor between link pose - * of current and next time steps + * TrapezoidalPoseCollocationFactor is a five-way nonlinear factor between link + * pose of current and next time steps */ class TrapezoidalPoseCollocationFactor - : public gtsam::NoiseModelFactor5 { private: using This = TrapezoidalPoseCollocationFactor; - using Base = gtsam::NoiseModelFactor5; public: @@ -154,12 +156,11 @@ class TrapezoidalPoseCollocationFactor gtsam::Vector evaluateError( const gtsam::Pose3 &pose_t0, const gtsam::Pose3 &pose_t1, const gtsam::Vector6 &twist_t0, const gtsam::Vector6 &twist_t1, - const double &dt, - boost::optional H_pose_t0 = boost::none, - boost::optional H_pose_t1 = boost::none, - boost::optional H_twist_t0 = boost::none, - boost::optional H_twist_t1 = boost::none, - boost::optional H_dt = boost::none) const override { + const double &dt, gtsam::OptionalMatrixType H_pose_t0 = nullptr, + gtsam::OptionalMatrixType H_pose_t1 = nullptr, + gtsam::OptionalMatrixType H_twist_t0 = nullptr, + gtsam::OptionalMatrixType H_twist_t1 = nullptr, + gtsam::OptionalMatrixType H_dt = nullptr) const override { gtsam::Vector6 twistdt = 0.5 * dt * (twist_t0 + twist_t1); gtsam::Matrix6 H_twistdt; auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); @@ -179,9 +180,167 @@ class TrapezoidalPoseCollocationFactor return error; } + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print contents + void print(const std::string &s = "", + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "trapezoidal collocation factor" << std::endl; + Base::print("", keyFormatter); + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE const &ar, const unsigned int version) { + ar &boost::serialization::make_nvp( + "NoiseModelFactorN", boost::serialization::base_object(*this)); + } +#endif +}; + + +/** + * FixTimeEulerPoseCollocationFactor imposes collocation between link + * poses of current and next time steps with fixed dt. + */ +class FixTimeEulerPoseCollocationFactor + : public gtsam::NoiseModelFactor3 { + private: + using This = FixTimeEulerPoseCollocationFactor; + using Base = gtsam::NoiseModelFactor3; + double dt_; + + public: + FixTimeEulerPoseCollocationFactor( + gtsam::Key pose_t0_key, gtsam::Key pose_t1_key, gtsam::Key twist_t0_key, + double dt, const gtsam::noiseModel::Base::shared_ptr &cost_model) + : Base(cost_model, pose_t0_key, pose_t1_key, twist_t0_key), dt_(dt) {} + + virtual ~FixTimeEulerPoseCollocationFactor() {} + + /** + * Evaluate link pose errors + + * @param pose_t0 link pose of current step + * @param pose_t1 link pose of next step + * @param twist_t0 link twist of current step + * @param twist_t1 link twist of next step + * @param dt duration of time step + */ + gtsam::Vector evaluateError( + const gtsam::Pose3 &pose_t0, const gtsam::Pose3 &pose_t1, + const gtsam::Vector6 &twist_t0, + gtsam::OptionalMatrixType H_pose_t0 = nullptr, + gtsam::OptionalMatrixType H_pose_t1 = nullptr, + gtsam::OptionalMatrixType H_twist_t0 = nullptr) const override { + gtsam::Vector6 twistdt = dt_ * twist_t0; + gtsam::Matrix6 H_twistdt; + auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); + gtsam::Vector6 error = pose_t1.logmap(pose_t1_hat); + if (H_pose_t1) { + *H_pose_t1 = -gtsam::I_6x6; + } + if (H_twist_t0) { + *H_twist_t0 = dt_ * H_twistdt; + } + return error; + } + //// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print contents + void print(const std::string &s = "", + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "Euler pose collocation factor" << std::endl; + Base::print("", keyFormatter); + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE const &ar, const unsigned int version) { + ar &boost::serialization::make_nvp( + "NoiseModelFactor3", boost::serialization::base_object(*this)); + } +#endif +}; + + +/** + * FixTimeTrapezoidalPoseCollocationFactor imposes collocation between link + * poses of current and next time steps with fixed dt. + */ +class FixTimeTrapezoidalPoseCollocationFactor + : public gtsam::NoiseModelFactorN { + private: + using This = FixTimeTrapezoidalPoseCollocationFactor; + using Base = gtsam::NoiseModelFactorN; + double dt_; + + public: + FixTimeTrapezoidalPoseCollocationFactor( + gtsam::Key pose_t0_key, gtsam::Key pose_t1_key, gtsam::Key twist_t0_key, + gtsam::Key twist_t1_key, double dt, + const gtsam::noiseModel::Base::shared_ptr &cost_model) + : Base(cost_model, pose_t0_key, pose_t1_key, twist_t0_key, twist_t1_key), + dt_(dt) {} + + virtual ~FixTimeTrapezoidalPoseCollocationFactor() {} + + /** + * Evaluate link pose errors + + * @param pose_t0 link pose of current step + * @param pose_t1 link pose of next step + * @param twist_t0 link twist of current step + * @param twist_t1 link twist of next step + * @param dt duration of time step + */ + gtsam::Vector evaluateError( + const gtsam::Pose3 &pose_t0, const gtsam::Pose3 &pose_t1, + const gtsam::Vector6 &twist_t0, const gtsam::Vector6 &twist_t1, + gtsam::OptionalMatrixType H_pose_t0 = nullptr, + gtsam::OptionalMatrixType H_pose_t1 = nullptr, + gtsam::OptionalMatrixType H_twist_t0 = nullptr, + gtsam::OptionalMatrixType H_twist_t1 = nullptr) const override { + gtsam::Vector6 twistdt = 0.5 * dt_ * (twist_t0 + twist_t1); + gtsam::Matrix6 H_twistdt; + auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); + gtsam::Vector6 error = pose_t1.logmap(pose_t1_hat); + if (H_pose_t1) { + *H_pose_t1 = -gtsam::I_6x6; + } + if (H_twist_t0) { + *H_twist_t0 = 0.5 * dt_ * H_twistdt; + } + if (H_twist_t1) { + *H_twist_t1 = 0.5 * dt_ * H_twistdt; + } + return error; + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -194,31 +353,33 @@ class TrapezoidalPoseCollocationFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE const &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor5", boost::serialization::base_object(*this)); + "NoiseModelFactor4", boost::serialization::base_object(*this)); } +#endif }; /** - * EulerTwistCollocationFactor is a four-way nonlinear factor between link twist of - * current and next time steps + * EulerTwistCollocationFactor is a four-way nonlinear factor between link twist + * of current and next time steps */ class EulerTwistCollocationFactor - : public gtsam::NoiseModelFactor4 { private: using This = EulerTwistCollocationFactor; - using Base = gtsam::NoiseModelFactor4; public: - EulerTwistCollocationFactor(gtsam::Key twist_t0_key, gtsam::Key twist_t1_key, - gtsam::Key accel_key, gtsam::Key dt_key, - const gtsam::noiseModel::Base::shared_ptr &cost_model) + EulerTwistCollocationFactor( + gtsam::Key twist_t0_key, gtsam::Key twist_t1_key, gtsam::Key accel_key, + gtsam::Key dt_key, const gtsam::noiseModel::Base::shared_ptr &cost_model) : Base(cost_model, twist_t0_key, twist_t1_key, accel_key, dt_key) {} virtual ~EulerTwistCollocationFactor() {} @@ -234,10 +395,10 @@ class EulerTwistCollocationFactor gtsam::Vector evaluateError( const gtsam::Vector6 &twist_t0, const gtsam::Vector6 &twist_t1, const gtsam::Vector6 &accel, const double &dt, - boost::optional H_twist_t0 = boost::none, - boost::optional H_twist_t1 = boost::none, - boost::optional H_accel = boost::none, - boost::optional H_dt = boost::none) const override { + gtsam::OptionalMatrixType H_twist_t0 = nullptr, + gtsam::OptionalMatrixType H_twist_t1 = nullptr, + gtsam::OptionalMatrixType H_accel = nullptr, + gtsam::OptionalMatrixType H_dt = nullptr) const override { gtsam::Vector6 error = twist_t0 + dt * accel - twist_t1; if (H_twist_t1) { *H_twist_t1 = -gtsam::I_6x6; @@ -254,9 +415,9 @@ class EulerTwistCollocationFactor return error; } - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -269,13 +430,15 @@ class EulerTwistCollocationFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE const &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor4", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; /** @@ -283,11 +446,11 @@ class EulerTwistCollocationFactor * twist of current and next time steps */ class TrapezoidalTwistCollocationFactor - : public gtsam::NoiseModelFactor5 { private: using This = TrapezoidalTwistCollocationFactor; - using Base = gtsam::NoiseModelFactor5; public: @@ -312,12 +475,11 @@ class TrapezoidalTwistCollocationFactor gtsam::Vector evaluateError( const gtsam::Vector6 &twist_t0, const gtsam::Vector6 &twist_t1, const gtsam::Vector6 &accel_t0, const gtsam::Vector6 &accel_t1, - const double &dt, - boost::optional H_twist_t0 = boost::none, - boost::optional H_twist_t1 = boost::none, - boost::optional H_accel_t0 = boost::none, - boost::optional H_accel_t1 = boost::none, - boost::optional H_dt = boost::none) const override { + const double &dt, gtsam::OptionalMatrixType H_twist_t0 = nullptr, + gtsam::OptionalMatrixType H_twist_t1 = nullptr, + gtsam::OptionalMatrixType H_accel_t0 = nullptr, + gtsam::OptionalMatrixType H_accel_t1 = nullptr, + gtsam::OptionalMatrixType H_dt = nullptr) const override { gtsam::Vector6 error = twist_t0 + 0.5 * dt * (accel_t0 + accel_t1) - twist_t1; if (H_twist_t1) { @@ -338,9 +500,167 @@ class TrapezoidalTwistCollocationFactor return error; } + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print contents + void print(const std::string &s = "", + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "trapezoidal twist collocation factor" << std::endl; + Base::print("", keyFormatter); + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE const &ar, const unsigned int version) { + ar &boost::serialization::make_nvp( + "NoiseModelFactorN", boost::serialization::base_object(*this)); + } +#endif +}; + + +/** + * FixTimeEulerTwistCollocationFactor is a three-way nonlinear factor + * between link twist of current and next time steps + */ +class FixTimeEulerTwistCollocationFactor + : public gtsam::NoiseModelFactor3 { + private: + using This = FixTimeEulerTwistCollocationFactor; + using Base = gtsam::NoiseModelFactor3; + double dt_; + + public: + FixTimeEulerTwistCollocationFactor( + gtsam::Key twist_t0_key, gtsam::Key twist_t1_key, + gtsam::Key accel_t0_key, double dt, + const gtsam::noiseModel::Base::shared_ptr &cost_model) + : Base(cost_model, twist_t0_key, twist_t1_key, accel_t0_key), dt_(dt) {} + + virtual ~FixTimeEulerTwistCollocationFactor() {} + + /** + * Evaluate link twist errors + * + * @param twist_t0 link twist of current step + * @param twist_t1 link twist of next step + * @param accel_t0 link twist acceleration of current step + */ + gtsam::Vector evaluateError( + const gtsam::Vector6 &twist_t0, const gtsam::Vector6 &twist_t1, + const gtsam::Vector6 &accel_t0, + gtsam::OptionalMatrixType H_twist_t0 = nullptr, + gtsam::OptionalMatrixType H_twist_t1 = nullptr, + gtsam::OptionalMatrixType H_accel_t0 = nullptr) const override { + gtsam::Vector6 error = twist_t0 + dt_ * accel_t0 - twist_t1; + if (H_twist_t1) { + *H_twist_t1 = -gtsam::I_6x6; + } + if (H_twist_t0) { + *H_twist_t0 = gtsam::I_6x6; + } + if (H_accel_t0) { + *H_accel_t0 = dt_ * gtsam::I_6x6; + } + return error; + } + //// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print contents + void print(const std::string &s = "", + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "Euler twist collocation factor" << std::endl; + Base::print("", keyFormatter); + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE const &ar, const unsigned int version) { + ar &boost::serialization::make_nvp( + "NoiseModelFactor3", boost::serialization::base_object(*this)); + } +#endif +}; + + +/** + * FixTimeTrapezoidalTwistCollocationFactor is a four-way nonlinear factor + * between link twist of current and next time steps + */ +class FixTimeTrapezoidalTwistCollocationFactor + : public gtsam::NoiseModelFactorN { + private: + using This = FixTimeTrapezoidalTwistCollocationFactor; + using Base = gtsam::NoiseModelFactorN; + double dt_; + + public: + FixTimeTrapezoidalTwistCollocationFactor( + gtsam::Key twist_t0_key, gtsam::Key twist_t1_key, gtsam::Key accel_t0_key, + gtsam::Key accel_t1_key, double dt, + const gtsam::noiseModel::Base::shared_ptr &cost_model) + : Base(cost_model, twist_t0_key, twist_t1_key, accel_t0_key, + accel_t1_key), + dt_(dt) {} + + virtual ~FixTimeTrapezoidalTwistCollocationFactor() {} + + /** + * Evaluate link twist errors + * + * @param twist_t0 link twist of current step + * @param twist_t1 link twist of next step + * @param accel_t0 link twist acceleration of current step + * @param accel_t1 link twist acceleration of next step + */ + gtsam::Vector evaluateError( + const gtsam::Vector6 &twist_t0, const gtsam::Vector6 &twist_t1, + const gtsam::Vector6 &accel_t0, const gtsam::Vector6 &accel_t1, + gtsam::OptionalMatrixType H_twist_t0 = nullptr, + gtsam::OptionalMatrixType H_twist_t1 = nullptr, + gtsam::OptionalMatrixType H_accel_t0 = nullptr, + gtsam::OptionalMatrixType H_accel_t1 = nullptr) const override { + gtsam::Vector6 error = + twist_t0 + 0.5 * dt_ * (accel_t0 + accel_t1) - twist_t1; + if (H_twist_t1) { + *H_twist_t1 = -gtsam::I_6x6; + } + if (H_twist_t0) { + *H_twist_t0 = gtsam::I_6x6; + } + if (H_accel_t0) { + *H_accel_t0 = 0.5 * dt_ * gtsam::I_6x6; + } + if (H_accel_t1) { + *H_accel_t1 = 0.5 * dt_ * gtsam::I_6x6; + } + return error; + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -353,13 +673,15 @@ class TrapezoidalTwistCollocationFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE const &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor5", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/factors/ConstVarFactor.cpp b/gtdynamics/factors/ConstVarFactor.cpp new file mode 100644 index 000000000..02a8415c0 --- /dev/null +++ b/gtdynamics/factors/ConstVarFactor.cpp @@ -0,0 +1,123 @@ +#include +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +KeyVector ConstVarFactor::computeNewKeys(const Base::shared_ptr& base_factor, + const KeySet& fixed_keys) { + KeyVector new_keys; + for (const Key& key : base_factor->keys()) { + if (!fixed_keys.exists(key)) { + new_keys.push_back(key); + } + } + return new_keys; +} + +/* ************************************************************************* */ +KeySet ConstVarFactor::computeFixedKeys(const Base::shared_ptr& base_factor, + const KeySet& fixed_keys) { + KeySet fixed_keys_in_factor; + for (const Key& key : base_factor->keys()) { + if (fixed_keys.exists(key)) { + fixed_keys_in_factor.insert(key); + } + } + return fixed_keys_in_factor; +} + +/* ************************************************************************* */ +std::vector ConstVarFactor::computeBaseKeyIndex( + const Base::shared_ptr& base_factor, const KeySet& fixed_keys) { + std::vector base_key_index; + const KeyVector& base_keys = base_factor->keys(); + for (size_t i = 0; i < base_factor->size(); i++) { + if (!fixed_keys.exists(base_keys.at(i))) { + base_key_index.emplace_back(i); + } + } + return base_key_index; +} + +/* ************************************************************************* */ +void ConstVarFactor::setFixedValues(const Values& values) { + for (const Key& key : values.keys()) { + if (fixed_keys_.exists(key)) { + if (fixed_values_.exists(key)) { + fixed_values_.update(key, values.at(key)); + } else { + fixed_values_.insert(key, values.at(key)); + } + } + } +} + +/* ************************************************************************* */ +Vector ConstVarFactor::unwhitenedError(const Values& x, + gtsam::OptionalMatrixVecType H) const { + // Construct values for base factor. + Values base_x = x; + base_x.insert(fixed_values_); + + // compute jacobian + if (H) { + // Compute Jacobian and error using base factor. + std::vector base_H(base_factor_->size()); + Vector unwhitened_error = base_factor_->unwhitenedError(base_x, base_H); + // Compute Jacobian for new variables + for (size_t variable_idx = 0; variable_idx < keys().size(); + variable_idx++) { + (*H)[variable_idx] = base_H.at(base_key_index_.at(variable_idx)); + } + return unwhitened_error; + } else { + return base_factor_->unwhitenedError(base_x); + } +} + +std::pair ConstVarGraph( + const NonlinearFactorGraph& graph, const KeySet& fixed_keys) { + ConstVarFactors const_var_factors; + NonlinearFactorGraph new_graph; + for (const auto& factor : graph) { + // check if the factor contains fixed keys + bool contain_fixed_keys = false; + for (const Key& key : factor->keys()) { + if (fixed_keys.exists(key)) { + contain_fixed_keys = true; + break; + } + } + // update the factor if it constains fixed keys + if (contain_fixed_keys) { + NoiseModelFactor::shared_ptr noise_factor = + std::dynamic_pointer_cast(factor); + auto const_var_factor = + std::make_shared(noise_factor, fixed_keys); + if (const_var_factor->checkActive()) { + new_graph.add(const_var_factor); + const_var_factors.push_back(const_var_factor); + } + } else { + new_graph.add(factor); + } + } + return std::make_pair(new_graph, const_var_factors); +} + +NonlinearFactorGraph ConstVarGraph(const NonlinearFactorGraph& graph, + const Values& fixed_values) { + ConstVarFactors const_var_factors; + NonlinearFactorGraph new_graph; + KeyVector key_vec = fixed_values.keys(); + KeySet key_set(key_vec.begin(), key_vec.end()); + std::tie(new_graph, const_var_factors) = ConstVarGraph(graph, key_set); + + for (auto& factor : const_var_factors) { + factor->setFixedValues(fixed_values); + } + return new_graph; +} + +} // namespace gtdynamics diff --git a/gtdynamics/factors/ConstVarFactor.h b/gtdynamics/factors/ConstVarFactor.h new file mode 100644 index 000000000..acb474052 --- /dev/null +++ b/gtdynamics/factors/ConstVarFactor.h @@ -0,0 +1,107 @@ +#pragma once + +#include +#include + +namespace gtdynamics { + +using gtsam::Key; +using gtsam::KeySet; +using gtsam::KeyVector; +using gtsam::NoiseModelFactor; +using gtsam::NonlinearFactorGraph; +using gtsam::Values; +using gtsam::Vector; + +/** A factor that substitute certain variables of a base factor with constraint + * manifold variables. + * It is used for constraint manifold optimization, since the variables + * connected with constraint factors will be replaced by constraint manifold + * variables. + * Note that the noise model is stored twice (both in base factor and the + * noisemodel of substitute factor. The noisemodel in the base factor will be + * ignored. */ + +class ConstVarFactor : public NoiseModelFactor { + protected: + typedef NoiseModelFactor Base; + typedef ConstVarFactor This; + Base::shared_ptr base_factor_; + KeySet fixed_keys_; + Values fixed_values_; + std::vector base_key_index_; + + public: + /** Default constructor for I/O only */ + ConstVarFactor() {} + + /** Destructor */ + ~ConstVarFactor() override {} + + /** + * Constructor + * @param base_factor original factor on X + * @param fixed_keys keys of fixed variables (may contain more variables + * apart from ones in the factor) + */ + ConstVarFactor(const Base::shared_ptr& base_factor, const KeySet& fixed_keys) + : Base(base_factor->noiseModel(), + computeNewKeys(base_factor, fixed_keys)), + base_factor_(base_factor), + fixed_keys_(computeFixedKeys(base_factor, fixed_keys)), + base_key_index_(computeBaseKeyIndex(base_factor, fixed_keys)) { + if (!checkActive()) { + return; + } + } + + /// Set values of fixed variables. + void setFixedValues(const Values& values); + + protected: + /** Compute keys for the new factor */ + static KeyVector computeNewKeys(const Base::shared_ptr& base_factor, + const KeySet& fixed_keys); + + static KeySet computeFixedKeys(const Base::shared_ptr& base_factor, + const KeySet& fixed_keys); + + static std::vector computeBaseKeyIndex( + const Base::shared_ptr& base_factor, const KeySet& fixed_keys); + + public: + /** + * Error function *without* the NoiseModel, \f$ z-h(x) \f$. + * Override this method to finish implementing an N-way factor. + * If the optional arguments is specified, it should compute + * both the function evaluation and its derivative(s) in H. + */ + virtual Vector unwhitenedError( + const Values& x, gtsam::OptionalMatrixVecType H = nullptr) const override; + + /** Return a deep copy of this factor. */ + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** Check if a variable in the base factor is fixed. */ + inline bool isFixed(const Key& key) const { return fixed_keys_.exists(key); } + + /** Check if the factor is active. Note: if all the variables of the original + * factor are fully constrained, no updates can be made.*/ + inline bool checkActive() const { return size() > 0; } + + const Values& fixedValues() const { return fixed_values_; } + +}; // \class ConstVarFactor + +typedef std::vector> ConstVarFactors; + +std::pair ConstVarGraph( + const NonlinearFactorGraph& graph, const KeySet& fixed_keys); + +NonlinearFactorGraph ConstVarGraph(const NonlinearFactorGraph& graph, + const Values& fixed_values); + +} // namespace gtdynamics diff --git a/gtdynamics/factors/ContactDynamicsFrictionConeFactor.h b/gtdynamics/factors/ContactDynamicsFrictionConeFactor.h index ba5bcf1b1..8697cdfa0 100644 --- a/gtdynamics/factors/ContactDynamicsFrictionConeFactor.h +++ b/gtdynamics/factors/ContactDynamicsFrictionConeFactor.h @@ -13,18 +13,17 @@ #pragma once +#include #include #include #include -#include +#include -#include #include +#include #include #include -#include "gtdynamics/utils/utils.h" - namespace gtdynamics { /** @@ -32,10 +31,10 @@ namespace gtdynamics { * that the linear contact force lies within a friction cone. */ class ContactDynamicsFrictionConeFactor - : public gtsam::NoiseModelFactor2 { + : public gtsam::NoiseModelFactorN { private: using This = ContactDynamicsFrictionConeFactor; - using Base = gtsam::NoiseModelFactor2; + using Base = gtsam::NoiseModelFactorN; int up_axis_; // Which axis is up (assuming flat ground)? {0: x, 1: y, 2: z}. double mu_prime_; // static friction coefficient squared. @@ -52,7 +51,7 @@ class ContactDynamicsFrictionConeFactor public: /** * Contact dynamics factor for zero moment at contact. - + * * @param pose_key Key corresponding to the link's CoM pose. * @param contact_wrench_key Key corresponding to this link's contact wrench. * @param cost_model Noise model for this factor. @@ -70,18 +69,17 @@ class ContactDynamicsFrictionConeFactor else up_axis_ = 2; // z. } + virtual ~ContactDynamicsFrictionConeFactor() {} - public: /** * Evaluate contact point moment errors. * @param contact_wrench Contact wrench on this link. */ gtsam::Vector evaluateError( const gtsam::Pose3 &pose, const gtsam::Vector6 &contact_wrench, - boost::optional H_pose = boost::none, - boost::optional H_contact_wrench = - boost::none) const override { + gtsam::OptionalMatrixType H_pose = nullptr, + gtsam::OptionalMatrixType H_contact_wrench = nullptr) const override { // Linear component of the contact wrench. gtsam::Vector3 f_c = H_wrench_ * contact_wrench; @@ -142,9 +140,9 @@ class ContactDynamicsFrictionConeFactor return error; } - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -157,13 +155,15 @@ class ContactDynamicsFrictionConeFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE const &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor1", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/factors/ContactDynamicsMomentFactor.h b/gtdynamics/factors/ContactDynamicsMomentFactor.h index 894238ae3..c618e528d 100644 --- a/gtdynamics/factors/ContactDynamicsMomentFactor.h +++ b/gtdynamics/factors/ContactDynamicsMomentFactor.h @@ -13,6 +13,7 @@ #pragma once +#include #include #include #include @@ -20,26 +21,24 @@ #include #include -#include #include +#include #include #include -#include "gtdynamics/utils/utils.h" - namespace gtdynamics { - /** - * ContactDynamicsMomentConstraint is a 3-dimensional constraint which enforces zero - * moment at the contact point for the link. + * ContactDynamicsMomentConstraint is a 3-dimensional constraint which enforces + * zero moment at the contact point for the link. */ inline gtsam::Vector3_ ContactDynamicsMomentConstraint( gtsam::Key contact_wrench_key, const gtsam::Pose3 &cTcom) { gtsam::Matrix36 H_contact_wrench; H_contact_wrench << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0; - gtsam::Matrix36 H = H_contact_wrench * cTcom.inverse().AdjointMap().transpose(); + gtsam::Matrix36 H = + H_contact_wrench * cTcom.inverse().AdjointMap().transpose(); gtsam::Vector6_ contact_wrench(contact_wrench_key); const std::function f = [H](const gtsam::Vector6 &F) { return H * F; }; @@ -47,6 +46,24 @@ inline gtsam::Vector3_ ContactDynamicsMomentConstraint( return error; } +/** + * ContactDynamicsMomentConstraint is a 3-dimensional constraint which enforces + * zero moment at the contact point for the link. This is an alternative + * interface for expressions as inputs + */ +inline gtsam::Vector3_ ContactDynamicsMomentConstraint( + gtsam::Vector6_ contact_wrench, const gtsam::Pose3 &cTcom) { + gtsam::Matrix36 H_contact_wrench; + H_contact_wrench << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0; + + gtsam::Matrix36 H = + H_contact_wrench * cTcom.inverse().AdjointMap().transpose(); + const std::function f = + [H](const gtsam::Vector6 &F) { return H * F; }; + gtsam::Vector3_ error = gtsam::linearExpression(f, contact_wrench, H); + return error; +} + /** * ContactDynamicsMomentFactor is unary nonlinear factor which enforces zero * moment at the contact point for the link. @@ -77,9 +94,9 @@ class ContactDynamicsMomentFactor virtual ~ContactDynamicsMomentFactor() {} - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -92,13 +109,15 @@ class ContactDynamicsMomentFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { // NOLINT ar &boost::serialization::make_nvp( - "NoiseModelFactor1", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/factors/ContactEqualityFactor.h b/gtdynamics/factors/ContactEqualityFactor.h index d7d69f14d..61afa9244 100644 --- a/gtdynamics/factors/ContactEqualityFactor.h +++ b/gtdynamics/factors/ContactEqualityFactor.h @@ -13,18 +13,17 @@ #pragma once +#include +#include +#include #include #include #include -#include +#include #include #include -#include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/utils/PointOnLink.h" -#include "gtdynamics/utils/values.h" - namespace gtdynamics { /** @@ -32,21 +31,29 @@ namespace gtdynamics { * between contact points on the same link at two different time steps. */ class ContactEqualityFactor - : public gtsam::NoiseModelFactor2 { + : public gtsam::NoiseModelFactorN { private: using This = ContactEqualityFactor; - using Base = gtsam::NoiseModelFactor2; + using Base = gtsam::NoiseModelFactorN; + /// The point on the link at which to enforce equality. PointOnLink point_on_link_; public: + // shorthand for a smart pointer to a factor + using shared_ptr = std::shared_ptr; + + /** default constructor - only use for serialization */ + ContactEqualityFactor(){}; + /** * Constructor. * - * @param point_on_link Point on the link at which to enforce constraint. - * @param model The noise model for this factor. - * @param k1 First time index. - * @param k2 Next time index. + * @param point_on_link Point on the link at which to enforce + * constraint. + * @param model The noise model for this factor. + * @param k1 First time index. + * @param k2 Subsequent time index. */ ContactEqualityFactor(const PointOnLink &point_on_link, const gtsam::SharedNoiseModel &model, size_t k1, @@ -55,14 +62,20 @@ class ContactEqualityFactor PoseKey(point_on_link.link->id(), k2)), point_on_link_(point_on_link) {} - virtual ~ContactEqualityFactor() {} + ~ContactEqualityFactor() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } /// Generic method to compute difference between contact points and provide /// jacobians. gtsam::Vector3 contactPointsDifference( const gtsam::Pose3 &wT1, const gtsam::Pose3 &wT2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr) const { gtsam::Point3 p1_w = wT1.transformFrom(point_on_link_.point, H1); gtsam::Point3 p2_w = wT2.transformFrom(point_on_link_.point, H2); @@ -72,8 +85,8 @@ class ContactEqualityFactor gtsam::Vector evaluateError( const gtsam::Pose3 &wT1, const gtsam::Pose3 &wT2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr) const override { return contactPointsDifference(wT1, wT2, H1, H2); } @@ -81,10 +94,25 @@ class ContactEqualityFactor void print(const std::string &s = "", const gtsam::KeyFormatter &keyFormatter = gtsam::DefaultKeyFormatter) const override { - std::cout << s << "ContactEqualityFactor(" << keyFormatter(this->key1()) - << "," << keyFormatter(this->key2()) << ")\n"; - this->noiseModel_->print(" noise model: "); + std::cout << (s.empty() ? "" : s + " ") << "ContactEqualityFactor" + << std::endl; + Base::print("", keyFormatter); + } + + bool equals(const gtsam::NonlinearFactor &other, + double tol = 1e-9) const override { + const This *e = dynamic_cast(&other); + return e != nullptr && Base::equals(*e, tol) && + point_on_link_.equals(e->point_on_link_); } }; } // namespace gtdynamics + +namespace gtsam { + +template <> +struct traits + : public Testable {}; + +} // namespace gtsam diff --git a/gtdynamics/factors/ContactHeightFactor.h b/gtdynamics/factors/ContactHeightFactor.h index cfa27e69e..bd8a85913 100644 --- a/gtdynamics/factors/ContactHeightFactor.h +++ b/gtdynamics/factors/ContactHeightFactor.h @@ -18,7 +18,6 @@ #include #include #include - #include #include @@ -30,9 +29,9 @@ namespace gtdynamics { * height for the contact point. This factor assumes that the ground is flat and * level. */ -inline gtsam::Double_ ContactHeightConstraint(gtsam::Key pose_key, - const gtsam::Point3 &comPc, const gtsam::Vector3 &gravity, - const double &ground_plane_height = 0.0) { +inline gtsam::Double_ ContactHeightConstraint( + gtsam::Key pose_key, const gtsam::Point3 &comPc, + const gtsam::Vector3 &gravity, const double &ground_plane_height = 0.0) { gtsam::Point3_ gravity_s_unit(gravity.normalized().cwiseAbs()); gtsam::Pose3_ sTl(pose_key); gtsam::Point3_ sPc = gtsam::transformFrom(sTl, gtsam::Point3_(comPc)); @@ -73,9 +72,9 @@ class ContactHeightFactor : public gtsam::ExpressionFactor { virtual ~ContactHeightFactor() {} - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -89,13 +88,15 @@ class ContactHeightFactor : public gtsam::ExpressionFactor { } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { // NOLINT ar &boost::serialization::make_nvp( - "NoiseModelFactor1", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/factors/ContactKinematicsAccelFactor.h b/gtdynamics/factors/ContactKinematicsAccelFactor.h index 1d5b5809c..236cf7ae3 100644 --- a/gtdynamics/factors/ContactKinematicsAccelFactor.h +++ b/gtdynamics/factors/ContactKinematicsAccelFactor.h @@ -13,21 +13,19 @@ #pragma once +#include #include #include #include #include #include - #include -#include #include +#include #include #include -#include "gtdynamics/utils/utils.h" - namespace gtdynamics { /** @@ -73,9 +71,9 @@ class ContactKinematicsAccelFactor virtual ~ContactKinematicsAccelFactor() {} - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -88,13 +86,15 @@ class ContactKinematicsAccelFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { // NOLINT ar &boost::serialization::make_nvp( - "NoiseModelFactor3", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/factors/ContactKinematicsTwistFactor.h b/gtdynamics/factors/ContactKinematicsTwistFactor.h index 302144556..06909fcdd 100644 --- a/gtdynamics/factors/ContactKinematicsTwistFactor.h +++ b/gtdynamics/factors/ContactKinematicsTwistFactor.h @@ -13,26 +13,24 @@ #pragma once +#include #include #include #include #include #include - #include -#include #include +#include #include #include -#include "gtdynamics/utils/utils.h" - namespace gtdynamics { /** - * ContactKinematicsTwistConstraint is a 3-dimensional constraint which enforces zero - * linear velocity at the contact point for a link. + * ContactKinematicsTwistConstraint is a 3-dimensional constraint which enforces + * zero linear velocity at the contact point for a link. */ inline gtsam::Vector3_ ContactKinematicsTwistConstraint( gtsam::Key twist_key, const gtsam::Pose3 &cTcom) { @@ -72,9 +70,9 @@ class ContactKinematicsTwistFactor ContactKinematicsTwistConstraint(twist_key, cTcom)) {} virtual ~ContactKinematicsTwistFactor() {} - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -87,13 +85,15 @@ class ContactKinematicsTwistFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { // NOLINT ar &boost::serialization::make_nvp( - "NoiseModelFactor3", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/factors/ContactPointFactor.h b/gtdynamics/factors/ContactPointFactor.h index 47ff5b8d8..a9780ecc2 100644 --- a/gtdynamics/factors/ContactPointFactor.h +++ b/gtdynamics/factors/ContactPointFactor.h @@ -13,15 +13,14 @@ #pragma once +#include #include #include #include -#include +#include #include -#include "gtdynamics/utils/PointOnLink.h" - namespace gtdynamics { /** @@ -33,10 +32,10 @@ namespace gtdynamics { * contact. */ class ContactPointFactor - : public gtsam::NoiseModelFactor2 { + : public gtsam::NoiseModelFactorN { private: using This = ContactPointFactor; - using Base = gtsam::NoiseModelFactor2; + using Base = gtsam::NoiseModelFactorN; // The contact point in the link's CoM frame. gtsam::Point3 contact_in_com_; @@ -68,7 +67,7 @@ class ContactPointFactor ContactPointFactor(const PointOnLink &point_on_link, gtsam::Key point_key, const gtsam::noiseModel::Base::shared_ptr &cost_model, size_t t = 0) - : ContactPointFactor(gtdynamics::PoseKey(point_on_link.link->id(), t), + : ContactPointFactor(PoseKey(point_on_link.link->id(), t), point_key, cost_model, point_on_link.point) {} virtual ~ContactPointFactor() {} @@ -80,17 +79,17 @@ class ContactPointFactor */ gtsam::Vector evaluateError( const gtsam::Pose3 &wTl, const gtsam::Point3 &wPc, - boost::optional H_pose = boost::none, - boost::optional H_point = boost::none) const override { + gtsam::OptionalMatrixType H_pose = nullptr, + gtsam::OptionalMatrixType H_point = nullptr) const override { gtsam::Vector error = wPc - wTl.transformFrom(contact_in_com_, H_pose); if (H_pose) *H_pose = -gtsam::Matrix3::Identity() * (*H_pose); if (H_point) *H_point = gtsam::Matrix3::Identity(); return error; } - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -104,13 +103,183 @@ class ContactPointFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int version) { // NOLINT + ar &boost::serialization::make_nvp( + "NoiseModelFactorN", boost::serialization::base_object(*this)); + } +#endif +}; + +/** + * ContactPoseFactor is a two-way nonlinear factor which constrains a link pose + * and a reference frame defined at a point of contact P in the world/spatial + * frame. + * + * This factor is useful for implementing flat foot constraints. + */ +class ContactPoseFactor + : public gtsam::NoiseModelFactorN { + private: + using This = ContactPoseFactor; + using Base = gtsam::NoiseModelFactorN; + + // The contact point reference frame in the link's CoM frame. + gtsam::Pose3 comTcontact_; + + public: + /** + * Constructor. + * + * @param link_pose_key Key for the CoM pose of the link in contact. + * @param contact_pose_key Key for the contact point pose in the world/spatial + * frame. + * @param cost_model Noise model associated with this factor. + * @param contact_in_com Static transform from point of contact to link CoM. + */ + ContactPoseFactor(gtsam::Key link_pose_key, gtsam::Key contact_pose_key, + const gtsam::noiseModel::Base::shared_ptr &cost_model, + const gtsam::Pose3 &comTcontact) + : Base(cost_model, link_pose_key, contact_pose_key), + comTcontact_(comTcontact) {} + + /** + * Convenience constructor which uses PointOnLink. + * + * @param point_on_link PointOnLink object which encapsulates the link and its + * contact point. + * @param contact_pose_key Key for the contact point pose in the world/spatial + * frame. + * @param cost_model Noise model associated with this factor. + * @param t The time step at which to add the factor (default t=0). + */ + ContactPoseFactor(const PointOnLink &point_on_link, + gtsam::Key contact_pose_key, + const gtsam::noiseModel::Base::shared_ptr &cost_model, + size_t t = 0) + : ContactPoseFactor( + PoseKey(point_on_link.link->id(), t), contact_pose_key, + cost_model, + // Contact reference frame has same rotation as the link CoM + gtsam::Pose3(gtsam::Rot3(), point_on_link.point)) {} + + virtual ~ContactPoseFactor() {} + + /** + * Evaluate error. + * @param wTl The link's COM pose in the world frame. + * @param wTcontact The environment contact point frame in the world frame. + */ + gtsam::Vector evaluateError( + const gtsam::Pose3 &wTl, const gtsam::Pose3 &wTcontact, + gtsam::OptionalMatrixType H_link = nullptr, + gtsam::OptionalMatrixType H_contact = nullptr) const override { + gtsam::Pose3 measured_wTcontact = wTl.compose(comTcontact_, H_link); + gtsam::Matrix6 H1, H2; + gtsam::Vector error = + measured_wTcontact.localCoordinates(wTcontact, H1, H2); + if (H_link) *H_link = H1 * (*H_link); + if (H_contact) *H_contact = H2; + return error; + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print contents + void print(const std::string &s = "", + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << (s.empty() ? "" : s + " ") << "ContactPoseFactor" << std::endl; + Base::print("", keyFormatter); + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int version) { // NOLINT + ar &boost::serialization::make_nvp( + "NoiseModelFactorN", boost::serialization::base_object(*this)); + } +#endif +}; + +/** Contact Point Factor with a static contact point. */ +class FixedContactPointFactor : public gtsam::NoiseModelFactorN { + private: + using This = FixedContactPointFactor; + using Base = gtsam::NoiseModelFactorN; + + // The contact point in the link's CoM frame. + gtsam::Point3 contact_in_world_; + gtsam::Point3 contact_in_com_; + + public: + /** + * Constructor. + * + * @param link_pose_key Key for the CoM pose of the link in contact. + * @param cost_model Noise model associated with this factor. + * @param contacet_in_world Static contact point expressed in world frame. + * @param contact_in_com Static transform from point of contact to link CoM. + */ + FixedContactPointFactor(gtsam::Key link_pose_key, + const gtsam::noiseModel::Base::shared_ptr &cost_model, + const gtsam::Point3 &contact_in_world, + const gtsam::Point3 &contact_in_com) + : Base(cost_model, link_pose_key), + contact_in_world_(contact_in_world), + contact_in_com_(contact_in_com) {} + + virtual ~FixedContactPointFactor() {} + + /** + * Evaluate error. + * @param wTl The link's COM pose in the world frame. + * @param wPc The environment contact point in the world frame. + */ + gtsam::Vector evaluateError( + const gtsam::Pose3 &wTl, + gtsam::OptionalMatrixType H_pose = nullptr) const override { + gtsam::Vector error = + contact_in_world_ - wTl.transformFrom(contact_in_com_, H_pose); + if (H_pose) *H_pose = -gtsam::Matrix3::Identity() * (*H_pose); + return error; + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print contents + void print(const std::string &s = "", + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << (s.empty() ? "" : s + " ") << "FixedContactPointFactor" + << std::endl; + Base::print("", keyFormatter); + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { // NOLINT ar &boost::serialization::make_nvp( - "NonlinearEquality2", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/factors/ForwardKinematicsFactor.h b/gtdynamics/factors/ForwardKinematicsFactor.h index 18a63143b..8c71568d9 100644 --- a/gtdynamics/factors/ForwardKinematicsFactor.h +++ b/gtdynamics/factors/ForwardKinematicsFactor.h @@ -13,6 +13,9 @@ #pragma once +#include +#include +#include #include #include #include @@ -22,10 +25,6 @@ #include #include -#include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/utils/PointOnLink.h" -#include "gtdynamics/utils/values.h" - namespace gtdynamics { /** diff --git a/gtdynamics/factors/GeneralPriorFactor.h b/gtdynamics/factors/GeneralPriorFactor.h new file mode 100644 index 000000000..5b9e26930 --- /dev/null +++ b/gtdynamics/factors/GeneralPriorFactor.h @@ -0,0 +1,190 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GeneralPriorFactor.h + * @author Yetong Zhang + **/ +#pragma once + +#include +#include +#include +#include + +#include + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#endif + +namespace gtdynamics { + +using gtsam::DefaultKeyFormatter; +using gtsam::Key; +using gtsam::KeyFormatter; +using gtsam::KeyVector; +using gtsam::Matrix; +using gtsam::NoiseModelFactor; +using gtsam::NonlinearFactor; +using gtsam::NonlinearFactorGraph; +using gtsam::SharedNoiseModel; +using gtsam::Value; +using gtsam::Values; +using gtsam::Vector; +using gtsam::VectorValues; + +/** + * A class for a soft prior on any Value type + */ +class GeneralPriorFactor : public NoiseModelFactor { + private: + typedef NoiseModelFactor Base; + Values prior_; /** The measurement */ + size_t dim_; + + public: + /// shorthand for a smart pointer to a factor + typedef typename std::shared_ptr shared_ptr; + + /// Typedef to this class + typedef GeneralPriorFactor This; + + /** default constructor - only use for serialization */ + GeneralPriorFactor() {} + + ~GeneralPriorFactor() override {} + + /** Constructor */ + GeneralPriorFactor(Key key, const Value &prior, + const SharedNoiseModel &model = nullptr) + : Base(model, KeyVector{key}) { + prior_.insert(key, prior); + dim_ = prior.dim(); + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** implement functions needed for Testable */ + + /** print */ + void print(const std::string &s, const KeyFormatter &keyFormatter = + DefaultKeyFormatter) const override { + std::cout << s << "GeneralPriorFactor on " << keyFormatter(this->keys()[0]) + << "\n"; + if (this->noiseModel_) + this->noiseModel_->print(" noise model: "); + else + std::cout << "no noise model" << std::endl; + } + + /** equals */ + bool equals(const NonlinearFactor &expected, + double tol = 1e-9) const override { + const This *e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + prior_.equals(e->prior_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector unwhitenedError(const Values &x, gtsam::OptionalMatrixVecType H = + nullptr) const override { + if (H) (*H)[0] = Matrix::Identity(dim_, dim_); + return -x.at(keys_[0]).localCoordinates_(prior_.at(keys_[0])); + } + + const Value &prior() const { return prior_.at(keys_[0]); } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); + ar &BOOST_SERIALIZATION_NVP(prior_); + } +#endif +}; + +template +inline void AddGeneralPriors(const Values &values, const CONTAINER &keys, + double sigma, NonlinearFactorGraph &graph) { + for (const Key &key : keys) { + const Value &value = values.at(key); + graph.emplace_shared( + key, value, gtsam::noiseModel::Isotropic::Sigma(value.dim(), sigma)); + } +} + +inline void AddGeneralPriors(const Values &values, double sigma, + NonlinearFactorGraph &graph) { + AddGeneralPriors(values, values.keys(), sigma, graph); +} + +inline void AddGeneralPriors(const Values &values, + const VectorValues &all_sigmas, + NonlinearFactorGraph &graph) { + for (const auto &it : all_sigmas) { + const Key &key = it.first; + const Vector &sigmas = it.second; + graph.emplace_shared( + key, values.at(key), gtsam::noiseModel::Diagonal::Sigmas(sigmas)); + } +} + +template +inline void AddGeneralPriors(const Values &values, const CONTAINER &keys, + const VectorValues &all_sigmas, + NonlinearFactorGraph &graph) { + for (const Key &key : keys) { + if (all_sigmas.exists(key)) { + graph.emplace_shared( + key, values.at(key), + gtsam::noiseModel::Diagonal::Sigmas(all_sigmas.at(key))); + } + } +} + +template +inline void AddGeneralPriors(const Values &values, const CONTAINER &keys, + const VectorValues &all_sigmas, + NonlinearFactorGraph &graph, + double nominal_sigma) { + + double min_sigma = 1e10; + for (const Key &key : keys) { + if (all_sigmas.exists(key)) { + min_sigma = std::min(min_sigma, all_sigmas.at(key).minCoeff()); + } + } + double scale = nominal_sigma / min_sigma; + // std::cout << "scale: " << scale << "\n"; + + for (const Key &key : keys) { + if (all_sigmas.exists(key)) { + graph.emplace_shared( + key, values.at(key), + gtsam::noiseModel::Diagonal::Sigmas(scale * all_sigmas.at(key))); + } + } +} + + +} // namespace gtdynamics diff --git a/gtdynamics/factors/JointLimitFactor.h b/gtdynamics/factors/JointLimitFactor.h index 712e451a2..11b74ff42 100644 --- a/gtdynamics/factors/JointLimitFactor.h +++ b/gtdynamics/factors/JointLimitFactor.h @@ -13,29 +13,27 @@ #pragma once +#include #include #include -#include +#include -#include #include #include #include #include #include -#include "gtdynamics/universal_robot/Joint.h" - namespace gtdynamics { /** - * JointLimitFactor is a class which enforces joint angle, velocity, - * acceleration and torque value to be within limi + * JointLimitFactor is a class which enforces joint angle value + * to be within specified limits. */ -class JointLimitFactor : public gtsam::NoiseModelFactor1 { +class JointLimitFactor : public gtsam::NoiseModelFactorN { private: using This = JointLimitFactor; - using Base = gtsam::NoiseModelFactor1; + using Base = gtsam::NoiseModelFactorN; double low_, high_; public: @@ -57,7 +55,6 @@ class JointLimitFactor : public gtsam::NoiseModelFactor1 { virtual ~JointLimitFactor() {} - public: /** * Evaluate joint limit errors * @@ -68,9 +65,8 @@ class JointLimitFactor : public gtsam::NoiseModelFactor1 { * * @param q joint value */ - gtsam::Vector evaluateError( - const double &q, - boost::optional H_q = boost::none) const override { + gtsam::Vector evaluateError(const double &q, + gtsam::OptionalMatrixType H_q) const override { if (q < low_) { if (H_q) *H_q = -gtsam::I_1x1; return gtsam::Vector1(low_ - q); @@ -83,9 +79,9 @@ class JointLimitFactor : public gtsam::NoiseModelFactor1 { } } - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -98,15 +94,17 @@ class JointLimitFactor : public gtsam::NoiseModelFactor1 { } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { // NOLINT ar &boost::serialization::make_nvp( - "NoiseModelFactor1", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); ar &low_; ar &high_; } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/factors/JointMeasurementFactor.h b/gtdynamics/factors/JointMeasurementFactor.h index 23d2d788b..00f37eb96 100644 --- a/gtdynamics/factors/JointMeasurementFactor.h +++ b/gtdynamics/factors/JointMeasurementFactor.h @@ -13,10 +13,9 @@ */ #pragma once -#include - -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/universal_robot/Link.h" +#include +#include +#include namespace gtdynamics { @@ -25,15 +24,18 @@ namespace gtdynamics { * the joint coordinate as a measurement. */ class JointMeasurementFactor - : public gtsam::NoiseModelFactor2 { + : public gtsam::NoiseModelFactorN { private: using This = JointMeasurementFactor; - using Base = gtsam::NoiseModelFactor2; + using Base = gtsam::NoiseModelFactorN; JointConstSharedPtr joint_; double measured_joint_coordinate_; public: + // shorthand for a smart pointer to a factor + using shared_ptr = std::shared_ptr; + /** * @brief Construct a new Link Pose Factor object * @@ -70,8 +72,8 @@ class JointMeasurementFactor gtsam::Vector evaluateError( const gtsam::Pose3& wTp, const gtsam::Pose3& wTc, - boost::optional H_wTp = boost::none, - boost::optional H_wTc = boost::none) const override { + gtsam::OptionalMatrixType H_wTp = nullptr, + gtsam::OptionalMatrixType H_wTc = nullptr) const override { gtsam::Matrix6 H; gtsam::Pose3 wTc_hat = joint_->poseOf(joint_->child(), wTp, measured_joint_coordinate_, H_wTp); @@ -83,6 +85,9 @@ class JointMeasurementFactor return error; } + /// Return measurement + double measured() { return measured_joint_coordinate_; } + /// print contents void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = @@ -92,5 +97,21 @@ class JointMeasurementFactor gtsam::traits::Print(measured_joint_coordinate_, " measured: "); this->noiseModel_->print(" noise model: "); } + + bool equals(const gtsam::NonlinearFactor& other, + double tol = 1e-9) const override { + const This* e = dynamic_cast(&other); + return e != nullptr && Base::equals(*e, tol) && + joint_->equals(*e->joint_) && + measured_joint_coordinate_ == e->measured_joint_coordinate_; + } }; } // namespace gtdynamics + +namespace gtsam { + +template <> +struct traits + : public Testable {}; + +} // namespace gtsam diff --git a/gtdynamics/factors/MinTorqueFactor.h b/gtdynamics/factors/MinTorqueFactor.h index 06c02d61f..53fb3f1e2 100644 --- a/gtdynamics/factors/MinTorqueFactor.h +++ b/gtdynamics/factors/MinTorqueFactor.h @@ -15,18 +15,18 @@ #include #include -#include +#include -#include +#include #include namespace gtdynamics { /// MinTorqueFactor is a unary factor which minimizes torque. -class MinTorqueFactor : public gtsam::NoiseModelFactor1 { +class MinTorqueFactor : public gtsam::NoiseModelFactorN { private: using This = MinTorqueFactor; - using Base = gtsam::NoiseModelFactor1; + using Base = gtsam::NoiseModelFactorN; public: /** @@ -38,9 +38,9 @@ class MinTorqueFactor : public gtsam::NoiseModelFactor1 { MinTorqueFactor(gtsam::Key torque_key, const gtsam::noiseModel::Base::shared_ptr &cost_model) : Base(cost_model, torque_key) {} + virtual ~MinTorqueFactor() {} - public: /** * Evaluate wrench balance errors * @param wrench wrench on this link @@ -48,7 +48,7 @@ class MinTorqueFactor : public gtsam::NoiseModelFactor1 { */ gtsam::Vector evaluateError( const double &torque, - boost::optional H_torque = boost::none) const override { + gtsam::OptionalMatrixType H_torque = nullptr) const override { gtsam::Vector error = (gtsam::Vector(1) << torque).finished(); if (H_torque) *H_torque = gtsam::I_1x1; @@ -56,9 +56,9 @@ class MinTorqueFactor : public gtsam::NoiseModelFactor1 { return error; } - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -71,13 +71,15 @@ class MinTorqueFactor : public gtsam::NoiseModelFactor1 { } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE const &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor1", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/factors/PenaltyFactor.h b/gtdynamics/factors/PenaltyFactor.h new file mode 100644 index 000000000..39d37d628 --- /dev/null +++ b/gtdynamics/factors/PenaltyFactor.h @@ -0,0 +1,140 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#endif + +namespace gtdynamics { + +using gtsam::Matrix; +using gtsam::NoiseModelFactor; +using gtsam::OptionalJacobian; +using gtsam::SharedNoiseModel; +using gtsam::Values; +using gtsam::Vector; + +inline std::function H)> +RampFunction(const double offset) { + auto func = [=](const double &x, OptionalJacobian<1, 1> H = {}) -> double { + if (x < offset) { + if (H) { + H->setConstant(0); + } + return 0; + } else { + if (H) { + H->setConstant(1); + } + return x - offset; + } + }; + return func; +} + +/** A factor that implements the penalty function for inequality constraint + * for g(x)>=0. the penalty function is ramp(-g(x)) + * for g(x)<=0, the penalty function is ramp(g(x)) + * Note that the noise model is stored twice (both in base factor and the + * noisemodel of substitute factor. The noisemodel in the base factor will be + * ignored. */ +class PenaltyFactor : public NoiseModelFactor { +protected: + typedef NoiseModelFactor Base; + typedef PenaltyFactor This; + + // original factor + Base::shared_ptr base_factor_; + bool positive_; + +public: + typedef std::shared_ptr shared_ptr; + + /** Default constructor for I/O only */ + PenaltyFactor() {} + + /** Destructor */ + ~PenaltyFactor() override {} + + /** + * Constructor + * @param base_factor original factor on X + * @param bias the bias term + */ + PenaltyFactor(const Base::shared_ptr &base_factor, bool positive = true) + : Base(base_factor->noiseModel(), base_factor->keys()), + base_factor_(base_factor), positive_(positive) {} + + PenaltyFactor(const Base::shared_ptr &base_factor, + const SharedNoiseModel &noise_model, bool positive = true) + : Base(noise_model, base_factor->keys()), base_factor_(base_factor), + positive_(positive) {} + + /** + * Error function *without* the NoiseModel, \f$ z-h(x) \f$. + * Override this method to finish implementing an N-way factor. + * If the optional arguments is specified, it should compute + * both the function evaluation and its derivative(s) in H. + */ + virtual Vector + unwhitenedError(const Values &x, + gtsam::OptionalMatrixVecType H = nullptr) const override { + Vector error = base_factor_->unwhitenedError(x, H); + if (positive_) { + error = -error; + if (H) { + for (Matrix &m : *H) { + m = -m; + } + } + } + for (int i = 0; i < error.size(); i++) { + if (error(i) < 0) { + error(i) = 0; + if (H) { + for (Matrix &m : *H) { + m.row(i).setZero(); + } + } + } + } + + return error; + } + + /** Return a deep copy of this factor. */ + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** Print function. */ + void print(const std::string &s = "", + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "penalty factor" << std::endl; + Base::print("", keyFormatter); + } + +private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &boost::serialization::make_nvp( + "PenaltyFactor", boost::serialization::base_object(*this)); + ar &BOOST_SERIALIZATION_NVP(base_factor_); + } +#endif + +}; // \class PenaltyFactor + +} // namespace gtdynamics diff --git a/gtdynamics/factors/PointGoalFactor.h b/gtdynamics/factors/PointGoalFactor.h index 3ccd41bee..f0fe1650a 100644 --- a/gtdynamics/factors/PointGoalFactor.h +++ b/gtdynamics/factors/PointGoalFactor.h @@ -13,20 +13,17 @@ #pragma once +#include #include #include #include #include -#include #include - #include #include #include -#include "gtdynamics/utils/values.h" - namespace gtdynamics { /** @@ -34,9 +31,9 @@ namespace gtdynamics { * reaches a desired goal point. */ inline gtsam::Vector3_ PointGoalConstraint(gtsam::Key pose_key, - const gtsam::Point3 &point_com, - const gtsam::Point3 &goal_point) { - gtsam::Vector3_ point_com_expr(point_com); + const gtsam::Point3 &point_com, + const gtsam::Point3 &goal_point) { + gtsam::Vector3_ point_com_expr(point_com); gtsam::Pose3_ wTcom_expr(pose_key); gtsam::Vector3_ point_world_expr(wTcom_expr, >sam::Pose3::transformFrom, point_com_expr); @@ -72,13 +69,15 @@ class PointGoalFactor : public gtsam::ExpressionFactor { const gtsam::Point3 &goalPoint() const { return goal_point_; } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { // NOLINT ar &boost::serialization::make_nvp( - "NoiseModelFactor1", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; /** @@ -99,7 +98,8 @@ inline gtsam::NonlinearFactorGraph PointGoalFactors( const std::vector &goal_trajectory) { gtsam::NonlinearFactorGraph factors; for (auto &&goal_point : goal_trajectory) { - factors.emplace_shared(first_key, cost_model, point_com, goal_point); + factors.emplace_shared(first_key, cost_model, point_com, + goal_point); first_key += 1; } return factors; diff --git a/gtdynamics/factors/PoseFactor.h b/gtdynamics/factors/PoseFactor.h index a33a13f59..a39376ebb 100644 --- a/gtdynamics/factors/PoseFactor.h +++ b/gtdynamics/factors/PoseFactor.h @@ -13,6 +13,8 @@ #pragma once +#include +#include #include #include #include @@ -20,21 +22,15 @@ #include #include -#include #include #include -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/universal_robot/Link.h" - namespace gtdynamics { -using boost::assign::cref_list_of; - /** * Create single factor relating this link's pose (COM) with previous one. * Note: this function is provided for BW compatibility only, and will in time - * be replaced with EqualityConstraint. + * be replaced with a gtsam equality constraint. * * PoseFactor is a three-way nonlinear factor between a joint's parent link * pose, child link pose, and the joint angle relating the two poses. @@ -49,18 +45,17 @@ using boost::assign::cref_list_of; inline gtsam::NoiseModelFactor::shared_ptr PoseFactor( const gtsam::SharedNoiseModel &cost_model, const JointConstSharedPtr &joint, int time) { - return boost::make_shared>( + return std::make_shared>( cost_model, gtsam::Vector6::Zero(), joint->poseConstraint(time)); } /** * Create single factor relating this link's pose (COM) with previous one. * Note: this function is provided for BW compatibility only, and will in time - * be replaced with EqualityConstraint. + * be replaced with a gtsam equality constraint. * * @param wTp_key Key for parent link's CoM pose in world frame. * @param wTc_key Key for child link's CoM pose in world frame. - * @param q_key Key for joint value. * @param cost_model The noise model for this factor. * @param joint The joint connecting the two poses */ @@ -68,9 +63,9 @@ inline gtsam::NoiseModelFactor::shared_ptr PoseFactor( DynamicsSymbol wTp_key, DynamicsSymbol wTc_key, DynamicsSymbol q_key, const gtsam::noiseModel::Base::shared_ptr &cost_model, JointConstSharedPtr joint) { - return boost::make_shared>( + return std::make_shared>( cost_model, gtsam::Vector6::Zero(), - joint->poseConstraint(wTp_key.time())); + joint->poseConstraint(wTp_key, wTc_key, q_key)); } } // namespace gtdynamics diff --git a/gtdynamics/factors/PreintegratedContactFactors.h b/gtdynamics/factors/PreintegratedContactFactors.h index 7b1d4c3bc..50486e90a 100644 --- a/gtdynamics/factors/PreintegratedContactFactors.h +++ b/gtdynamics/factors/PreintegratedContactFactors.h @@ -13,20 +13,19 @@ #pragma once +#include +#include #include #include #include #include #include -#include +#include #include #include #include -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/universal_robot/Link.h" - namespace gtdynamics { /** @@ -89,11 +88,11 @@ class PreintegratedPointContactMeasurements { * Hartley18icra. */ class PreintegratedPointContactFactor - : public gtsam::NoiseModelFactor4 { private: using This = PreintegratedPointContactFactor; - using Base = gtsam::NoiseModelFactor4; public: @@ -131,10 +130,10 @@ class PreintegratedPointContactFactor gtsam::Vector evaluateError( const gtsam::Pose3 &wTb_i, const gtsam::Pose3 &wTc_i, const gtsam::Pose3 &wTb_j, const gtsam::Pose3 &wTc_j, - boost::optional H_wTb_i = boost::none, - boost::optional H_wTc_i = boost::none, - boost::optional H_wTb_j = boost::none, - boost::optional H_wTc_j = boost::none) const override { + gtsam::OptionalMatrixType H_wTb_i = nullptr, + gtsam::OptionalMatrixType H_wTc_i = nullptr, + gtsam::OptionalMatrixType H_wTb_j = nullptr, + gtsam::OptionalMatrixType H_wTc_j = nullptr) const override { // Compute the error. gtsam::Vector3 error = wTb_i.rotation().transpose() * (wTc_j.translation() - wTc_i.translation()); @@ -171,29 +170,31 @@ class PreintegratedPointContactFactor return error; } - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print contents void print(const std::string &s = "", const gtsam::KeyFormatter &keyFormatter = - gtdynamics::GTDKeyFormatter) const override { + GTDKeyFormatter) const override { std::cout << (s.empty() ? s : s + " ") << "Preintegrated Point Contact Factor" << std::endl; Base::print("", keyFormatter); } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { // NOLINT ar &boost::serialization::make_nvp( - "NoiseModelFactor4", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; /** @@ -287,22 +288,23 @@ class PreintegratedRigidContactFactor virtual ~PreintegratedRigidContactFactor() {} - //// @return a deep copy of this factor + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print contents void print(const std::string &s = "", const gtsam::KeyFormatter &keyFormatter = - gtdynamics::GTDKeyFormatter) const override { + GTDKeyFormatter) const override { std::cout << (s.empty() ? s : s + " ") << "Preintegrated Rigid Contact Factor" << std::endl; Base::print("", keyFormatter); } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template @@ -310,6 +312,7 @@ class PreintegratedRigidContactFactor ar &boost::serialization::make_nvp( "BetweenFactor", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/factors/SmoothPenaltyFactor.cpp b/gtdynamics/factors/SmoothPenaltyFactor.cpp new file mode 100644 index 000000000..2544c6dff --- /dev/null +++ b/gtdynamics/factors/SmoothPenaltyFactor.cpp @@ -0,0 +1,96 @@ +#include + +namespace gtdynamics { + +using gtsam::Double_; + +/* ************************************************************************* */ +DoublePenaltyFunc SmoothBarrierFunction(const double &min_val, + const double &b) { + double k = 0.5 / b; + double offset = 0.5 * b; + auto func = [=](const double &x, OptionalJacobian<1, 1> H = {}) -> double { + if (x >= min_val) { + if (H) { + H->setConstant(0); + } + return 0; + } + double error = min_val - x; + if (error < b) { + if (H) { + H->setConstant(-2 * k * error); + } + return k * error * error; + } else { + if (H) { + H->setConstant(-1); + } + return error - offset; + } + }; + return func; +} + +// /* ************************************************************************* +// */ DoublePenaltyFunc SmoothBarrierFunction(const double &min_val, +// const double &max_val, +// const double &b) { + +// double k = 0.5 / b; +// double offset = 0.5 * b; +// auto func = [=](const double &x, OptionalJacobian<1, 1> H = {}) -> double { +// int sign; +// double error; +// if (x < min_val) { +// error = min_val - x; +// sign = -1; +// } else if (x > max_val) { +// error = x - max_val; +// sign = 1; +// } else { +// if (H) { +// H->setConstant(0); +// } +// return 0; +// } +// if (error < b) { +// if (H) { +// H->setConstant(2 * sign * k * error); +// } +// return k * error * error; +// } else { +// if (H) { +// H->setConstant(sign); +// } +// return error - offset; +// } +// }; +// return func; +// } + +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr SmoothPenaltyFactor( + const Key &key, const double &min_val, const double &b, + const SharedNoiseModel &model) { + Double_ x_expr(key); + auto penalty_function = SmoothBarrierFunction(min_val, b); + Double_ error_expr(penalty_function, x_expr); + return std::make_shared>(model, 0.0, error_expr); +} + +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr SmoothPenaltyFactor( + const Key &key, const double &min_val, const double &max_val, + const double &b, const SharedNoiseModel &model) { + Double_ x_expr(key); + auto penalty_function_min = SmoothBarrierFunction(min_val, b); + auto penalty_function_negmax = SmoothBarrierFunction(-max_val, b); + + Double_ error_min_expr(penalty_function_min, x_expr); + Double_ error_max_expr(penalty_function_negmax, -1 * x_expr); + Double_ error_expr = error_min_expr + error_max_expr; + return std::make_shared>(model, 0.0, error_expr); +} + +} // namespace gtdynamics \ No newline at end of file diff --git a/gtdynamics/factors/SmoothPenaltyFactor.h b/gtdynamics/factors/SmoothPenaltyFactor.h new file mode 100644 index 000000000..09534a12d --- /dev/null +++ b/gtdynamics/factors/SmoothPenaltyFactor.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include + +#include + +namespace gtdynamics { + +using gtsam::Key; +using gtsam::NoiseModelFactor; +using gtsam::OptionalJacobian; +using gtsam::SharedNoiseModel; + +typedef std::function H)> + DoublePenaltyFunc; + +/** Smooth penalty function for i-constraint x >= min + * Function f(x) = 0 | min <= x + * k * (min-x)^2 | min - b < x < min + * min - x - offset | x < min - b + */ +DoublePenaltyFunc SmoothBarrierFunction(const double &min_val, const double &b); + +// /** Smooth penalty function. +// * Function f(x) = x - max - offset | x > max + b +// * k * (x-max)^2 | max < x < max + b +// * 0 | min < x < max +// * k * (min-x)^2 | min - b < x < min +// * min - x - offset | x < min - b +// */ +// DoublePenaltyFunc SmoothBarrierFunction(const double &min_val, +// const double &max_val, const double &b); + +/** Smooth penalty factor corresponding to the constraint x>=min. */ +NoiseModelFactor::shared_ptr SmoothPenaltyFactor(const Key &key, + const double &min_val, + const double &b, + const SharedNoiseModel &model); + +/** Smooth penalty factor corresponding to the constraint max>=x>=min. */ +NoiseModelFactor::shared_ptr SmoothPenaltyFactor(const Key &key, + const double &min_val, + const double &max_val, + const double &b, + const SharedNoiseModel &model); + +} // namespace gtdynamics diff --git a/gtdynamics/factors/SubstituteFactor.cpp b/gtdynamics/factors/SubstituteFactor.cpp new file mode 100644 index 000000000..78b4505f4 --- /dev/null +++ b/gtdynamics/factors/SubstituteFactor.cpp @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SubstituteFactor.cpp + * @brief Substitute factor implementations. + * @author: Yetong Zhang + */ + +#include +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +KeyVector SubstituteFactor::computeNewKeys( + const Base::shared_ptr& base_factor, + const std::map& replacement_map, const Values& fc_manifolds) { + const KeyVector& base_keys = base_factor->keys(); + KeySet cmanifold_keys; + KeyVector new_keys; + for (const Key& base_key : base_keys) { + if (replacement_map.find(base_key) != replacement_map.end()) { + Key new_key = replacement_map.at(base_key); + if (!fc_manifolds.exists(new_key)) { + if (!cmanifold_keys.exists(new_key)) { + new_keys.push_back(new_key); + cmanifold_keys.insert(new_key); + } + } + } else { + new_keys.push_back(base_key); + } + } + return new_keys; +} + +/* ************************************************************************* */ +void SubstituteFactor::computeBaseKeyIndex() { + const KeyVector& base_keys = base_factor_->keys(); + for (size_t key_index = 0; key_index < base_keys.size(); key_index++) { + const Key& base_key = base_keys.at(key_index); + base_key_index_[base_key] = key_index; + } +} + +/* ************************************************************************* */ +void SubstituteFactor::classifyKeys(const Values& fc_manifolds) { + const KeyVector& base_keys = base_factor_->keys(); + for (const Key& base_key : base_keys) { + if (isReplaced(base_key)) { + Key new_key = replacement_map_.at(base_key); + if (fc_manifolds.exists(new_key)) { + fc_values_.insert( + base_key, + fc_manifolds.at(new_key).cast().recover(base_key)); + } else { + cmanifold_keys_.insert(new_key); + } + } else { + unconstrained_keys_.insert(base_key); + } + } +} + +/* ************************************************************************* */ +/** Insert values (for key in keys) to output_values */ +void InsertSelected(const Values& values, const KeyVector& keys, + Values& output_values) { + for (const Key& key : keys) { + if (values.exists(key)) { + output_values.insert(key, values.at(key)); + } + } +} + +/* ************************************************************************* */ +Vector SubstituteFactor::unwhitenedError( + const Values& x, gtsam::OptionalMatrixVecType H) const { + // Construct values for base factor. + Values base_x = fc_values_; + for (const Key& key : unconstrained_keys_) { + base_x.insert(key, x.at(key)); + } + for (const Key& key : cmanifold_keys_) { + const auto& cmanifold = x.at(key).cast(); + InsertSelected(cmanifold.values(), base_factor_->keys(), base_x); + } + + // compute jacobian + if (H) { + // Compute Jacobian and error using base factor. + std::vector base_H(base_x.size()); + Vector unwhitened_error = base_factor_->unwhitenedError(base_x, base_H); + + // Compute Jacobian for new variables + for (size_t variable_idx = 0; variable_idx < keys().size(); + variable_idx++) { + const Key& key = keys().at(variable_idx); + if (unconstrained_keys_.exists(key)) { + (*H)[variable_idx] = base_H.at(base_key_index_.at(key)); + } else { + const auto& cmanifold = x.at(key).cast(); + bool H_initialized = false; + for (const Key& base_key : cmanifold.values().keys()) { + if (base_key_index_.find(base_key) != base_key_index_.end()) { + const size_t& base_key_index = base_key_index_.at(base_key); + Matrix H_recover; + cmanifold.recover(base_key, H_recover); + if (!H_initialized) { + (*H)[variable_idx] = base_H.at(base_key_index) * H_recover; + H_initialized = true; + continue; + } + (*H)[variable_idx] += base_H.at(base_key_index) * H_recover; + } + } + } + } + return unwhitened_error; + } else { + return base_factor_->unwhitenedError(base_x); + } +} + +} // namespace gtdynamics diff --git a/gtdynamics/factors/SubstituteFactor.h b/gtdynamics/factors/SubstituteFactor.h new file mode 100644 index 000000000..bf161b2c3 --- /dev/null +++ b/gtdynamics/factors/SubstituteFactor.h @@ -0,0 +1,143 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SubstituteFactor.h + * @brief Factor that substitute certain variables with its corresponding + * recover function from the constraint manifold. It is used to represent the + * equivalent new cost factors on manifold varaibles. + * @author: Yetong Zhang + */ + +#pragma once + +#include + +#include + +namespace gtdynamics { + +using gtsam::Key; +using gtsam::KeySet; +using gtsam::KeyVector; +using gtsam::NoiseModelFactor; +using gtsam::Values; +using gtsam::Vector; + +/** A factor that substitute certain variables of a base factor with constraint + * manifold variables. + * It is used for constraint manifold optimization, since the variables + * connected with constraint factors will be replaced by constraint manifold + * variables. + * Note that the noise model is stored twice (both in base factor and the + * noisemodel of substitute factor. The noisemodel in the base factor will be + * ignored. */ +class SubstituteFactor : public NoiseModelFactor { + protected: + typedef NoiseModelFactor Base; + typedef SubstituteFactor This; + + // original factor + Base::shared_ptr base_factor_; + // map from base key to key of corresponding constraint manifold + std::map replacement_map_; + // keys of constraint manifold variables + KeySet cmanifold_keys_; + // keys of unconstrianed variables + KeySet unconstrained_keys_; + // base variables that are in fully constrained CCCs + Values fc_values_; + // map from base_key to index of the key in base factor + std::map base_key_index_; + + public: + typedef std::shared_ptr shared_ptr; + + /// Default constructor for I/O only. + SubstituteFactor() {} + + /// Destructor. + ~SubstituteFactor() override {} + + /** + * Constructor + * @param base_factor original factor on X + * @param replacement_map map from X to CCC + * @param fc_manifolds CCC that are fully constrained (dimension 0) + */ + SubstituteFactor(const Base::shared_ptr& base_factor, + const std::map& replacement_map, + const Values& fc_manifolds = Values()) + : Base(base_factor->noiseModel(), + computeNewKeys(base_factor, replacement_map, fc_manifolds)), + base_factor_(base_factor), + replacement_map_(replacement_map) { + if (!checkActive()) { + return; + } + computeBaseKeyIndex(); + classifyKeys(fc_manifolds); + } + + protected: + /// Compute keys for the new factor. + static KeyVector computeNewKeys(const Base::shared_ptr& base_factor, + const std::map& replacement_map, + const Values& fc_manifolds); + + /// Construct map from base key to key index in base factor. + void computeBaseKeyIndex(); + + /** Classify the variables as either constarined or unconstrained in the + * subsitute factor. */ + void classifyKeys(const Values& fc_manifolds); + + public: + /** + * Error function *without* the NoiseModel, \f$ z-h(x) \f$. + * Override this method to finish implementing an N-way factor. + * If the optional arguments is specified, it should compute + * both the function evaluation and its derivative(s) in H. + */ + virtual Vector unwhitenedError( + const Values& x, gtsam::OptionalMatrixVecType H = nullptr) const override; + + /// Return a deep copy of this factor. + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// Check if a variable in the base factor is substituted. + inline bool isReplaced(const Key& key) const { + return replacement_map_.find(key) != replacement_map_.end(); + } + + /** Check if the factor is active. Note: if all the variables of the original + * factor are fully constrained, no updates can be made.*/ + inline bool checkActive() const { return size() > 0; } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function. + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "SubstituteFactor", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(base_factor_); + ar& BOOST_SERIALIZATION_NVP(replacement_map_); + ar& BOOST_SERIALIZATION_NVP(cmanifold_keys_); + ar& BOOST_SERIALIZATION_NVP(unconstrained_keys_); + ar& BOOST_SERIALIZATION_NVP(fc_values_); + ar& BOOST_SERIALIZATION_NVP(base_key_index_); + } +#endif + +}; // \class SubstituteFactor + +} // namespace gtdynamics diff --git a/gtdynamics/factors/TorqueFactor.h b/gtdynamics/factors/TorqueFactor.h index f8c61d164..509eb39ef 100644 --- a/gtdynamics/factors/TorqueFactor.h +++ b/gtdynamics/factors/TorqueFactor.h @@ -13,19 +13,18 @@ #pragma once +#include +#include +#include #include #include #include #include -#include #include +#include #include -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/utils/values.h" - namespace gtdynamics { /** @@ -45,7 +44,7 @@ namespace gtdynamics { inline gtsam::NoiseModelFactor::shared_ptr TorqueFactor( const gtsam::noiseModel::Base::shared_ptr &cost_model, const JointConstSharedPtr &joint, size_t k = 0) { - return boost::make_shared>( + return std::make_shared>( cost_model, 0.0, joint->torqueConstraint(k)); } diff --git a/gtdynamics/factors/TwistAccelFactor.h b/gtdynamics/factors/TwistAccelFactor.h index 20ee90aa5..0ce34933c 100644 --- a/gtdynamics/factors/TwistAccelFactor.h +++ b/gtdynamics/factors/TwistAccelFactor.h @@ -14,18 +14,17 @@ #pragma once +#include #include #include #include #include #include -#include +#include #include #include -#include "gtdynamics/universal_robot/Joint.h" - namespace gtdynamics { /** @@ -45,7 +44,7 @@ namespace gtdynamics { inline gtsam::NoiseModelFactor::shared_ptr TwistAccelFactor( const gtsam::noiseModel::Base::shared_ptr &cost_model, JointConstSharedPtr joint, int time) { - return boost::make_shared>( + return std::make_shared>( cost_model, gtsam::Vector6::Zero(), joint->twistAccelConstraint(time)); } diff --git a/gtdynamics/factors/TwistFactor.h b/gtdynamics/factors/TwistFactor.h index 65a6e5f54..6b739d833 100644 --- a/gtdynamics/factors/TwistFactor.h +++ b/gtdynamics/factors/TwistFactor.h @@ -13,17 +13,16 @@ #pragma once +#include #include #include #include #include #include -#include +#include #include -#include "gtdynamics/universal_robot/Joint.h" - namespace gtdynamics { /** @@ -41,7 +40,7 @@ namespace gtdynamics { inline gtsam::NoiseModelFactor::shared_ptr TwistFactor( const gtsam::noiseModel::Base::shared_ptr &cost_model, JointConstSharedPtr joint, int time) { - return boost::make_shared>( + return std::make_shared>( cost_model, gtsam::Vector6::Zero(), joint->twistConstraint(time)); } diff --git a/gtdynamics/factors/WrenchEquivalenceFactor.h b/gtdynamics/factors/WrenchEquivalenceFactor.h index db4c8445d..d3506325e 100644 --- a/gtdynamics/factors/WrenchEquivalenceFactor.h +++ b/gtdynamics/factors/WrenchEquivalenceFactor.h @@ -14,21 +14,20 @@ #pragma once +#include +#include +#include #include #include #include #include #include -#include +#include #include #include #include -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/utils/values.h" - namespace gtdynamics { /** WrenchEquivalenceFactor is a 3-way nonlinear factor which enforces @@ -38,10 +37,10 @@ namespace gtdynamics { * Wrench eq factor, enforce same wrench expressed in different link frames. * @param joint JointConstSharedPtr to the joint */ -inline gtsam::NoiseModelFactor::shared_ptr -WrenchEquivalenceFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model, - const JointConstSharedPtr &joint, size_t k = 0) { - return boost::make_shared>( +inline gtsam::NoiseModelFactor::shared_ptr WrenchEquivalenceFactor( + const gtsam::noiseModel::Base::shared_ptr &cost_model, + const JointConstSharedPtr &joint, size_t k = 0) { + return std::make_shared>( cost_model, gtsam::Vector6::Zero(), joint->wrenchEquivalenceConstraint(k)); } diff --git a/gtdynamics/factors/WrenchFactor.h b/gtdynamics/factors/WrenchFactor.h index 251e7fa88..b1bfc96e0 100644 --- a/gtdynamics/factors/WrenchFactor.h +++ b/gtdynamics/factors/WrenchFactor.h @@ -13,6 +13,9 @@ #pragma once +#include +#include +#include #include #include #include @@ -20,16 +23,10 @@ #include #include -#include -#include +#include #include #include -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/utils/DynamicsSymbol.h" -#include "gtdynamics/utils/utils.h" - namespace gtdynamics { /** @@ -45,9 +42,9 @@ namespace gtdynamics { */ inline gtsam::NoiseModelFactor::shared_ptr WrenchFactor( const gtsam::SharedNoiseModel &cost_model, const LinkConstSharedPtr &link, - const std::vector &wrench_keys, int time, - const boost::optional &gravity = boost::none) { - return boost::make_shared>( + const std::vector &wrench_keys, int time, + const std::optional &gravity = {}) { + return std::make_shared>( cost_model, gtsam::Vector6::Zero(), link->wrenchConstraint(wrench_keys, time, gravity)); } diff --git a/gtdynamics/factors/WrenchPlanarFactor.h b/gtdynamics/factors/WrenchPlanarFactor.h index 6d3dc9daf..b7a0c2611 100644 --- a/gtdynamics/factors/WrenchPlanarFactor.h +++ b/gtdynamics/factors/WrenchPlanarFactor.h @@ -13,21 +13,20 @@ #pragma once +#include +#include +#include +#include +#include #include #include #include #include #include -#include +#include #include -#include "gtdynamics/dynamics/Dynamics.h" -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/utils/utils.h" -#include "gtdynamics/utils/values.h" - namespace gtdynamics { /** @@ -36,13 +35,13 @@ namespace gtdynamics { inline gtsam::Vector3_ WrenchPlanarConstraint(gtsam::Vector3 planar_axis, const JointConstSharedPtr &joint, size_t k = 0) { - gtsam::Matrix36 H_wrench; + gtsam::Matrix36 H_wrench = gtsam::Matrix36::Zero(); if (planar_axis[0] == 1) { // x axis - H_wrench << 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0; + H_wrench << 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0; } else if (planar_axis[1] == 1) { // y axis - H_wrench << 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0; + H_wrench << 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0; } else if (planar_axis[2] == 1) { // z axis - H_wrench << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; + H_wrench << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; } auto wrench_key = WrenchKey(joint->child()->id(), joint->id(), k); @@ -64,7 +63,7 @@ inline gtsam::NoiseModelFactor::shared_ptr WrenchPlanarFactor( const gtsam::noiseModel::Base::shared_ptr &cost_model, gtsam::Vector3 planar_axis, const JointConstSharedPtr &joint, size_t k = 0) { - return boost::make_shared>( + return std::make_shared>( cost_model, gtsam::Vector3::Zero(), WrenchPlanarConstraint(planar_axis, joint, k)); } diff --git a/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h b/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h index 68ecdcf16..726ff4114 100644 --- a/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h +++ b/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h @@ -20,7 +20,6 @@ #include #include -#include #include #include diff --git a/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h b/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h index 969ee16c6..8395ad59f 100644 --- a/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h +++ b/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h @@ -16,9 +16,8 @@ #include #include #include -#include +#include -#include #include #include @@ -27,10 +26,10 @@ namespace gtdynamics { /** ForceBalanceFactor is a three-way nonlinear factor which characterize * the relationship between pressure, contraction length and force */ class ForceBalanceFactor - : public gtsam::NoiseModelFactor3 { + : public gtsam::NoiseModelFactorN { private: typedef ForceBalanceFactor This; - typedef gtsam::NoiseModelFactor3 Base; + typedef gtsam::NoiseModelFactorN Base; double k_, r_, q_rest_; bool positive_; gtsam::Matrix H_delta_x_, H_q_, H_f_; @@ -71,9 +70,9 @@ class ForceBalanceFactor */ gtsam::Vector evaluateError( const double &delta_x, const double &q, const double &f, - boost::optional H_delta_x = boost::none, - boost::optional H_q = boost::none, - boost::optional H_f = boost::none) const override { + gtsam::OptionalMatrixType H_delta_x = nullptr, + gtsam::OptionalMatrixType H_q = nullptr, + gtsam::OptionalMatrixType H_f = nullptr) const override { if (H_delta_x) *H_delta_x = H_delta_x_; if (H_q) *H_q = H_q_; if (H_f) *H_f = H_f_; @@ -82,7 +81,7 @@ class ForceBalanceFactor // @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -95,22 +94,24 @@ class ForceBalanceFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor3", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; /** JointTorqueFactor is a three-way nonlinear factor which characterize * the relationship between pressure, contraction length and force */ class JointTorqueFactor - : public gtsam::NoiseModelFactor4 { + : public gtsam::NoiseModelFactorN { private: typedef JointTorqueFactor This; - typedef gtsam::NoiseModelFactor4 Base; + typedef gtsam::NoiseModelFactorN Base; double q_limit_, ka_, r_, b_; bool positive_; gtsam::Matrix H_v_, H_f_, H_torque_; @@ -155,10 +156,10 @@ class JointTorqueFactor */ gtsam::Vector evaluateError( const double &q, const double &v, const double &f, const double &torque, - boost::optional H_q = boost::none, - boost::optional H_v = boost::none, - boost::optional H_f = boost::none, - boost::optional H_torque = boost::none) const override { + gtsam::OptionalMatrixType H_q = nullptr, + gtsam::OptionalMatrixType H_v = nullptr, + gtsam::OptionalMatrixType H_f = nullptr, + gtsam::OptionalMatrixType H_torque = nullptr) const override { bool antagonistic_active = (positive_ && q > q_limit_) || (!positive_ && q < q_limit_); double delta_q = antagonistic_active ? q - q_limit_ : 0; @@ -179,7 +180,7 @@ class JointTorqueFactor // @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -192,22 +193,24 @@ class JointTorqueFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor4", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; /** SmoothActuatorFactor fits a smooth relationship between pressure, * contraction length and force */ class SmoothActuatorFactor - : public gtsam::NoiseModelFactor3 { + : public gtsam::NoiseModelFactorN { private: typedef SmoothActuatorFactor This; - typedef gtsam::NoiseModelFactor3 Base; + typedef gtsam::NoiseModelFactorN Base; const gtsam::Vector5 x0_coeffs_ = (gtsam::Vector(5) << 3.05583930e+00, 7.58361626e-02, -4.91579771e-04, 1.42792618e-06, -1.54817477e-09) @@ -235,9 +238,9 @@ class SmoothActuatorFactor */ gtsam::Vector evaluateError( const double &delta_x, const double &p, const double &f, - boost::optional H_delta_x = boost::none, - boost::optional H_p = boost::none, - boost::optional H_f = boost::none) const override { + gtsam::OptionalMatrixType H_delta_x = nullptr, + gtsam::OptionalMatrixType H_p = nullptr, + gtsam::OptionalMatrixType H_f = nullptr) const override { double gauge_p = p - 101.325; if (H_f) { @@ -308,7 +311,7 @@ class SmoothActuatorFactor // @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -321,6 +324,7 @@ class SmoothActuatorFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -328,15 +332,16 @@ class SmoothActuatorFactor ar &boost::serialization::make_nvp( "SmoothActuatorFactor", boost::serialization::base_object(*this)); } +#endif }; /** ClippingActuatorFactor (deprecated) fits a non-smooth relationship between * pressure, contraction length and force, and use clipping for force < 0 */ class ClippingActuatorFactor - : public gtsam::NoiseModelFactor3 { + : public gtsam::NoiseModelFactorN { private: typedef ClippingActuatorFactor This; - typedef gtsam::NoiseModelFactor3 Base; + typedef gtsam::NoiseModelFactorN Base; std::vector coeffs_{-17.39, 1.11, 2.22, -0.9486, -0.4481, -0.0003159, 0.1745, 0.01601, 0.0001081, -7.703e-07}; @@ -370,9 +375,9 @@ class ClippingActuatorFactor */ gtsam::Vector evaluateError( const double &delta_x, const double &p, const double &f, - boost::optional H_delta_x = boost::none, - boost::optional H_p = boost::none, - boost::optional H_f = boost::none) const override { + gtsam::OptionalMatrixType H_delta_x = nullptr, + gtsam::OptionalMatrixType H_p = nullptr, + gtsam::OptionalMatrixType H_f = nullptr) const override { std::vector p_powers(4, 1); std::vector delta_x_powers(4, 1); for (size_t i = 1; i < 4; i++) { @@ -460,7 +465,7 @@ class ClippingActuatorFactor // @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -473,21 +478,23 @@ class ClippingActuatorFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor3", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; /** ActuatorVolumeFactor characterize the relationship between actuator volume * and contraction length */ -class ActuatorVolumeFactor : public gtsam::NoiseModelFactor2 { +class ActuatorVolumeFactor : public gtsam::NoiseModelFactorN { private: typedef ActuatorVolumeFactor This; - typedef gtsam::NoiseModelFactor2 Base; + typedef gtsam::NoiseModelFactorN Base; gtsam::Vector4 c_ = gtsam::Vector4(4.243e-5, 3.141e-5, -3.251e-6, 1.28e-7); double D_, L_; @@ -499,9 +506,9 @@ class ActuatorVolumeFactor : public gtsam::NoiseModelFactor2 { virtual ~ActuatorVolumeFactor() {} public: - double computeVolume(const double &l, boost::optional H_l = - boost::none) const { - gtsam::Vector4 l_powers(1, l, l*l, l*l*l); + double computeVolume(const double &l, + gtsam::OptionalMatrixType H_l = nullptr) const { + gtsam::Vector4 l_powers(1, l, l * l, l * l * l); double expected_v = L_ * M_PI * pow(D_ / 2, 2); expected_v += c_.dot(l_powers); @@ -516,9 +523,8 @@ class ActuatorVolumeFactor : public gtsam::NoiseModelFactor2 { } gtsam::Vector evaluateError( - const double &v, const double &l, - boost::optional H_v = boost::none, - boost::optional H_l = boost::none) const override { + const double &v, const double &l, gtsam::OptionalMatrixType H_v = nullptr, + gtsam::OptionalMatrixType H_l = nullptr) const override { double expected_v = computeVolume(l, H_l); if (H_v) { *H_v = -gtsam::I_1x1; @@ -528,7 +534,7 @@ class ActuatorVolumeFactor : public gtsam::NoiseModelFactor2 { // @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -541,13 +547,15 @@ class ActuatorVolumeFactor : public gtsam::NoiseModelFactor2 { } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor2", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/jumpingrobot/factors/PneumaticFactors.h b/gtdynamics/jumpingrobot/factors/PneumaticFactors.h index 922e74f68..8cc5ba665 100644 --- a/gtdynamics/jumpingrobot/factors/PneumaticFactors.h +++ b/gtdynamics/jumpingrobot/factors/PneumaticFactors.h @@ -18,19 +18,18 @@ #include #include #include -#include +#include -#include #include #include namespace gtdynamics { /** GasLawFactor: P*V=Rs*T */ -class GasLawFactor : public gtsam::NoiseModelFactor3 { +class GasLawFactor : public gtsam::NoiseModelFactorN { private: typedef GasLawFactor This; - typedef gtsam::NoiseModelFactor3 Base; + typedef gtsam::NoiseModelFactorN Base; double c_; public: @@ -44,9 +43,9 @@ class GasLawFactor : public gtsam::NoiseModelFactor3 { public: gtsam::Vector evaluateError( const double &p, const double &v, const double &m, - boost::optional H_p = boost::none, - boost::optional H_v = boost::none, - boost::optional H_m = boost::none) const override { + gtsam::OptionalMatrixType H_p = nullptr, + gtsam::OptionalMatrixType H_v = nullptr, + gtsam::OptionalMatrixType H_m = nullptr) const override { if (H_p) { H_p->setConstant(1, 1, 1e3 * v); } @@ -61,7 +60,7 @@ class GasLawFactor : public gtsam::NoiseModelFactor3 { } gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -73,20 +72,22 @@ class GasLawFactor : public gtsam::NoiseModelFactor3 { } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor3", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; /** MassFlowRateFactor: compute mdot from pressures*/ class MassFlowRateFactor - : public gtsam::NoiseModelFactor3 { + : public gtsam::NoiseModelFactorN { private: typedef MassFlowRateFactor This; - typedef gtsam::NoiseModelFactor3 Base; + typedef gtsam::NoiseModelFactorN Base; double D_, L_, mu_, epsilon_, k_; double term1_, term2_, c1_, coeff_; @@ -114,9 +115,9 @@ class MassFlowRateFactor */ double computeExpectedMassFlow( const double &pm, const double &ps, const double &mdot, - boost::optional H_pm = boost::none, - boost::optional H_ps = boost::none, - boost::optional H_mdot = boost::none) const { + gtsam::OptionalMatrixType H_pm = nullptr, + gtsam::OptionalMatrixType H_ps = nullptr, + gtsam::OptionalMatrixType H_mdot = nullptr) const { double tmp = term1_ / abs(mdot) + term2_; double fD = c1_ * pow(log(tmp), -2); double p_square_diff = abs(ps * ps - pm * pm); @@ -142,9 +143,9 @@ class MassFlowRateFactor gtsam::Vector evaluateError( const double &pm, const double &ps, const double &mdot, - boost::optional H_pm = boost::none, - boost::optional H_ps = boost::none, - boost::optional H_mdot = boost::none) const override { + gtsam::OptionalMatrixType H_pm = nullptr, + gtsam::OptionalMatrixType H_ps = nullptr, + gtsam::OptionalMatrixType H_mdot = nullptr) const override { double expected_mdot = computeExpectedMassFlow(pm, ps, mdot, H_pm, H_ps, H_mdot); if (H_mdot) { @@ -155,7 +156,7 @@ class MassFlowRateFactor // @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -168,18 +169,20 @@ class MassFlowRateFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor3", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; -/** Sigmoid function, 1/(1+e^-x), used to model the change of mass flow +/** Sigmoid function, 1/(1+e^-x), used to model the change of mass flow * rate when valve is open/closed. */ -double sigmoid(double x, boost::optional H_x = boost::none) { +double sigmoid(double x, gtsam::OptionalMatrixType H_x = nullptr) { double neg_exp = exp(-x); if (H_x) { H_x->setConstant(1, 1, neg_exp / pow(1.0 + neg_exp, 2)); @@ -189,10 +192,10 @@ double sigmoid(double x, boost::optional H_x = boost::none) { /** ValveControlFactor: compute true mdot based on valve open/close time. */ class ValveControlFactor - : public gtsam::NoiseModelFactor5 { + : public gtsam::NoiseModelFactorN { private: typedef ValveControlFactor This; - typedef gtsam::NoiseModelFactor5 Base; + typedef gtsam::NoiseModelFactorN Base; double ct_inv_; public: @@ -211,10 +214,10 @@ class ValveControlFactor */ double computeExpectedTrueMassFlow( const double &t, const double &to, const double &tc, const double &mdot, - boost::optional H_t = boost::none, - boost::optional H_to = boost::none, - boost::optional H_tc = boost::none, - boost::optional H_mdot = boost::none) const { + gtsam::OptionalMatrixType H_t = nullptr, + gtsam::OptionalMatrixType H_to = nullptr, + gtsam::OptionalMatrixType H_tc = nullptr, + gtsam::OptionalMatrixType H_mdot = nullptr) const { double dto = (t - to) * ct_inv_; double dtc = (t - tc) * ct_inv_; double coeff = sigmoid(dto, H_to) - sigmoid(dtc, H_tc); @@ -236,13 +239,11 @@ class ValveControlFactor gtsam::Vector evaluateError( const double &t, const double &to, const double &tc, const double &mdot, - const double &true_mdot, - boost::optional H_t = boost::none, - boost::optional H_to = boost::none, - boost::optional H_tc = boost::none, - boost::optional H_mdot = boost::none, - boost::optional H_true_mdot = - boost::none) const override { + const double &true_mdot, gtsam::OptionalMatrixType H_t = nullptr, + gtsam::OptionalMatrixType H_to = nullptr, + gtsam::OptionalMatrixType H_tc = nullptr, + gtsam::OptionalMatrixType H_mdot = nullptr, + gtsam::OptionalMatrixType H_true_mdot = nullptr) const override { double expected_true_mdot = computeExpectedTrueMassFlow(t, to, tc, mdot, H_t, H_to, H_tc, H_mdot); if (H_true_mdot) { @@ -253,7 +254,7 @@ class ValveControlFactor // @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -266,13 +267,15 @@ class ValveControlFactor } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int version) { ar &boost::serialization::make_nvp( - "NoiseModelFactor5", boost::serialization::base_object(*this)); + "NoiseModelFactorN", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics \ No newline at end of file diff --git a/gtdynamics/jumpingrobot/src/actuation_graph_builder.py b/gtdynamics/jumpingrobot/src/actuation_graph_builder.py index 70f689294..a65383d6e 100644 --- a/gtdynamics/jumpingrobot/src/actuation_graph_builder.py +++ b/gtdynamics/jumpingrobot/src/actuation_graph_builder.py @@ -74,9 +74,9 @@ def actuator_dynamics_graph(self, jr: JumpingRobot, actuator: Actuator, V_a_key = Actuator.VolumeKey(j, k) delta_x_key = Actuator.ContractionKey(j, k) f_a_key = Actuator.ForceKey(j, k) - torque_key = gtd.TorqueKey(j, k).key() - q_key = gtd.JointAngleKey(j, k).key() - v_key = gtd.JointVelKey(j, k).key() + torque_key = gtd.TorqueKey(j, k) + q_key = gtd.JointAngleKey(j, k) + v_key = gtd.JointVelKey(j, k) graph = NonlinearFactorGraph() graph.add( @@ -116,7 +116,7 @@ def mass_flow_graph(self, jr, actuator, k): mdot_sigma_key = Actuator.MassRateActualKey(j, k) To_a_key = Actuator.ValveOpenTimeKey(j) Tc_a_key = Actuator.ValveCloseTimeKey(j) - t_key = gtd.TimeKey(k).key() + t_key = gtd.TimeKey(k) graph = gtsam.NonlinearFactorGraph() graph.add( @@ -144,7 +144,7 @@ def collocation_graph(self, jr: JumpingRobot, phase = step_phases[time_step] k_prev = time_step k_curr = time_step + 1 - dt_key = gtd.PhaseKey(phase).key() + dt_key = gtd.PhaseKey(phase) # collcoation on actuator mass mdot_prev_keys = [] diff --git a/gtdynamics/jumpingrobot/src/jr_graph_builder.py b/gtdynamics/jumpingrobot/src/jr_graph_builder.py index 5ba6b4022..402afa069 100644 --- a/gtdynamics/jumpingrobot/src/jr_graph_builder.py +++ b/gtdynamics/jumpingrobot/src/jr_graph_builder.py @@ -9,16 +9,14 @@ * @author Yetong Zhang """ - - import inspect import os.path as osp import sys currentdir = osp.dirname(osp.abspath(inspect.getfile(inspect.currentframe()))) parentdir = osp.dirname(currentdir) -sys.path.insert(0,parentdir) -sys.path.insert(0,currentdir) +sys.path.insert(0, parentdir) +sys.path.insert(0, currentdir) import gtdynamics as gtd import gtsam @@ -32,7 +30,6 @@ class JRGraphBuilder: """ Class that constructs factor graphs for a jumping robot. """ - def __init__(self): """Initialize the graph builder, specify all noise models.""" self.robot_graph_builder = RobotGraphBuilder() @@ -41,17 +38,19 @@ def __init__(self): def collocation_graph(self, jr: JumpingRobot, step_phases: list): """ Create a factor graph containing collocation constraints. """ graph = self.actuation_graph_builder.collocation_graph(jr, step_phases) - graph.push_back(self.robot_graph_builder.collocation_graph(jr, step_phases)) + graph.push_back( + self.robot_graph_builder.collocation_graph(jr, step_phases)) # add collocation factors for time for time_step in range(len(step_phases)): phase = step_phases[time_step] k_prev = time_step - k_curr = time_step+1 - dt_key = gtd.PhaseKey(phase).key() - time_prev_key = gtd.TimeKey(k_prev).key() - time_curr_key = gtd.TimeKey(k_curr).key() - time_col_cost_model = self.robot_graph_builder.graph_builder.opt().time_cost_model + k_curr = time_step + 1 + dt_key = gtd.PhaseKey(phase) + time_prev_key = gtd.TimeKey(k_prev) + time_curr_key = gtd.TimeKey(k_curr) + time_col_cost_model = self.robot_graph_builder.graph_builder.opt( + ).time_cost_model gtd.AddTimeCollocationFactor(graph, time_prev_key, time_curr_key, dt_key, time_col_cost_model) diff --git a/gtdynamics/jumpingrobot/src/jr_simulator.py b/gtdynamics/jumpingrobot/src/jr_simulator.py index 1497bfe37..30f7b1876 100644 --- a/gtdynamics/jumpingrobot/src/jr_simulator.py +++ b/gtdynamics/jumpingrobot/src/jr_simulator.py @@ -55,9 +55,9 @@ def step_integration(self, k, dt, values, include_actuation=True): JRValues.integrate_mass(self.jr, values, k, dt) # integrate time - t_prev = values.atDouble(gtd.TimeKey(k - 1).key()) + t_prev = values.atDouble(gtd.TimeKey(k - 1)) t_curr = t_prev + dt - values.insert(gtd.TimeKey(k).key(), t_curr) + values.insert(gtd.TimeKey(k), t_curr) def step_actuation_dynamics(self, k, values): """ Perform actuation dynamics by solving the actuation dynamics factor @@ -86,8 +86,8 @@ def step_actuation_dynamics(self, k, values): graph = self.jr_graph_builder.actuation_graph_builder.actuator_dynamics_graph( self.jr, actuator, k) m_a_key = Actuator.MassKey(j, k) - q_key = gtd.JointAngleKey(j, k).key() - v_key = gtd.JointVelKey(j, k).key() + q_key = gtd.JointAngleKey(j, k) + v_key = gtd.JointVelKey(j, k) m_a = values.atDouble(m_a_key) q = values.atDouble(q_key) v = values.atDouble(v_key) @@ -145,14 +145,14 @@ def step_robot_dynamics_by_layer(self, k, values): # solve q level graph_q = robot_graph_builder.graph_builder.qFactors( self.jr.robot, k, None) - pose_key = gtd.PoseKey(torso_i, k).key() + pose_key = gtd.PoseKey(torso_i, k) torso_pose = gtd.Pose(values, torso_i, k) graph_q.add( gtsam.PriorFactorPose3(pose_key, torso_pose, opt.p_cost_model)) if "ground" not in link_names: for joint in self.jr.robot.joints(): j = joint.id() - q_key = gtd.JointAngleKey(j, k).key() + q_key = gtd.JointAngleKey(j, k) graph_q.add( gtd.PriorFactorDouble(q_key, gtd.JointAngle(values, j, k), opt.prior_q_cost_model)) @@ -164,20 +164,20 @@ def step_robot_dynamics_by_layer(self, k, values): # solve v level graph_v = robot_graph_builder.graph_builder.vFactors( self.jr.robot, k, None) - twist_key = gtd.TwistKey(torso_i, k).key() + twist_key = gtd.TwistKey(torso_i, k) torso_twist = gtd.Twist(values, torso_i, k) graph_v.add( gtd.PriorFactorVector6(twist_key, torso_twist, opt.v_cost_model)) for joint in self.jr.robot.joints(): j = joint.id() - q_key = gtd.JointAngleKey(j, k).key() + q_key = gtd.JointAngleKey(j, k) graph_v.add( gtd.PriorFactorDouble(q_key, init_values.atDouble(q_key), opt.prior_q_cost_model)) if not "ground" in link_names: for joint in self.jr.robot.joints(): j = joint.id() - v_key = gtd.JointVelKey(j, k).key() + v_key = gtd.JointVelKey(j, k) graph_v.add( gtd.PriorFactorDouble(v_key, gtd.JointVel(values, j, k), opt.prior_qv_cost_model)) @@ -196,9 +196,9 @@ def step_robot_dynamics_by_layer(self, k, values): for joint in self.jr.robot.joints(): j = joint.id() - q_key = gtd.JointAngleKey(j, k).key() - v_key = gtd.JointVelKey(j, k).key() - torque_key = gtd.TorqueKey(j, k).key() + q_key = gtd.JointAngleKey(j, k) + v_key = gtd.JointVelKey(j, k) + torque_key = gtd.TorqueKey(j, k) graph_dynamics.add( gtd.PriorFactorDouble(q_key, init_values.atDouble(q_key), opt.prior_q_cost_model)) @@ -211,8 +211,8 @@ def step_robot_dynamics_by_layer(self, k, values): opt.prior_t_cost_model)) for link in self.jr.robot.links(): i = link.id() - pose_key = gtd.PoseKey(i, k).key() - twist_key = gtd.TwistKey(i, k).key() + pose_key = gtd.PoseKey(i, k) + twist_key = gtd.TwistKey(i, k) graph_dynamics_keys = [ key for key in gtd.KeySetToKeyVector(graph_dynamics.keys()) ] @@ -257,7 +257,7 @@ def step_robot_dynamics(self, k, values): graph = robot_graph_builder.dynamics_graph(self.jr, k) for acutator in self.jr.actuators: j = acutator.j - torque_key = gtd.TorqueKey(j, k).key() + torque_key = gtd.TorqueKey(j, k) torque = values.atDouble(torque_key) graph.add( gtd.PriorFactorDouble(torque_key, torque, @@ -265,11 +265,11 @@ def step_robot_dynamics(self, k, values): # prior on torso link pose and twist i = self.jr.robot.link("torso").id() - pose_key = gtd.PoseKey(i, k).key() + pose_key = gtd.PoseKey(i, k) torso_pose = gtd.Pose(values, i, k) graph.add( gtsam.PriorFactorPose3(pose_key, torso_pose, opt.p_cost_model)) - twist_key = gtd.TwistKey(i, k).key() + twist_key = gtd.TwistKey(i, k) torso_twist = gtd.Twist(values, i, k) graph.add( gtd.PriorFactorVector6(twist_key, torso_twist, opt.v_cost_model)) @@ -278,11 +278,11 @@ def step_robot_dynamics(self, k, values): if "ground" not in link_names: for joint in self.jr.robot.joints(): j = joint.id() - q_key = gtd.JointAngleKey(j, k).key() + q_key = gtd.JointAngleKey(j, k) graph.add( gtd.PriorFactorDouble(q_key, gtd.JointAngle(values, j, k), opt.prior_q_cost_model)) - v_key = gtd.JointVelKey(j, k).key() + v_key = gtd.JointVelKey(j, k) graph.add( gtd.PriorFactorDouble(v_key, gtd.JointVel(values, j, k), opt.prior_v_cost_model)) diff --git a/gtdynamics/jumpingrobot/src/jr_values.py b/gtdynamics/jumpingrobot/src/jr_values.py index 357c34fcc..adb7d55fd 100644 --- a/gtdynamics/jumpingrobot/src/jr_values.py +++ b/gtdynamics/jumpingrobot/src/jr_values.py @@ -60,8 +60,8 @@ def init_config_values(jr, controls) -> gtsam.Values: for joint in jr.robot.joints(): j = joint.id() name = joint.name() - q_key = gtd.JointAngleKey(j, 0).key() - v_key = gtd.JointVelKey(j, 0).key() + q_key = gtd.JointAngleKey(j, 0) + v_key = gtd.JointVelKey(j, 0) q = float(jr.init_config["qs"][name]) v = float(jr.init_config["vs"][name]) values.insert(q_key, q) @@ -82,14 +82,14 @@ def init_config_values(jr, controls) -> gtsam.Values: values.insert(Tc_key, float(controls["Tcs"][name])) # time for the first step - values.insert(gtd.TimeKey(0).key(), 0) + values.insert(gtd.TimeKey(0), 0) return values @staticmethod def get_known_values_actuator(j, k, values): """ Construct known values for actuator dynamics graph. """ - q_key = gtd.JointAngleKey(j, k).key() - v_key = gtd.JointVelKey(j, k).key() + q_key = gtd.JointAngleKey(j, k) + v_key = gtd.JointVelKey(j, k) m_a_key = Actuator.MassKey(j, k) init_values = gtsam.Values() @@ -110,7 +110,7 @@ def init_values_from_prev_actuator(j, k, values): V_a_key = Actuator.VolumeKey(j, k) delta_x_key = Actuator.ContractionKey(j, k) f_a_key = Actuator.ForceKey(j, k) - torque_key = gtd.TorqueKey(j, k).key() + torque_key = gtd.TorqueKey(j, k) init_values.insert(P_s_key, values.atDouble(Actuator.SourcePressureKey(k - 1))) @@ -120,8 +120,8 @@ def init_values_from_prev_actuator(j, k, values): values.atDouble(Actuator.ContractionKey(j, k - 1))) init_values.insert(f_a_key, values.atDouble(Actuator.ForceKey(j, k - 1))) - init_values.insert(torque_key, - values.atDouble(gtd.TorqueKey(j, k - 1).key())) + init_values.insert(torque_key, values.atDouble(gtd.TorqueKey(j, + k - 1))) init_values.insert(V_a_key, values.atDouble(Actuator.VolumeKey(j, k - 1))) return init_values @@ -185,11 +185,11 @@ def compute_mass_flow(jr, values, j, k): To_a_key = Actuator.ValveOpenTimeKey(j) Tc_a_key = Actuator.ValveCloseTimeKey(j) - t_key = gtd.TimeKey(k).key() + t_key = gtd.TimeKey(k) mdot_sigma_key = Actuator.MassRateActualKey(j, k) To = values.atDouble(To_a_key) Tc = values.atDouble(Tc_a_key) - curr_time = values.atDouble(gtd.TimeKey(k).key()) + curr_time = values.atDouble(gtd.TimeKey(k)) valve_control_factor = gtd.ValveControlFactor(t_key, To_a_key, Tc_a_key, mdot_key, mdot_sigma_key, @@ -210,7 +210,7 @@ def init_values_from_init_config_actuator(jr, j, k, values): V_a_key = Actuator.VolumeKey(j, k) delta_x_key = Actuator.ContractionKey(j, k) f_a_key = Actuator.ForceKey(j, k) - torque_key = gtd.TorqueKey(j, k).key() + torque_key = gtd.TorqueKey(j, k) init_values.insert(P_s_key, values.atDouble(P_s_key)) init_values.insert(P_a_key, 101.325) @@ -231,8 +231,8 @@ def init_values_from_prev_robot(robot, k, values): init_values = gtsam.Values() for joint in robot.joints(): j = joint.id() - q_key = gtd.JointAngleKey(j, k).key() - v_key = gtd.JointVelKey(j, k).key() + q_key = gtd.JointAngleKey(j, k) + v_key = gtd.JointVelKey(j, k) JRValues.copy_value(values, init_values, q_key) JRValues.copy_value(values, init_values, v_key) gtd.InsertJointAccel(init_values, j, k, @@ -243,16 +243,16 @@ def init_values_from_prev_robot(robot, k, values): gtd.Wrench(values, i1, j, k - 1)) gtd.InsertWrench(init_values, i2, j, k, gtd.Wrench(values, i2, j, k - 1)) - torque_key = gtd.TorqueKey(j, k).key() + torque_key = gtd.TorqueKey(j, k) JRValues.copy_value(values, init_values, torque_key) for link in robot.links(): i = link.id() - if values.exists(gtd.PoseKey(i, k).key()): + if values.exists(gtd.PoseKey(i, k)): gtd.InsertPose(init_values, i, k, gtd.Pose(values, i, k)) else: gtd.InsertPose(init_values, i, k, gtd.Pose(values, i, k - 1)) - if values.exists(gtd.TwistKey(i, k).key()): + if values.exists(gtd.TwistKey(i, k)): gtd.InsertTwist(init_values, i, k, gtd.Twist(values, i, k)) else: gtd.InsertTwist(init_values, i, k, gtd.Twist(values, i, k - 1)) @@ -278,8 +278,8 @@ def init_values_from_fk_robot(jr, k, values): init_values = gtsam.Values() for link in jr.robot.links(): i = link.id() - pose_key = gtd.PoseKey(i, k).key() - twist_key = gtd.TwistKey(i, k).key() + pose_key = gtd.PoseKey(i, k) + twist_key = gtd.TwistKey(i, k) pose = fk_results.atPose3(pose_key) if values.exists(pose_key): pose = gtd.Pose(values, i, k) @@ -293,8 +293,8 @@ def init_values_from_fk_robot(jr, k, values): # assign zeros to unknown values for joint in jr.robot.joints(): j = joint.id() - q_key = gtd.JointAngleKey(j, k).key() - v_key = gtd.JointVelKey(j, k).key() + q_key = gtd.JointAngleKey(j, k) + v_key = gtd.JointVelKey(j, k) q = 0.0 v = 0.0 if values.exists(q_key): @@ -308,7 +308,7 @@ def init_values_from_fk_robot(jr, k, values): i2 = joint.child().id() gtd.InsertWrench(init_values, i1, j, k, np.zeros(6)) gtd.InsertWrench(init_values, i2, j, k, np.zeros(6)) - torque_key = gtd.TorqueKey(j, k).key() + torque_key = gtd.TorqueKey(j, k) if values.exists(torque_key): gtd.InsertTorque(init_values, j, k, values.atDouble(torque_key)) diff --git a/gtdynamics/jumpingrobot/src/jr_visualizer.py b/gtdynamics/jumpingrobot/src/jr_visualizer.py index 6134861a3..63bdd87c6 100644 --- a/gtdynamics/jumpingrobot/src/jr_visualizer.py +++ b/gtdynamics/jumpingrobot/src/jr_visualizer.py @@ -43,10 +43,10 @@ def update_jr_frame(ax, values, jr, k): z = pose.z() theta = pose.rotation().roll() l = 0.55 - start_y = y - l/2 * np.cos(theta) - start_z = z - l/2 * np.sin(theta) - end_y = y + l/2 * np.cos(theta) - end_z = z + l/2 * np.sin(theta) + start_y = y - l / 2 * np.cos(theta) + start_z = z - l / 2 * np.sin(theta) + end_y = y + l / 2 * np.cos(theta) + end_z = z + l / 2 * np.sin(theta) ax.plot([start_y, end_y], [start_z, end_z], color=color) @@ -92,6 +92,7 @@ def visualize_jr_trajectory(values, jr, num_steps, step=1): def animate(i): update_jr_frame(ax, values, jr, i) + frames = np.arange(0, num_steps, step) FuncAnimation(fig, animate, frames=frames, interval=10) plt.show() @@ -100,11 +101,13 @@ def animate(i): def make_plot(values, jr, num_steps): """ Draw plots of all quantities with time. """ joint_names = ["knee_r", "hip_r", "hip_l", "knee_l"] - colors = {"knee_r": "red", - "hip_r": "orange", - "hip_l": "green", - "knee_l": "blue", - "source": "black"} + colors = { + "knee_r": "red", + "hip_r": "orange", + "hip_l": "green", + "knee_l": "blue", + "source": "black" + } qs_dict = {name: [] for name in joint_names} vs_dict = {name: [] for name in joint_names} @@ -124,39 +127,62 @@ def make_plot(values, jr, num_steps): qs_dict[name].append(gtd.JointAngle(values, j, k)) vs_dict[name].append(gtd.JointVel(values, j, k)) torques_dict[name].append(gtd.Torque(values, j, k)) - pressures_dict[name].append(values.atDouble(Actuator.PressureKey(j, k))) + pressures_dict[name].append( + values.atDouble(Actuator.PressureKey(j, k))) masses_dict[name].append(values.atDouble(Actuator.MassKey(j, k))) - mdots_dict[name].append(values.atDouble(Actuator.MassRateActualKey(j, k))) - contractions_dict[name].append(values.atDouble(Actuator.ContractionKey(j, k))) + mdots_dict[name].append( + values.atDouble(Actuator.MassRateActualKey(j, k))) + contractions_dict[name].append( + values.atDouble(Actuator.ContractionKey(j, k))) forces_dict[name].append(values.atDouble(Actuator.ForceKey(j, k))) - masses_dict["source"].append(values.atDouble(Actuator.SourceMassKey(k))) - pressures_dict["source"].append(values.atDouble(Actuator.SourcePressureKey(k))) - time_list.append(values.atDouble(gtd.TimeKey(k).key())) + masses_dict["source"].append(values.atDouble( + Actuator.SourceMassKey(k))) + pressures_dict["source"].append( + values.atDouble(Actuator.SourcePressureKey(k))) + time_list.append(values.atDouble(gtd.TimeKey(k))) fig, axs = plt.subplots(2, 3, sharex=True, figsize=(10, 6.7), dpi=80) for name in qs_dict.keys(): - axs[0, 0].plot(time_list, qs_dict[name], label=name, color=colors[name]) + axs[0, 0].plot(time_list, + qs_dict[name], + label=name, + color=colors[name]) axs[0, 0].set_title("joint angle") for name in torques_dict.keys(): - axs[0, 1].plot(time_list, torques_dict[name], label=name, color=colors[name]) + axs[0, 1].plot(time_list, + torques_dict[name], + label=name, + color=colors[name]) axs[0, 1].set_title("torque") for name in forces_dict.keys(): - axs[0, 2].plot(time_list, forces_dict[name], label=name, color=colors[name]) + axs[0, 2].plot(time_list, + forces_dict[name], + label=name, + color=colors[name]) axs[0, 2].set_title("force") for name in pressures_dict.keys(): - axs[1, 0].plot(time_list, pressures_dict[name], label=name, color=colors[name]) + axs[1, 0].plot(time_list, + pressures_dict[name], + label=name, + color=colors[name]) axs[1, 0].set_title("pressure") for name in masses_dict.keys(): - axs[1, 1].plot(time_list, masses_dict[name], label=name, color=colors[name]) + axs[1, 1].plot(time_list, + masses_dict[name], + label=name, + color=colors[name]) axs[1, 1].set_title("mass") for name in contractions_dict.keys(): - axs[1, 2].plot(time_list, contractions_dict[name], label=name, color=colors[name]) + axs[1, 2].plot(time_list, + contractions_dict[name], + label=name, + color=colors[name]) axs[1, 2].set_title("contraction") plt.show() diff --git a/gtdynamics/jumpingrobot/src/jumping_robot.py b/gtdynamics/jumpingrobot/src/jumping_robot.py index 39cf6ddd1..f2495684f 100644 --- a/gtdynamics/jumpingrobot/src/jumping_robot.py +++ b/gtdynamics/jumpingrobot/src/jumping_robot.py @@ -9,18 +9,16 @@ * @author Yetong Zhang """ - -import yaml -import numpy as np -import gtsam -from gtsam import Pose2, Pose3, Point3, Rot3, Values -from gtsam import NonlinearFactorGraph, RangeFactorPose2, PriorFactorPose2 import gtdynamics as gtd +import gtsam +import numpy as np +import yaml +from gtsam import (NonlinearFactorGraph, Point3, Pose2, Pose3, + PriorFactorPose2, RangeFactorPose2, Rot3, Values) class Actuator: """ Class that stores all parameters for an actuator. """ - def __init__(self, name, robot, actuator_config, positive): self.name = name self.j = robot.joint(name).id() @@ -30,6 +28,7 @@ def __init__(self, name, robot, actuator_config, positive): """ actuator specific keys: all keys use the same naming convention as paper """ + @staticmethod def PressureKey(j, t): return gtd.DynamicsSymbol.JointSymbol("Pa", j, t).key() @@ -82,7 +81,6 @@ def ValveCloseTimeKey(j): class JumpingRobot: """ Class that stores a GTDynamics robot class and all parameters for a jumping robot. """ - def __init__(self, yaml_file_path, init_config, phase=0): """ Constructor @@ -98,10 +96,12 @@ def __init__(self, yaml_file_path, init_config, phase=0): self.params = self.load_file(yaml_file_path) self.init_config = init_config self.robot = self.create_robot(self.params, phase) - self.actuators = [Actuator("knee_r", self.robot, self.params["knee"], False), - Actuator("hip_r", self.robot, self.params["hip"], True), - Actuator("hip_l", self.robot, self.params["hip"], True), - Actuator("knee_l", self.robot, self.params["knee"], False)] + self.actuators = [ + Actuator("knee_r", self.robot, self.params["knee"], False), + Actuator("hip_r", self.robot, self.params["hip"], True), + Actuator("hip_l", self.robot, self.params["hip"], True), + Actuator("knee_l", self.robot, self.params["knee"], False) + ] Rs = self.params["pneumatic"]["Rs"] temperature = self.params["pneumatic"]["T"] @@ -115,8 +115,9 @@ def load_file(yaml_file_path: str): return params @staticmethod - def create_init_config(torso_pose=gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0, 0, 1.1)), - torso_twist = np.zeros(6), + def create_init_config(torso_pose=gtsam.Pose3(gtsam.Rot3(), + gtsam.Point3(0, 0, 1.1)), + torso_twist=np.zeros(6), rest_angles=[0, 0, 0, 0, 0, 0], init_angles=[0, 0, 0, 0, 0, 0], init_vels=[0, 0, 0, 0, 0, 0]): @@ -132,8 +133,9 @@ def create_init_config(torso_pose=gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0, 0, 1 Returns: Dict: specifiction of initial configuration """ - joint_names = ["foot_r", "knee_r", - "hip_r", "hip_l", "knee_l", "foot_l"] + joint_names = [ + "foot_r", "knee_r", "hip_r", "hip_l", "knee_l", "foot_l" + ] init_config = {} init_config["torso_pose"] = torso_pose init_config["torso_twist"] = torso_twist @@ -181,36 +183,42 @@ def create_robot(params, phase) -> gtd.Robot: foot_distance = params["morphology"]["foot_dist"] # compute link, joint poses - link_poses, joint_poses = JumpingRobot.compute_poses(length_list, foot_distance) - + link_poses, joint_poses = JumpingRobot.compute_poses( + length_list, foot_distance) + # cosntruct links ground = gtd.Link(0, "ground", 1, np.eye(3), Pose3(), Pose3(), True) - shank_r = JumpingRobot.construct_link( - 1, "shank_r", mass_list[4], length_list[4], link_radius, link_poses["shank_r"]) - thigh_r = JumpingRobot.construct_link( - 2, "thigh_r", mass_list[3], length_list[3], link_radius, link_poses["thigh_r"]) - torso = JumpingRobot.construct_link( - 3, "torso", mass_list[2], length_list[2], link_radius, link_poses["torso"]) - thigh_l = JumpingRobot.construct_link( - 4, "thigh_l", mass_list[1], length_list[1], link_radius, link_poses["thigh_l"]) - shank_l = JumpingRobot.construct_link( - 5, "shank_l", mass_list[0], length_list[0], link_radius, link_poses["shank_l"]) + shank_r = JumpingRobot.construct_link(1, "shank_r", mass_list[4], + length_list[4], link_radius, + link_poses["shank_r"]) + thigh_r = JumpingRobot.construct_link(2, "thigh_r", mass_list[3], + length_list[3], link_radius, + link_poses["thigh_r"]) + torso = JumpingRobot.construct_link(3, "torso", mass_list[2], + length_list[2], link_radius, + link_poses["torso"]) + thigh_l = JumpingRobot.construct_link(4, "thigh_l", mass_list[1], + length_list[1], link_radius, + link_poses["thigh_l"]) + shank_l = JumpingRobot.construct_link(5, "shank_l", mass_list[0], + length_list[0], link_radius, + link_poses["shank_l"]) # construct joints axis_r = np.array([1, 0, 0]) axis_l = np.array([-1, 0, 0]) - foot_r = gtd.RevoluteJoint( - 0, "foot_r", joint_poses["foot_r"], ground, shank_r, axis_r, gtd.JointParams()) - knee_r = gtd.RevoluteJoint( - 1, "knee_r", joint_poses["knee_r"], shank_r, thigh_r, axis_r, gtd.JointParams()) - hip_r = gtd.RevoluteJoint( - 2, "hip_r", joint_poses["hip_r"], thigh_r, torso, axis_r, gtd.JointParams()) - hip_l = gtd.RevoluteJoint( - 3, "hip_l", joint_poses["hip_l"], thigh_l, torso, axis_l, gtd.JointParams()) - knee_l = gtd.RevoluteJoint( - 4, "knee_l", joint_poses["knee_l"], shank_l, thigh_l, axis_l, gtd.JointParams()) - foot_l = gtd.RevoluteJoint( - 5, "foot_l", joint_poses["foot_l"], ground, shank_l, axis_l, gtd.JointParams()) + foot_r = gtd.RevoluteJoint(0, "foot_r", joint_poses["foot_r"], ground, + shank_r, axis_r, gtd.JointParams()) + knee_r = gtd.RevoluteJoint(1, "knee_r", joint_poses["knee_r"], shank_r, + thigh_r, axis_r, gtd.JointParams()) + hip_r = gtd.RevoluteJoint(2, "hip_r", joint_poses["hip_r"], thigh_r, + torso, axis_r, gtd.JointParams()) + hip_l = gtd.RevoluteJoint(3, "hip_l", joint_poses["hip_l"], thigh_l, + torso, axis_l, gtd.JointParams()) + knee_l = gtd.RevoluteJoint(4, "knee_l", joint_poses["knee_l"], shank_l, + thigh_l, axis_l, gtd.JointParams()) + foot_l = gtd.RevoluteJoint(5, "foot_l", joint_poses["foot_l"], ground, + shank_l, axis_l, gtd.JointParams()) # use links, joints to create robot if phase == 0: @@ -269,22 +277,27 @@ def compute_poses(length_list: list, foot_distance: float): # of links and joints link_poses = {} link_poses["shank_r"] = Pose3(rot_r, Point3(0, p0.x(), p0.y())) - link_poses["thigh_r"] = Pose3(rot_r, Point3(0, (p0.x() + p1.x())/2, (p0.y() + p1.y())/2)) + link_poses["thigh_r"] = Pose3( + rot_r, Point3(0, (p0.x() + p1.x()) / 2, (p0.y() + p1.y()) / 2)) link_poses["torso"] = Pose3(rot_m, Point3(0, p2.x(), p2.y())) - link_poses["thigh_l"] = Pose3(rot_l, Point3(0, (p2.x() + p3.x())/2, (p2.y()+p3.y())/2)) + link_poses["thigh_l"] = Pose3( + rot_l, Point3(0, (p2.x() + p3.x()) / 2, (p2.y() + p3.y()) / 2)) link_poses["shank_l"] = Pose3(rot_l, Point3(0, p3.x(), p3.y())) joint_poses = {} joint_poses["foot_r"] = Pose3(Rot3(), Point3(0, p0.x(), p0.y())) - joint_poses["knee_r"] = Pose3(Rot3(), Point3(0, (p0.x() + p1.x())/2, (p0.y() + p1.y())/2)) + joint_poses["knee_r"] = Pose3( + Rot3(), Point3(0, (p0.x() + p1.x()) / 2, (p0.y() + p1.y()) / 2)) joint_poses["hip_r"] = Pose3(Rot3(), Point3(0, p1.x(), p1.y())) joint_poses["hip_l"] = Pose3(Rot3(), Point3(0, p2.x(), p2.y())) - joint_poses["knee_l"] = Pose3(Rot3(), Point3(0, (p2.x() + p3.x())/2, (p2.y()+p3.y())/2)) + joint_poses["knee_l"] = Pose3( + Rot3(), Point3(0, (p2.x() + p3.x()) / 2, (p2.y() + p3.y()) / 2)) joint_poses["foot_l"] = Pose3(Rot3(), Point3(0, p3.x(), p3.y())) return link_poses, joint_poses @staticmethod - def compute_poses_helper(length_list: list, foot_distance: float) -> Values: + def compute_poses_helper(length_list: list, + foot_distance: float) -> Values: """ compute the configuration by solving a simple factor graph: - under the constraint that thigh and shank are in the same line, the robot can be simplied as 3 links @@ -298,10 +311,10 @@ def compute_poses_helper(length_list: list, foot_distance: float) -> Values: length_mid = length_list[2] init_values = Values() - init_values.insert(0, Pose2(foot_distance/2, 0, 0)) - init_values.insert(1, Pose2(foot_distance/2, length_right, 0)) - init_values.insert(2, Pose2(-foot_distance/2, length_left, 0)) - init_values.insert(3, Pose2(-foot_distance/2, 0, 0)) + init_values.insert(0, Pose2(foot_distance / 2, 0, 0)) + init_values.insert(1, Pose2(foot_distance / 2, length_right, 0)) + init_values.insert(2, Pose2(-foot_distance / 2, length_left, 0)) + init_values.insert(3, Pose2(-foot_distance / 2, 0, 0)) range_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([1])) prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([1, 1, 1])) @@ -309,15 +322,18 @@ def compute_poses_helper(length_list: list, foot_distance: float) -> Values: graph.add(RangeFactorPose2(0, 1, length_right, range_noise)) graph.add(RangeFactorPose2(1, 2, length_mid, range_noise)) graph.add(RangeFactorPose2(2, 3, length_left, range_noise)) - graph.add(PriorFactorPose2(0, Pose2(foot_distance/2, 0, 0), prior_noise)) - graph.add(PriorFactorPose2(3, Pose2(-foot_distance/2, 0, 0), prior_noise)) + graph.add( + PriorFactorPose2(0, Pose2(foot_distance / 2, 0, 0), prior_noise)) + graph.add( + PriorFactorPose2(3, Pose2(-foot_distance / 2, 0, 0), prior_noise)) return gtsam.LevenbergMarquardtOptimizer(graph, init_values).optimize() @staticmethod - def construct_link(link_id: int, link_name: str, mass: float, length: float, radius: float, pose: Pose3): + def construct_link(link_id: int, link_name: str, mass: float, + length: float, radius: float, pose: Pose3): """ Construct a link. """ - lTcom = Pose3(Rot3(), Point3(0, length/2, 0)) + lTcom = Pose3(Rot3(), Point3(0, length / 2, 0)) bMcom = pose.compose(lTcom) inertia = JumpingRobot.compute_link_inertia(mass, length, radius) return gtd.Link(link_id, link_name, mass, inertia, bMcom, pose, False) @@ -325,8 +341,8 @@ def construct_link(link_id: int, link_name: str, mass: float, length: float, rad @staticmethod def compute_link_inertia(mass: float, length: float, radius: float): """ Compute inertia matrix for a link. """ - Ixx = 1/12 * mass * (3*radius**2 + length**2) - Iyy = 1/2 * mass * radius**2 + Ixx = 1 / 12 * mass * (3 * radius**2 + length**2) + Iyy = 1 / 2 * mass * radius**2 Izz = Ixx inertia = np.diag([Ixx, Iyy, Izz]) return inertia diff --git a/gtdynamics/jumpingrobot/src/robot_graph_builder.py b/gtdynamics/jumpingrobot/src/robot_graph_builder.py index 8cf5ca298..55232dc5b 100644 --- a/gtdynamics/jumpingrobot/src/robot_graph_builder.py +++ b/gtdynamics/jumpingrobot/src/robot_graph_builder.py @@ -73,7 +73,7 @@ def dynamics_graph(self, jr: JumpingRobot, k: int) -> NonlinearFactorGraph: for name in ["foot_l", "foot_r"]: if name in joint_names: j = jr.robot.joint(name).id() - torque_key = gtd.TorqueKey(j, k).key() + torque_key = gtd.TorqueKey(j, k) graph.add( gtd.PriorFactorDouble( torque_key, 0.0, @@ -94,7 +94,7 @@ def collocation_graph(self, jr: JumpingRobot, step_phases: list): phase = step_phases[time_step] k_prev = time_step k_curr = time_step + 1 - dt_key = gtd.PhaseKey(phase).key() + dt_key = gtd.PhaseKey(phase) # collocation on joint angles if phase == 3: @@ -113,12 +113,12 @@ def collocation_graph(self, jr: JumpingRobot, step_phases: list): # collocation on torso link link = jr.robot.link("torso") i = link.id() - pose_prev_key = gtd.PoseKey(i, k_prev).key() - pose_curr_key = gtd.PoseKey(i, k_curr).key() - twist_prev_key = gtd.TwistKey(i, k_prev).key() - twist_curr_key = gtd.TwistKey(i, k_curr).key() - twistaccel_prev_key = gtd.TwistAccelKey(i, k_prev).key() - twistaccel_curr_key = gtd.TwistAccelKey(i, k_curr).key() + pose_prev_key = gtd.PoseKey(i, k_prev) + pose_curr_key = gtd.PoseKey(i, k_curr) + twist_prev_key = gtd.TwistKey(i, k_prev) + twist_curr_key = gtd.TwistKey(i, k_curr) + twistaccel_prev_key = gtd.TwistAccelKey(i, k_prev) + twistaccel_curr_key = gtd.TwistAccelKey(i, k_curr) pose_col_cost_model = self.graph_builder.opt().pose_col_cost_model graph.add( diff --git a/gtdynamics/jumpingrobot/src/test_jr_values.py b/gtdynamics/jumpingrobot/src/test_jr_values.py index ac60b597d..6ea0cfe6a 100644 --- a/gtdynamics/jumpingrobot/src/test_jr_values.py +++ b/gtdynamics/jumpingrobot/src/test_jr_values.py @@ -29,7 +29,6 @@ class TestJRValues(unittest.TestCase): """ Tests for jumping robot. """ - def setUp(self): """ Set up the jumping robot. """ self.yaml_file_path = osp.join(parentdir, "yaml", "robot_config.yaml") @@ -46,9 +45,9 @@ def test_compute_mass_flow_convergence(self): values.insert(Actuator.ValveCloseTimeKey(j), 1) P_a_key = Actuator.PressureKey(j, k) P_s_key = Actuator.SourcePressureKey(k) - t_key = gtd.TimeKey(k).key() + t_key = gtd.TimeKey(k) values.insert(P_a_key, 101.325) - values.insert(P_s_key, 65 * 6894.76/1000) + values.insert(P_s_key, 65 * 6894.76 / 1000) values.insert(t_key, 0.0001) mdot, mdot_sigma = JRValues.compute_mass_flow(self.jr, values, j, k) diff --git a/gtdynamics/jumpingrobot/tests/testPneumaticActuatorFactors.cpp b/gtdynamics/jumpingrobot/tests/testPneumaticActuatorFactors.cpp index 3b16fbd23..0ba4d0199 100644 --- a/gtdynamics/jumpingrobot/tests/testPneumaticActuatorFactors.cpp +++ b/gtdynamics/jumpingrobot/tests/testPneumaticActuatorFactors.cpp @@ -12,6 +12,7 @@ **/ #include +#include #include #include #include @@ -24,8 +25,6 @@ #include -#include "gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h" - using gtdynamics::ForceBalanceFactor, gtdynamics::JointTorqueFactor, gtdynamics::ActuatorVolumeFactor, gtdynamics::SmoothActuatorFactor, gtdynamics::ClippingActuatorFactor; @@ -33,8 +32,8 @@ using gtsam::Symbol, gtsam::Vector1, gtsam::Values, gtsam::Key, gtsam::assert_equal, gtsam::noiseModel::Isotropic; namespace example { - auto cost_model = Isotropic::Sigma(1, 0.001); - gtsam::Symbol q_key('q', 0), v_key('v', 0), f_key('f', 0), torque_key('T', 0), +auto cost_model = Isotropic::Sigma(1, 0.001); +gtsam::Symbol q_key('q', 0), v_key('v', 0), f_key('f', 0), torque_key('T', 0), l_key('l', 0), p_key('p', 0), delta_x_key('x', 0); } // namespace example @@ -92,7 +91,7 @@ TEST(ForceBalanceFactor, Expand) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } -/** Test the joint torque factor with expand configuration of the actuator, +/** Test the joint torque factor with expand configuration of the actuator, * and anatagnostic spring inactive. */ TEST(JointTorqueFactor, ExpandInactive) { double q_limit = 0.4; @@ -265,7 +264,6 @@ TEST(SmoothActuatorFactor, zero_force) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } -//// following tests are deprecated TEST(ClippingActuatorFactor, Factor) { const double delta_x = 1; const double p = 120; diff --git a/gtdynamics/jumpingrobot/tests/testPneumaticFactors.cpp b/gtdynamics/jumpingrobot/tests/testPneumaticFactors.cpp index 5f685f86e..30752272e 100644 --- a/gtdynamics/jumpingrobot/tests/testPneumaticFactors.cpp +++ b/gtdynamics/jumpingrobot/tests/testPneumaticFactors.cpp @@ -12,6 +12,7 @@ **/ #include +#include #include #include #include @@ -24,8 +25,6 @@ #include -#include "gtdynamics/jumpingrobot/factors/PneumaticFactors.h" - using gtdynamics::GasLawFactor, gtdynamics::MassFlowRateFactor, gtdynamics::ValveControlFactor; using gtsam::Symbol, gtsam::Vector1, gtsam::Values, gtsam::Key, diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 0197a3416..5ce89567e 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -120,7 +120,9 @@ struct KinematicsParameters : public OptimizationParameters { prior_q_cost_model; // joint angle prior factor // TODO(yetong): replace noise model with tolerance. - KinematicsParameters(double p_cost_model_sigma = 1e-4, double g_cost_model_sigma = 1e-2, double prior_q_cost_model_sigma = 0.5) + KinematicsParameters(double p_cost_model_sigma = 1e-4, + double g_cost_model_sigma = 1e-2, + double prior_q_cost_model_sigma = 0.5) : p_cost_model(Isotropic::Sigma(6, p_cost_model_sigma)), g_cost_model(Isotropic::Sigma(3, g_cost_model_sigma)), prior_q_cost_model(Isotropic::Sigma(1, prior_q_cost_model_sigma)) {} @@ -155,8 +157,8 @@ class Kinematics : public Optimizer { * @returns Equality constraints. */ template - EqualityConstraints constraints(const CONTEXT& context, - const Robot& robot) const; + gtsam::NonlinearEqualityConstraints constraints(const CONTEXT& context, + const Robot& robot) const; /** * @fn Create point goal objectives. @@ -175,7 +177,7 @@ class Kinematics : public Optimizer { * @returns Equality constraints with point goal constraints. */ template - EqualityConstraints pointGoalConstraints( + gtsam::NonlinearEqualityConstraints pointGoalConstraints( const CONTEXT& context, const ContactGoals& contact_goals) const; /** @@ -230,7 +232,7 @@ class Kinematics : public Optimizer { */ template gtsam::Values initialValues( - const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.1, + const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.0, const gtsam::Values& initial_joints = gtsam::Values(), bool use_fk = false) const; diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index e31c440a3..d3b50d0e6 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -35,19 +35,21 @@ NonlinearFactorGraph Kinematics::graph(const Interval& interval, } template <> -EqualityConstraints Kinematics::constraints( +gtsam::NonlinearEqualityConstraints Kinematics::constraints( const Interval& interval, const Robot& robot) const { - EqualityConstraints constraints; + gtsam::NonlinearEqualityConstraints constraints; for (size_t k = interval.k_start; k <= interval.k_end; k++) { - constraints.add(this->constraints(Slice(k), robot)); + auto slice_constraints = this->constraints(Slice(k), robot); + for (const auto& constraint : slice_constraints) { + constraints.push_back(constraint); + } } return constraints; } template <> NonlinearFactorGraph Kinematics::poseGoalObjectives( - const Interval& interval, - const PoseGoals& pose_goals) const { + const Interval& interval, const PoseGoals& pose_goals) const { NonlinearFactorGraph graph; for (size_t k = interval.k_start; k <= interval.k_end; k++) { graph.add(poseGoalObjectives(Slice(k), pose_goals)); @@ -66,11 +68,14 @@ NonlinearFactorGraph Kinematics::pointGoalObjectives( } template <> -EqualityConstraints Kinematics::pointGoalConstraints( +gtsam::NonlinearEqualityConstraints Kinematics::pointGoalConstraints( const Interval& interval, const ContactGoals& contact_goals) const { - EqualityConstraints constraints; + gtsam::NonlinearEqualityConstraints constraints; for (size_t k = interval.k_start; k <= interval.k_end; k++) { - constraints.add(pointGoalConstraints(Slice(k), contact_goals)); + auto slice_constraints = pointGoalConstraints(Slice(k), contact_goals); + for (const auto& constraint : slice_constraints) { + constraints.push_back(constraint); + } } return constraints; } @@ -96,12 +101,15 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( } template <> -Values Kinematics::initialValues( - const Interval& interval, const Robot& robot, double gaussian_noise, - const gtsam::Values& joint_priors, bool use_fk) const { +Values Kinematics::initialValues(const Interval& interval, + const Robot& robot, + double gaussian_noise, + const gtsam::Values& joint_priors, + bool use_fk) const { Values values; for (size_t k = interval.k_start; k <= interval.k_end; k++) { - values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors, use_fk)); + values.insert( + initialValues(Slice(k), robot, gaussian_noise, joint_priors, use_fk)); } return values; } diff --git a/gtdynamics/kinematics/KinematicsPhase.cpp b/gtdynamics/kinematics/KinematicsPhase.cpp index 0be126e26..21001bef7 100644 --- a/gtdynamics/kinematics/KinematicsPhase.cpp +++ b/gtdynamics/kinematics/KinematicsPhase.cpp @@ -23,6 +23,4 @@ using gtsam::Values; using std::string; using std::vector; - - } // namespace gtdynamics diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 9a01cb11b..c507c189f 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -69,16 +69,17 @@ NonlinearFactorGraph Kinematics::graph(const Slice& slice, } template <> -EqualityConstraints Kinematics::constraints(const Slice& slice, - const Robot& robot) const { - EqualityConstraints constraints; +gtsam::NonlinearEqualityConstraints Kinematics::constraints( + const Slice& slice, const Robot& robot) const { + gtsam::NonlinearEqualityConstraints constraints; // Constrain kinematics at joints. gtsam::Vector6 tolerance = p_.p_cost_model->sigmas(); for (auto&& joint : robot.joints()) { auto constraint_expr = joint->poseConstraint(slice.k); - constraints.emplace_shared>(constraint_expr, - tolerance); + constraints + .emplace_shared>( + constraint_expr, gtsam::Vector6::Zero(), tolerance); } // Constrain fixed links @@ -96,8 +97,9 @@ EqualityConstraints Kinematics::constraints(const Slice& slice, auto constraint_expr = gtsam::logmap(bTcom, bMcom); // Add constraint - constraints.emplace_shared>(constraint_expr, - tolerance); + constraints + .emplace_shared>( + constraint_expr, gtsam::Vector6::Zero(), tolerance); } } @@ -120,9 +122,9 @@ NonlinearFactorGraph Kinematics::pointGoalObjectives( } template <> -EqualityConstraints Kinematics::pointGoalConstraints( +gtsam::NonlinearEqualityConstraints Kinematics::pointGoalConstraints( const Slice& slice, const ContactGoals& contact_goals) const { - EqualityConstraints constraints; + gtsam::NonlinearEqualityConstraints constraints; // Add objectives. gtsam::Vector3 tolerance = p_.g_cost_model->sigmas(); @@ -130,8 +132,9 @@ EqualityConstraints Kinematics::pointGoalConstraints( const gtsam::Key pose_key = PoseKey(goal.link()->id(), slice.k); auto constraint_expr = PointGoalConstraint(pose_key, goal.contactInCoM(), goal.goal_point); - constraints.emplace_shared>(constraint_expr, - tolerance); + constraints + .emplace_shared>( + constraint_expr, gtsam::Vector3::Zero(), tolerance); } return constraints; } @@ -146,7 +149,7 @@ NonlinearFactorGraph Kinematics::poseGoalObjectives( const auto& pose_goal = it->second; auto pose_key = PoseKey(pose_goal.link()->id(), slice.k); const gtsam::Pose3& desired_pose = pose_goal.wTcom(); - // TODO(toni): use poseprior from unstable gtsam slam or create new + // TODO(toni): use pose prior from unstable gtsam slam or create new // factors, to add pose from link7 graph.addPrior(pose_key, desired_pose, p_.p_cost_model); } @@ -175,10 +178,11 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( NonlinearFactorGraph graph; for (auto&& joint : robot.joints()) { graph.add(JointLimitFactor( - JointAngleKey(joint->id(), slice.k), gtsam::noiseModel::Isotropic::Sigma(1, 0.001), + JointAngleKey(joint->id(), slice.k), + gtsam::noiseModel::Isotropic::Sigma(1, 0.001), joint->parameters().scalar_limits.value_lower_limit, joint->parameters().scalar_limits.value_upper_limit, - 0.04));//joint->parameters().scalar_limits.value_limit_threshold)); + 0.04)); // joint->parameters().scalar_limits.value_limit_threshold)); } return graph; } @@ -232,7 +236,10 @@ Values Kinematics::inverse(const Slice& slice, const Robot& robot, // Contact goals if (contact_goals_as_constraints) { - constraints.add(this->pointGoalConstraints(slice, contact_goals)); + auto goal_constraints = this->pointGoalConstraints(slice, contact_goals); + for (const auto& constraint : goal_constraints) { + constraints.push_back(constraint); + } } else { graph.add(pointGoalObjectives(slice, contact_goals)); } @@ -240,7 +247,7 @@ Values Kinematics::inverse(const Slice& slice, const Robot& robot, // Target joint angles. graph.add(jointAngleObjectives(slice, robot)); - // TODO(frank): allo pose prior as well. + // TODO(frank): allow pose prior as well. // graph.addPrior(PoseKey(0, slice.k), // gtsam::Pose3(), nullptr); diff --git a/gtdynamics/kinematics/KinematicsTrajectory.cpp b/gtdynamics/kinematics/KinematicsTrajectory.cpp index a35ee1088..4629705a7 100644 --- a/gtdynamics/kinematics/KinematicsTrajectory.cpp +++ b/gtdynamics/kinematics/KinematicsTrajectory.cpp @@ -35,11 +35,14 @@ NonlinearFactorGraph Kinematics::graph(const Trajectory& trajectory, } template <> -EqualityConstraints Kinematics::constraints( +gtsam::NonlinearEqualityConstraints Kinematics::constraints( const Trajectory& trajectory, const Robot& robot) const { - EqualityConstraints constraints; + gtsam::NonlinearEqualityConstraints constraints; for (auto&& phase : trajectory.phases()) { - constraints.add(this->constraints(phase, robot)); + auto phase_constraints = this->constraints(phase, robot); + for (const auto& constraint : phase_constraints) { + constraints.push_back(constraint); + } } return constraints; } @@ -55,11 +58,15 @@ NonlinearFactorGraph Kinematics::pointGoalObjectives( } template <> -EqualityConstraints Kinematics::pointGoalConstraints( +gtsam::NonlinearEqualityConstraints Kinematics::pointGoalConstraints( const Trajectory& trajectory, const ContactGoals& contact_goals) const { - EqualityConstraints constraints; + gtsam::NonlinearEqualityConstraints constraints; for (auto&& phase : trajectory.phases()) { - constraints.add(pointGoalConstraints(phase, contact_goals)); + auto phase_constraints = + pointGoalConstraints(phase, contact_goals); + for (const auto& constraint : phase_constraints) { + constraints.push_back(constraint); + } } return constraints; } diff --git a/gtdynamics/optimizer/AugmentedLagrangianOptimizer.cpp b/gtdynamics/optimizer/AugmentedLagrangianOptimizer.cpp deleted file mode 100644 index 49db011da..000000000 --- a/gtdynamics/optimizer/AugmentedLagrangianOptimizer.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file AugmentedLagrangianOptimizer.cpp - * @brief Augmented Lagrangian optimization routines. - * @author: Yetong Zhang, Frank Dellaert - */ - -#include "gtdynamics/optimizer/AugmentedLagrangianOptimizer.h" - -namespace gtdynamics { - -/** Update penalty parameter and Lagrangian multipliers from unconstrained - * optimization result. */ -void update_parameters(const EqualityConstraints& constraints, - const gtsam::Values& previous_values, - const gtsam::Values& current_values, double& mu, - std::vector& z) { - double previous_error = 0; - double current_error = 0; - for (size_t constraint_index = 0; constraint_index < constraints.size(); - constraint_index++) { - auto constraint = constraints.at(constraint_index); - - // Update Lagrangian multipliers. - auto violation = (*constraint)(current_values); - z[constraint_index] += mu * violation; - - // Sum errors for updating penalty parameter. - previous_error += - pow(constraint->toleranceScaledViolation(previous_values).norm(), 2); - current_error += - pow(constraint->toleranceScaledViolation(current_values).norm(), 2); - } - - // Update penalty parameter. - if (sqrt(current_error) >= 0.25 * sqrt(previous_error)) { - mu *= 2; - } -} - -gtsam::Values AugmentedLagrangianOptimizer::optimize( - const gtsam::NonlinearFactorGraph& graph, - const EqualityConstraints& constraints, const gtsam::Values& initial_values, - ConstrainedOptResult* intermediate_result) const { - gtsam::Values values = initial_values; - - // Set initial values for penalty parameter and Lagrangian multipliers. - double mu = 1.0; // penalty parameter - std::vector z; // Lagrangian multiplier - for (const auto& constraint : constraints) { - z.push_back(gtsam::Vector::Zero(constraint->dim())); - } - - // Solve the constrained optimization problem by solving a sequence of - // unconstrained optimization problems. - for (int i = 0; i < p_.num_iterations; i++) { - // Construct merit function. - gtsam::NonlinearFactorGraph merit_graph = graph; - - // Create factors corresponding to penalty terms of constraints. - for (size_t constraint_index = 0; constraint_index < constraints.size(); - constraint_index++) { - auto constraint = constraints.at(constraint_index); - gtsam::Vector bias = z[constraint_index] / mu; - merit_graph.add(constraint->createFactor(mu, bias)); - } - - // Run LM optimization. - gtsam::LevenbergMarquardtOptimizer optimizer(merit_graph, values, - p_.lm_parameters); - auto result = optimizer.optimize(); - - // Update parameters. - update_parameters(constraints, values, result, mu, z); - - // Update values. - values = result; - - /// Store intermediate results. - if (intermediate_result != nullptr) { - intermediate_result->intermediate_values.push_back(values); - intermediate_result->num_iters.push_back(optimizer.getInnerIterations()); - intermediate_result->mu_values.push_back(mu); - } - } - return values; -} - -} // namespace gtdynamics diff --git a/gtdynamics/optimizer/AugmentedLagrangianOptimizer.h b/gtdynamics/optimizer/AugmentedLagrangianOptimizer.h deleted file mode 100644 index eafa633ea..000000000 --- a/gtdynamics/optimizer/AugmentedLagrangianOptimizer.h +++ /dev/null @@ -1,56 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file AugmentedLagrangianOptimizer.h - * @brief Augmented Lagrangian method for constrained optimization. - * @author Yetong Zhang, Frank Dellaert - */ - -#pragma once - -#include "gtdynamics/optimizer/ConstrainedOptimizer.h" - -namespace gtdynamics { - -/// Parameters for Augmented Lagrangian method -struct AugmentedLagrangianParameters - : public ConstrainedOptimizationParameters { - using Base = ConstrainedOptimizationParameters; - size_t num_iterations; - - AugmentedLagrangianParameters() - : Base(gtsam::LevenbergMarquardtParams()), num_iterations(12) {} - - AugmentedLagrangianParameters( - const gtsam::LevenbergMarquardtParams& _lm_parameters, - const size_t& _num_iterations = 12) - : Base(_lm_parameters), num_iterations(_num_iterations) {} -}; - -/// Augmented Lagrangian method only considering equality constraints. -class AugmentedLagrangianOptimizer : public ConstrainedOptimizer { - protected: - const AugmentedLagrangianParameters p_; - - public: - /// Default constructor - AugmentedLagrangianOptimizer() : p_(AugmentedLagrangianParameters()) {} - - /// Construct from parameters. - AugmentedLagrangianOptimizer(const AugmentedLagrangianParameters& parameters) - : p_(parameters) {} - - /// Run optimization. - gtsam::Values optimize( - const gtsam::NonlinearFactorGraph& graph, - const EqualityConstraints& constraints, - const gtsam::Values& initial_values, - ConstrainedOptResult* intermediate_result = nullptr) const override; -}; - -} // namespace gtdynamics diff --git a/gtdynamics/optimizer/ConstrainedOptimizer.h b/gtdynamics/optimizer/ConstrainedOptimizer.h deleted file mode 100644 index 2c1d3c54f..000000000 --- a/gtdynamics/optimizer/ConstrainedOptimizer.h +++ /dev/null @@ -1,69 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file ConstrainedOptimizer.h - * @brief Base class constrained optimization. - * @author Yetong Zhang - */ - -#pragma once - -#include -#include -#include -#include - -#include "gtdynamics/optimizer/EqualityConstraint.h" - -namespace gtdynamics { - -/// Constrained optimization parameters shared between all solvers. -struct ConstrainedOptimizationParameters { - gtsam::LevenbergMarquardtParams lm_parameters; // LM parameters - - /// Constructor. - ConstrainedOptimizationParameters() {} - - /// Constructor with LM parameters. - ConstrainedOptimizationParameters( - const gtsam::LevenbergMarquardtParams& _lm_parameters) - : lm_parameters(_lm_parameters) {} -}; - -/// Intermediate results for constrained optimization process. -struct ConstrainedOptResult { - std::vector intermediate_values; // values after each inner loop - std::vector num_iters; // number of LM iterations for each inner loop - std::vector mu_values; // penalty parameter for each inner loop -}; - -/// Base class for constrained optimizer. -class ConstrainedOptimizer { - - public: - /** - * @brief Constructor. - */ - ConstrainedOptimizer() {} - - /** - * @brief Solve constrained optimization problem with optimizer settings. - * - * @param graph A Nonlinear factor graph representing cost. - * @param cosntraints All the constraints. - * @param initial_values Initial values for all variables. - * @param intermediate_result (optional) intermediate results during optimization. - * @return Values The result of the constrained optimization. - */ - virtual gtsam::Values optimize( - const gtsam::NonlinearFactorGraph& graph, - const EqualityConstraints& constraints, - const gtsam::Values& initial_values, - ConstrainedOptResult* intermediate_result = nullptr) const = 0; -}; -} // namespace gtdynamics diff --git a/gtdynamics/optimizer/ConvexIQPSolver.cpp b/gtdynamics/optimizer/ConvexIQPSolver.cpp new file mode 100644 index 000000000..5ff435800 --- /dev/null +++ b/gtdynamics/optimizer/ConvexIQPSolver.cpp @@ -0,0 +1,236 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ConvexIQPSolver.cpp + * @brief Convex IQP solver implementations. + * @author: Yetong Zhang + */ + +#include + +namespace gtdynamics { + +using gtsam::GaussianFactorGraph; +using gtsam::IndeterminantLinearSystemException; +using gtsam::Key; +using gtsam::Matrix; +using gtsam::Vector; +using gtsam::VectorValues; + +/* ************************************************************************* */ +GaussianFactorGraph +ConstraintGraph(const LinearInequalityConstraints &constraints, + const IndexSet &active_indices) { + GaussianFactorGraph graph; + for (const auto &constraint_idx : active_indices) { + graph.push_back(constraints.at(constraint_idx)->createConstrainedFactor()); + } + return graph; +} + +/* ************************************************************************* */ +std::pair +SolveEQP(const GaussianFactorGraph &cost, + const GaussianFactorGraph &constriant_graph) { + GaussianFactorGraph graph = cost; + for (const auto &factor : constriant_graph) { + graph.push_back(factor); + } + + try { + VectorValues solution = graph.optimize(); + return {solution, true}; + } catch (const IndeterminantLinearSystemException &) { + return {VectorValues(), false}; + } +} + +/* ************************************************************************* */ +std::map +ComputeLagrangianMultipliers(const GaussianFactorGraph &cost, + const LinearInequalityConstraints &constraints, + const VectorValues &x, + const IndexSet &active_indices) { + std::map lambdas; + + auto grad_x = cost.gradient(x); + std::map start_row_map; + size_t total_rows = 0; + for (const auto &idx : active_indices) { + start_row_map.insert({idx, total_rows}); + total_rows += constraints.at(idx)->dim(); + } + std::map start_col_map; + size_t total_cols = 0; + for (const auto &[key, v] : x) { + start_col_map.insert({key, total_cols}); + total_cols += v.size(); + } + + Matrix jac_mat = Matrix::Zero(total_rows, total_cols); + for (const auto &idx : active_indices) { + const auto &constraint = constraints.at(idx); + size_t start_row = start_row_map.at(idx); + size_t block_rows = constraint->dim(); + auto multi_jacobian = constraint->jacobian(); + for (const auto &[key, mat] : multi_jacobian) { + size_t start_col = start_col_map.at(key); + size_t block_cols = x.at(key).size(); + jac_mat.block(start_row, start_col, block_rows, block_cols) = mat; + } + } + Vector b = Vector::Zero(total_cols); + for (const auto &[key, grad] : grad_x) { + size_t start_row = start_col_map.at(key); + b.middleRows(start_row, grad.size()) = grad; + } + + Vector lambda = jac_mat.transpose().colPivHouseholderQr().solve(b); + for (const auto &idx : active_indices) { + lambdas.insert({idx, lambda.middleRows(start_row_map.at(idx), + constraints.at(idx)->dim())}); + } + + return lambdas; +} + +/* ************************************************************************* */ +double VectorMax(const Vector &vec) { + double max_val = vec(0); + for (size_t i = 1; i < vec.size(); i++) { + max_val = std::max(max_val, vec(i)); + } + return max_val; +} + +/* ************************************************************************* */ +std::pair GetMinimum(const std::map &vals) { + if (vals.size() == 0) { + return {0, 0}; + } + size_t min_idx = vals.begin()->first; + double min_val = VectorMax(vals.begin()->second); + + for (const auto &[idx, vec] : vals) { + double val = VectorMax(vec); + if (val < min_val) { + min_val = val; + min_idx = idx; + } + } + return {min_idx, min_val}; +} + +double SubDot(const VectorValues& v1, const VectorValues& v2) { + double result = 0; + for (const auto&[key, vec]: v1) { + result += vec.dot(v2.at(key)); + } + return result; +} + +/* ************************************************************************* */ +std::pair +ClipUpdate(const VectorValues &x, const VectorValues &p, + const LinearInequalityConstraints &constraints, + const IndexSet &active_indices) { + double alpha = 1; + size_t blocking_idx = 0; + + for (size_t idx = 0; idx < constraints.size(); idx++) { + if (active_indices.exists(idx)) { + continue; + } + const auto &constraint = constraints.at(idx); + auto jacobian = constraint->jacobian(); + + for (size_t row_idx = 0; row_idx < constraint->dim(); row_idx++) { + VectorValues A_i = jacobian.row(row_idx); + // A_i.print("A_i", GTDKeyFormatter); + // p.print("p", GTDKeyFormatter); + double Aip = SubDot(A_i, p); + if (Aip < 0) { + double clip_rate = -SubDot(A_i, x) / Aip; + if (clip_rate < alpha) { + alpha = clip_rate; + blocking_idx = idx; + } + } + } + } + + return {alpha, blocking_idx}; +} + +/* ************************************************************************* */ +std::tuple +SolveConvexIQP(const GaussianFactorGraph &cost, + const LinearInequalityConstraints &constraints, + const IndexSet &init_active_indices, + const VectorValues &init_values, + size_t max_iters) { + VectorValues x = init_values; + IndexSet active_indices = init_active_indices; + + size_t num_solves = 0; + while (true) { + num_solves++; + + // compute update + auto constraint_graph = ConstraintGraph(constraints, active_indices); + auto [x_new, solve_successful] = SolveEQP(cost, constraint_graph); + if (!solve_successful) { + return {init_values, init_active_indices, num_solves, false}; + } + + VectorValues p = x_new - x; + if (p.norm() < 1e-5) { + // no update is made + if (active_indices.size() == 0) { + return {x_new, active_indices, num_solves, true}; + } + auto lambdas = ComputeLagrangianMultipliers(cost, constraints, x_new, + active_indices); + auto [min_idx, min_lambda] = GetMinimum(lambdas); + if (min_lambda >= -1e-2) { + // all constraints are tight + return {x_new, active_indices, num_solves, true}; + } else { + // can relieve constraint + active_indices.erase(min_idx); + if (num_solves > max_iters) { + std::cout << "min_lambda: " << min_lambda << "\n"; + std::cout << "removing_idx: " << min_idx << "\n"; + return {x_new, active_indices, num_solves, true}; + } + } + } else { + // has a non-zero update + auto [alpha, blocking_idx] = + ClipUpdate(x, p, constraints, active_indices); + if (alpha < 1) { + // new constraints blocking + active_indices.insert(blocking_idx); + x = x + alpha * p; + if (num_solves > max_iters) { + std::cout << "alpha: " << alpha << "\n"; + std::cout << "blocking index: " << blocking_idx << "\n"; + // constraints.at(blocking_idx)->print(GTDKeyFormatter); + } + } else { + // no constraints blocking + x = x_new; + if (num_solves > max_iters) { + return {x, active_indices, num_solves, true}; + } + } + } + } +} + +} // namespace gtdynamics diff --git a/gtdynamics/optimizer/ConvexIQPSolver.h b/gtdynamics/optimizer/ConvexIQPSolver.h new file mode 100644 index 000000000..931fc0aa0 --- /dev/null +++ b/gtdynamics/optimizer/ConvexIQPSolver.h @@ -0,0 +1,42 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file IQPSolver.h + * @brief Solver of a convex linear inequality-constrained quadratic programming + * problem. + * @author: Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::GaussianFactorGraph; +using gtsam::VectorValues; + +/** Solve a convex inequality constrained QP problem defined in + * Nocedal Sec. 16.5 using the algorithm in Nocedal Alg. 16.3. + * @param graph Cost for IQP problem. + * @param constraints Linear inequality constraints of IQP problem. + * @param init_active_indices Initial indices of inequality constraints. + * @param init_values Initial values for optimization. + * @return [solution vector, active constraint indices, num solves, solve + * successful]. */ +std::tuple +SolveConvexIQP(const GaussianFactorGraph &graph, + const LinearInequalityConstraints &constraints, + const IndexSet &init_active_indices, + const VectorValues &init_values, + size_t max_iters = 100); + +} // namespace gtdynamics diff --git a/gtdynamics/optimizer/EqualityConstraint-inl.h b/gtdynamics/optimizer/EqualityConstraint-inl.h deleted file mode 100644 index 2f2e0e897..000000000 --- a/gtdynamics/optimizer/EqualityConstraint-inl.h +++ /dev/null @@ -1,66 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file EqualityConstraint-inl.h - * @brief Equality constraints in constrained optimization. - * @author: Yetong Zhang, Frank Dellaert - */ - -#pragma once - -#include "gtdynamics/optimizer/EqualityConstraint.h" - -namespace gtdynamics { - -template -gtsam::NoiseModelFactor::shared_ptr VectorExpressionEquality

::createFactor( - const double mu, boost::optional bias) const { - auto noise = gtsam::noiseModel::Diagonal::Sigmas(tolerance_ / sqrt(mu)); - VectorP measure = VectorP::Zero(); - if (bias) { - measure = -*bias; - } - return gtsam::NoiseModelFactor::shared_ptr( - new gtsam::ExpressionFactor(noise, measure, expression_)); -} - -template -bool VectorExpressionEquality

::feasible(const gtsam::Values& x) const { - VectorP result = expression_.value(x); - for (int i = 0; i < P; i++) { - if (abs(result[i]) > tolerance_[i]) { - return false; - } - } - return true; -} - -template -gtsam::Vector VectorExpressionEquality

::operator()( - const gtsam::Values& x) const { - return expression_.value(x); -} - -template -gtsam::Vector VectorExpressionEquality

::toleranceScaledViolation( - const gtsam::Values& x) const { - auto violation = expression_.value(x); - // TODO(yetong): figure out how to perform element-wise division - VectorP scaled_violation; - for (int i = 0; i < P; i++) { - scaled_violation(i) = violation(i) / tolerance_(i); - } - return scaled_violation; -} - -template -size_t VectorExpressionEquality

::dim() const { - return P; -} - -} // namespace gtdynamics diff --git a/gtdynamics/optimizer/EqualityConstraint.cpp b/gtdynamics/optimizer/EqualityConstraint.cpp deleted file mode 100644 index 85bcdfeb6..000000000 --- a/gtdynamics/optimizer/EqualityConstraint.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file EqualityConstraint.cpp - * @brief Equality constraints in constrained optimization. - * @author: Yetong Zhang, Frank Dellaert - */ - -#include "gtdynamics/optimizer/EqualityConstraint.h" - -namespace gtdynamics { - -gtsam::NoiseModelFactor::shared_ptr DoubleExpressionEquality::createFactor( - const double mu, boost::optional bias) const { - auto noise = gtsam::noiseModel::Isotropic::Sigma(1, tolerance_ / sqrt(mu)); - double measure = 0.0; - if (bias) { - measure = -(*bias)(0); - } - return gtsam::NoiseModelFactor::shared_ptr( - new gtsam::ExpressionFactor(noise, measure, expression_)); -} - -bool DoubleExpressionEquality::feasible(const gtsam::Values& x) const { - double result = expression_.value(x); - return abs(result) <= tolerance_; -} - -gtsam::Vector DoubleExpressionEquality::operator()( - const gtsam::Values& x) const { - double result = expression_.value(x); - return (gtsam::Vector(1) << result).finished(); -} - -gtsam::Vector DoubleExpressionEquality::toleranceScaledViolation( - const gtsam::Values& x) const { - double result = expression_.value(x); - return (gtsam::Vector(1) << result / tolerance_).finished(); -} - -} // namespace gtdynamics diff --git a/gtdynamics/optimizer/EqualityConstraint.h b/gtdynamics/optimizer/EqualityConstraint.h deleted file mode 100644 index 55776065b..000000000 --- a/gtdynamics/optimizer/EqualityConstraint.h +++ /dev/null @@ -1,166 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file EqualityConstraint.h - * @brief Equality constraints in constrained optimization. - * @author: Yetong Zhang, Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtdynamics { - -/** - * Equality constraint base class. - */ -class EqualityConstraint { - public: - typedef EqualityConstraint This; - typedef boost::shared_ptr shared_ptr; - - /** Default constructor. */ - EqualityConstraint() {} - - /** Destructor. */ - virtual ~EqualityConstraint() {} - - /** - * @brief Create a factor representing the component in the merit function. - * - * @param mu penalty parameter. - * @param bias additional bias. - * @return a factor representing 1/2 mu||g(x)+bias||_Diag(tolerance^2)^2. - */ - virtual gtsam::NoiseModelFactor::shared_ptr createFactor( - const double mu, - boost::optional bias = boost::none) const = 0; - - /** - * @brief Check if constraint violation is within tolerance. - * - * @param x values to evalute constraint at. - * @return bool representing if is feasible. - */ - virtual bool feasible(const gtsam::Values& x) const = 0; - - /** - * @brief Evaluate the constraint violation, g(x). - * - * @param x values to evalute constraint at. - * @return a vector representing the constraint violation in each dimension. - */ - virtual gtsam::Vector operator()(const gtsam::Values& x) const = 0; - - /** @brief Constraint violation scaled by tolerance, e.g. g(x)/tolerance. */ - virtual gtsam::Vector toleranceScaledViolation( - const gtsam::Values& x) const = 0; - - /** @brief return the dimension of the constraint. */ - virtual size_t dim() const = 0; -}; - -/** Equality constraint that force g(x) = 0, where g(x) is a scalar-valued - * function. */ -class DoubleExpressionEquality : public EqualityConstraint { - public: - protected: - gtsam::Expression expression_; - double tolerance_; - - public: - /** - * @brief Constructor. - * - * @param expression expression representing g(x). - * @param tolerance scalar representing tolerance. - */ - DoubleExpressionEquality(const gtsam::Expression& expression, - const double& tolerance) - : expression_(expression), tolerance_(tolerance) {} - - gtsam::NoiseModelFactor::shared_ptr createFactor( - const double mu, - boost::optional bias = boost::none) const override; - - bool feasible(const gtsam::Values& x) const override; - - gtsam::Vector operator()(const gtsam::Values& x) const override; - - gtsam::Vector toleranceScaledViolation(const gtsam::Values& x) const override; - - size_t dim() const override { return 1; } -}; - -/** Equality constraint that force g(x) = 0, where g(x) is a vector-valued - * function. */ -template -class VectorExpressionEquality : public EqualityConstraint { - public: - using VectorP = Eigen::Matrix; - - protected: - gtsam::Expression expression_; - VectorP tolerance_; - - public: - /** - * @brief Constructor. - * - * @param expression expression representing g(x). - * @param tolerance vector representing tolerance in each dimension. - */ - VectorExpressionEquality(const gtsam::Expression& expression, - const VectorP& tolerance) - : expression_(expression), tolerance_(tolerance) {} - - gtsam::NoiseModelFactor::shared_ptr createFactor( - const double mu, - boost::optional bias = boost::none) const override; - - bool feasible(const gtsam::Values& x) const override; - - gtsam::Vector operator()(const gtsam::Values& x) const override; - - gtsam::Vector toleranceScaledViolation(const gtsam::Values& x) const override; - - size_t dim() const override; -}; - -/// Container of EqualityConstraint. -class EqualityConstraints : public std::vector { - private: - using Base = std::vector; - - template - using IsDerived = typename std::enable_if< - std::is_base_of::value>::type; - - public: - EqualityConstraints() : Base() {} - - /// Add a set of equality constraints. - void add (const EqualityConstraints& other) { - insert(end(), other.begin(), other.end()); - } - - /// Emplace a shared pointer to constraint of given type. - template - IsDerived emplace_shared(Args&&... args) { - push_back(boost::allocate_shared( - Eigen::aligned_allocator(), - std::forward(args)...)); - } - -}; - -} // namespace gtdynamics - -#include diff --git a/gtdynamics/optimizer/HistoryLMOptimizer.h b/gtdynamics/optimizer/HistoryLMOptimizer.h new file mode 100644 index 000000000..a8ef9a027 --- /dev/null +++ b/gtdynamics/optimizer/HistoryLMOptimizer.h @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file HistoryLMOptimizer.h + * @brief Modified Levenberg Marquardt optimizer that stores history states. + * @author: Yetong Zhang, Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::GaussianFactorGraph; +using gtsam::LevenbergMarquardtOptimizer; +using gtsam::LevenbergMarquardtParams; +using gtsam::NonlinearFactorGraph; +using gtsam::Ordering; +using gtsam::Values; +using gtsam::VectorValues; + +/** LM optimizer that stores history states. */ +class HistoryLMOptimizer : public LevenbergMarquardtOptimizer { +public: + typedef gtsam::internal::LevenbergMarquardtState State; + typedef std::vector LMStates; + +protected: + std::shared_ptr states_ = std::make_shared(); + +public: + HistoryLMOptimizer( + const NonlinearFactorGraph &graph, const Values &initialValues, + const LevenbergMarquardtParams ¶ms = LevenbergMarquardtParams()) + : LevenbergMarquardtOptimizer(graph, initialValues, params), + states_(std::make_shared()) { + auto currentState = static_cast(state_.get()); + states_->emplace_back(*currentState); + } + + HistoryLMOptimizer( + const NonlinearFactorGraph &graph, const Values &initialValues, + const Ordering &ordering, + const LevenbergMarquardtParams ¶ms = LevenbergMarquardtParams()) + : LevenbergMarquardtOptimizer(graph, initialValues, ordering, params), + states_(std::make_shared()) { + auto currentState = static_cast(state_.get()); + states_->emplace_back(*currentState); + } + + const LMStates &states() const { return *states_; } + + GaussianFactorGraph::shared_ptr iterate() override { + gttic(LM_iterate); + + // Linearize graph + GaussianFactorGraph::shared_ptr linear = linearize(); + + // Only calculate diagonal of Hessian (expensive) once per outer iteration, + // if we need it + VectorValues sqrtHessianDiagonal; + if (params_.getDiagonalDamping()) { + sqrtHessianDiagonal = linear->hessianDiagonal(); + for (auto &[key, value] : sqrtHessianDiagonal) { + value = value.cwiseMax(params_.dampingParams.minDiagonal) + .cwiseMin(params_.dampingParams.maxDiagonal) + .cwiseSqrt(); + } + } + + // Keep increasing lambda until we make make progress + while (!tryLambda(*linear, sqrtHessianDiagonal)) { + } + auto newState = static_cast(state_.get()); + states_->emplace_back(*newState); + + return linear; + } +}; + +} // namespace gtdynamics diff --git a/gtdynamics/optimizer/MutableLMOptimizer.cpp b/gtdynamics/optimizer/MutableLMOptimizer.cpp new file mode 100644 index 000000000..78ed9b389 --- /dev/null +++ b/gtdynamics/optimizer/MutableLMOptimizer.cpp @@ -0,0 +1,353 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file MutableLMOptimizer.cpp + * @brief A nonlinear optimizer that uses the Levenberg-Marquardt trust-region + * scheme + * @author Richard Roberts + * @author Frank Dellaert + * @author Luca Carlone + * @date Feb 26, 2012 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtdynamics { + +typedef gtsam::internal::LevenbergMarquardtState State; + +/* ************************************************************************* */ +MutableLMOptimizer::MutableLMOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, + const LevenbergMarquardtParams& params) + : NonlinearOptimizer( + graph, std::unique_ptr( + new State(initialValues, graph.error(initialValues), + params.lambdaInitial, params.lambdaFactor))), + params_(LevenbergMarquardtParams::EnsureHasOrdering(params, graph)) {} + +MutableLMOptimizer::MutableLMOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, + const Ordering& ordering, + const LevenbergMarquardtParams& params) + : NonlinearOptimizer( + graph, std::unique_ptr( + new State(initialValues, graph.error(initialValues), + params.lambdaInitial, params.lambdaFactor))), + params_(LevenbergMarquardtParams::ReplaceOrdering(params, ordering)) {} + +/* ************************************************************************* */ +void MutableLMOptimizer::initTime() { + startTime_ = std::chrono::high_resolution_clock::now(); +} + +/* ************************************************************************* */ +double MutableLMOptimizer::lambda() const { + auto currentState = static_cast(state_.get()); + return currentState->lambda; +} + +/* ************************************************************************* */ +int MutableLMOptimizer::getInnerIterations() const { + auto currentState = static_cast(state_.get()); + return currentState->totalNumberInnerIterations; +} + +/* ************************************************************************* */ +GaussianFactorGraph::shared_ptr MutableLMOptimizer::linearize() const { + return graph_.linearize(state_->values); +} + +/* ************************************************************************* */ +GaussianFactorGraph MutableLMOptimizer::buildDampedSystem( + const GaussianFactorGraph& linear, + const VectorValues& sqrtHessianDiagonal) const { + gttic(damp); + auto currentState = static_cast(state_.get()); + + if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED) + std::cout << "building damped system with lambda " << currentState->lambda + << std::endl; + + if (params_.getDiagonalDamping()) + return currentState->buildDampedSystem(linear, sqrtHessianDiagonal); + else + return currentState->buildDampedSystem(linear); +} + +/* ************************************************************************* */ +// Log current error/lambda to file +inline void MutableLMOptimizer::writeLogFile(double currentError) { + auto currentState = static_cast(state_.get()); + + if (!params_.logFile.empty()) { + ofstream os(params_.logFile.c_str(), ios::app); + // use chrono to measure time in microseconds + auto currentTime = std::chrono::high_resolution_clock::now(); + // Get the time spent in seconds and print it + double timeSpent = std::chrono::duration_cast( + currentTime - startTime_) + .count() / + 1e6; + os << /*inner iterations*/ currentState->totalNumberInnerIterations << "," + << timeSpent << "," << /*current error*/ currentError << "," + << currentState->lambda << "," + << /*outer iterations*/ currentState->iterations << endl; + } +} + +/* ************************************************************************* */ +bool MutableLMOptimizer::tryLambda(const GaussianFactorGraph& linear, + const VectorValues& sqrtHessianDiagonal) { + auto currentState = static_cast(state_.get()); + bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA); + + if (verbose) cout << "trying lambda = " << currentState->lambda << endl; + + // Build damped system for this lambda (adds prior factors that make it like + // gradient descent) + auto dampedSystem = buildDampedSystem(linear, sqrtHessianDiagonal); + + // Try solving + double modelFidelity = 0.0; + bool step_is_successful = false; + bool stopSearchingLambda = false; + double newError = numeric_limits::infinity(), costChange; + Values newValues; + VectorValues delta; + + bool systemSolvedSuccessfully; + try { + // ============ Solve is where most computation happens !! ================= + delta = solve(dampedSystem, params_); + systemSolvedSuccessfully = true; + } catch (const gtsam::IndeterminantLinearSystemException&) { + systemSolvedSuccessfully = false; + } + + if (systemSolvedSuccessfully) { + if (verbose) cout << "linear delta norm = " << delta.norm() << endl; + if (params_.verbosityLM >= LevenbergMarquardtParams::TRYDELTA) + delta.print("delta"); + + // Compute the old linearized error as it is not the same + // as the nonlinear error when robust noise models are used. + double oldLinearizedError = linear.error(VectorValues::Zero(delta)); + double newlinearizedError = linear.error(delta); + + // cost change in the linearized system (old - new) + double linearizedCostChange = oldLinearizedError - newlinearizedError; + if (verbose) + cout << "newlinearizedError = " << newlinearizedError + << " linearizedCostChange = " << linearizedCostChange << endl; + + if (linearizedCostChange >= 0) { // step is valid + // update values + gttic(retract); + // ============ This is where the solution is updated ==================== + newValues = currentState->values.retract(delta); + // ======================================================================= + gttoc(retract); + + // compute new error + gttic(compute_error); + if (verbose) cout << "calculating error:" << endl; + newError = graph_.error(newValues); + gttoc(compute_error); + + if (verbose) + cout << "old error (" << currentState->error + << ") new (tentative) error (" << newError << ")" << endl; + + // cost change in the original, nonlinear system (old - new) + costChange = currentState->error - newError; + + if (linearizedCostChange > + std::numeric_limits::epsilon() * oldLinearizedError) { + // the (linear) error has to decrease to satisfy this condition + // fidelity of linearized model VS original system between + modelFidelity = costChange / linearizedCostChange; + // if we decrease the error in the nonlinear system and modelFidelity is + // above threshold + step_is_successful = modelFidelity > params_.minModelFidelity; + if (verbose) cout << "modelFidelity: " << modelFidelity << endl; + } // else we consider the step non successful and we either increase + // lambda or stop if error change is small + + double minAbsoluteTolerance = + params_.relativeErrorTol * currentState->error; + // if the change is small we terminate + if (std::abs(costChange) < minAbsoluteTolerance) { + if (verbose) + cout << "abs(costChange)=" << std::abs(costChange) + << " minAbsoluteTolerance=" << minAbsoluteTolerance + << " (relativeErrorTol=" << params_.relativeErrorTol << ")" + << endl; + stopSearchingLambda = true; + } + } + } // if (systemSolvedSuccessfully) + + if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { + double iterationTime = 0; + if (currentState->iterations == 0) + cout << "iter cost cost_change lambda success iter_time" + << endl; + + cout << std::setw(4) << currentState->iterations << " " << std::setw(8) + << newError << " " << std::setw(3) << std::setprecision(2) + << costChange << " " << std::setw(3) << std::setprecision(2) + << currentState->lambda << " " << std::setw(4) + << systemSolvedSuccessfully << " " << std::setw(3) + << std::setprecision(2) << iterationTime << endl; + } + + if (step_is_successful) { + // we have successfully decreased the cost and we have good modelFidelity + // NOTE(frank): As we return immediately after this, we move the newValues + // TODO(frank): make Values actually support move. Does not seem to happen + // now. + state_ = currentState->decreaseLambda(params_, modelFidelity, + std::move(newValues), newError); + return true; + } else if (!stopSearchingLambda) { // we failed to solved the system or had + // no decrease in cost + if (verbose) cout << "increasing lambda" << endl; + State* modifiedState = static_cast(state_.get()); + modifiedState->increaseLambda( + params_); // TODO(frank): make this functional with Values move + + // check if lambda is too big + if (modifiedState->lambda >= params_.lambdaUpperBound) { + if (params_.verbosity >= NonlinearOptimizerParams::TERMINATION || + params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) + cout << "Warning: Levenberg-Marquardt giving up because " + "cannot decrease error with maximum lambda" + << endl; + return true; + } else { + return false; // only case where we will keep trying + } + } else { // the change in the cost is very small and it is not worth trying + // bigger lambdas + if (verbose) + cout + << "Levenberg-Marquardt: stopping as relative cost reduction is small" + << endl; + return true; + } +} + +/* ************************************************************************* */ +GaussianFactorGraph::shared_ptr MutableLMOptimizer::iterate() { + auto currentState = static_cast(state_.get()); + + gttic(LM_iterate); + + // Linearize graph + if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED) + cout << "linearizing = " << endl; + GaussianFactorGraph::shared_ptr linear = linearize(); + + if (currentState->totalNumberInnerIterations == 0) { // write initial error + writeLogFile(currentState->error); + + if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { + cout << "Initial error: " << currentState->error + << ", values: " << currentState->values.size() << std::endl; + } + } + + // Only calculate diagonal of Hessian (expensive) once per outer iteration, if + // we need it + VectorValues sqrtHessianDiagonal; + if (params_.getDiagonalDamping()) { + sqrtHessianDiagonal = linear->hessianDiagonal(); + for (auto& kvp : sqrtHessianDiagonal) { + gtsam::Vector& v = kvp.second; + v = v.cwiseMax(params_.dampingParams.minDiagonal) + .cwiseMin(params_.dampingParams.maxDiagonal) + .cwiseSqrt(); + } + } + + // Keep increasing lambda until we make make progress + while (!tryLambda(*linear, sqrtHessianDiagonal)) { + auto newState = static_cast(state_.get()); + writeLogFile(newState->error); + } + + return linear; +} + +/* ************************************************************************* */ +MutableLMOptimizer::MutableLMOptimizer(const LevenbergMarquardtParams& params) + : NonlinearOptimizer( + NonlinearFactorGraph(), + std::unique_ptr(new State(Values(), 0., params.lambdaInitial, + params.lambdaFactor))), + params_(params) {} + +/* ************************************************************************* */ +MutableLMOptimizer::MutableLMOptimizer(const NonlinearFactorGraph& graph, + const LevenbergMarquardtParams& params) + : NonlinearOptimizer( + graph, std::unique_ptr(new State( + Values(), 0., params.lambdaInitial, params.lambdaFactor))), + params_(LevenbergMarquardtParams::EnsureHasOrdering(params, graph)) {} + +/* ************************************************************************* */ +void MutableLMOptimizer::setGraph(const NonlinearFactorGraph& graph) { + graph_ = graph; + params_ = LevenbergMarquardtParams::EnsureHasOrdering(params_, graph); +} + +/* ************************************************************************* */ +void MutableLMOptimizer::setGraph(const NonlinearFactorGraph& graph, + const Ordering& ordering) { + graph_ = graph; + params_ = LevenbergMarquardtParams::ReplaceOrdering(params_, ordering); +} + +/* ************************************************************************* */ +void MutableLMOptimizer::setValues(const Values& values) { + state_ = std::unique_ptr(new State((values), graph_.error(values), + params_.lambdaInitial, + params_.lambdaFactor)); +} + +/* ************************************************************************* */ +void MutableLMOptimizer::setValues(Values&& values) { + // Evaluate error before moving values into State to avoid use-after-move UB. + const double error = graph_.error(values); + state_ = std::unique_ptr(new State( + std::move(values), error, params_.lambdaInitial, params_.lambdaFactor)); +} + +} /* namespace gtdynamics */ diff --git a/gtdynamics/optimizer/MutableLMOptimizer.h b/gtdynamics/optimizer/MutableLMOptimizer.h new file mode 100644 index 000000000..d461de607 --- /dev/null +++ b/gtdynamics/optimizer/MutableLMOptimizer.h @@ -0,0 +1,162 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file MutableLMOptimizer.h + * @brief A nonlinear optimizer that uses + * the Levenberg-Marquardt trust-region + * scheme + * @author Richard Roberts + * @author Frank Dellaert + * @author Luca Carlone + * @date Feb 26, 2012 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +class NonlinearOptimizerMoreOptimizationTest; + +namespace gtdynamics { + +using gtsam::GaussianFactorGraph; +using gtsam::LevenbergMarquardtParams; +using gtsam::NonlinearFactorGraph; +using gtsam::NonlinearOptimizer; +using gtsam::NonlinearOptimizerParams; +using gtsam::Ordering; +using gtsam::Values; +using gtsam::VectorValues; + +/** + * This class performs Levenberg-Marquardt nonlinear optimization + */ +class GTSAM_EXPORT MutableLMOptimizer : public NonlinearOptimizer { + protected: + LevenbergMarquardtParams params_; ///< LM parameters + + // startTime_ is a chrono time point + std::chrono::time_point + startTime_; ///< time when optimization started + + void initTime(); + + public: + typedef std::shared_ptr shared_ptr; + + NonlinearFactorGraph& mutableGraph() { return graph_; } + + /// @name Constructors/Destructor + /// @{ + + MutableLMOptimizer( + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()); + + MutableLMOptimizer( + const NonlinearFactorGraph& graph, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()); + + /** Standard constructor, requires a nonlinear factor graph, initial + * variable assignments, and optimization parameters. For convenience this + * version takes plain objects instead of shared pointers, but internally + * copies the objects. + * @param graph The nonlinear factor graph to optimize + * @param initialValues The initial variable assignments + * @param params The optimization parameters + */ + MutableLMOptimizer( + const NonlinearFactorGraph& graph, const Values& initialValues, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()); + + /** Standard constructor, requires a nonlinear factor graph, initial + * variable assignments, and optimization parameters. For convenience this + * version takes plain objects instead of shared pointers, but internally + * copies the objects. + * @param graph The nonlinear factor graph to optimize + * @param initialValues The initial variable assignments + */ + MutableLMOptimizer( + const NonlinearFactorGraph& graph, const Values& initialValues, + const Ordering& ordering, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()); + + /** Virtual destructor */ + ~MutableLMOptimizer() override {} + + /// @} + + /// @name Standard interface + /// @{ + + /// Access the current damping value + double lambda() const; + + /// Access the current number of inner iterations + int getInnerIterations() const; + + /// print + void print(const std::string& str = "") const { + std::cout << str << "MutableLMOptimizer" << std::endl; + this->params_.print(" parameters:\n"); + } + + /// @} + + void setGraph(const NonlinearFactorGraph& graph); + + void setGraph(const NonlinearFactorGraph& graph, const Ordering& ordering); + + void setValues(Values&& values); + + void setValues(const Values& values); + + /// @name Advanced interface + /// @{ + + /** + * Perform a single iteration, returning GaussianFactorGraph corresponding to + * the linearized factor graph. + */ + GaussianFactorGraph::shared_ptr iterate() override; + + /** Read-only access the parameters */ + const LevenbergMarquardtParams& params() const { return params_; } + + void writeLogFile(double currentError); + + /** linearize, can be overwritten */ + virtual GaussianFactorGraph::shared_ptr linearize() const; + + /** Build a damped system for a specific lambda -- for testing only */ + GaussianFactorGraph buildDampedSystem( + const GaussianFactorGraph& linear, + const VectorValues& sqrtHessianDiagonal) const; + + /** Inner loop, changes state, returns true if successful or giving up */ + bool tryLambda(const GaussianFactorGraph& linear, + const VectorValues& sqrtHessianDiagonal); + + /// @} + + protected: + /** Access the parameters (base class version) */ + const NonlinearOptimizerParams& _params() const override { return params_; } +}; + +} // namespace gtdynamics diff --git a/gtdynamics/optimizer/Optimizer.cpp b/gtdynamics/optimizer/Optimizer.cpp index 6a515c4f4..7b53ba747 100644 --- a/gtdynamics/optimizer/Optimizer.cpp +++ b/gtdynamics/optimizer/Optimizer.cpp @@ -11,9 +11,10 @@ * @author: Frank Dellaert */ -#include #include -#include +#include +#include +#include #include namespace gtdynamics { @@ -21,6 +22,17 @@ namespace gtdynamics { using gtsam::NonlinearFactorGraph; using gtsam::Values; +namespace { + +gtsam::ConstrainedOptProblem ToGtsamProblem( + const gtsam::NonlinearFactorGraph& graph, + const gtsam::NonlinearEqualityConstraints& constraints) { + return gtsam::ConstrainedOptProblem::EqConstrainedOptProblem(graph, + constraints); +} + +} // namespace + Values Optimizer::optimize(const NonlinearFactorGraph& graph, const Values& initial_values) const { gtsam::LevenbergMarquardtOptimizer optimizer(graph, initial_values, @@ -30,25 +42,27 @@ Values Optimizer::optimize(const NonlinearFactorGraph& graph, } Values Optimizer::optimize(const gtsam::NonlinearFactorGraph& graph, - const EqualityConstraints& constraints, + const gtsam::NonlinearEqualityConstraints& constraints, const gtsam::Values& initial_values) const { if (p_.method == OptimizationParameters::Method::SOFT_CONSTRAINTS) { auto merit_graph = graph; - for (const auto& constraint : constraints) { - merit_graph.add(constraint->createFactor(1.0)); - } + merit_graph.add(constraints.penaltyGraph(1.0)); return optimize(merit_graph, initial_values); } else if (p_.method == OptimizationParameters::Method::PENALTY) { - PenaltyMethodParameters params = p_.lm_parameters; - PenaltyMethodOptimizer optimizer(params); - return optimizer.optimize(graph, constraints, initial_values); + auto params = std::make_shared(); + params->lmParams = p_.lm_parameters; + gtsam::PenaltyOptimizer optimizer(ToGtsamProblem(graph, constraints), + initial_values, params); + return optimizer.optimize(); } else if (p_.method == OptimizationParameters::Method::AUGMENTED_LAGRANGIAN) { - AugmentedLagrangianParameters params = p_.lm_parameters; - AugmentedLagrangianOptimizer optimizer(params); - return optimizer.optimize(graph, constraints, initial_values); + auto params = std::make_shared(); + params->lmParams = p_.lm_parameters; + gtsam::AugmentedLagrangianOptimizer optimizer( + ToGtsamProblem(graph, constraints), initial_values, params); + return optimizer.optimize(); } else { throw std::runtime_error("optimization method not recognized."); diff --git a/gtdynamics/optimizer/Optimizer.h b/gtdynamics/optimizer/Optimizer.h index a4d6f39f8..ff8371ecd 100644 --- a/gtdynamics/optimizer/Optimizer.h +++ b/gtdynamics/optimizer/Optimizer.h @@ -13,8 +13,8 @@ #pragma once -#include #include +#include #include // Forward declarations. @@ -68,7 +68,7 @@ class Optimizer { * @return Values The result of the optimization. */ gtsam::Values optimize(const gtsam::NonlinearFactorGraph& graph, - const EqualityConstraints& constraints, + const gtsam::NonlinearEqualityConstraints& constraints, const gtsam::Values& initial_values) const; }; } // namespace gtdynamics diff --git a/gtdynamics/optimizer/PenaltyMethodOptimizer.cpp b/gtdynamics/optimizer/PenaltyMethodOptimizer.cpp deleted file mode 100644 index e4c6b4a2e..000000000 --- a/gtdynamics/optimizer/PenaltyMethodOptimizer.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file PenaltyMethodOptimizer.cpp - * @brief Penalty method optimization routines. - * @author: Yetong Zhang - */ - -#include "gtdynamics/optimizer/PenaltyMethodOptimizer.h" - -namespace gtdynamics { - -gtsam::Values PenaltyMethodOptimizer::optimize( - const gtsam::NonlinearFactorGraph& graph, - const EqualityConstraints& constraints, const gtsam::Values& initial_values, - ConstrainedOptResult* intermediate_result) const { - gtsam::Values values = initial_values; - double mu = p_.initial_mu; - - // Solve the constrained optimization problem by solving a sequence of - // unconstrained optimization problems. - for (int i = 0; i < p_.num_iterations; i++) { - gtsam::NonlinearFactorGraph merit_graph = graph; - - // Create factors corresponding to penalty terms of constraints. - for (auto& constraint : constraints) { - merit_graph.add(constraint->createFactor(mu)); - } - - // Run optimization. - gtsam::LevenbergMarquardtOptimizer optimizer(merit_graph, values, - p_.lm_parameters); - auto result = optimizer.optimize(); - - // Save results and update parameters. - values = result; - mu *= p_.mu_increase_rate; - - /// Store intermediate results. - if (intermediate_result != nullptr) { - intermediate_result->intermediate_values.push_back(values); - intermediate_result->num_iters.push_back(optimizer.getInnerIterations()); - intermediate_result->mu_values.push_back(mu); - } - } - return values; -} - -} // namespace gtdynamics diff --git a/gtdynamics/optimizer/PenaltyMethodOptimizer.h b/gtdynamics/optimizer/PenaltyMethodOptimizer.h deleted file mode 100644 index 1a3dea35d..000000000 --- a/gtdynamics/optimizer/PenaltyMethodOptimizer.h +++ /dev/null @@ -1,67 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file PenaltyMethodOptimizer.h - * @brief Penalty method optimizer for constrained optimization. - * @author Yetong Zhang - */ - -#pragma once - -#include "gtdynamics/optimizer/ConstrainedOptimizer.h" - -namespace gtdynamics { - -/// Parameters for penalty method -struct PenaltyMethodParameters : public ConstrainedOptimizationParameters { - using Base = ConstrainedOptimizationParameters; - size_t num_iterations; - double initial_mu; // initial penalty parameter - double mu_increase_rate; // increase rate of penalty parameter - - /** Constructor. */ - PenaltyMethodParameters() - : Base(gtsam::LevenbergMarquardtParams()), - num_iterations(15), - initial_mu(1.0), - mu_increase_rate(2.0) {} - - PenaltyMethodParameters(const gtsam::LevenbergMarquardtParams& _lm_parameters, - const size_t& _num_iterations = 15, - const double& _initial_mu = 1.0, - const double& _mu_increase_rate = 2.0) - : Base(_lm_parameters), - num_iterations(_num_iterations), - initial_mu(_initial_mu), - mu_increase_rate(_mu_increase_rate) {} -}; - -/// Penalty method only considering equality constraints. -class PenaltyMethodOptimizer : public ConstrainedOptimizer { - protected: - const PenaltyMethodParameters p_; - - public: - /** Default constructor. */ - PenaltyMethodOptimizer() : p_(PenaltyMethodParameters()) {} - - /** - * Construct from parameters. - */ - PenaltyMethodOptimizer(const PenaltyMethodParameters& parameters) - : p_(parameters) {} - - /// Run optimization. - gtsam::Values optimize( - const gtsam::NonlinearFactorGraph& graph, - const EqualityConstraints& constraints, - const gtsam::Values& initial_values, - ConstrainedOptResult* intermediate_result = nullptr) const override; -}; - -} // namespace gtdynamics diff --git a/gtdynamics/optimizer/QPSolver.cpp b/gtdynamics/optimizer/QPSolver.cpp new file mode 100644 index 000000000..82a0df8b6 --- /dev/null +++ b/gtdynamics/optimizer/QPSolver.cpp @@ -0,0 +1,57 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file QPSolver.cpp + * @brief Quadratic programming solver implementations. + * @author: Yetong Zhang + */ + +#include + +namespace gtdynamics { + +using gtsam::Key; +using gtsam::JacobianFactor; + +std::pair SolveEQP(const Matrix &A_cost, const Vector &b_cost, + const Matrix &A_constraint, + const Vector &b_constraint) { + gtsam::GaussianFactorGraph graph; + // Key key = 0; + // size_t cost_dim = A_cost.rows(); + // size_t constraint_dim = A_constraint.rows(); + // auto cost_noise = noiseModel::Unit::Create(cost_dim); + // auto constrained_noise = noiseModel::Constrained::All(constraint_dim); + + // graph.emplace_shared(key, A_cost, b_cost, cost_noise); + // graph.emplace_shared(key, A_constraint, b_constraint, + // constrained_noise); + + // Vector x = graph.optimize().at(key); + + // // Solve for lambda. + // GaussianFactorGraph lambda_graph; + + Key x_key = 0; + Key lambda_key = 1; + Matrix G = A_cost.transpose() * A_cost; + Vector c = -b_cost.transpose() * A_cost; + graph.emplace_shared( + x_key, G, lambda_key, -A_constraint.transpose(), -c, + gtsam::noiseModel::Unit::Create(c.size())); + graph.emplace_shared( + x_key, A_constraint, b_constraint, + gtsam::noiseModel::Unit::Create(b_constraint.size())); + + auto result = graph.optimize(); + Vector x = result.at(x_key); + Vector lambda = result.at(lambda_key); + return std::make_pair(x, lambda); +} + +} // namespace gtdynamics diff --git a/gtdynamics/optimizer/QPSolver.h b/gtdynamics/optimizer/QPSolver.h new file mode 100644 index 000000000..62c838e46 --- /dev/null +++ b/gtdynamics/optimizer/QPSolver.h @@ -0,0 +1,34 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file QPSolver.h + * @brief Solver of a quadratic programming problem. + * @author: Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::Matrix; +using gtsam::Vector; + +/** Solve an equality constrained QP problem defined by + * Nocedal 16.39. + * Returns resulting vector and Lagrangian multipliers. */ +std::pair SolveEQP(const Matrix &A_cost, const Vector &b_cost, + const Matrix &A_constraint, + const Vector &b_constraint); + +} // namespace gtdynamics diff --git a/gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h b/gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h index afd5319cc..61da4d1aa 100644 --- a/gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h +++ b/gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h @@ -5,6 +5,13 @@ #include #include + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#include +#endif + namespace gtdynamics { class GaussianProcessPrior : public gtsam::NoiseModelFactor4 { @@ -31,18 +38,17 @@ class GaussianProcessPrior /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// factor error function gtsam::Vector evaluateError( const double& theta1, const double& thetad1, const double& theta2, - const double& thetad2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none) const override { - // using namespace gtsam; + const double& thetad2, gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr, + gtsam::OptionalMatrixType H3 = nullptr, + gtsam::OptionalMatrixType H4 = nullptr) const override { // state vector gtsam::Vector2 error; @@ -101,14 +107,17 @@ class GaussianProcessPrior } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE& ar, const unsigned int version) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar& BOOST_SERIALIZATION_NVP(delta_t_); + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp( + "NoiseModelFactorN", boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(delta_t_); } +#endif }; // GaussianProcessPrior -} // namespace gtdynamics \ No newline at end of file +} // namespace gtdynamics diff --git a/gtdynamics/statics/StaticWrenchFactor.cpp b/gtdynamics/statics/StaticWrenchFactor.cpp index a2766f76b..c1e0ba649 100644 --- a/gtdynamics/statics/StaticWrenchFactor.cpp +++ b/gtdynamics/statics/StaticWrenchFactor.cpp @@ -11,19 +11,17 @@ * @author Frank Dellaert, Mandy Xie, Yetong Zhang, and Gerry Chen */ -#include "gtdynamics/statics/StaticWrenchFactor.h" - +#include +#include #include #include #include #include #include -#include +#include #include -#include "gtdynamics/statics/Statics.h" - using gtsam::Matrix; using gtsam::Pose3; using gtsam::Values; @@ -33,15 +31,15 @@ using gtsam::Vector6; namespace gtdynamics { StaticWrenchFactor::StaticWrenchFactor( - const std::vector &wrench_keys, gtsam::Key pose_key, + const std::vector &wrench_keys, gtsam::Key pose_key, const gtsam::noiseModel::Base::shared_ptr &cost_model, double mass, - const boost::optional &gravity) + const std::optional &gravity) : Base(cost_model, wrench_keys), mass_(mass), gravity_(gravity) { keys_.push_back(pose_key); } Vector StaticWrenchFactor::unwhitenedError( - const Values &x, boost::optional &> H) const { + const Values &x, gtsam::OptionalMatrixVecType H) const { if (!this->active(x)) { return Vector::Zero(this->dim()); } diff --git a/gtdynamics/statics/StaticWrenchFactor.h b/gtdynamics/statics/StaticWrenchFactor.h index a98cc1bde..2df970eab 100644 --- a/gtdynamics/statics/StaticWrenchFactor.h +++ b/gtdynamics/statics/StaticWrenchFactor.h @@ -13,20 +13,18 @@ #pragma once +#include +#include #include #include #include #include #include -#include -#include +#include #include #include -#include "gtdynamics/utils/DynamicsSymbol.h" -#include "gtdynamics/utils/utils.h" - namespace gtdynamics { /** @@ -38,7 +36,7 @@ class StaticWrenchFactor : public gtsam::NoiseModelFactor { using This = StaticWrenchFactor; using Base = gtsam::NoiseModelFactor; double mass_; - boost::optional gravity_; + std::optional gravity_; public: /** @@ -49,24 +47,24 @@ class StaticWrenchFactor : public gtsam::NoiseModelFactor { * @param mass Mass for this link. * @param gravity (optional) Gravity vector in world frame. */ - StaticWrenchFactor( - const std::vector &wrench_keys, gtsam::Key pose_key, - const gtsam::noiseModel::Base::shared_ptr &cost_model, double mass, - const boost::optional &gravity = boost::none); + StaticWrenchFactor(const std::vector &wrench_keys, + gtsam::Key pose_key, + const gtsam::noiseModel::Base::shared_ptr &cost_model, + double mass, + const std::optional &gravity = {}); - public: /** * Evaluate ResultantWrench, which should be zero and is factor error. * @param values contains the pose and wrenches acting on the link. * @param H Jacobians, in the order: *wrenches, pose */ - gtsam::Vector unwhitenedError(const gtsam::Values &x, - boost::optional &> - H = boost::none) const override; + gtsam::Vector unwhitenedError( + const gtsam::Values &x, + gtsam::OptionalMatrixVecType H = nullptr) const override; /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -79,6 +77,7 @@ class StaticWrenchFactor : public gtsam::NoiseModelFactor { } private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template @@ -86,6 +85,7 @@ class StaticWrenchFactor : public gtsam::NoiseModelFactor { ar &boost::serialization::make_nvp( "NoiseModelFactor", boost::serialization::base_object(*this)); } +#endif }; } // namespace gtdynamics diff --git a/gtdynamics/statics/Statics.cpp b/gtdynamics/statics/Statics.cpp index 0c0294e3e..fd90114f4 100644 --- a/gtdynamics/statics/Statics.cpp +++ b/gtdynamics/statics/Statics.cpp @@ -11,6 +11,7 @@ * @author Frank Dellaert, Mandy Xie, Yetong Zhang, and Gerry Chen */ +#include #include #include #include @@ -21,7 +22,7 @@ using gtsam::Vector6; Vector6 GravityWrench(const gtsam::Vector3 &gravity, double mass, const gtsam::Pose3 &wTcom, - gtsam::OptionalJacobian<6, 6> H_wTcom = boost::none) { + gtsam::OptionalJacobian<6, 6> H_wTcom) { // Transform gravity from base frame to link COM frame. gtsam::Matrix33 H_unrotate; const gtsam::Rot3 wRcom = wTcom.rotation(); @@ -41,7 +42,7 @@ Vector6 GravityWrench(const gtsam::Vector3 &gravity, double mass, } Vector6 ResultantWrench(const std::vector &wrenches, - boost::optional &> H) { + gtsam::OptionalMatrixVecType H) { Vector6 sum = gtsam::Z_6x1; const size_t n = wrenches.size(); for (size_t i = 0; i < n; i++) { @@ -55,12 +56,12 @@ Vector6 ResultantWrench(const std::vector &wrenches, Vector6 ResultantWrench(const std::vector &wrenches, double mass, const gtsam::Pose3 &wTcom, - boost::optional gravity, - boost::optional &> H) { + std::optional gravity, + gtsam::OptionalMatrixVecType H) { // Calculate resultant wrench, fills up H with identity matrices if asked. const Vector6 external_wrench = ResultantWrench(wrenches, H); - // Potentiall add gravity wrench. + // Potentially add gravity wrench. if (gravity) { gtsam::Matrix6 H_wTcom; auto gravity_wrench = diff --git a/gtdynamics/statics/Statics.h b/gtdynamics/statics/Statics.h index a0a49168e..d74eac16e 100644 --- a/gtdynamics/statics/Statics.h +++ b/gtdynamics/statics/Statics.h @@ -13,13 +13,12 @@ #pragma once +#include +#include #include #include #include -#include "gtdynamics/kinematics/Kinematics.h" -#include "gtdynamics/utils/Slice.h" - namespace gtdynamics { /** @@ -30,9 +29,9 @@ namespace gtdynamics { * @param H_wTcom optional 6x6 Jacobian of wrench wrt COM pose * @returns 6x1 gravity wrench in CoM frame */ -gtsam::Vector6 GravityWrench( - const gtsam::Vector3& gravity, double mass, const gtsam::Pose3& wTcom, - gtsam::OptionalJacobian<6, 6> H_wTcom = boost::none); +gtsam::Vector6 GravityWrench(const gtsam::Vector3& gravity, double mass, + const gtsam::Pose3& wTcom, + gtsam::OptionalJacobian<6, 6> H_wTcom = {}); /** * @fn Calculate sum of wrenches with optional Jacobians (all identity!). @@ -40,9 +39,8 @@ gtsam::Vector6 GravityWrench( * @param H optional Jacobians, if given needs to be same size as wrenches. */ /// -gtsam::Vector6 ResultantWrench( - const std::vector& wrenches, - boost::optional&> H = boost::none); +gtsam::Vector6 ResultantWrench(const std::vector& wrenches, + gtsam::OptionalMatrixVecType H = nullptr); /** * @fn Calculate sum of external wrenches and gravity wrench on a body. @@ -54,12 +52,12 @@ gtsam::Vector6 ResultantWrench( */ gtsam::Vector6 ResultantWrench(const std::vector& wrenches, double mass, const gtsam::Pose3& wTcom, - boost::optional gravity, - boost::optional&> H); + std::optional gravity, + gtsam::OptionalMatrixVecType H = nullptr); /// Noise models etc specific to Statics class struct StaticsParameters : public KinematicsParameters { - boost::optional gravity, planar_axis; + std::optional gravity, planar_axis; using Isotropic = gtsam::noiseModel::Isotropic; const gtsam::SharedNoiseModel fs_cost_model, // statics cost model @@ -68,10 +66,9 @@ struct StaticsParameters : public KinematicsParameters { planar_cost_model; // planar factor /// Constructor with default arguments - StaticsParameters( - double sigma_dynamics = 1e-5, - const boost::optional& gravity = boost::none, - const boost::optional& planar_axis = boost::none) + StaticsParameters(double sigma_dynamics = 1e-5, + const std::optional& gravity = {}, + const std::optional& planar_axis = {}) : gravity(gravity), planar_axis(planar_axis), fs_cost_model(Isotropic::Sigma(6, 1e-4)), @@ -113,22 +110,25 @@ class Statics : public Kinematics { const Robot& robot) const; /** - * Create keys for unkowns and initial values. + * Create keys for unknowns and initial values. * TODO(frank): if we inherit, should we have *everything below us? * @param slice Slice instance. * @param robot Robot specification from URDF/SDF. + * @param gaussian_noise noise (stddev) added to initial values (default 0.0). */ gtsam::Values initialValues(const Slice& slice, const Robot& robot, - double gaussian_noise = 1e-6) const; + double gaussian_noise = 0.0) const; /** * Solve for wrenches given kinematics configuration. * @param slice Slice instance. * @param robot Robot specification from URDF/SDF. * @param configuration A known kinematics configuration. + * @param gaussian_noise noise (stddev) added to initial values (default 0.0). */ gtsam::Values solve(const Slice& slice, const Robot& robot, - const gtsam::Values& configuration) const; + const gtsam::Values& configuration, + double gaussian_noise = 0.0) const; /** * Solve for wrenches and kinematics configuration. diff --git a/gtdynamics/statics/StaticsSlice.cpp b/gtdynamics/statics/StaticsSlice.cpp index 408f6a488..1f651feb0 100644 --- a/gtdynamics/statics/StaticsSlice.cpp +++ b/gtdynamics/statics/StaticsSlice.cpp @@ -11,17 +11,16 @@ * @author: Frank Dellaert */ +#include // TODO: move +#include // TODO: move +#include // TODO: move +#include +#include #include #include #include #include -#include "gtdynamics/factors/TorqueFactor.h" // TODO: move -#include "gtdynamics/factors/WrenchEquivalenceFactor.h" // TODO: move -#include "gtdynamics/factors/WrenchPlanarFactor.h" // TODO: move -#include "gtdynamics/statics/StaticWrenchFactor.h" -#include "gtdynamics/statics/Statics.h" - namespace gtdynamics { using gtsam::assert_equal; using gtsam::Point3; @@ -68,7 +67,7 @@ gtsam::NonlinearFactorGraph Statics::graph(const Slice& slice, int i = link->id(); if (link->isFixed()) continue; const auto& connected_joints = link->joints(); - std::vector wrench_keys; + std::vector wrench_keys; // Add wrench keys for joints. for (auto&& joint : connected_joints) @@ -104,8 +103,8 @@ gtsam::Values Statics::initialValues(const Slice& slice, const Robot& robot, // Initialize wrenches and torques to 0. for (auto&& joint : robot.joints()) { int j = joint->id(); - InsertWrench(&values, joint->parent()->id(), j, k, gtsam::Z_6x1); - InsertWrench(&values, joint->child()->id(), j, k, gtsam::Z_6x1); + InsertWrench(&values, joint->parent()->id(), j, k, sampler.sample()); + InsertWrench(&values, joint->child()->id(), j, k, sampler.sample()); InsertTorque(&values, j, k, 0.0); } @@ -113,10 +112,11 @@ gtsam::Values Statics::initialValues(const Slice& slice, const Robot& robot, } gtsam::Values Statics::solve(const Slice& slice, const Robot& robot, - const gtsam::Values& configuration) const { + const gtsam::Values& configuration, + double gaussian_noise) const { auto graph = this->graph(slice, robot); gtsam::Values initial_values; - initial_values.insert(initialValues(slice, robot)); + initial_values.insert(initialValues(slice, robot, gaussian_noise)); // In this function we assume the kinematics is given, and we add priors to // the graph to enforce this. Would be much nicer with constant expressions. @@ -125,7 +125,7 @@ gtsam::Values Statics::solve(const Slice& slice, const Robot& robot, for (auto&& link : robot.links()) { auto key = PoseKey(link->id(), slice.k); auto pose = configuration.at(key); - graph.emplace_shared>(pose, key); + graph.emplace_shared>(key, pose); initial_values.insert(key, pose); } @@ -133,7 +133,7 @@ gtsam::Values Statics::solve(const Slice& slice, const Robot& robot, for (auto&& joint : robot.joints()) { auto key = JointAngleKey(joint->id(), slice.k); auto q = configuration.at(key); - graph.emplace_shared>(q, key); + graph.emplace_shared>(key, q); initial_values.insert(key, q); } diff --git a/gtdynamics/universal_robot/FixedJoint.h b/gtdynamics/universal_robot/FixedJoint.h new file mode 100644 index 000000000..35b82bae9 --- /dev/null +++ b/gtdynamics/universal_robot/FixedJoint.h @@ -0,0 +1,80 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file FixedJoint.h + * @brief Representation of a fixed joint. + * @author Varun Agrawal + */ + +#pragma once + +#include "gtdynamics/universal_robot/Joint.h" + +namespace gtdynamics { + +/** + * @class FixedJoint is a joint which has 0 degrees of freedom. + */ +class FixedJoint : public Joint { + private: + JointParams fixedJointParams() { + JointParams params; + params.effort_type = JointEffortType::Unactuated; + params.velocity_limit = 0.0; + params.acceleration_limit = 0.0; + params.torque_limit = 0.0; + return params; + } + + public: + /** + * @brief Create FixedJoint using joint name, joint pose in + * base frame, and parent and child links. + * + * Since a fixed joint has 0 degrees of freedom, we set the screw axis to + * Vector6::Zero. + * + * @param[in] id id for keys + * @param[in] name Name of the joint + * @param[in] bTj joint pose expressed in the base frame + * @param[in] parent_link Shared pointer to the parent Link. + * @param[in] child_link Shared pointer to the child Link. + */ + FixedJoint(unsigned char id, const std::string &name, const gtsam::Pose3 &bTj, + const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link) + : Joint(id, name, bTj, parent_link, child_link, gtsam::Vector6::Zero(), + fixedJointParams()) {} + + /// Return joint type for use in reconstructing robot from JointParams. + Type type() const override { return Type::Fixed; } + + private: + /// @name Advanced Interface + /// @{ + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Joint); + } +#endif + + /// @} +}; + +} // namespace gtdynamics + +namespace gtsam { + +template <> +struct traits + : public Testable {}; + +} // namespace gtsam diff --git a/gtdynamics/universal_robot/HelicalJoint.h b/gtdynamics/universal_robot/HelicalJoint.h index 3fb8e84b3..e56c6d8da 100644 --- a/gtdynamics/universal_robot/HelicalJoint.h +++ b/gtdynamics/universal_robot/HelicalJoint.h @@ -18,7 +18,7 @@ #pragma once -#include "gtdynamics/universal_robot/Joint.h" +#include namespace gtdynamics { @@ -73,12 +73,14 @@ class HelicalJoint : public Joint { /// @name Advanced Interface /// @{ +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int /*version*/) { ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Joint); } +#endif /// @} }; diff --git a/gtdynamics/universal_robot/Joint.cpp b/gtdynamics/universal_robot/Joint.cpp index 57bd5e63d..1f1ca06ce 100644 --- a/gtdynamics/universal_robot/Joint.cpp +++ b/gtdynamics/universal_robot/Joint.cpp @@ -11,43 +11,67 @@ /** * @file Joint.cpp - * @brief Absract representation of a robot joint. + * @brief Abstract representation of a robot joint. */ -#include "gtdynamics/universal_robot/Joint.h" - +#include +#include +#include #include #include -#include "gtdynamics/factors/JointLimitFactor.h" -#include "gtdynamics/universal_robot/Link.h" - using gtsam::Pose3; using gtsam::Vector6; namespace gtdynamics { /* ************************************************************************* */ +/* + * This constructor initializes the joint with its unique ID, name, pose, parent + * and child links, screw axis, and joint parameters. It computes several + * important quantities for kinematic and dynamic analysis: + * + * - `jMp_`: The pose of the parent link's COM expressed in the joint frame. + * - `jMc_`: The pose of the child link's COM expressed in the joint frame. + * - `pScrewAxis_`: The screw axis of the joint expressed in parent COM frame. + * - `cScrewAxis_`: The screw axis expressed in child link COM frame. + * + * The computation involves transforming the provided screw axis from the joint + * frame to the respective COM frames using the adjoint map of the + * transformation matrices. + * + * ASCII Illustration: + * + * Parent Link (COM) Joint Frame Child Link (COM) + * o----------------------X---------------------o + * bMcom() bTj (joint pose) bMcom() + * | | | + * |<--- jMp_ ------>|<------ jMc_ --------->| + * + * The screw axis is transformed from the joint frame to each COM frame: + * - pScrewAxis_: joint screw axis in parent COM frame + * - cScrewAxis_: joint screw axis in child COM frame + */ 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), + 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(); } /* ************************************************************************* */ @@ -83,27 +107,29 @@ Pose3 Joint::childTparent(double q, /* ************************************************************************* */ Vector6 Joint::transformTwistTo( const LinkSharedPtr &link, double q, double q_dot, - boost::optional other_twist, gtsam::OptionalJacobian<6, 1> H_q, + std::optional other_twist, gtsam::OptionalJacobian<6, 1> H_q, gtsam::OptionalJacobian<6, 1> H_q_dot, gtsam::OptionalJacobian<6, 6> H_other_twist) const { Vector6 other_twist_ = other_twist ? *other_twist : Vector6::Zero(); - auto other = otherLink(link); - auto this_ad_other = relativePoseOf(other, q).AdjointMap(); + + // Intermediate Jacobian declarations + gtsam::Matrix61 T_H_q; + gtsam::Matrix6 H_T; + + // Calculations + auto twist_from_other = + relativePoseOf(other, q, H_q ? &T_H_q : nullptr) + .Adjoint(other_twist_, H_q ? &H_T : nullptr, H_other_twist); if (H_q) { - // TODO(frank): really, zero below? Check derivatives - *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screwAxis(link)) * - other_twist_; + *H_q = H_T * T_H_q; } if (H_q_dot) { *H_q_dot = screwAxis(link); } - if (H_other_twist) { - *H_other_twist = this_ad_other; - } - return this_ad_other * other_twist_ + screwAxis(link) * q_dot; + return twist_from_other + screwAxis(link) * q_dot; } /* ************************************************************************* */ @@ -112,25 +138,24 @@ Vector6 Joint::transformWrenchCoordinate( gtsam::OptionalJacobian<6, 1> H_q, gtsam::OptionalJacobian<6, 6> H_wrench) const { auto other = otherLink(link); - gtsam::Pose3 T_21 = relativePoseOf(other, q); - gtsam::Matrix6 Ad_21_T = T_21.AdjointMap().transpose(); - gtsam::Vector6 transformed_wrench = Ad_21_T * wrench; - if (H_wrench) { - *H_wrench = Ad_21_T; - } + // Intermediate Jacobian declarations + gtsam::Matrix6 H_T_21; + gtsam::Matrix61 T_21_H_q; + + gtsam::Pose3 T_21 = relativePoseOf(other, q, H_q ? &T_21_H_q : nullptr); + gtsam::Vector6 transformed_wrench = + T_21.AdjointTranspose(wrench, H_q ? &H_T_21 : nullptr, H_wrench); + if (H_q) { - // TODO(frank): really, child? Double-check derivatives - *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screwAxis(link)) - .transpose() * - wrench; + *H_q = H_T_21 * T_21_H_q; } return transformed_wrench; } /* ************************************************************************* */ double Joint::transformWrenchToTorque( - const LinkSharedPtr &link, boost::optional wrench, + const LinkSharedPtr &link, std::optional wrench, gtsam::OptionalJacobian<1, 6> H_wrench) const { auto screw_axis_ = screwAxis(link); if (H_wrench) { @@ -154,14 +179,14 @@ gtsam::GaussianFactorGraph Joint::linearFDPriors( /* ************************************************************************* */ gtsam::GaussianFactorGraph Joint::linearAFactors( size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis) const { + const std::optional &planar_axis) const { gtsam::GaussianFactorGraph graph; 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 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 @@ -178,13 +203,13 @@ gtsam::GaussianFactorGraph Joint::linearAFactors( /* ************************************************************************* */ gtsam::GaussianFactorGraph Joint::linearDynamicsFactors( size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis) const { + const std::optional &planar_axis) const { gtsam::GaussianFactorGraph graph; 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 @@ -248,8 +273,8 @@ gtsam::NonlinearFactorGraph Joint::jointLimitFactors( /* ************************************************************************* */ std::ostream &Joint::to_stream(std::ostream &os) const { - os << name_ << "\n\tid=" << size_t(id_) - << "\n\tparent link: " << parent()->name() + os << name_ << " (" << JointTypeString(type()) << ")" + << "\n\tid=" << size_t(id_) << "\n\tparent link: " << parent()->name() << "\n\tchild link: " << child()->name() << "\n\tscrew axis (parent): " << screwAxis(parent()).transpose(); return os; @@ -268,12 +293,20 @@ std::ostream &operator<<(std::ostream &os, const JointSharedPtr &j) { /* ************************************************************************* */ gtsam::Vector6_ Joint::poseConstraint(uint64_t t) const { + return poseConstraint(PoseKey(parent()->id(), t), PoseKey(child()->id(), t), + JointAngleKey(id(), t)); +} + +/* ************************************************************************* */ +gtsam::Vector6_ Joint::poseConstraint(const DynamicsSymbol &wTp_key, + const DynamicsSymbol &wTc_key, + const DynamicsSymbol &q_key) const { using gtsam::Pose3_; // Get an expression for parent pose. - Pose3_ wTp(PoseKey(parent()->id(), t)); - Pose3_ wTc(PoseKey(child()->id(), t)); - gtsam::Double_ q(JointAngleKey(id(), t)); + Pose3_ wTp(wTp_key); + Pose3_ wTc(wTc_key); + gtsam::Double_ q(q_key); // Compute the expected pose of the child link. Pose3_ pTc(std::bind(&Joint::parentTchild, this, std::placeholders::_1, @@ -282,7 +315,7 @@ gtsam::Vector6_ Joint::poseConstraint(uint64_t t) const { Pose3_ wTc_hat = wTp * pTc; // Return the error in tangent space - return gtsam::logmap(wTc, wTc_hat); + return logmap(wTc, wTc_hat); } /* ************************************************************************* */ @@ -312,24 +345,23 @@ gtsam::Vector6_ Joint::twistAccelConstraint(uint64_t t) const { gtsam::Double_ qAccel(JointAccelKey(id(), t)); /// The following 2 lambda functions computes the expected twist acceleration - /// of the child link. (Note: we split it into 2 functions because the - /// expression constructor currently only support up to tenary expression.) + /// of the child link. + /// (Note: we split it this into 2 functions because the + /// expression constructor currently only supports at most ternary expressions.) auto transformTwistAccelTo1 = [this](double q, const Vector6 &other_twist_accel, gtsam::OptionalJacobian<6, 1> H_q, gtsam::OptionalJacobian<6, 6> H_other_twist_accel) { - Pose3 jTi = relativePoseOf(parent(), q); - Vector6 this_twist_accel = jTi.AdjointMap() * other_twist_accel; + // Intermediate Jacobian declarations + gtsam::Matrix61 jTi_H_q; + gtsam::Matrix6 H_jTi; + + Pose3 jTi = relativePoseOf(parent(), q, H_q ? &jTi_H_q : nullptr); + Vector6 this_twist_accel = jTi.Adjoint( + other_twist_accel, H_q ? &H_jTi : nullptr, H_other_twist_accel); - if (H_other_twist_accel) { - *H_other_twist_accel = jTi.AdjointMap(); - } if (H_q) { - // TODO(frank): really, zero below? Check derivatives. Also, - // copy/pasta from above? - *H_q = AdjointMapJacobianQ(q, relativePoseOf(parent(), 0.0), - cScrewAxis_) * - other_twist_accel; + *H_q = H_jTi * jTi_H_q; } return this_twist_accel; }; @@ -357,6 +389,20 @@ gtsam::Vector6_ Joint::twistAccelConstraint(uint64_t t) const { return twistAccel_c_hat1 + twistAccel_c_hat2 - twistAccel_c; } +/* ************************************************************************* */ +gtsam::Vector6_ Joint::childWrenchAdjoint(gtsam::Vector6_ &wrench_p, + uint64_t t) const { + gtsam::Double_ q(JointAngleKey(id(), t)); + + gtsam::Vector6_ wrench_c_adjoint( + std::bind(&Joint::transformWrenchCoordinate, this, parent(), + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), + q, wrench_p); + + return wrench_c_adjoint; +} + /* ************************************************************************* */ gtsam::Vector6_ Joint::wrenchEquivalenceConstraint(uint64_t t) const { gtsam::Vector6_ wrench_p(WrenchKey(parent()->id(), id(), t)); diff --git a/gtdynamics/universal_robot/Joint.h b/gtdynamics/universal_robot/Joint.h index 7cef42ab8..eb3f0fc8f 100644 --- a/gtdynamics/universal_robot/Joint.h +++ b/gtdynamics/universal_robot/Joint.h @@ -7,7 +7,7 @@ /** * @file Joint.h - * @brief Absract representation of a robot joint. + * @brief Abstract representation of a robot joint. * @author: Frank Dellaert * @author: Mandy Xie * @author: Alejandro Escontrela @@ -17,6 +17,9 @@ #pragma once +#include +#include +#include #include #include #include @@ -24,16 +27,15 @@ #include #include -#include -#include #include #include #include #include -#include "gtdynamics/dynamics/OptimizerSetting.h" -#include "gtdynamics/universal_robot/RobotTypes.h" -#include "gtdynamics/utils/DynamicsSymbol.h" +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#endif namespace gtdynamics { @@ -64,6 +66,7 @@ struct JointScalarLimit { JointScalarLimit() {} +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -72,6 +75,7 @@ struct JointScalarLimit { ar &BOOST_SERIALIZATION_NVP(value_upper_limit); ar &BOOST_SERIALIZATION_NVP(value_limit_threshold); } +#endif }; /** @@ -92,6 +96,7 @@ struct JointParams { /// Constructor JointParams() {} +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -107,10 +112,11 @@ struct JointParams { ar &BOOST_SERIALIZATION_NVP(damping_coefficient); ar &BOOST_SERIALIZATION_NVP(spring_coefficient); } +#endif }; /// Joint is the base class for a joint connecting two Link objects. -class Joint : public boost::enable_shared_from_this { +class Joint : public std::enable_shared_from_this { /// Robot class should have access to the internals of its joints. friend class Robot; @@ -124,8 +130,19 @@ class Joint : public boost::enable_shared_from_this { Revolute = 'R', Prismatic = 'P', Screw = 'H', + Fixed = 'F', }; + static std::string JointTypeString(const Type &type) { + std::map joint_type_map = { + {Revolute, "Revolute"}, + {Prismatic, "Prismatic"}, + {Screw, "Screw"}, + {Fixed, "Fixed"}, + }; + return joint_type_map[type]; + } + protected: /// This joint's name. std::string name_; @@ -138,9 +155,12 @@ class Joint : public boost::enable_shared_from_this { /// Rest transform to child link CoM frame from joint frame. Pose3 jMc_; - using LinkSharedPtr = boost::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 cycles. + // 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_; @@ -170,7 +190,7 @@ class Joint : public boost::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()); @@ -214,21 +234,35 @@ class Joint : public boost::enable_shared_from_this { /// Return joint name. const std::string &name() const { return name_; } + /** + * @brief Rename the joint. + * + * @param new_name The new name of the joint. + */ + void rename(const std::string& new_name) {name_ = new_name; } + + /** + * @brief Reassign the joint ID. + * + * @param new_id The new ID of the joint. + */ + void reassign(const uint8_t new_id) {id_ = new_id; } + /// 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_; } @@ -251,7 +285,7 @@ class Joint : public boost::enable_shared_from_this { /// Helper print function void print(const std::string &s = "") const { - std::cout << (s.empty() ? s : s + " ") << *this; + std::cout << (s.empty() ? s : s + " ") << *this << std::endl; } /// Helper function for overloading stream operator @@ -274,20 +308,20 @@ class Joint : public boost::enable_shared_from_this { * Return transform of child link CoM frame w.r.t parent link CoM frame */ Pose3 parentTchild(double q, - gtsam::OptionalJacobian<6, 1> pMc_H_q = boost::none) const; + gtsam::OptionalJacobian<6, 1> pMc_H_q = {}) const; /** * Return transform of parent link CoM frame w.r.t child link CoM frame */ Pose3 childTparent(double q, - gtsam::OptionalJacobian<6, 1> cMp_H_q = boost::none) const; + gtsam::OptionalJacobian<6, 1> cMp_H_q = {}) const; /** * Return the relative pose of the specified link [link2] in the other link's * [link1] reference frame. */ Pose3 relativePoseOf(const LinkSharedPtr &link2, double q, - gtsam::OptionalJacobian<6, 1> H_q = boost::none) const { + gtsam::OptionalJacobian<6, 1> H_q = {}) const { return isChildLink(link2) ? parentTchild(q, H_q) : childTparent(q, H_q); } @@ -296,8 +330,8 @@ class Joint : public boost::enable_shared_from_this { * the world pose of the other link [link1]. */ Pose3 poseOf(const LinkSharedPtr &link2, const Pose3 &wT1, double q, - gtsam::OptionalJacobian<6, 6> H_wT1 = boost::none, - gtsam::OptionalJacobian<6, 1> H_q = boost::none) const { + gtsam::OptionalJacobian<6, 6> H_wT1 = {}, + gtsam::OptionalJacobian<6, 1> H_q = {}) const { auto T12 = relativePoseOf(link2, q, H_q); return wT1.compose(T12, H_wT1); // H_wT2_T12 is identity } @@ -307,10 +341,10 @@ class Joint : public boost::enable_shared_from_this { */ gtsam::Vector6 transformTwistTo( const LinkSharedPtr &link, double q, double q_dot, - boost::optional other_twist = boost::none, - gtsam::OptionalJacobian<6, 1> H_q = boost::none, - gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const; + std::optional other_twist = {}, + gtsam::OptionalJacobian<6, 1> H_q = {}, + gtsam::OptionalJacobian<6, 1> H_q_dot = {}, + gtsam::OptionalJacobian<6, 6> H_other_twist = {}) const; /** * Express the same wrench in the coordinate frame of the other link. (This @@ -318,14 +352,13 @@ class Joint : public boost::enable_shared_from_this { */ gtsam::Vector6 transformWrenchCoordinate( const LinkSharedPtr &link, double q, const gtsam::Vector6 &wrench, - gtsam::OptionalJacobian<6, 1> H_q = boost::none, - gtsam::OptionalJacobian<6, 6> H_wrench = boost::none) const; + gtsam::OptionalJacobian<6, 1> H_q = {}, + gtsam::OptionalJacobian<6, 6> H_wrench = {}) const; /// Return the torque on this joint given the wrench double transformWrenchToTorque( - const LinkSharedPtr &link, - boost::optional wrench = boost::none, - gtsam::OptionalJacobian<1, 6> H_wrench = boost::none) const; + const LinkSharedPtr &link, std::optional wrench = {}, + gtsam::OptionalJacobian<1, 6> H_wrench = {}) const; /// Returns forward dynamics priors on torque gtsam::GaussianFactorGraph linearFDPriors(size_t t, @@ -343,7 +376,7 @@ class Joint : public boost::enable_shared_from_this { */ gtsam::GaussianFactorGraph linearAFactors( size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis = boost::none) const; + const std::optional &planar_axis = {}) const; /** * Returns linear dynamics factors in the dynamics graph. @@ -358,7 +391,7 @@ class Joint : public boost::enable_shared_from_this { */ gtsam::GaussianFactorGraph linearDynamicsFactors( size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis = boost::none) const; + const std::optional &planar_axis = {}) const; /** * Return joint limit factors in the dynamics graph. @@ -408,6 +441,15 @@ class Joint : public boost::enable_shared_from_this { */ gtsam::Vector6_ poseConstraint(uint64_t t = 0) const; + /** + * @brief Create expression that constraint the pose of two links imposed by + * the joint angle. Alternative version using custom keys. + * TODO(Varun) Need to do the same for all other constraints below + */ + gtsam::Vector6_ poseConstraint(const DynamicsSymbol &wTp_key, + const DynamicsSymbol &wTc_key, + const DynamicsSymbol &q_key) const; + /** * @brief Create expression that constraint the twist of two links imposed by * the joint angle and velocity. @@ -426,6 +468,12 @@ class Joint : public boost::enable_shared_from_this { */ gtsam::Vector6_ wrenchEquivalenceConstraint(uint64_t t = 0) const; + /** + * @brief Create expression for child wrench adjoint from parent link + */ + gtsam::Vector6_ childWrenchAdjoint(gtsam::Vector6_ &wrench_p, + uint64_t t = 0) const; + /** * @brief Create expression that constraint the relation between * wrench and torque on each link. @@ -436,6 +484,7 @@ class Joint : public boost::enable_shared_from_this { /// @name Advanced Interface /// @{ +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -450,6 +499,7 @@ class Joint : public boost::enable_shared_from_this { ar &BOOST_SERIALIZATION_NVP(cScrewAxis_); ar &BOOST_SERIALIZATION_NVP(parameters_); } +#endif /// @} }; diff --git a/gtdynamics/universal_robot/Link.cpp b/gtdynamics/universal_robot/Link.cpp index e29b4c038..3ceb6a714 100644 --- a/gtdynamics/universal_robot/Link.cpp +++ b/gtdynamics/universal_robot/Link.cpp @@ -10,24 +10,23 @@ * -------------------------------------------------------------------------- */ /** - * @file Joint.cpp - * @brief Absract representation of a robot joint. + * @file Link.cpp + * @brief Abstract representation of a robot link. */ -#include "gtdynamics/universal_robot/Link.h" - +#include +#include +#include #include #include -#include "gtdynamics/dynamics/Dynamics.h" -#include "gtdynamics/statics/Statics.h" - namespace gtdynamics { +/* ************************************************************************* */ gtsam::Vector6_ Link::wrenchConstraint( - const std::vector& wrench_keys, uint64_t t, - const boost::optional& gravity) const { + const std::vector& wrench_keys, uint64_t t, + const std::optional& gravity) const { // Collect wrenches to implement L&P Equation 8.48 (F = ma) std::vector wrenches; @@ -72,4 +71,40 @@ gtsam::Vector6_ Link::wrenchConstraint( return error; } +/* ************************************************************************* */ +gtsam::Matrix6 Link::inertiaMatrix() const { + std::vector gmm; + gmm.push_back(inertia_); + gmm.push_back(gtsam::I_3x3 * mass_); + return gtsam::diag(gmm); +} + +/* ************************************************************************* */ +bool Link::operator==(const Link& other) const { + return (this->name_ == other.name_ && this->id_ == other.id_ && + this->mass_ == other.mass_ && + this->centerOfMass_.equals(other.centerOfMass_) && + this->inertia_ == other.inertia_ && + this->bMcom_.equals(other.bMcom_) && + this->bMlink_.equals(other.bMlink_) && + this->is_fixed_ == other.is_fixed_ && + this->fixed_pose_.equals(other.fixed_pose_)); +} + +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Link& link) { + std::string fixed = link.isFixed() ? " (fixed)" : ""; + os << link.name() << ", id=" << size_t(link.id()) << fixed << ":\n"; + os << "\tcom pose: " << link.bMcom().rotation().rpy().transpose() << ", " + << link.bMcom().translation().transpose() << "\n"; + os << "\tlink pose: " << link.bMlink().rotation().rpy().transpose() << ", " + << link.bMlink().translation().transpose() << std::endl; + return os; +} + +/* ************************************************************************* */ +void Link::print(const std::string& s) const { + std::cout << (s.empty() ? s : s + " ") << *this; +} + } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/Link.h b/gtdynamics/universal_robot/Link.h index d9c350b0b..fd2a5f88e 100644 --- a/gtdynamics/universal_robot/Link.h +++ b/gtdynamics/universal_robot/Link.h @@ -7,12 +7,18 @@ /** * @file Link.h - * @brief only link part of a robot, does not include joint part - * @author: Frank Dellaert, Mandy Xie, and Alejandro Escontrela + * @brief Abstract representation of a robot link. + * @author: Frank Dellaert, Mandy Xie, Varun Agrawal, and Alejandro Escontrela */ #pragma once +#include +#include +#include +#include +#include +#include #include #include #include @@ -23,31 +29,22 @@ #include #include -#include -#include -#include #include +#include #include #include -#include "gtdynamics/dynamics/OptimizerSetting.h" -#include "gtdynamics/universal_robot/RobotTypes.h" -#include "gtdynamics/utils/DynamicsSymbol.h" -#include "gtdynamics/utils/utils.h" -#include "gtdynamics/utils/values.h" +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#endif namespace gtdynamics { -class Link; // forward declaration -class Joint; // forward declaration - -LINK_TYPEDEF_CLASS_POINTER(Link); -LINK_TYPEDEF_CLASS_POINTER(Joint); - /** - * @class Base class for links taking different format of parameters. + * @class Abstract base class for robot links. */ -class Link : public boost::enable_shared_from_this { +class Link : public std::enable_shared_from_this { private: uint8_t id_; std::string name_; @@ -74,6 +71,7 @@ class Link : public boost::enable_shared_from_this { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// Default constructor Link() {} /** @@ -101,16 +99,7 @@ class Link : public boost::enable_shared_from_this { /** destructor */ virtual ~Link() = default; - bool operator==(const Link &other) const { - return (this->name_ == other.name_ && this->id_ == other.id_ && - this->mass_ == other.mass_ && - this->centerOfMass_.equals(other.centerOfMass_) && - this->inertia_ == other.inertia_ && - this->bMcom_.equals(other.bMcom_) && - this->bMlink_.equals(other.bMlink_) && - this->is_fixed_ == other.is_fixed_ && - this->fixed_pose_.equals(other.fixed_pose_)); - } + bool operator==(const Link &other) const; bool operator!=(const Link &other) const { return !(*this == other); } @@ -135,8 +124,12 @@ class Link : public boost::enable_shared_from_this { /// Relative pose at rest from link’s COM to the base frame. inline const gtsam::Pose3 &bMcom() const { return bMcom_; } - /// Relative pose at rest from link frame to the base frame. mainly for - /// interoperability uses + /** + * @brief Relative pose at rest from link frame to the base frame. + * Mainly for interoperability uses. + * + * @return const gtsam::Pose3 + */ inline const gtsam::Pose3 bMlink() const { return bMlink_; } /// the fixed pose of the link @@ -157,24 +150,24 @@ class Link : public boost::enable_shared_from_this { /// Return link mass. double mass() const { return mass_; } + // Set Mass + inline void setMass(const double mass) { mass_ = mass; } + /// Return center of mass (gtsam::Pose3) const gtsam::Pose3 ¢erOfMass() const { return centerOfMass_; } /// Return inertia. const gtsam::Matrix3 &inertia() const { return inertia_; } + // Set Inertia + inline void setInertia(const gtsam::Matrix3 &inertia) { inertia_ = inertia; } + /// Return general mass gtsam::Matrix - gtsam::Matrix6 inertiaMatrix() const { - std::vector gmm; - gmm.push_back(inertia_); - gmm.push_back(gtsam::I_3x3 * mass_); - return gtsam::diag(gmm); - } + gtsam::Matrix6 inertiaMatrix() const; /// Functional way to fix a link - static Link fix( - const Link &link, - const boost::optional fixed_pose = boost::none) { + static Link fix(const Link &link, + const std::optional fixed_pose = {}) { // Copy construct Link fixed_link(link); // Fix the link @@ -191,17 +184,25 @@ class Link : public boost::enable_shared_from_this { return unfixed_link; } + /** + * @brief Rename the link. + * + * @param new_name The new name of the link. + */ + void rename(const std::string& new_name) {name_ = new_name; } + + /** + * @brief Reassign the link ID. + * + * @param new_id The new ID of the link. + */ + void reassign(const uint8_t new_id) {id_ = new_id; } + /// Print to ostream - friend std::ostream &operator<<(std::ostream &os, const Link &l) { - os << l.name(); - return os; - } + friend std::ostream &operator<<(std::ostream &os, const Link &link); /// Helper print function - void print(const std::string &s = "") const { - std::cout << (s.empty() ? s : s + " ") << *this; - } - + void print(const std::string &s = "") const; /** * @brief Create expression that constraint the wrench balance on the link. @@ -210,12 +211,12 @@ class Link : public boost::enable_shared_from_this { * @param gravity Gravitional constant. */ gtsam::Vector6_ wrenchConstraint( - const std::vector &wrench_keys, uint64_t t = 0, - const boost::optional &gravity = boost::none) const; + const std::vector &wrench_keys, uint64_t t = 0, + const std::optional &gravity = {}) const; private: /// fix the link to fixed_pose. If fixed_pose is not specified, use bTcom. - void fix(const boost::optional fixed_pose = boost::none) { + void fix(const std::optional fixed_pose = {}) { is_fixed_ = true; fixed_pose_ = fixed_pose ? *fixed_pose : bMcom(); } @@ -226,6 +227,7 @@ class Link : public boost::enable_shared_from_this { /// @name Advanced Interface /// @{ +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -240,6 +242,7 @@ class Link : public boost::enable_shared_from_this { ar &BOOST_SERIALIZATION_NVP(is_fixed_); ar &BOOST_SERIALIZATION_NVP(fixed_pose_); } +#endif /// @} }; diff --git a/gtdynamics/universal_robot/PrismaticJoint.h b/gtdynamics/universal_robot/PrismaticJoint.h index 2a8603a7d..b55b0f2e5 100644 --- a/gtdynamics/universal_robot/PrismaticJoint.h +++ b/gtdynamics/universal_robot/PrismaticJoint.h @@ -18,7 +18,7 @@ #pragma once -#include "gtdynamics/universal_robot/Joint.h" +#include namespace gtdynamics { @@ -66,12 +66,14 @@ class PrismaticJoint : public Joint { /// @name Advanced Interface /// @{ +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int /*version*/) { ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Joint); } +#endif /// @} }; diff --git a/gtdynamics/universal_robot/RevoluteJoint.h b/gtdynamics/universal_robot/RevoluteJoint.h index a5d96fc35..0e0648c45 100644 --- a/gtdynamics/universal_robot/RevoluteJoint.h +++ b/gtdynamics/universal_robot/RevoluteJoint.h @@ -18,7 +18,7 @@ #pragma once -#include "gtdynamics/universal_robot/Joint.h" +#include namespace gtdynamics { @@ -65,12 +65,14 @@ class RevoluteJoint : public Joint { /// @name Advanced Interface /// @{ +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int /*version*/) { ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Joint); } +#endif /// @} }; diff --git a/gtdynamics/universal_robot/Robot.cpp b/gtdynamics/universal_robot/Robot.cpp index 6857f26cb..3ca15d41d 100644 --- a/gtdynamics/universal_robot/Robot.cpp +++ b/gtdynamics/universal_robot/Robot.cpp @@ -11,7 +11,11 @@ * @author: Frank Dellaert, Mandy Xie, and Alejandro Escontrela */ -#include "gtdynamics/universal_robot/Robot.h" +#include +#include +#include +#include +#include #include #include @@ -19,11 +23,6 @@ #include #include -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/universal_robot/RobotTypes.h" -#include "gtdynamics/utils/utils.h" -#include "gtdynamics/utils/values.h" - using gtsam::Pose3; using gtsam::Vector3; using gtsam::Vector6; @@ -121,16 +120,13 @@ void Robot::print(const std::string &s) const { // Print links in sorted id order. cout << "LINKS:" << endl; - for (const auto &link : sorted_links) { - std::string fixed = link->isFixed() ? " (fixed)" : ""; - cout << link->name() << ", id=" << size_t(link->id()) << fixed << ":\n"; - cout << "\tcom pose: " << link->bMcom().rotation().rpy().transpose() << ", " - << link->bMcom().translation().transpose() << "\n"; + for (auto &&link : sorted_links) { + cout << *link; cout << "\tjoints: "; - for (const auto &joint : link->joints()) { + for (auto &&joint : link->joints()) { cout << joint->name() << " "; } - cout << "\n"; + cout << std::endl; } // Sort joints by id. @@ -141,7 +137,7 @@ void Robot::print(const std::string &s) const { // Print joints in sorted id order. cout << "JOINTS:" << endl; - for (const auto &joint : sorted_joints) { + for (auto &&joint : sorted_joints) { cout << joint << endl; auto pTc = joint->parentTchild(0.0); @@ -152,7 +148,7 @@ void Robot::print(const std::string &s) const { LinkSharedPtr Robot::findRootLink( const gtsam::Values &values, - const boost::optional &prior_link_name) const { + const std::optional &prior_link_name) const { LinkSharedPtr root_link; // Use prior_link if given. @@ -226,7 +222,7 @@ static bool InsertWithCheck(size_t i, size_t t, gtsam::Values Robot::forwardKinematics( const gtsam::Values &known_values, size_t t, - const boost::optional &prior_link_name) const { + const std::optional &prior_link_name) const { gtsam::Values values = known_values; // Set root link. @@ -269,4 +265,56 @@ gtsam::Values Robot::forwardKinematics( return values; } + +void Robot::renameLinks(const std::map& name_map) { + LinkMap new_links; + for (const auto& it : name_to_link_) { + const std::string& old_name = it.first; + const std::string& new_name = name_map.at(old_name); + it.second->rename(new_name); + new_links.insert({new_name, it.second}); + } + name_to_link_ = new_links; +} + +void Robot::renameJoints(const std::map& name_map) { + JointMap new_joints; + for (const auto& it : name_to_joint_) { + const std::string& old_name = it.first; + const std::string& new_name = name_map.at(old_name); + it.second->rename(new_name); + new_joints.insert({new_name, it.second}); + } + name_to_joint_ = new_joints; +} + +void Robot::reassignLinks(const std::vector &ordered_link_names) { + for (size_t i = 0; i < ordered_link_names.size(); i++) { + name_to_link_.at(ordered_link_names[i])->reassign(i); + } +} + +void Robot::reassignJoints( + const std::vector &ordered_joint_names) { + for (size_t i = 0; i < ordered_joint_names.size(); i++) { + name_to_joint_.at(ordered_joint_names[i])->reassign(i); + } +} + +std::vector Robot::orderedLinks() const { + std::map ordered_links; + for (const auto& it: name_to_link_) { + ordered_links.insert({it.second->id(), it.second}); + } + return getValues(ordered_links); +} + +std::vector Robot::orderedJoints() const { + std::map ordered_joints; + for (const auto& it: name_to_joint_) { + ordered_joints.insert({it.second->id(), it.second}); + } + return getValues(ordered_joints); +} + } // namespace gtdynamics. diff --git a/gtdynamics/universal_robot/Robot.h b/gtdynamics/universal_robot/Robot.h index 1ea88ef6a..edf4502b2 100644 --- a/gtdynamics/universal_robot/Robot.h +++ b/gtdynamics/universal_robot/Robot.h @@ -13,16 +13,21 @@ #pragma once -#include +#include +#include +#include +#include + #include +#include #include #include #include -#include "gtdynamics/config.h" -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/universal_robot/RobotTypes.h" +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#endif namespace gtdynamics { @@ -79,6 +84,66 @@ class Robot { /// remove specified joint from the robot void removeJoint(const JointSharedPtr &joint); + /** + * @brief Rename the links. + * + * @param name_map Map from old link name to new link name. This map must + * contain entries for all links in the robot; if a link name is missing, + * the implementation will throw (e.g., due to use of std::map::at()). + */ + void renameLinks(const std::map& name_map); + + /** + * @brief Rename the joints. + * + * The provided map must contain an entry for every joint in the robot. + * Internally, this function uses std::map::at() to look up the new name + * for each existing joint name, so missing keys will result in an + * exception being thrown. + * + * @param name_map Map from old joint name to new joint name, containing + * entries for all joints in the robot. + */ + void renameJoints(const std::map& name_map); + + /** + * @brief Reassign the link IDs. + * + * The provided list must contain the names of all links in the robot, + * with no omissions or duplicates. Internally the implementation uses + * a map lookup that will throw if any name is missing. The index of + * each name in the vector (0-indexed) determines the new ID assigned + * to the corresponding link. + * + * @param ordered_link_names List of all link names in the desired new + * ID order (0-indexed). + */ + void reassignLinks(const std::vector& ordered_link_names); + + /** + * @brief Reassign the joint IDs. + * + * @param ordered_joint_names List of joint names in the desired new order. + * This vector must contain the names of all joints in the robot, + * each appearing exactly once. The index of each name in the vector + * determines the joint's new ID (0-indexed). + */ + void reassignJoints(const std::vector& ordered_joint_names); + + /** + * @brief Return links ordered by their IDs. + * + * @return std::vector + */ + std::vector orderedLinks() const; + + /** + * @brief Return joints ordered by their IDs. + * + * @return std::vector + */ + std::vector orderedJoints() const; + /// Return the link corresponding to the input string. LinkSharedPtr link(const std::string &name) const; @@ -156,17 +221,18 @@ class Robot { */ gtsam::Values forwardKinematics( const gtsam::Values &known_values, size_t t = 0, - const boost::optional &prior_link_name = boost::none) const; + const std::optional &prior_link_name = {}) const; private: /// Find root link for forward kinematics LinkSharedPtr findRootLink( const gtsam::Values &values, - const boost::optional &prior_link_name) const; + const std::optional &prior_link_name) const; /// @name Advanced Interface /// @{ +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -174,6 +240,7 @@ class Robot { ar &BOOST_SERIALIZATION_NVP(name_to_link_); ar &BOOST_SERIALIZATION_NVP(name_to_joint_); } +#endif /// @} }; diff --git a/gtdynamics/universal_robot/RobotModels.h b/gtdynamics/universal_robot/RobotModels.h index e1a205bd9..07e6f3267 100644 --- a/gtdynamics/universal_robot/RobotModels.h +++ b/gtdynamics/universal_robot/RobotModels.h @@ -13,42 +13,39 @@ #pragma once -#include - -#include "gtdynamics/dynamics/DynamicsGraph.h" -#include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/universal_robot/sdf.h" +#include +#include +#include -using gtdynamics::kSdfPath; -using gtdynamics::kUrdfPath; +#include namespace four_bar_linkage_pure { gtdynamics::Robot getRobot() { gtdynamics::Robot four_bar = gtdynamics::CreateRobotFromFile( - kSdfPath + std::string("test/four_bar_linkage_pure.sdf")); + gtdynamics::kSdfPath + std::string("test/four_bar_linkage_pure.sdf")); return four_bar; } -gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, 0).finished(); -gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished(); +gtsam::Vector3 gravity = gtsam::Vector3::Zero(); +gtsam::Vector3 planar_axis(1, 0, 0); } // namespace four_bar_linkage_pure namespace simple_urdf { gtdynamics::Robot getRobot() { auto robot = gtdynamics::CreateRobotFromFile( - kUrdfPath + std::string("test/simple_urdf.urdf")); + gtdynamics::kUrdfPath + std::string("test/simple_urdf.urdf")); robot = robot.fixLink("l1"); return robot; } -gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, 0).finished(); -gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished(); +gtsam::Vector3 gravity = gtsam::Vector3::Zero(); +gtsam::Vector3 planar_axis(1, 0, 0); } // namespace simple_urdf namespace simple_urdf_prismatic { gtdynamics::Robot getRobot() { auto robot = gtdynamics::CreateRobotFromFile( - kUrdfPath + std::string("test/simple_urdf_prismatic.urdf")); + gtdynamics::kUrdfPath + std::string("test/simple_urdf_prismatic.urdf")); robot = robot.fixLink("l1"); return robot; } @@ -57,45 +54,47 @@ gtdynamics::Robot getRobot() { namespace simple_urdf_zero_inertia { gtdynamics::Robot getRobot() { auto robot = gtdynamics::CreateRobotFromFile( - kUrdfPath + std::string("test/simple_urdf_zero_inertia.urdf")); + gtdynamics::kUrdfPath + std::string("test/simple_urdf_zero_inertia.urdf")); robot = robot.fixLink("l1"); return robot; } -gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, 0).finished(); -gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished(); +gtsam::Vector3 gravity = gtsam::Vector3::Zero(); +gtsam::Vector3 planar_axis(1, 0, 0); } // namespace simple_urdf_zero_inertia namespace simple_urdf_eq_mass { gtdynamics::Robot getRobot() { auto robot = gtdynamics::CreateRobotFromFile( - kUrdfPath + std::string("test/simple_urdf_eq_mass.urdf")); + gtdynamics::kUrdfPath + std::string("test/simple_urdf_eq_mass.urdf")); return robot; } -gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, 0).finished(); -gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished(); +gtsam::Vector3 gravity = gtsam::Vector3::Zero(); +gtsam::Vector3 planar_axis(1, 0, 0); } // namespace simple_urdf_eq_mass namespace simple_rr { gtdynamics::Robot getRobot() { auto robot = gtdynamics::CreateRobotFromFile( - kSdfPath + std::string("test/simple_rr.sdf"), "simple_rr_sdf"); + gtdynamics::kSdfPath + std::string("test/simple_rr.sdf"), "simple_rr_sdf"); return robot; } -gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, 0).finished(); -gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished(); + +gtsam::Vector3 gravity = gtsam::Vector3::Zero(); +gtsam::Vector3 planar_axis(1, 0, 0); } // namespace simple_rr namespace jumping_robot { gtdynamics::Robot getRobot() { gtdynamics::Robot jumping_robot = gtdynamics::CreateRobotFromFile( - kSdfPath + std::string("test/jumping_robot.sdf")); + gtdynamics::kSdfPath + std::string("test/jumping_robot.sdf")); jumping_robot = jumping_robot.fixLink("l0"); return jumping_robot; } -gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished(); -gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished(); + +gtsam::Vector3 gravity(0, 0, -9.8); +gtsam::Vector3 planar_axis(1, 0, 0); } // namespace jumping_robot diff --git a/gtdynamics/universal_robot/RobotTypes.h b/gtdynamics/universal_robot/RobotTypes.h index f3d1b112a..2933e20b8 100644 --- a/gtdynamics/universal_robot/RobotTypes.h +++ b/gtdynamics/universal_robot/RobotTypes.h @@ -13,10 +13,10 @@ #pragma once -#include +#include -#define LINK_TYPEDEF_CLASS_POINTER(Class) \ - class Class; \ - typedef boost::shared_ptr Class##SharedPtr; \ - typedef boost::shared_ptr Class##ConstSharedPtr; \ - typedef boost::weak_ptr Class##WeakPtr +#define LINK_TYPEDEF_CLASS_POINTER(Class) \ + class Class; \ + typedef std::shared_ptr Class##SharedPtr; \ + typedef std::shared_ptr Class##ConstSharedPtr; \ + typedef std::weak_ptr Class##WeakPtr diff --git a/gtdynamics/universal_robot/sdf.cpp b/gtdynamics/universal_robot/sdf.cpp index dcc9bab50..f9753632f 100644 --- a/gtdynamics/universal_robot/sdf.cpp +++ b/gtdynamics/universal_robot/sdf.cpp @@ -11,51 +11,76 @@ * @author Frank Dellaert, Alejandro Escontrela, Stephanie McCormick */ -#include "gtdynamics/universal_robot/sdf.h" +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/universal_robot/PrismaticJoint.h" -#include "gtdynamics/universal_robot/RevoluteJoint.h" -#include "gtdynamics/universal_robot/HelicalJoint.h" -#include "gtdynamics/universal_robot/sdf_internal.h" - namespace gtdynamics { using gtsam::Pose3; sdf::Model GetSdf(const std::string &sdf_file_path, - const std::string &model_name) { - sdf::SDFPtr sdf = sdf::readFile(sdf_file_path); - if (sdf==nullptr) + const std::string &model_name, + const sdf::ParserConfig &config) { + sdf::Errors errors; + + sdf::SDFPtr sdf = sdf::readFile(sdf_file_path, config, errors); + if (sdf == nullptr || errors.size() > 0) { + for (auto &&error : errors) { + std::cout << "ERROR: " << error << std::endl; + } + throw std::runtime_error("SDF library could not parse " + sdf_file_path); + } - sdf::Model model = sdf::Model(); - model.Load(sdf->Root()->GetElement("model")); + sdf::Root root; + errors = root.Load(sdf, config); + if (errors.size() > 0) { + for (auto &&error : errors) { + std::cout << "ERROR: " << error.Message() << std::endl; + } + throw std::runtime_error("Error loading SDF file " + sdf_file_path); + } // Check whether this is a world file, in which case we have to first // access the world element then check whether one of its child models // corresponds to model_name. - if (model.Name() != "__default__") return model; - - // Load the world element. - sdf::World world = sdf::World(); - world.Load(sdf->Root()->GetElement("world")); + if (root.WorldCount() > 0) { + // Iterate through all the worlds and all the models in each world to find + // the desired model. + for (size_t widx = 0; widx < root.WorldCount(); widx++) { + const sdf::World *world = root.WorldByIndex(widx); + + for (uint midx = 0; midx < world->ModelCount(); midx++) { + const sdf::Model *curr_model = world->ModelByIndex(midx); + if (curr_model->Name() == model_name) { + return *curr_model; + } + } + } - for (uint i = 0; i < world.ModelCount(); i++) { - sdf::Model curr_model = *world.ModelByIndex(i); - if (curr_model.Name() == model_name) return curr_model; + } else { + // There is no world element so we directly access the model element. + const sdf::Model *model = root.Model(); + if (model->Name() != "__default__") { + return *model; + } } throw std::runtime_error("Model not found in: " + sdf_file_path); } -gtsam::Pose3 Pose3FromIgnition(const ignition::math::Pose3d &ignition_pose) { - const auto &rot = ignition_pose.Rot(); - const auto &pos = ignition_pose.Pos(); +gtsam::Pose3 Pose3FromGazebo(const gz::math::Pose3d &gazebo_pose) { + const auto &rot = gazebo_pose.Rot(); + const auto &pos = gazebo_pose.Pos(); return gtsam::Pose3( gtsam::Rot3(gtsam::Quaternion(rot.W(), rot.X(), rot.Y(), rot.Z())), gtsam::Point3(pos[0], pos[1], pos[2])); @@ -75,48 +100,47 @@ JointParams ParametersFromSdfJoint(const sdf::Joint &sdf_joint) { // Get Link pose in base frame from sdf::Link object Pose3 GetSdfLinkFrame(const sdf::Link *sdf_link) { - // Call SemanticPose::Resolve so the pose is resolved to the correct frame - // http://sdformat.org/tutorials?tut=pose_frame_semantics&ver=1.7&cat=specification& - // Get non-const pose of link in the frame of the joint it is connect to // (http://wiki.ros.org/urdf/XML/link). auto raw_pose = sdf_link->RawPose(); // Update from joint frame to base frame in-place. - // Base frame is denoted by "". - auto errors = sdf_link->SemanticPose().Resolve(raw_pose, ""); + // Call SemanticPose::Resolve so the pose is resolved to the correct frame. + // Default base frame is "". + // http://sdformat.org/tutorials?tut=pose_frame_semantics&ver=1.7&cat=specification& + auto errors = sdf_link->SemanticPose().Resolve(raw_pose); // If any errors in the resolution, throw an exception. if (errors.size() > 0) { throw std::runtime_error(errors[0].Message()); } - // Pose is updated from joint frame to base frame. - const auto bMl = Pose3FromIgnition(raw_pose); + // Pose is updated from joint frame to base frame. + const Pose3 bMl = Pose3FromGazebo(raw_pose); return bMl; } Pose3 GetJointFrame(const sdf::Joint &sdf_joint, const sdf::Link *parent_sdf_link, const sdf::Link *child_sdf_link) { - // Name of the coordinate frame the joint's pose is relative to. // Specified by `relative_to` in the SDF file. std::string frame_name = sdf_joint.PoseRelativeTo(); // Get the pose of the joint in the parent or child link's frame depending on // the value of `frame_name`. - Pose3 lTj = Pose3FromIgnition(sdf_joint.RawPose()); + Pose3 lTj = Pose3FromGazebo(sdf_joint.RawPose()); if (frame_name.empty() || frame_name == child_sdf_link->Name()) { // If `frame_name` is empty or has the same name as the child_link, it means // the joint frame is relative to the child link. So to get the joint pose // in the base frame, we pre-multiply by the child link's frame. - // The child link frame here is not COM of the link, + // The child link frame here is not COM of the link, // it is the pose of the link as described in the sdf file. // This is done here once in order to avoid saving the bTl transform // as part of the Link class. - // We need the bTl here because the joint is defined in the link frame in the sdf. + // We need the bTl here because the joint is defined in the link frame in + // the sdf. const Pose3 bTcl = GetSdfLinkFrame(child_sdf_link); return bTcl * lTj; @@ -151,12 +175,12 @@ LinkSharedPtr LinkFromSdf(uint8_t id, const sdf::Link &sdf_link) { // Get the pose of the link in the base frame // we only save the bMcom rest matrix as part of the Link class const Pose3 bMl = GetSdfLinkFrame(&sdf_link); - const Pose3 lMcom = Pose3FromIgnition(sdf_link.Inertial().Pose()); + const Pose3 lMcom = Pose3FromGazebo(sdf_link.Inertial().Pose()); const Pose3 bMcom = bMl * lMcom; - return boost::make_shared(id, sdf_link.Name(), - sdf_link.Inertial().MassMatrix().Mass(), - inertia, bMcom, bMl); + return std::make_shared(id, sdf_link.Name(), + sdf_link.Inertial().MassMatrix().Mass(), + inertia, bMcom, bMl); } LinkSharedPtr LinkFromSdf(uint8_t id, const std::string &link_name, @@ -173,27 +197,38 @@ JointSharedPtr JointFromSdf(uint8_t id, const LinkSharedPtr &parent_link, const sdf::Joint &sdf_joint) { JointSharedPtr joint; - // Generate a joint parameters struct with values from the SDF. - JointParams parameters = ParametersFromSdfJoint(sdf_joint); - std::string name(sdf_joint.Name()); + Pose3 bMj = GetJointFrame(sdf_joint, parent_sdf_link, child_sdf_link); - const gtsam::Vector3 axis = GetSdfAxis(sdf_joint); + JointParams parameters; + gtsam::Vector3 axis; + // Fixed joints don't have parameters or an axis. + if (sdf_joint.Type() != sdf::JointType::FIXED) { + // Generate a joint parameters struct with values from the SDF. + parameters = ParametersFromSdfJoint(sdf_joint); + // Get the joint axis. + axis = GetSdfAxis(sdf_joint); + } + switch (sdf_joint.Type()) { case sdf::JointType::PRISMATIC: - joint = boost::make_shared(id, name, bMj, parent_link, - child_link, axis, parameters); + joint = std::make_shared(id, name, bMj, parent_link, + child_link, axis, parameters); break; case sdf::JointType::REVOLUTE: - joint = boost::make_shared(id, name, bMj, parent_link, - child_link, axis, parameters); + joint = std::make_shared(id, name, bMj, parent_link, + child_link, axis, parameters); break; case sdf::JointType::SCREW: - joint = boost::make_shared( + joint = std::make_shared( id, name, bMj, parent_link, child_link, axis, sdf_joint.ThreadPitch(), parameters); break; + case sdf::JointType::FIXED: + joint = + std::make_shared(id, name, bMj, parent_link, child_link); + break; default: throw std::runtime_error("Joint type for [" + name + "] not yet supported"); @@ -221,13 +256,13 @@ static LinkJointPair ExtractRobotFromSdf(const sdf::Model &sdf) { sdf::Joint sdf_joint = *sdf.JointByIndex(j); // Get this joint's parent and child links. - std::string parent_link_name = sdf_joint.ParentLinkName(); - std::string child_link_name = sdf_joint.ChildLinkName(); + std::string parent_link_name = sdf_joint.ParentName(); + std::string child_link_name = sdf_joint.ChildName(); if (parent_link_name == "world") { // This joint fixes the child link in the world frame. LinkSharedPtr child_link = name_to_link[child_link_name]; Pose3 fixed_pose = child_link->bMcom(); - child_link = boost::make_shared(Link::fix(*child_link, fixed_pose)); + child_link = std::make_shared(Link::fix(*child_link, fixed_pose)); continue; } LinkSharedPtr parent_link = name_to_link[parent_link_name]; @@ -236,7 +271,8 @@ static LinkJointPair ExtractRobotFromSdf(const sdf::Model &sdf) { const sdf::Link *child_sdf_link = sdf.LinkByName(child_link_name); // Construct Joint and insert into name_to_joint. - JointSharedPtr joint = JointFromSdf(j, parent_link, parent_sdf_link, child_link, child_sdf_link, sdf_joint); + JointSharedPtr joint = JointFromSdf(j, parent_link, parent_sdf_link, + child_link, child_sdf_link, sdf_joint); name_to_joint.emplace(joint->name(), joint); // Update list of parent and child links/joints for each Link. @@ -253,10 +289,13 @@ static LinkJointPair ExtractRobotFromSdf(const sdf::Model &sdf) { * robot description. * @param[in] model_name name of the robot we care about. Must be specified in * case sdf_file_path points to a world file. + * @param[in] preserve_fixed_joint Flag indicating if the fixed joints in the + * URDF file should be preserved and not merged. * @return LinkMap and JointMap as a pair */ static LinkJointPair ExtractRobotFromFile(const std::string &file_path, - const std::string &model_name) { + const std::string &model_name, + bool preserve_fixed_joint) { std::ifstream is(file_path); if (!is.good()) throw std::runtime_error("ExtractRobotFromFile: no file found at " + @@ -266,17 +305,23 @@ static LinkJointPair ExtractRobotFromFile(const std::string &file_path, std::string file_ext = file_path.substr(file_path.find_last_of(".") + 1); std::transform(file_ext.begin(), file_ext.end(), file_ext.begin(), ::tolower); - if (file_ext == "urdf") - return ExtractRobotFromSdf(GetSdf(file_path)); - else if (file_ext == "sdf") - return ExtractRobotFromSdf(GetSdf(file_path, model_name)); + sdf::ParserConfig _config = sdf::ParserConfig::GlobalConfig(); + _config.URDFSetPreserveFixedJoint(preserve_fixed_joint); + + if (file_ext == "urdf") { + return ExtractRobotFromSdf(GetSdf(file_path, "", _config)); + } else if (file_ext == "sdf") { + return ExtractRobotFromSdf(GetSdf(file_path, model_name, _config)); + } throw std::runtime_error("Invalid file extension."); } Robot CreateRobotFromFile(const std::string &file_path, - const std::string &model_name) { - auto links_joints_pair = ExtractRobotFromFile(file_path, model_name); + const std::string &model_name, + bool preserve_fixed_joint) { + auto links_joints_pair = + ExtractRobotFromFile(file_path, model_name, preserve_fixed_joint); return Robot(links_joints_pair.first, links_joints_pair.second); } diff --git a/gtdynamics/universal_robot/sdf.h b/gtdynamics/universal_robot/sdf.h index 63eeb9b4a..ab87c8831 100644 --- a/gtdynamics/universal_robot/sdf.h +++ b/gtdynamics/universal_robot/sdf.h @@ -13,9 +13,9 @@ #pragma once -#include +#include -#include "gtdynamics/universal_robot/Robot.h" +#include namespace gtdynamics { @@ -24,8 +24,11 @@ namespace gtdynamics { * @param[in] file_path path to the file. * @param[in] model_name name of the robot we care about. Must be specified in * case sdf_file_path points to a world file. + * @param[in] preserve_fixed_joint Flag indicating if the fixed joints in the + * URDF file should be preserved and not merged. */ Robot CreateRobotFromFile(const std::string &file_path, - const std::string &model_name = ""); + const std::string &model_name = "", + bool preserve_fixed_joint = false); } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/sdf_internal.h b/gtdynamics/universal_robot/sdf_internal.h index 16f66c00d..e72570842 100644 --- a/gtdynamics/universal_robot/sdf_internal.h +++ b/gtdynamics/universal_robot/sdf_internal.h @@ -13,12 +13,12 @@ #pragma once -#include +#include + +#include #include #include -#include "gtdynamics/universal_robot/Robot.h" - namespace gtdynamics { /** @@ -29,8 +29,9 @@ namespace gtdynamics { * sdf_file_path points to a world file. * @return SDF Model */ -sdf::Model GetSdf(const std::string &sdf_file_path, - const std::string &model_name = ""); +sdf::Model GetSdf( + const std::string &sdf_file_path, const std::string &model_name = "", + const sdf::ParserConfig &config = sdf::ParserConfig::GlobalConfig()); /** * @fn Construct a Link class from sdf::Link @@ -63,11 +64,11 @@ JointSharedPtr JointFromSdf(uint8_t id, const LinkSharedPtr &parent_link, const sdf::Joint &sdf_joint); /** - * Parse a ignition::math Pose object into a gtsam::Pose. + * Parse a gz::math Pose object into a gtsam::Pose. * - * @param ignition_pose An ignition::math::Pose object to be parsed. + * @param gazebo_pose An gz::math::Pose object to be parsed. */ -gtsam::Pose3 Pose3FromIgnition(const ignition::math::Pose3d &ignition_pose); +gtsam::Pose3 Pose3FromGazebo(const gz::math::Pose3d &gazebo_pose); /** * @fn Extract joint parameter values from an input sdf::Joint. diff --git a/gtdynamics/utils/ChainInitializer.cpp b/gtdynamics/utils/ChainInitializer.cpp new file mode 100644 index 000000000..9636dad8a --- /dev/null +++ b/gtdynamics/utils/ChainInitializer.cpp @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ChainInitializer.h + * @brief Utility methods for initializing trajectory optimization solutions for + * chain graph. + * @author: Dan Barladeanu + */ + +#include + +namespace gtdynamics { + +gtsam::Values ChainInitializer::ZeroValues( + const Robot& robot, const int t, double gaussian_noise, + const std::optional& contact_points) const { + gtsam::Values values; + + auto sampler_noise_model = + gtsam::noiseModel::Isotropic::Sigma(6, gaussian_noise); + gtsam::Sampler sampler(sampler_noise_model); + + // Initialize link dynamics to 0. + for (auto&& link : robot.links()) { + const int i = link->id(); + // TODO(Varun): Not sure what's happening here. + // The link IDs are not guaranteed to be consistent across robots. + // If these IDs are for the feet, they should be passed in as an argument. + if (i == 0 || i == 3 || i == 6 || i == 9 || i == 12) + InsertPose(&values, i, t, AddGaussianNoiseToPose(link->bMcom(), sampler)); + } + InsertTwist(&values, 0, t, sampler.sample()); + InsertTwistAccel(&values, 0, t, sampler.sample()); + + // Initialize joint kinematics/dynamics to 0. + for (auto &&joint : robot.joints()) { + const int j = joint->id(); + if (joint->parent()->name().find("trunk") != std::string::npos) { + InsertWrench(&values, joint->parent()->id(), j, t, sampler.sample()); + } + std::vector keys = {JointAngleKey(j, t), + JointVelKey(j, t), JointAccelKey(j, t)}; + for (size_t i = 0; i < keys.size(); i++) + values.insert(keys[i], sampler.sample()[0]); + } + + // TODO(Varun): Why are these commented out? + //if (contact_points) { + //for (auto&& cp : *contact_points) { + // TODO(frank): allow multiple contact points on one link, id = 0,1,2... + //values.insert(ContactWrenchKey(cp.link->id(), 0, t), sampler.sample()); + //std::cout<id(), t, AddGaussianNoiseToPose(cp.link->bMcom(), sampler)); + //} + //} + + return values; +} + +} // namespace gtdynamics \ No newline at end of file diff --git a/gtdynamics/utils/ChainInitializer.h b/gtdynamics/utils/ChainInitializer.h new file mode 100644 index 000000000..e47717999 --- /dev/null +++ b/gtdynamics/utils/ChainInitializer.h @@ -0,0 +1,38 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ChainInitializer.h + * @brief Utility methods for initializing trajectory optimization solutions for chain graph. + * @author: Dan Barladeanu + */ + +#pragma once + +#include + +namespace gtdynamics { + +class ChainInitializer : public Initializer { + + public: + /** + * @fn Return zero values for all variables for initial value of optimization. + * + * @param[in] robot A Robot object. + * @param[in] t Timestep to return zero initial values for. + * @param[in] gaussian_noise Optional gaussian noise to add to initial values. + * Noise drawn from a zero-mean gaussian distribution with a standard + * deviation of gaussian_noise. + * @param[in] contact_points Contact points for timestep t. + */ + gtsam::Values ZeroValues( + const Robot& robot, const int t, double gaussian_noise = 0.0, + const std::optional& contact_points = {}) const override; +}; + +} // namespace gtdynamics diff --git a/gtdynamics/utils/DynamicsSymbol.cpp b/gtdynamics/utils/DynamicsSymbol.cpp index d2d71a70f..a248428dd 100644 --- a/gtdynamics/utils/DynamicsSymbol.cpp +++ b/gtdynamics/utils/DynamicsSymbol.cpp @@ -13,8 +13,6 @@ #include -#include -#include #include using gtsam::Key; @@ -33,8 +31,10 @@ DynamicsSymbol::DynamicsSymbol(const DynamicsSymbol& key) t_(key.t_) {} /* ************************************************************************* */ -DynamicsSymbol::DynamicsSymbol(const std::string& s, uint8_t link_idx, - uint8_t joint_idx, uint64_t t) +DynamicsSymbol::DynamicsSymbol(const std::string& s, // + uint8_t link_idx, // + uint8_t joint_idx, // + uint64_t t) : link_idx_(link_idx), joint_idx_(joint_idx), t_(t) { if (s.length() > 2) { throw std::runtime_error( @@ -54,19 +54,20 @@ DynamicsSymbol::DynamicsSymbol(const std::string& s, uint8_t link_idx, DynamicsSymbol DynamicsSymbol::LinkJointSymbol(const std::string& s, uint8_t link_idx, - uint8_t joint_idx, + uint8_t joint_idx, // uint64_t t) { return DynamicsSymbol(s, link_idx, joint_idx, t); } DynamicsSymbol DynamicsSymbol::JointSymbol(const std::string& s, - uint8_t joint_idx, + uint8_t joint_idx, // uint64_t t) { return DynamicsSymbol(s, kMax_uchar_, joint_idx, t); } DynamicsSymbol DynamicsSymbol::LinkSymbol(const std::string& s, - uint8_t link_idx, uint64_t t) { + uint8_t link_idx, // + uint64_t t) { return DynamicsSymbol(s, link_idx, kMax_uchar_, t); } @@ -130,6 +131,16 @@ std::string _GTDKeyFormatter(Key key) { return std::string(DynamicsSymbol(key)); } +bool IsQLevel(const gtsam::Key &key) { + DynamicsSymbol symb(key); + return symb.label() == "p" || symb.label() == "q"; +} + +bool IsVLevel(const gtsam::Key &key) { + DynamicsSymbol symb(key); + return symb.label() == "V" || symb.label() == "v"; +} + /* ************************************************************************* */ } // namespace gtdynamics diff --git a/gtdynamics/utils/DynamicsSymbol.h b/gtdynamics/utils/DynamicsSymbol.h index f526d64bb..ada42d4fb 100644 --- a/gtdynamics/utils/DynamicsSymbol.h +++ b/gtdynamics/utils/DynamicsSymbol.h @@ -16,6 +16,13 @@ #include #include +#include + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#endif + namespace gtdynamics { class DynamicsSymbol { @@ -32,8 +39,8 @@ class DynamicsSymbol { * @param[in] joint_idx index of the joint * @param[in] t time step */ - DynamicsSymbol(const std::string& s, uint8_t link_idx, - uint8_t joint_idx, uint64_t t); + DynamicsSymbol(const std::string& s, uint8_t link_idx, uint8_t joint_idx, + uint64_t t); public: /** Default constructor */ @@ -46,8 +53,7 @@ class DynamicsSymbol { * Constructor for symbol related to both link and joint. * See private constructor */ - static DynamicsSymbol LinkJointSymbol(const std::string& s, - uint8_t link_idx, + static DynamicsSymbol LinkJointSymbol(const std::string& s, uint8_t link_idx, uint8_t joint_idx, uint64_t t); /** @@ -57,8 +63,8 @@ class DynamicsSymbol { * @param[in] joint_idx index of the joint * @param[in] t time step */ - static DynamicsSymbol JointSymbol(const std::string& s, - uint8_t joint_idx, uint64_t t); + static DynamicsSymbol JointSymbol(const std::string& s, uint8_t joint_idx, + uint64_t t); /** * Constructor for symbol related to only link (e.g. link pose). @@ -113,6 +119,7 @@ class DynamicsSymbol { operator std::string() const; private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template @@ -123,13 +130,13 @@ class DynamicsSymbol { ar& BOOST_SERIALIZATION_NVP(joint_idx_); ar& BOOST_SERIALIZATION_NVP(t_); } +#endif /** * \defgroup Bitfield bit field constants * @{ */ - static constexpr size_t kMax_uchar_ = - std::numeric_limits::max(); + static constexpr size_t kMax_uchar_ = std::numeric_limits::max(); // bit counts static constexpr size_t key_bits = sizeof(gtsam::Key) * 8; static constexpr size_t ch1_bits = sizeof(uint8_t) * 8; @@ -156,4 +163,92 @@ std::string _GTDKeyFormatter(gtsam::Key key); static const gtsam::KeyFormatter GTDKeyFormatter = &_GTDKeyFormatter; +/** + * @brief Check if the key corresponds to a configuration level variable (q, p). + * + * @param key The key to check. + * @return true If the key is a Q level variable. + * @return false otherwise. + */ +bool IsQLevel(const gtsam::Key &key); + +/** + * @brief Check if the key corresponds to a velocity level variable (v, V). + * + * @param key The key to check. + * @return true If the key is a V level variable. + * @return false otherwise. + */ +bool IsVLevel(const gtsam::Key &key); + +/** + * @brief Identify the highest level of the keys in the container. + * 0: configuration level, 1: velocity level, 2: acceleration/dynamics level. + * + * @tparam CONTAINER Type of the container. + * @param keys The container of keys. + * @return int The highest level found. + */ +template +inline int IdentifyLevel(const CONTAINER &keys) { + int lvl = 0; + for (const auto &key : keys) { + if (IsQLevel(key)) { + lvl = std::max(lvl, 0); + } else if (IsVLevel(key)) { + lvl = std::max(lvl, 1); + } else { + lvl = std::max(lvl, 2); + } + } + return lvl; +} + +/** + * @brief Classify keys into three levels: configuration, velocity, and acceleration/dynamics. + * + * @tparam CONTAINER Type of the container. + * @param keys The container of keys. + * @param q_keys Output set of configuration level keys. + * @param v_keys Output set of velocity level keys. + * @param ad_keys Output set of acceleration/dynamics level keys. + */ +template +inline void ClassifyKeysByLevel(const CONTAINER &keys, gtsam::KeySet &q_keys, + gtsam::KeySet &v_keys, gtsam::KeySet &ad_keys) { + for (const gtsam::Key &key : keys) { + if (IsQLevel(key)) { + q_keys.insert(key); + } else if (IsVLevel(key)) { + v_keys.insert(key); + } else { + ad_keys.insert(key); + } + } +} + +/** + * @brief Classify keys into three levels: configuration, velocity, and acceleration/dynamics. + * + * @tparam CONTAINER Type of the container. + * @param keys The container of keys. + * @param q_keys Output vector of configuration level keys. + * @param v_keys Output vector of velocity level keys. + * @param ad_keys Output vector of acceleration/dynamics level keys. + */ +template +inline void ClassifyKeysByLevel(const CONTAINER &keys, gtsam::KeyVector &q_keys, + gtsam::KeyVector &v_keys, gtsam::KeyVector &ad_keys) { + for (const gtsam::Key &key : keys) { + if (IsQLevel(key)) { + q_keys.push_back(key); + } else if (IsVLevel(key)) { + v_keys.push_back(key); + } else { + ad_keys.push_back(key); + } + } +} + + } // namespace gtdynamics diff --git a/gtdynamics/utils/FootContactConstraintSpec.h b/gtdynamics/utils/FootContactConstraintSpec.h index 97059e288..46b85c73c 100644 --- a/gtdynamics/utils/FootContactConstraintSpec.h +++ b/gtdynamics/utils/FootContactConstraintSpec.h @@ -99,6 +99,6 @@ class FootContactConstraintSpec : public ConstraintSpec { const ContactPointGoals &cp_goals) const; }; -using FootContactVector = std::vector>; +using FootContactVector = std::vector>; } // namespace gtdynamics diff --git a/gtdynamics/utils/GraphUtils.cpp b/gtdynamics/utils/GraphUtils.cpp new file mode 100644 index 000000000..4f5fbcf22 --- /dev/null +++ b/gtdynamics/utils/GraphUtils.cpp @@ -0,0 +1,204 @@ +#include +#include +#include +#include +#include +#include +#include + +#if GTSAM_ENABLE_BOOST_SERIALIZATION +GTSAM_VALUE_EXPORT(double) +GTSAM_VALUE_EXPORT(gtsam::Point3) +GTSAM_VALUE_EXPORT(gtsam::Rot3) +GTSAM_VALUE_EXPORT(gtsam::Pose3) +GTSAM_VALUE_EXPORT(gtsam::Vector6) +#endif +namespace gtdynamics { + +/* ************************************************************************* */ +void PrintGraphWithError(const NonlinearFactorGraph &graph, + const Values &values, double error_threshold, + const KeyFormatter &key_formatter) { + std::cout << "total error: " << graph.error(values) << std::endl; + for (const auto &factor : graph) { + double error = factor->error(values); + if (error > error_threshold) { + std::cout << "====================== factor ==========================\n"; + factor->print("", key_formatter); + std::cout << "error: " << error << std::endl; + } + } +} + +/* ************************************************************************* */ +bool CheckFeasible(const NonlinearFactorGraph &graph, const Values &values, + const std::string s, const double feasible_threshold, + bool print_details, const KeyFormatter &key_formatter) { + + if (graph.error(values) > feasible_threshold) { + std::cout << s << "fail: " << graph.error(values) << "\n"; + if (print_details) { + PrintGraphWithError(graph, values, feasible_threshold, key_formatter); + } + return false; + } + return true; +} + +/* ************************************************************************* */ +VectorValues SqrtHessianDiagonal(const GaussianFactorGraph &graph, + const LevenbergMarquardtParams ¶ms) { + VectorValues sqrt_hessian_diagonal; + if (params.getDiagonalDamping()) { + sqrt_hessian_diagonal = graph.hessianDiagonal(); + for (auto &[key, value] : sqrt_hessian_diagonal) { + value = value.cwiseMax(params.dampingParams.minDiagonal) + .cwiseMin(params.dampingParams.maxDiagonal) + .cwiseSqrt(); + } + } + return sqrt_hessian_diagonal; +} + +/* ************************************************************************* */ +VectorValues SolveLinear(const GaussianFactorGraph &gfg, + const NonlinearOptimizerParams ¶ms) { + // solution of linear solver is an update to the linearization point + VectorValues delta; + + // Check which solver we are using + if (params.isMultifrontal()) { + // Multifrontal QR or Cholesky (decided by params.getEliminationFunction()) + if (params.ordering) + delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); + else + delta = gfg.optimize(params.getEliminationFunction()); + } else if (params.isSequential()) { + // Sequential QR or Cholesky (decided by params.getEliminationFunction()) + if (params.ordering) + delta = gfg.eliminateSequential(*params.ordering, + params.getEliminationFunction()) + ->optimize(); + else + delta = gfg.eliminateSequential(params.orderingType, + params.getEliminationFunction()) + ->optimize(); + } else if (params.isIterative()) { + // Conjugate Gradient -> needs params.iterativeParams + if (!params.iterativeParams) + throw std::runtime_error( + "NonlinearOptimizer::solve: cg parameter has to be assigned ..."); + + if (auto pcg = std::dynamic_pointer_cast( + params.iterativeParams)) { + delta = gtsam::PCGSolver(*pcg).optimize(gfg); + } else if (auto spcg = std::dynamic_pointer_cast( + params.iterativeParams)) { + if (!params.ordering) + throw std::runtime_error("SubgraphSolver needs an ordering"); + delta = gtsam::SubgraphSolver(gfg, *spcg, *params.ordering).optimize(); + } else { + throw std::runtime_error( + "NonlinearOptimizer::solve: special cg parameter type is not handled " + "in LM solver ..."); + } + } else { + throw std::runtime_error( + "NonlinearOptimizer::solve: Optimization parameter is invalid"); + } + + // return update + return delta; +} + +/* ************************************************************************* */ + +#if GTSAM_ENABLE_BOOST_SERIALIZATION +void ExportValuesToFile(const Values &values, const std::string &file_path) { + gtsam::serializeToBinaryFile(values, file_path); +} + +/* ************************************************************************* */ +Values LoadValuesFromFile(const std::string &file_path) { + Values values; + gtsam::deserializeFromBinaryFile(file_path, values); + return values; +} +#endif + +/* ************************************************************************* */ +double ComputeErrorNorm(const double &graph_error, const double &sigma) { + return sqrt(graph_error * 2) * sigma; +} + +/* ************************************************************************* */ +void IndexSetMapTranslator::insert(size_t index, Key key, size_t index_in_key) { + decoder.insert({index, {key, index_in_key}}); + encoder.insert({{key, index_in_key}, index}); +} + +/* ************************************************************************* */ +IndexSet +IndexSetMapTranslator::encodeIndices(const IndexSetMap &index_set_map) const { + IndexSet indices; + for (const auto &[key, man_indices] : index_set_map) { + for (const auto &constraint_idx : man_indices) { + indices.insert(encoder.at({key, constraint_idx})); + } + } + return indices; +} + +/* ************************************************************************* */ +IndexSetMap +IndexSetMapTranslator::decodeIndices(const IndexSet &indices) const { + IndexSetMap index_set_map; + for (const auto &index : indices) { + const auto [key, constraint_idx] = decoder.at(index); + index_set_map.addIndex(key, constraint_idx); + } + return index_set_map; +} + +/* ************************************************************************* */ +GaussianFactorGraph ScaledBiasedFactors(const GaussianFactorGraph &graph_in, + double mu, double b_scale) { + GaussianFactorGraph graph; + double sigma = 1 / sqrt(mu); + for (const auto &factor : graph_in) { + auto [A, b] = factor->jacobian(); + std::map terms; + size_t start_col = 0; + for (auto it = factor->begin(); it != factor->end(); it++) { + size_t num_cols = factor->getDim(it); + terms.insert({*it, A.middleCols(start_col, num_cols)}); + start_col += num_cols; + } + b *= b_scale; + graph.emplace_shared( + terms, b, gtsam::noiseModel::Isotropic::Sigma(b.size(), sigma)); + } + return graph; +} + +/* ************************************************************************* */ +JacobianFactor::shared_ptr +ZerobFactor(const JacobianFactor::shared_ptr factor) { + // auto [A, b] = factor->jacobian(); + std::map terms; + for (auto it = factor->begin(); it != factor->end(); it++) { + terms.insert({*it, factor->getA(it)}); + } + // b = Vector::Zero(b.size()); + // auto A = factor->getA(); + auto b = Vector::Zero(factor->getb().size()); + if (factor->get_model()) { + return std::make_shared(terms, b, factor->get_model()); + } else { + return std::make_shared(terms, b, + gtsam::noiseModel::Unit::Create( + b.size())); + } +} + +} // namespace gtdynamics diff --git a/gtdynamics/utils/GraphUtils.h b/gtdynamics/utils/GraphUtils.h new file mode 100644 index 000000000..9619d3587 --- /dev/null +++ b/gtdynamics/utils/GraphUtils.h @@ -0,0 +1,238 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file GraphUtils.h + * @brief Utility methods. + * @author Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::GaussianFactorGraph; +using gtsam::JacobianFactor; +using gtsam::Key; +using gtsam::KeyFormatter; +using gtsam::LevenbergMarquardtParams; +using gtsam::Matrix; +using gtsam::NonlinearFactorGraph; +using gtsam::NonlinearOptimizerParams; +using gtsam::SharedDiagonal; +using gtsam::Values; +using gtsam::Vector; +using gtsam::VectorValues; + +/** Print the factors in the graph that has error larger than the specified + * threshold. */ +void PrintGraphWithError( + const NonlinearFactorGraph &graph, const Values &values, + double error_threshold = 1e-3, + const KeyFormatter &key_formatter = GTDKeyFormatter); + +/** Check if the values is feasible to the constraints corresponding to the + * factor graph. */ +bool CheckFeasible( + const NonlinearFactorGraph &graph, const Values &values, + const std::string s = "", const double feasible_threshold = 1e-3, + bool print_details = false, + const KeyFormatter &key_formatter = GTDKeyFormatter); + +/// Solve gaussian factor graph using specified parameters. +VectorValues SolveLinear(const GaussianFactorGraph &gfg, + const NonlinearOptimizerParams ¶ms); + +/** + * @brief Compute the diagonal of the square root Hessian. + * + * @param graph The Gaussian factor graph. + * @param params The Levenberg-Marquardt parameters. + * @return VectorValues The diagonal of the square root Hessian. + */ +VectorValues SqrtHessianDiagonal(const GaussianFactorGraph &graph, + const LevenbergMarquardtParams ¶ms); + +/** Return a subset of the values of variables specified by keys. */ +template +inline VALUES SubValues(const VALUES &values, const CONTAINER &keys) { + VALUES sub_values; + for (const gtsam::Key &key : keys) { + sub_values.insert(key, values.at(key)); + } + return sub_values; +} + +/** Create values by picking variables specified by the keys. Will try to pick + * from the priority_values first, if key does not exist in priority_values, + * then pick from supplementary_values. */ +template +inline Values PickValues(const CONTAINER &keys, const Values &priority_values, + const Values &supplementary_values) { + Values values; + for (const Key &key : keys) { + if (priority_values.exists(key)) { + values.insert(key, priority_values.at(key)); + } else if (supplementary_values.exists(key)) { + values.insert(key, supplementary_values.at(key)); + } else { + throw std::runtime_error("key " + GTDKeyFormatter(key) + + " does not exist in both values."); + } + } + return values; +} + +/** + * @struct LMCachedModel + * @brief Cached matrices and noise model for Levenberg-Marquardt optimization. + */ +struct LMCachedModel { + LMCachedModel() {} // default int makes zero-size matrices + LMCachedModel(int dim, double sigma) + : A(Matrix::Identity(dim, dim)), b(Vector::Zero(dim)), + model(gtsam::noiseModel::Isotropic::Sigma(dim, sigma)) {} + LMCachedModel(int dim, double sigma, const Vector &diagonal) + : A(Eigen::DiagonalMatrix(diagonal)), + b(Vector::Zero(dim)), + model(gtsam::noiseModel::Isotropic::Sigma(dim, sigma)) {} + Matrix A; + Vector b; + SharedDiagonal model; +}; + +/** + * @brief Calculate the total dimension of the graph factors. + * + * @param graph The nonlinear factor graph. + * @return size_t The total dimension. + */ +inline size_t GraphDim(const NonlinearFactorGraph &graph) { + size_t dim = 0; + for (const auto &factor : graph) { + dim += factor->dim(); + } + return dim; +} + +/// Compute the error norm in standard units given graph error +double ComputeErrorNorm(const double &graph_error, const double &sigma); + +/** + * @brief Export values to a file. + * + * @param values The values to export. + * @param file_path The path to the file. + */ +void ExportValuesToFile(const Values &values, const std::string &file_path); + +#if GTSAM_ENABLE_BOOST_SERIALIZATION +/** + * @brief Load values from a file. + * + * @param file_path The path to the file. + * @return Values The loaded values. + */ +Values LoadValuesFromFile(const std::string &file_path); +#endif + +/** + * @class IndexSet + * @brief A set of indices. + */ +class IndexSet : public std::set { +public: + using base = std::set; + using base::base; + + bool exists(const size_t idx) const { return find(idx) != end(); } + + void print(const std::string &s = "") const { + std::cout << s; + for (const auto &val : *this) { + std::cout << val << "\t"; + } + std::cout << std::endl; + } +}; + +/** + * @class IndexSetMap + * @brief A map from Key to IndexSet. + */ +class IndexSetMap : public std::map { +public: + bool exists(const Key &key) const { return find(key) != end(); } + + void addIndex(const Key &key, const size_t &idx) { + IndexSet idxset; + idxset.insert(idx); + addIndices(key, idxset); + } + + void addIndices(const Key &key, const IndexSet &index_set) { + if (!exists(key)) { + insert({key, index_set}); + } else { + IndexSet ¤t_indices = at(key); + current_indices.insert(index_set.begin(), index_set.end()); + } + } + + void mergeWith(const IndexSetMap &new_map) { + for (const auto &it : new_map) { + addIndices(it.first, it.second); + } + } +}; + +/** + * @class IndexSetMapTranslator + * @brief Translates between global index and (Key, index_in_key) pair. + */ +class IndexSetMapTranslator { +public: + std::map, size_t> encoder; + std::map> decoder; + + IndexSetMapTranslator() {} + + void insert(size_t index, Key key, size_t index_in_key); + + IndexSet encodeIndices(const IndexSetMap &index_set_map) const; + + IndexSetMap decodeIndices(const IndexSet &indices) const; +}; + + +/// Return factors representing mu*\|Ax-b*b_scale\|^2. +GaussianFactorGraph ScaledBiasedFactors(const GaussianFactorGraph &graph, + double mu, double b_scale); + +/// Return factor by setting b to zero. +JacobianFactor::shared_ptr ZerobFactor(const JacobianFactor::shared_ptr factor); + +} // namespace gtdynamics diff --git a/gtdynamics/utils/initialize_solution_utils.cpp b/gtdynamics/utils/Initializer.cpp similarity index 83% rename from gtdynamics/utils/initialize_solution_utils.cpp rename to gtdynamics/utils/Initializer.cpp index 8164dc2d7..d21a94868 100644 --- a/gtdynamics/utils/initialize_solution_utils.cpp +++ b/gtdynamics/utils/Initializer.cpp @@ -11,11 +11,10 @@ * @authors Alejandro Escontrela, Yetong Zhang, Varun Agrawal */ -#include "gtdynamics/utils/initialize_solution_utils.h" - #include #include #include +#include #include #include #include @@ -38,15 +37,15 @@ using gtsam::Vector6; namespace gtdynamics { -Pose3 AddGaussianNoiseToPose(const Pose3& T, const Sampler& sampler) { +Pose3 Initializer::AddGaussianNoiseToPose(const Pose3& T, + const Sampler& sampler) const { Vector6 xi = sampler.sample(); return T.expmap(xi); } -std::vector InterpolatePoses(const Pose3& wTl_i, - const std::vector& wTl_t, double t_i, - const std::vector& timesteps, - double dt) { +std::vector Initializer::InterpolatePoses( + const Pose3& wTl_i, const std::vector& wTl_t, double t_i, + const std::vector& timesteps, double dt) { std::vector wTl_dt; Pose3 wTl = wTl_i; @@ -69,12 +68,11 @@ std::vector InterpolatePoses(const Pose3& wTl_i, return wTl_dt; } -Values InitializePosesAndJoints(const Robot& robot, const Pose3& wTl_i, - const std::vector& wTl_t, - const std::string& link_name, double t_i, - const std::vector& timesteps, double dt, - const Sampler& sampler, - std::vector* wTl_dt) { +Values Initializer::InitializePosesAndJoints( + const Robot& robot, const Pose3& wTl_i, const std::vector& wTl_t, + const std::string& link_name, double t_i, + const std::vector& timesteps, double dt, const Sampler& sampler, + std::vector* wTl_dt) { // Linearly interpolated pose for link at each discretized timestep. *wTl_dt = InterpolatePoses(wTl_i, wTl_t, t_i, timesteps, dt); @@ -105,11 +103,10 @@ Values InitializePosesAndJoints(const Robot& robot, const Pose3& wTl_i, return values; } -Values InitializeSolutionInterpolation( +Values Initializer::InitializeSolutionInterpolation( const Robot& robot, const std::string& link_name, const Pose3& wTl_i, const Pose3& wTl_f, double T_s, double T_f, double dt, - double gaussian_noise, - const boost::optional& contact_points) { + double gaussian_noise, const std::optional& contact_points) { auto sampler_noise_model = gtsam::noiseModel::Isotropic::Sigma(6, gaussian_noise); Sampler sampler(sampler_noise_model); @@ -146,8 +143,12 @@ Values InitializeSolutionInterpolation( InsertTwist(&init_vals, link->id(), t, gtsam::Z_6x1); init_vals = robot.forwardKinematics(init_vals, t, link_name); - for (auto&& kvp : ZeroValues(robot, t, gaussian_noise, contact_points)) { - init_vals.tryInsert(kvp.key, kvp.value); + const Values zero_values = + ZeroValues(robot, t, gaussian_noise, contact_points); + for (auto&& key : zero_values.keys()) { + if (!init_vals.exists(key)) { + init_vals.insert(key, zero_values.at(key)); + } } t_elapsed += dt; @@ -156,11 +157,10 @@ Values InitializeSolutionInterpolation( return init_vals; } -Values InitializeSolutionInterpolationMultiPhase( +Values Initializer::InitializeSolutionInterpolationMultiPhase( const Robot& robot, const std::string& link_name, const Pose3& wTl_i, const std::vector& wTl_t, const std::vector& ts, double dt, - double gaussian_noise, - const boost::optional& contact_points) { + double gaussian_noise, const std::optional& contact_points) { Values init_vals; Pose3 pose = wTl_i; double curr_t = 0.0; @@ -169,8 +169,10 @@ Values InitializeSolutionInterpolationMultiPhase( robot, link_name, pose, wTl_t[i], curr_t, ts[i], dt, gaussian_noise, contact_points); - for (auto&& key_value_pair : phase_vals) { - init_vals.tryInsert(key_value_pair.key, key_value_pair.value); + for (auto&& key : phase_vals.keys()) { + if (!init_vals.exists(key)) { + init_vals.insert(key, phase_vals.at(key)); + } } pose = wTl_t[i]; curr_t = ts[i]; @@ -178,11 +180,11 @@ Values InitializeSolutionInterpolationMultiPhase( return init_vals; } -Values InitializeSolutionInverseKinematics( +Values Initializer::InitializeSolutionInverseKinematics( const Robot& robot, const std::string& link_name, const Pose3& wTl_i, const std::vector& wTl_t, const std::vector& timesteps, double dt, double gaussian_noise, - const boost::optional& contact_points) { + const std::optional& contact_points) { double t_i = 0.0; // Time elapsed. Vector3 gravity(0, 0, -9.8); @@ -230,19 +232,20 @@ Values InitializeSolutionInverseKinematics( return init_vals; } -Values MultiPhaseZeroValuesTrajectory( +Values Initializer::MultiPhaseZeroValuesTrajectory( const Robot& robot, const std::vector& phase_steps, std::vector transition_graph_init, double dt_i, double gaussian_noise, - const boost::optional>& phase_contact_points) { + const std::optional>& phase_contact_points) + const { Values values; int num_phases = phase_steps.size(); // Return either PointOnLinks or None if none specified for phase p auto contact_points = - [&phase_contact_points](int p) -> boost::optional { + [&phase_contact_points](int p) -> std::optional { if (phase_contact_points) return (*phase_contact_points)[p]; - return boost::none; + return {}; }; // First slice, k==0 @@ -270,12 +273,12 @@ Values MultiPhaseZeroValuesTrajectory( return values; } -Values MultiPhaseInverseKinematicsTrajectory( +Values Initializer::MultiPhaseInverseKinematicsTrajectory( const Robot& robot, const std::string& link_name, const std::vector& phase_steps, const Pose3& wTl_i, const std::vector& wTl_t, const std::vector& ts, std::vector transition_graph_init, double dt, double gaussian_noise, - const boost::optional>& phase_contact_points) { + const std::optional>& phase_contact_points) { double t_i = 0; // Time elapsed. Vector3 gravity = (Vector(3) << 0, 0, -9.8).finished(); @@ -331,15 +334,18 @@ Values MultiPhaseInverseKinematicsTrajectory( MultiPhaseZeroValuesTrajectory(robot, phase_steps, transition_graph_init, dt, gaussian_noise, phase_contact_points); - for (auto&& key_value_pair : zero_values) { - init_vals.tryInsert(key_value_pair.key, key_value_pair.value); + for (auto&& key : zero_values.keys()) { + if (!init_vals.exists(key)) { + init_vals.insert(key, zero_values.at(key)); + } } return init_vals; } -Values ZeroValues(const Robot& robot, const int t, double gaussian_noise, - const boost::optional& contact_points) { +Values Initializer::ZeroValues( + const Robot& robot, const int t, double gaussian_noise, + const std::optional& contact_points) const { Values values; auto sampler_noise_model = @@ -359,6 +365,8 @@ Values ZeroValues(const Robot& robot, const int t, double gaussian_noise, int j = joint->id(); InsertWrench(&values, joint->parent()->id(), j, t, sampler.sample()); InsertWrench(&values, joint->child()->id(), j, t, sampler.sample()); + + // Joint angles, velocities, accelerations, and torques. std::vector keys = {TorqueKey(j, t), JointAngleKey(j, t), JointVelKey(j, t), JointAccelKey(j, t)}; for (size_t i = 0; i < keys.size(); i++) @@ -375,10 +383,9 @@ Values ZeroValues(const Robot& robot, const int t, double gaussian_noise, return values; } -Values ZeroValuesTrajectory( +Values Initializer::ZeroValuesTrajectory( const Robot& robot, const int num_steps, const int num_phases, - double gaussian_noise, - const boost::optional& contact_points) { + double gaussian_noise, const std::optional& contact_points) { Values z_values; for (int t = 0; t <= num_steps; t++) z_values.insert(ZeroValues(robot, t, gaussian_noise, contact_points)); diff --git a/gtdynamics/utils/Initializer.h b/gtdynamics/utils/Initializer.h new file mode 100644 index 000000000..30591edcb --- /dev/null +++ b/gtdynamics/utils/Initializer.h @@ -0,0 +1,223 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file initialize_solution_utils.h + * @brief Utility methods for initializing trajectory optimization solutions. + * @author: Alejandro Escontrela, Yetong Zhang + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtdynamics { + +class Initializer { + + public: + + // Default Constructor + Initializer() {} + + /** + * Add zero-mean gaussian noise to a Pose3. + * + * @param T Pose3 which to add noise to. + * @param sampler Helper to sample values from a gaussian distribution. + */ + gtsam::Pose3 AddGaussianNoiseToPose(const gtsam::Pose3& T, + const gtsam::Sampler& sampler) const; + + /** + * Linearly interpolate between initial pose and desired poses at each + * specified, discrete timestep. + * + * @param wTl_i Initial pose of the link. + * @param wTl_t Vector of desired poses. + * @param t_i Initial time. + * @param timesteps Times at which poses start and end. + * @param dt The duration of a single timestep. + */ + std::vector InterpolatePoses( + const gtsam::Pose3& wTl_i, const std::vector& wTl_t, + double t_i, const std::vector& timesteps, double dt); + + /** + * Initialize the poses and joints needed to perform trajectory optimization. + * + * @param robot The robot model. + * @param wTl_i Initial pose of the link. + * @param wTl_t Vector of poses at each desired timestep. + * @param link_name The name of the link whose pose to initialize. + * @param t_i The initial time of the trajectory. + * @param timesteps A vector of desired timesteps. + * @param dt The duration of a single timestep. + * @param sampler Helper to sample values from a gaussian distribution. + */ + gtsam::Values InitializePosesAndJoints(const Robot& robot, + const gtsam::Pose3& wTl_i, + const std::vector& wTl_t, + const std::string& link_name, double t_i, + const std::vector& timesteps, + double dt, const gtsam::Sampler& sampler, + std::vector* wTl_dt); + + /** + * @fn Initialize solution via linear interpolation of initial and final pose. + * + * @param[in] robot A Robot object. + * @param[in] link_name The name of the link whose pose to interpolate. + * @param[in] wTl_i The initial pose of the link. + * @param[in] wTl_f The final pose of the link. + * @param[in] T_s Time at which to start interpolation. + * @param[in] T_f Time at which to end interpolation. + * @param[in] dt The duration of a single timestep. + * @param[in] gaussian_noise Optional gaussian noise to add to initial values. + * Noise drawn from a zero-mean gaussian distribution with a standard + * deviation of gaussian_noise. + * @param[in] contact_points PointOnLink objects. + * @return Initial solution stored in gtsam::Values object. + */ + gtsam::Values InitializeSolutionInterpolation( + const Robot& robot, const std::string& link_name, const gtsam::Pose3& wTl_i, + const gtsam::Pose3& wTl_f, double T_s, double T_f, double dt, + double gaussian_noise = 0.0, + const std::optional& contact_points = {}); + + /** + * @fn Initialize interpolated solution for multiple phases. + * + * @param[in] robot A Robot object. + * @param[in] link_name The name of the link whose pose to interpolate. + * @param[in] wTl_i The initial pose of the link. + * @param[in] wTl_t A vector of desired poses. + * @param[in] ts Times at which poses start and end. + * @param[in] dt The duration of a single timestep. + * @param[in] gaussian_noise Optional gaussian noise to add to initial values. + * Noise drawn from a zero-mean gaussian distribution with a standard + * deviation of gaussian_noise. + * @param[in] contact_points PointOnLink objects. + * @return Initial solution stored in gtsam::Values object. + */ + gtsam::Values InitializeSolutionInterpolationMultiPhase( + const Robot& robot, const std::string& link_name, const gtsam::Pose3& wTl_i, + const std::vector& wTl_t, const std::vector& ts, + double dt, double gaussian_noise = 0.0, + const std::optional& contact_points = {}); + + /** + * @fn Iteratively solve for the robot kinematics with contacts. + * + * @param[in] robot A Robot object. + * @param[in] link_name The name of the link whose pose to interpolate. + * @param[in] wTl_i The initial pose of the link. + * @param[in] wTl_t A vector of desired poses. + * @param[in] ts Times at which poses start and end. + * @param[in] dt The duration of a single timestep. + * @param[in] gaussian_noise Optional gaussian noise to add to initial values. + * Noise drawn from a zero-mean gaussian distribution with a standard + * deviation of gaussian_noise. + * @param[in] contact_points PointOnLink objects. + * @return Initial solution stored in gtsam::Values object. + */ + gtsam::Values InitializeSolutionInverseKinematics( + const Robot& robot, const std::string& link_name, const gtsam::Pose3& wTl_i, + const std::vector& wTl_t, const std::vector& ts, + double dt, double gaussian_noise = 1e-8, + const std::optional& contact_points = {}); + + /** + * @fn Initialize solution for multi-phase trajectory to nominal pose. + * + * @param[in] robots The Robot object to use for each phase. + * @param[in] phase_steps Number of steps for each phase. + * @param[in] transition_graph_init Initial values for transition graphs. + * @param[in] dt_i Initial phase duration, + * @param[in] gaussian_noise Optional gaussian noise to add to initial values. + * @param[in] phase_contact_points Contact points at each phase. + */ + gtsam::Values MultiPhaseZeroValuesTrajectory( + const Robot& robot, const std::vector& phase_steps, + std::vector transition_graph_init, double dt_i = 1. / 240, + const double gaussian_noise = 1e-8, + const std::optional>& phase_contact_points = + {}) const; + + /** + * @fn Multi-phase initialize solution inverse kinematics. + * + * @param[in] robots A Robot object for each phase. + * @param[in] link_name The name of the link whose pose to + * interpolate. + * @param[in] phase_steps Number of steps for each phase. + * @param[in] wTl_i The initial pose of the link. + * @param[in] wTl_t A vector of desired poses. + * @param[in] ts Discretized times at which poses start and + * end. + * @param[in] transition_graph_init Initial values for transition graphs. + * @param[in] dt_i Initial value for phase duration. + * @param[in] gaussian_noise Optional gaussian noise to add to initial values. + * Noise drawn from a zero-mean gaussian distribution with a standard + * deviation of gaussian_noise. + * @param[in] phase_contact_points Contact points at each phase. + * @return Initial solution stored in gtsam::Values object. + */ + gtsam::Values MultiPhaseInverseKinematicsTrajectory( + const Robot& robot, const std::string& link_name, + const std::vector& phase_steps, const gtsam::Pose3& wTl_i, + const std::vector& wTl_t, const std::vector& ts, + std::vector transition_graph_init, double dt_i = 1. / 240, + double gaussian_noise = 1e-8, + const std::optional>& phase_contact_points = + {}); + + /** + * @fn Return zero values for all variables for initial value of optimization. + * + * @param[in] robot A Robot object. + * @param[in] t Timestep to return zero initial values for. + * @param[in] gaussian_noise Optional gaussian noise to add to initial values. + * Noise drawn from a zero-mean gaussian distribution with a standard + * deviation of gaussian_noise. + * @param[in] contact_points Contact points for timestep t. + */ + virtual gtsam::Values ZeroValues( + const Robot& robot, const int t, double gaussian_noise = 0.0, + const std::optional& contact_points = {}) const; + + /** + * @fn Return zero values of the trajectory for initial value of optimization. + * + * @param[in] robot A Robot object. + * @param[in] num_steps Total number of time steps. + * @param[in] num_phases Number of phases, -1 if not using. + * @param[in] gaussian_noise Optional gaussian noise to add to initial values. + * Noise drawn from a zero-mean gaussian distribution with a standard + * deviation of gaussian_noise. + * @param[in] contact_points Contact points along the trajectory. + * @return Initial solution stored in a gtsam::Values object. + */ + gtsam::Values ZeroValuesTrajectory( + const Robot& robot, const int num_steps, const int num_phases = -1, + double gaussian_noise = 0.0, + const std::optional& contact_points = {}); + +}; + +} // namespace gtdynamics diff --git a/gtdynamics/utils/JsonSaver.h b/gtdynamics/utils/JsonSaver.h index 7284a2ead..c1c37b2f7 100644 --- a/gtdynamics/utils/JsonSaver.h +++ b/gtdynamics/utils/JsonSaver.h @@ -9,34 +9,32 @@ #pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include -#include #include #include -#include #include #include #include +#include #include #include #include #include #include -#include "gtdynamics/factors/PoseFactor.h" -#include "gtdynamics/factors/TorqueFactor.h" -#include "gtdynamics/factors/TwistAccelFactor.h" -#include "gtdynamics/factors/TwistFactor.h" -#include "gtdynamics/factors/WrenchEquivalenceFactor.h" -#include "gtdynamics/factors/WrenchFactor.h" -#include "gtdynamics/factors/WrenchPlanarFactor.h" -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/utils/utils.h" - #define kQuote_ "\"" // using namespace gtdynamics; @@ -54,7 +52,7 @@ namespace gtdynamics { class JsonSaver { public: typedef std::pair AttributeType; - typedef boost::shared_ptr ValuePtr; + typedef std::shared_ptr ValuePtr; typedef std::map LocationType; typedef std::map StrLocationType; @@ -199,12 +197,12 @@ class JsonSaver { static inline std::string GetMeasurement( const gtsam::NonlinearFactor::shared_ptr& factor) { std::stringstream ss; - // if (const TorqueFactor* f = dynamic_cast(&(*factor))) { - // auto joint = f->getJoint(); - // ss << GetVector(joint->screwAxis(joint->child()).transpose()); + // if (const TorqueFactor* f = dynamic_cast(&(*factor))) { auto joint = f->getJoint(); ss << + // GetVector(joint->screwAxis(joint->child()).transpose()); if (const gtsam::PriorFactor* f = - dynamic_cast*>( - &(*factor))) { + dynamic_cast*>( + &(*factor))) { ss << GetVector(f->prior().transpose()); } else if (const gtsam::PriorFactor* f = dynamic_cast*>( @@ -259,8 +257,9 @@ class JsonSaver { dynamic_cast( &(*noise_model))) { // isotropic - ss << boost::format("isotropic dim=%1% sigma=%2%") % - true_noise_model->dim() % true_noise_model->sigma(); + ss << "isotropic dim=" << true_noise_model->dim() + << " sigma=" << true_noise_model->sigma(); + } else if (const gtsam::noiseModel::Constrained* true_noise_model = dynamic_cast( &(*noise_model))) { @@ -362,7 +361,7 @@ class JsonSaver { if (values.exists(key)) { // value - boost::optional location; + std::optional location; attributes.emplace_back(Quoted("value"), Quoted(GetValue(values.at(key)))); @@ -592,7 +591,7 @@ class JsonSaver { class StorageManager { private: typedef JsonSaver::AttributeType AttributeType; - typedef boost::shared_ptr ValuePtr; + typedef std::shared_ptr ValuePtr; typedef std::map> StorageMap; typedef std::pair> StorageEntry; StorageMap storage_; diff --git a/gtdynamics/utils/Phase.cpp b/gtdynamics/utils/Phase.cpp index c05b81a6e..eda034376 100644 --- a/gtdynamics/utils/Phase.cpp +++ b/gtdynamics/utils/Phase.cpp @@ -33,7 +33,7 @@ void Phase::print(const string &s) const { } Matrix Phase::jointMatrix(const Robot &robot, const gtsam::Values &results, - size_t k, boost::optional dt) const { + size_t k, std::optional dt) const { const auto &joints = robot.joints(); const size_t J = joints.size(); const int m = numTimeSteps(); diff --git a/gtdynamics/utils/Phase.h b/gtdynamics/utils/Phase.h index 92c3c0e45..f508f2b1d 100644 --- a/gtdynamics/utils/Phase.h +++ b/gtdynamics/utils/Phase.h @@ -32,16 +32,16 @@ namespace gtdynamics { class Phase : public Interval { protected: - boost::shared_ptr constraint_spec_; + std::shared_ptr constraint_spec_; public: /// Constructor Phase(size_t k_start, size_t k_end, - const boost::shared_ptr &constraint_spec) + const std::shared_ptr &constraint_spec) : Interval(k_start, k_end), constraint_spec_(constraint_spec) {} /// Return Constraint Spec pointer - const boost::shared_ptr constraintSpec() const { + const std::shared_ptr constraintSpec() const { return constraint_spec_; } @@ -53,7 +53,6 @@ class Phase : public Interval { /// Parse results into a matrix, in order: qs, qdots, qddots, taus, dt gtsam::Matrix jointMatrix(const Robot &robot, const gtsam::Values &results, - size_t k = 0, - boost::optional dt = boost::none) const; + size_t k = 0, std::optional dt = {}) const; }; } // namespace gtdynamics diff --git a/gtdynamics/utils/PointOnLink.h b/gtdynamics/utils/PointOnLink.h index 274c24436..567b7fe9e 100644 --- a/gtdynamics/utils/PointOnLink.h +++ b/gtdynamics/utils/PointOnLink.h @@ -33,29 +33,29 @@ struct PointOnLink { gtsam::Point3 point; PointOnLink() {} - PointOnLink(const LinkSharedPtr &link, const gtsam::Point3 &point) + PointOnLink(const LinkSharedPtr& link, const gtsam::Point3& point) : link(link), point(point) {} - bool operator==(const PointOnLink &other) const { + bool operator==(const PointOnLink& other) const { return (point == other.point && link == other.link); } - bool operator!=(const PointOnLink &other) const { return !(*this == other); } + bool operator!=(const PointOnLink& other) const { return !(*this == other); } /** * @fn For given values, predict where point on link is in world frame. * @param values a GTSAM Values instance that should contain link pose. * @param k time step to check (default 0). */ - gtsam::Point3 predict(const gtsam::Values &values, size_t k = 0) const; + gtsam::Point3 predict(const gtsam::Values& values, size_t k = 0) const; /// Print to stream. - friend std::ostream &operator<<(std::ostream &os, const PointOnLink &cp); + friend std::ostream& operator<<(std::ostream& os, const PointOnLink& cp); /// GTSAM-style print, works with wrapper. - void print(const std::string &s) const; + void print(const std::string& s = "") const; /// GTSAM-style equal, allows for using assert_equal. - bool equals(const PointOnLink &other, double tol = 1e-9) const; + bool equals(const PointOnLink& other, double tol = 1e-9) const; }; ///< Vector of `PointOnLink`s diff --git a/gtdynamics/utils/Timer.h b/gtdynamics/utils/Timer.h new file mode 100644 index 000000000..1d00c30fa --- /dev/null +++ b/gtdynamics/utils/Timer.h @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file Timer.h + * @brief Timing + * @author Yetong Zhang + */ + +#include +#include + +#pragma once + +namespace gtdynamics { + +/** + * @class Timer + * @brief Simple timer class using std::chrono. + */ +class Timer { +protected: + std::chrono::time_point start_time_; + std::chrono::time_point stop_time_; + +public: + /// Constructor. + Timer() {} + + /// Start timer. + void start() { start_time_ = std::chrono::system_clock::now(); } + + /// Stop timer. + void stop() { stop_time_ = std::chrono::system_clock::now(); } + + /// Return duration in seconds. + double seconds() const { + auto ns = std::chrono::duration_cast(stop_time_ - + start_time_); + return ns.count() * 1e-9; + } + + /// Return duration in milliseconds. + double milliSeconds() const { + auto ns = std::chrono::duration_cast(stop_time_ - + start_time_); + return ns.count() * 1e-6; + } + + /// Return duration in microseconds. + double microSeconds() const { + auto ns = std::chrono::duration_cast(stop_time_ - + start_time_); + return ns.count() * 1e-3; + } +}; + +} // namespace gtdynamics diff --git a/gtdynamics/utils/Trajectory.cpp b/gtdynamics/utils/Trajectory.cpp index e71789368..8226e80e3 100644 --- a/gtdynamics/utils/Trajectory.cpp +++ b/gtdynamics/utils/Trajectory.cpp @@ -18,7 +18,6 @@ #include #include -#include #include #include #include @@ -59,25 +58,27 @@ NonlinearFactorGraph Trajectory::multiPhaseFactorGraph( } vector Trajectory::transitionPhaseInitialValues( - const Robot &robot, double gaussian_noise) const { + const Robot &robot, const Initializer &initializer, + double gaussian_noise) const { vector trans_cps = transitionContactPoints(); vector transition_graph_init; vector final_timesteps = finalTimeSteps(); for (int p = 1; p < numPhases(); p++) { - transition_graph_init.push_back(ZeroValues( + transition_graph_init.push_back(initializer.ZeroValues( robot, final_timesteps[p - 1], gaussian_noise, trans_cps[p - 1])); } return transition_graph_init; } Values Trajectory::multiPhaseInitialValues(const Robot &robot, + const Initializer &initializer, double gaussian_noise, double dt) const { vector transition_graph_init = - transitionPhaseInitialValues(robot, gaussian_noise); - return MultiPhaseZeroValuesTrajectory(robot, phaseDurations(), - transition_graph_init, dt, - gaussian_noise, phaseContactPoints()); + transitionPhaseInitialValues(robot, initializer, gaussian_noise); + return initializer.MultiPhaseZeroValuesTrajectory( + robot, phaseDurations(), transition_graph_init, dt, gaussian_noise, + phaseContactPoints()); } NonlinearFactorGraph Trajectory::contactPointObjectives( @@ -162,7 +163,10 @@ void Trajectory::writeToFile(const Robot &robot, const std::string &name, for (auto &&joint : robot.joints()) { jnames.push_back(joint->name()); } - string jnames_str = boost::algorithm::join(jnames, ","); + string jnames_str = ""; + for (size_t j = 0; j < jnames.size(); j++) { + jnames_str += jnames[j] + (j != jnames.size() - 1 ? "," : ""); + } std::ofstream file(name); diff --git a/gtdynamics/utils/Trajectory.h b/gtdynamics/utils/Trajectory.h index 8760684eb..40f507585 100644 --- a/gtdynamics/utils/Trajectory.h +++ b/gtdynamics/utils/Trajectory.h @@ -19,7 +19,7 @@ #include #include #include -#include +#include namespace gtdynamics { @@ -123,20 +123,23 @@ class Trajectory { /** * @fn Returns Initial values for transition graphs. * @param[in] robot Robot specification from URDF/SDF. + * @param[in] initializer Initializer class to initialize with * @param[in] gaussian_noise Gaussian noise to add to initial values * @return Initial values for transition graphs */ std::vector transitionPhaseInitialValues( - const Robot &robot, double gaussian_noise) const; + const Robot &robot, const Initializer &initializer, double gaussian_noise) const; /** * @fn Returns Initial values for multi-phase factor graph. * @param[in] robot Robot specification from URDF/SDF. + * @param[in] initializer Initializer class to initialize with * @param[in] gaussian_noise Gaussian noise to add to initial values * @param[in] desired_dt integration timestep * @return Initial values for multi-phase factor graph */ gtsam::Values multiPhaseInitialValues(const Robot &robot, + const Initializer &initializer, double gaussian_noise, double dt) const; /** diff --git a/gtdynamics/utils/WalkCycle.cpp b/gtdynamics/utils/WalkCycle.cpp index 60bfae382..d2464e7a7 100644 --- a/gtdynamics/utils/WalkCycle.cpp +++ b/gtdynamics/utils/WalkCycle.cpp @@ -12,19 +12,19 @@ */ #include -#include +#include namespace gtdynamics { -using gtsam::Point3; using gtsam::NonlinearFactorGraph; +using gtsam::Point3; using gtsam::SharedNoiseModel; /// cast ConstraintSpec to FootContactConstraintSpec -const boost::shared_ptr +const std::shared_ptr castFootContactConstraintSpec( - const boost::shared_ptr &constraint_spec) { - return boost::dynamic_pointer_cast( + const std::shared_ptr &constraint_spec) { + return std::dynamic_pointer_cast( constraint_spec); } @@ -38,7 +38,7 @@ void WalkCycle::addPhaseContactPoints(const Phase &phase) { std::count_if(contact_points_.begin(), contact_points_.end(), [&](const PointOnLink &contact_point) { return contact_point.point == kv.point && - contact_point.link == kv.link; + contact_point.link == kv.link; }); if (link_count == 0) contact_points_.push_back(kv); } @@ -136,7 +136,7 @@ std::vector WalkCycle::getPhaseSwingLinks(size_t p) const { const PointOnLinks WalkCycle::getPhaseContactPoints(size_t p) const { PointOnLinks cp; auto foot_contact_spec = - castFootContactConstraintSpec(phases_[p].constraintSpec()); + castFootContactConstraintSpec(phases_[p].constraintSpec()); if (foot_contact_spec) { cp = foot_contact_spec->contactPoints(); } @@ -166,7 +166,7 @@ std::vector WalkCycle::transitionContactPoints() const { castFootContactConstraintSpec(phases_[p].constraintSpec()); auto foot_contact_spec2 = castFootContactConstraintSpec(phases_[p + 1].constraintSpec()); - + if (foot_contact_spec1) { phase_1_cps = foot_contact_spec1->contactPoints(); } diff --git a/gtdynamics/utils/initialize_solution_utils.h b/gtdynamics/utils/initialize_solution_utils.h deleted file mode 100644 index bfff9a3f3..000000000 --- a/gtdynamics/utils/initialize_solution_utils.h +++ /dev/null @@ -1,214 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file initialize_solution_utils.h - * @brief Utility methods for initializing trajectory optimization solutions. - * @author: Alejandro Escontrela, Yetong Zhang - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace gtdynamics { - -/** - * Add zero-mean gaussian noise to a Pose3. - * - * @param T Pose3 which to add noise to. - * @param sampler Helper to sample values from a gaussian distribution. - */ -inline gtsam::Pose3 AddGaussianNoiseToPose(const gtsam::Pose3& T, - const gtsam::Sampler& sampler); - -/** - * Linearly interpolate between initial pose and desired poses at each - * specified, discrete timestep. - * - * @param wTl_i Initial pose of the link. - * @param wTl_t Vector of desired poses. - * @param t_i Initial time. - * @param timesteps Times at which poses start and end. - * @param dt The duration of a single timestep. - */ -std::vector InterpolatePoses( - const gtsam::Pose3& wTl_i, const std::vector& wTl_t, - double t_i, const std::vector& timesteps, double dt); - -/** - * Initialize the poses and joints needed to perform trajectory optimization. - * - * @param robot The robot model. - * @param wTl_i Initial pose of the link. - * @param wTl_t Vector of poses at each desired timestep. - * @param link_name The name of the link whose pose to initialize. - * @param t_i The initial time of the trajectory. - * @param timesteps A vector of desired timesteps. - * @param dt The duration of a single timestep. - * @param sampler Helper to sample values from a gaussian distribution. - */ -gtsam::Values InitializePosesAndJoints(const Robot& robot, - const gtsam::Pose3& wTl_i, - const std::vector& wTl_t, - const std::string& link_name, double t_i, - const std::vector& timesteps, - double dt, const gtsam::Sampler& sampler, - std::vector* wTl_dt); - -/** - * @fn Initialize solution via linear interpolation of initial and final pose. - * - * @param[in] robot A Robot object. - * @param[in] link_name The name of the link whose pose to interpolate. - * @param[in] wTl_i The initial pose of the link. - * @param[in] wTl_f The final pose of the link. - * @param[in] T_s Time at which to start interpolation. - * @param[in] T_f Time at which to end interpolation. - * @param[in] dt The duration of a single timestep. - * @param[in] gaussian_noise Optional gaussian noise to add to initial values. - * Noise drawn from a zero-mean gaussian distribution with a standard - * deviation of gaussian_noise. - * @param[in] contact_points PointOnLink objects. - * @return Initial solution stored in gtsam::Values object. - */ -gtsam::Values InitializeSolutionInterpolation( - const Robot& robot, const std::string& link_name, const gtsam::Pose3& wTl_i, - const gtsam::Pose3& wTl_f, double T_s, double T_f, double dt, - double gaussian_noise = 0.0, - const boost::optional& contact_points = boost::none); - -/** - * @fn Initialize interpolated solution for multiple phases. - * - * @param[in] robot A Robot object. - * @param[in] link_name The name of the link whose pose to interpolate. - * @param[in] wTl_i The initial pose of the link. - * @param[in] wTl_t A vector of desired poses. - * @param[in] ts Times at which poses start and end. - * @param[in] dt The duration of a single timestep. - * @param[in] gaussian_noise Optional gaussian noise to add to initial values. - * Noise drawn from a zero-mean gaussian distribution with a standard - * deviation of gaussian_noise. - * @param[in] contact_points PointOnLink objects. - * @return Initial solution stored in gtsam::Values object. - */ -gtsam::Values InitializeSolutionInterpolationMultiPhase( - const Robot& robot, const std::string& link_name, const gtsam::Pose3& wTl_i, - const std::vector& wTl_t, const std::vector& ts, - double dt, double gaussian_noise = 0.0, - const boost::optional& contact_points = boost::none); - -/** - * @fn Iteratively solve for the robot kinematics with contacts. - * - * @param[in] robot A Robot object. - * @param[in] link_name The name of the link whose pose to interpolate. - * @param[in] wTl_i The initial pose of the link. - * @param[in] wTl_t A vector of desired poses. - * @param[in] ts Times at which poses start and end. - * @param[in] dt The duration of a single timestep. - * @param[in] gaussian_noise Optional gaussian noise to add to initial values. - * Noise drawn from a zero-mean gaussian distribution with a standard - * deviation of gaussian_noise. - * @param[in] contact_points PointOnLink objects. - * @return Initial solution stored in gtsam::Values object. - */ -gtsam::Values InitializeSolutionInverseKinematics( - const Robot& robot, const std::string& link_name, const gtsam::Pose3& wTl_i, - const std::vector& wTl_t, const std::vector& ts, - double dt, double gaussian_noise = 1e-8, - const boost::optional& contact_points = boost::none); - -/** - * @fn Initialize solution for multi-phase trajectory to nominal pose. - * - * @param[in] robots The Robot object to use for each phase. - * @param[in] phase_steps Number of steps for each phase. - * @param[in] transition_graph_init Initial values for transition graphs. - * @param[in] dt_i Initial phase duration, - * @param[in] gaussian_noise Optional gaussian noise to add to initial values. - * @param[in] phase_contact_points Contact points at each phase. - */ -gtsam::Values MultiPhaseZeroValuesTrajectory( - const Robot& robot, const std::vector& phase_steps, - std::vector transition_graph_init, double dt_i = 1. / 240, - const double gaussian_noise = 1e-8, - const boost::optional>& phase_contact_points = - boost::none); - -/** - * @fn Multi-phase initialize solution inverse kinematics. - * - * @param[in] robots A Robot object for each phase. - * @param[in] link_name The name of the link whose pose to - * interpolate. - * @param[in] phase_steps Number of steps for each phase. - * @param[in] wTl_i The initial pose of the link. - * @param[in] wTl_t A vector of desired poses. - * @param[in] ts Discretized times at which poses start and - * end. - * @param[in] transition_graph_init Initial values for transition graphs. - * @param[in] dt_i Initial value for phase duration. - * @param[in] gaussian_noise Optional gaussian noise to add to initial values. - * Noise drawn from a zero-mean gaussian distribution with a standard - * deviation of gaussian_noise. - * @param[in] phase_contact_points Contact points at each phase. - * @return Initial solution stored in gtsam::Values object. - */ -gtsam::Values MultiPhaseInverseKinematicsTrajectory( - const Robot& robot, const std::string& link_name, - const std::vector& phase_steps, const gtsam::Pose3& wTl_i, - const std::vector& wTl_t, const std::vector& ts, - std::vector transition_graph_init, double dt_i = 1. / 240, - double gaussian_noise = 1e-8, - const boost::optional>& phase_contact_points = - boost::none); - -/** - * @fn Return zero values for all variables for initial value of optimization. - * - * @param[in] robot A Robot object. - * @param[in] t Timestep to return zero initial values for. - * @param[in] gaussian_noise Optional gaussian noise to add to initial values. - * Noise drawn from a zero-mean gaussian distribution with a standard - * deviation of gaussian_noise. - * @param[in] contact_points Contact points for timestep t. - */ -gtsam::Values ZeroValues( - const Robot& robot, const int t, double gaussian_noise = 0.0, - const boost::optional& contact_points = boost::none); - -/** - * @fn Return zero values of the trajectory for initial value of optimization. - * - * @param[in] robot A Robot object. - * @param[in] num_steps Total number of time steps. - * @param[in] num_phases Number of phases, -1 if not using. - * @param[in] gaussian_noise Optional gaussian noise to add to initial values. - * Noise drawn from a zero-mean gaussian distribution with a standard - * deviation of gaussian_noise. - * @param[in] contact_points Contact points along the trajectory. - * @return Initial solution stored in a gtsam::Values object. - */ -gtsam::Values ZeroValuesTrajectory( - const Robot& robot, const int num_steps, const int num_phases = -1, - double gaussian_noise = 0.0, - const boost::optional& contact_points = boost::none); - -} // namespace gtdynamics diff --git a/gtdynamics/utils/utils.cpp b/gtdynamics/utils/utils.cpp index b2f064aad..2699d7eb7 100644 --- a/gtdynamics/utils/utils.cpp +++ b/gtdynamics/utils/utils.cpp @@ -11,7 +11,7 @@ * @author Frank Dellaert, Mandy Xie, Alejandro Escontrela */ -#include "gtdynamics/utils/utils.h" +#include #include @@ -33,27 +33,6 @@ gtsam::Vector radians(const gtsam::Vector °ree) { return radian; } -gtsam::Matrix6 AdjointMapJacobianQ(double q, const gtsam::Pose3 &jMi, - const gtsam::Vector6 &screw_axis) { - // taking opposite value of screw_axis_ is because - // jTi = Pose3::Expmap(-screw_axis_ * q) * jMi; - gtsam::Vector3 w = -screw_axis.head<3>(); - gtsam::Vector3 v = -screw_axis.tail<3>(); - gtsam::Pose3 kTj = gtsam::Pose3::Expmap(-screw_axis * q) * jMi; - auto w_skew = gtsam::skewSymmetric(w); - gtsam::Matrix3 H_expo = w_skew * cosf(q) + w_skew * w_skew * sinf(q); - gtsam::Matrix3 H_R = H_expo * jMi.rotation().matrix(); - gtsam::Vector3 H_T = - H_expo * (jMi.translation() - w_skew * v) + w * w.transpose() * v; - gtsam::Matrix3 H_TR = gtsam::skewSymmetric(H_T) * kTj.rotation().matrix() + - gtsam::skewSymmetric(kTj.translation()) * H_R; - gtsam::Matrix6 H = gtsam::Z_6x6; - gtsam::insertSub(H, H_R, 0, 0); - gtsam::insertSub(H, H_TR, 3, 0); - gtsam::insertSub(H, H_R, 3, 3); - return H; -} - gtsam::Matrix getQc(const gtsam::SharedNoiseModel Qc_model) { gtsam::noiseModel::Gaussian *Gassian_model = dynamic_cast(Qc_model.get()); @@ -179,3 +158,59 @@ gtsam::Matrix36 getPlanarJacobian(const gtsam::Vector3 &planar_axis) { } } // namespace gtdynamics + +namespace gtsam { +double point3_z(const gtsam::Point3 &p, gtsam::OptionalJacobian<1, 3> H) { + if (H) { + *H << 0, 0, 1; + } + return p.z(); +} + +double double_division(const double &x1, const double &x2, + gtsam::OptionalJacobian<1, 1> H_1, + gtsam::OptionalJacobian<1, 1> H_2) { + + double result = x1 / x2; + if (H_1) { + H_1->setConstant(1 / x2); + } + if (H_2) { + H_2->setConstant(-result / x2); + } + return result; +} + +double reciprocal(const double &x, gtsam::OptionalJacobian<1, 1> H) { + if (H) { + H->setConstant(-pow(x, -2)); + } + return 1 / x; +} + +double clip_by_one(const double &x, gtsam::OptionalJacobian<1, 1> H) { + if (x < 1) { + if (H) { + H->setZero(); + } + return 1; + } else { + if (H) { + H->setOnes(); + } + return x; + } +} + +Vector2 double_stack(const double &x1, const double &x2, + gtsam::OptionalJacobian<2, 1> H_1, + gtsam::OptionalJacobian<2, 1> H_2) { + if (H_1) { + (*H_1) << 1, 0; + } + if (H_2) { + (*H_2) << 0, 1; + } + return Vector2(x1, x2); +} +} diff --git a/gtdynamics/utils/utils.h b/gtdynamics/utils/utils.h index 67548aa67..c73f65133 100644 --- a/gtdynamics/utils/utils.h +++ b/gtdynamics/utils/utils.h @@ -20,7 +20,6 @@ #include #include -#include #include #include #include @@ -43,15 +42,6 @@ double radians(double degree); /// Convert a vector of angles to radians gtsam::Vector radians(const gtsam::Vector °ree); -/** - * Calculate AdjointMap jacobian w.r.t. joint coordinate q - * @param q joint angle - * @param jMi this COM frame, expressed in next link's COM frame at rest - * @param screw_axis screw axis expressed in kth link's COM frame - */ -gtsam::Matrix6 AdjointMapJacobianQ(double q, const gtsam::Pose3 &jMi, - const gtsam::Vector6 &screw_axis); - /** * Calculate Gaussian Process system transition matrix * @param tau timestep @@ -138,3 +128,59 @@ std::vector readFromTxt(std::string mat_dir, gtsam::Matrix36 getPlanarJacobian(const gtsam::Vector3 &planar_axis); } // namespace gtdynamics + +namespace gtsam { + +/** + * @brief Get z coordinate of a Point3. + * + * @param p The Point3 object. + * @param H Optional Jacobian. + * @return double The z coordinate. + */ +double point3_z(const gtsam::Point3 &p, gtsam::OptionalJacobian<1, 3> H = {}); + +/** + * @brief Division of two doubles, with derivatives. + * + * @param x1 Numerator. + * @param x2 Denominator. + * @param H_1 Optional Jacobian w.r.t x1. + * @param H_2 Optional Jacobian w.r.t x2. + * @return double x1 / x2. + */ +double double_division(const double &x1, const double &x2, + gtsam::OptionalJacobian<1, 1> H_1 = {}, + gtsam::OptionalJacobian<1, 1> H_2 = {}); + +/** + * @brief Reciprocal of a double, with derivatives. + * + * @param x The input value. + * @param H Optional Jacobian. + * @return double 1 / x. + */ +double reciprocal(const double& x, gtsam::OptionalJacobian<1, 1> H = {}); + +/** + * @brief Clip the value to be at least 1, with derivatives. + * + * @param x The input value. + * @param H Optional Jacobian. + * @return double max(x, 1.0). + */ +double clip_by_one(const double& x, gtsam::OptionalJacobian<1, 1> H = {}); + +/** + * @brief Stack two doubles into a Vector2. + * + * @param x1 First value. + * @param x2 Second value. + * @param H_1 Optional Jacobian w.r.t x1. + * @param H_2 Optional Jacobian w.r.t x2. + * @return Vector2 [x1, x2]. + */ +Vector2 double_stack(const double &x1, const double &x2, + gtsam::OptionalJacobian<2, 1> H_1 = {}, + gtsam::OptionalJacobian<2, 1> H_2 = {}); +} // namespace gtsam diff --git a/gtdynamics/utils/values.cpp b/gtdynamics/utils/values.cpp index 55ccc965a..226b92535 100644 --- a/gtdynamics/utils/values.cpp +++ b/gtdynamics/utils/values.cpp @@ -166,4 +166,14 @@ Vector6 Wrench(const Values &values, int i, int j, int t) { return at(values, WrenchKey(i, j, t)); } +/* ************************************************************************* */ +Values DynamicsValuesFromPrev(const Values &prev_values, + const int gap_steps) { + Values values; + for (const gtsam::Key &key : prev_values.keys()) { + values.insert(key + gap_steps, prev_values.at(key)); + } + return values; +} + } // namespace gtdynamics diff --git a/gtdynamics/utils/values.h b/gtdynamics/utils/values.h index 071231a44..27b3a8466 100644 --- a/gtdynamics/utils/values.h +++ b/gtdynamics/utils/values.h @@ -19,7 +19,7 @@ #include #include -#define GTD_PRINT(x) ((x).print(#x, gtdynamics::_GTDKeyFormatter)) +#define GTD_PRINT(x) ((x).print(#x, _GTDKeyFormatter)) namespace gtdynamics { @@ -39,45 +39,72 @@ class KeyDoesNotExist : public gtsam::ValuesKeyDoesNotExist { Key definitions. ************************************************************************* */ /// Shorthand for q_j_t, for j-th joint angle at time t. -inline DynamicsSymbol JointAngleKey(int j, int t = 0) { +inline gtsam::Key JointAngleKey(int j, int t = 0) { return DynamicsSymbol::JointSymbol("q", j, t); } /// Shorthand for v_j_t, for j-th joint velocity at time t. -inline DynamicsSymbol JointVelKey(int j, int t = 0) { +inline gtsam::Key JointVelKey(int j, int t = 0) { return DynamicsSymbol::JointSymbol("v", j, t); } /// Shorthand for a_j_t, for j-th joint acceleration at time t. -inline DynamicsSymbol JointAccelKey(int j, int t = 0) { +inline gtsam::Key JointAccelKey(int j, int t = 0) { return DynamicsSymbol::JointSymbol("a", j, t); } /// Shorthand for T_j_t, for torque on the j-th joint at time t. -inline DynamicsSymbol TorqueKey(int j, int t = 0) { +inline gtsam::Key TorqueKey(int j, int t = 0) { return DynamicsSymbol::JointSymbol("T", j, t); } /// Shorthand for p_i_t, for COM pose on the i-th link at time t. -inline DynamicsSymbol PoseKey(int i, int t = 0) { +inline gtsam::Key PoseKey(int i, int t = 0) { return DynamicsSymbol::LinkSymbol("p", i, t); } /// Shorthand for V_i_t, for 6D link twist vector on the i-th link. -inline DynamicsSymbol TwistKey(int i, int t = 0) { +inline gtsam::Key TwistKey(int i, int t = 0) { return DynamicsSymbol::LinkSymbol("V", i, t); } /// Shorthand for A_i_t, for twist accelerations on the i-th link at time t. -inline DynamicsSymbol TwistAccelKey(int i, int t = 0) { +inline gtsam::Key TwistAccelKey(int i, int t = 0) { return DynamicsSymbol::LinkSymbol("A", i, t); } /// Shorthand for F_i_j_t, wrenches at j-th joint on the i-th link at time t. -inline DynamicsSymbol WrenchKey(int i, int j, int t = 0) { +inline gtsam::Key WrenchKey(int i, int j, int t = 0) { return DynamicsSymbol::LinkJointSymbol("F", i, j, t); } +/// Shorthand for C_i_c_k, for contact wrench c on i-th link at time step k. +inline gtsam::Key ContactWrenchKey(int i, int c, int k = 0) { + return DynamicsSymbol::LinkJointSymbol("C", i, c, k); +} + +/** + * @brief Shorthand for CF_i_c_k, for contact force c on i-th link at time step k. + * + * @param i Link ID. + * @param c Contact ID. + * @param k Time step. + * @return gtsam::Key + */ +inline gtsam::Key ContactForceKey(int i, int c, int k = 0) { + return DynamicsSymbol::LinkJointSymbol("CF", i, c, k); +} + +/* Shorthand for dt_k, for duration for timestep dt_k during phase k. */ +inline gtsam::Key PhaseKey(int k) { + return DynamicsSymbol::SimpleSymbol("dt", k); +} + +/* Shorthand for t_k, time at time step k. */ +inline gtsam::Key TimeKey(int k) { + return DynamicsSymbol::SimpleSymbol("t", k); +} + /// Custom retrieval that throws KeyDoesNotExist template T at(const gtsam::Values &values, size_t key) { @@ -436,4 +463,14 @@ gtsam::Vector Wrench(const gtsam::VectorValues &values, int i, int j, */ gtsam::Vector6 Wrench(const gtsam::Values &values, int i, int j, int t = 0); +/** + * @brief Create values for the next time step by incrementing the time index of keys. + * + * @param prev_values Values at the previous time step. + * @param gap_steps The number of steps to increment. + * @return gtsam::Values + */ +gtsam::Values DynamicsValuesFromPrev(const gtsam::Values &prev_values, + const int gap_steps = 1); + } // namespace gtdynamics diff --git a/models/urdfs/cart_pole.urdf b/models/urdfs/cart_pole.urdf index 8f4703507..2c10ae267 100644 --- a/models/urdfs/cart_pole.urdf +++ b/models/urdfs/cart_pole.urdf @@ -59,6 +59,6 @@ - + \ No newline at end of file diff --git a/urdfs/nao.urdf b/models/urdfs/nao.urdf similarity index 100% rename from urdfs/nao.urdf rename to models/urdfs/nao.urdf diff --git a/models/urdfs/plane.urdf b/models/urdfs/plane.urdf new file mode 100644 index 000000000..ebed022e2 --- /dev/null +++ b/models/urdfs/plane.urdf @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 9dbf94624..7a9b8d772 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -13,10 +13,8 @@ set(GTDYNAMICS_PYTHON_PATH ${PROJECT_SOURCE_DIR}/python/) file(TO_NATIVE_PATH "${PROJECT_BINARY_DIR}/python" GTD_PYTHON_BINARY_DIR) -configure_file(${GTDYNAMICS_PYTHON_PATH}/templates/setup.py.in - ${GTD_PYTHON_BINARY_DIR}/setup.py) -configure_file(${GTDYNAMICS_PYTHON_PATH}/requirements.txt - ${GTD_PYTHON_BINARY_DIR}/requirements.txt COPYONLY) +configure_file(${GTDYNAMICS_PYTHON_PATH}/templates/pyproject.toml.in + ${GTD_PYTHON_BINARY_DIR}/pyproject.toml) configure_file(${GTDYNAMICS_PYTHON_PATH}/templates/${PROJECT_NAME}.tpl ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.tpl) file(COPY ${GTDYNAMICS_PYTHON_PATH}/${PROJECT_NAME} @@ -37,7 +35,7 @@ pybind_wrap( ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.tpl ${PROJECT_NAME} # libs "${PROJECT_NAME}" # dependencies - ON # use boost + ${GTDYNAMICS_ENABLE_BOOST_SERIALIZATION} # use_boost_serialization ) set_target_properties( @@ -50,7 +48,7 @@ set_target_properties( add_custom_target( python-install - COMMAND ${PYTHON_EXECUTABLE} ${GTD_PYTHON_BINARY_DIR}/setup.py install + COMMAND ${PYTHON_EXECUTABLE} -m pip install . DEPENDS ${PROJECT_NAME}_py WORKING_DIRECTORY ${GTD_PYTHON_BINARY_DIR}) diff --git a/python/README.md b/python/README.md new file mode 100644 index 000000000..45cfe4ed8 --- /dev/null +++ b/python/README.md @@ -0,0 +1,58 @@ +# GTDynamics Python Wrapper + +This directory is where the Pybind11-generated GTDynamics package lives and where the CI jobs assemble and verify the Python bindings. + +## Layout + +- `gtdynamics/` contains the Python package that mirrors the C++ API and exposes convenience helpers such as the `sim.py` sensor models. +- `templates/` and `pyproject.toml` drive the `pip install .` step that packages the compiled `.so`/`.pyd` into a wheel. +- `tests/` is the unit-test tree run by `make python-test.*` targets defined in `python/CMakeLists.txt`. + +## Build prerequisites + +1. **GTSAM** must be built with Python support (i.e., `-DGTSAM_BUILD_PYTHON=ON`) and installed to a prefix that GTDynamics can see via `GTSAM_DIR` or `CMAKE_PREFIX_PATH`. +2. **Python tooling**: the CI job installs `setuptools<70`, `wheel`, `numpy`, `pyparsing`, `pyyaml`, and `pybind11-stubgen` before configuring the project; matching this list locally avoids the same runtime issues. +3. On macOS, the workflow creates and activates a virtual environment (`pythonX -m venv venv`) so that `pip install` and the tests run in the same interpreter that baked the bindings. + +## Building and installing locally + +1. Create a build directory and configure with the wrapper flag enabled: + ```sh + mkdir -p build && cd build + cmake -DGTDYNAMICS_BUILD_PYTHON=ON -DGTSAM_DIR=/path/to/gtsam .. + ``` + Make sure the Python interpreter you pass to CMake has the same version that your `pip` commands will target. +2. Build the project and the python wrapper target: + ```sh + make -j$(nproc) + ``` +3. Install the package into the active Python environment (this is what both the CI job and `python-install` do): + ```sh + make python-install + ``` + This runs `${PYTHON_EXECUTABLE} -m pip install .` in `build/python`, which produces a wheel in `pip`'s cache before installing it. + +## Running Python tests + +- `make python-test` runs every suite captured by the `PYTHON_UNIT_TEST_SUITE` macro (base plus optional `cablerobot`/`jumpingrobot`). +- Individual suites can be executed via `make python-test.base`, `make python-test.cablerobot`, etc., and they rely on the package being built and discoverable via `PYTHONPATH`. +- The CI job runs the tests on macOS with `venv` activation and on Linux after `sudo make python-install` so you can copy those steps when reproducing test failures. + +## Packaging tips + +- `python/templates/setup.py.in` reads the CMake-generated `requirements.txt` and packages the shared library blobs (`.so` / `.pyd`) from `python/gtdynamics` so running `pip wheel .` in `build/python` yields a complete asset. +- Keep `python/requirements.txt` in sync with the requirements file copied to `build/python/requirements.txt` so that CI and a local `pip install` use the same dependency list. +- If you need to publish a wheel manually, the packaged wheel that `pip install .` writes to `~/.cache/pip` already encodes the GTDynamics version reported by `CMakeLists.txt`. + +## Wheels +### CI wheel pipeline +The Common CI workflow (`.github/workflows/common-ci.yml`) performs the exact steps above (configure with `GTDYNAMICS_BUILD_PYTHON=ON`, build, then `make python-install`) across Ubuntu and macOS runners. That workflow serves as the basis for every wheel that leaves the repo: +- **Develop wheels** mirror whatever commit triggered the workflow (typically a push or a PR targeting the `develop` integration branch) so that nightly or pre-release users get the latest patches. +- **Production wheels** are generated from an official release branch or tag (e.g., `main`/`master` after a version bump). The same commands run, but the resulting wheel is considered release-quality and is the artifact uploaded to PyPI. + +### Cleanup script (`.github/scripts/python_wheels/cleanup_gtsam_develop.sh`) +Before building a new develop wheel locally or rerunning the develop CI job, execute the cleanup script from the repository root: +```sh +bash .github/scripts/python_wheels/cleanup_gtsam_develop.sh +``` +This removes stale `gtsam` develop wheel artifacts and/or cached build output so the subsequent `pip install`/`make python-install` matches the clean state CI assumes. diff --git a/python/gtdynamics/__init__.py b/python/gtdynamics/__init__.py index 44ea44b5f..42aad55d3 100644 --- a/python/gtdynamics/__init__.py +++ b/python/gtdynamics/__init__.py @@ -1,3 +1,5 @@ +# Python needs to know about gtsam base classes before it can import module classes +# Else will throw cryptic "referenced unknown base type" error. import gtsam from gtdynamics.gtdynamics import * diff --git a/python/gtdynamics/preamble/gtdynamics.h b/python/gtdynamics/preamble/gtdynamics.h index f52326b34..e69de29bb 100644 --- a/python/gtdynamics/preamble/gtdynamics.h +++ b/python/gtdynamics/preamble/gtdynamics.h @@ -1,4 +0,0 @@ -namespace pybind11 { namespace detail { - template - struct type_caster> : optional_caster> {}; -}} diff --git a/python/gtdynamics/sim.py b/python/gtdynamics/sim.py index 5f5f1504e..2d601438d 100644 --- a/python/gtdynamics/sim.py +++ b/python/gtdynamics/sim.py @@ -23,7 +23,7 @@ def set_joint_angles(pyb, """ for joint_id in joint_to_jid_map.values(): pyb.setJointMotorControl2(bodyUniqueId=robot, - jointindex=joint_id, + jointIndex=joint_id, controlMode=pyb.VELOCITY_CONTROL, force=force) diff --git a/python/notebooks/cmopt_benchmark_dashboard.ipynb b/python/notebooks/cmopt_benchmark_dashboard.ipynb new file mode 100644 index 000000000..677158d15 --- /dev/null +++ b/python/notebooks/cmopt_benchmark_dashboard.ipynb @@ -0,0 +1,3295 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# CM-OPT Benchmark Dashboard\n", + "\n", + "Run and compare `example_constraint_manifold_arm`, `example_constraint_manifold_connected_poses`, and `example_constraint_manifold_cable_robot` from Python.\n", + "\n", + "This notebook:\n", + "- runs each benchmark with `subprocess`\n", + "- captures logs per benchmark\n", + "- reads a CSV emitted by C++ benchmark helpers\n", + "- prints tables via `tabulate`\n", + "- renders Plotly comparisons" + ] + }, + { + "cell_type": "code", + "execution_count": 19, + "id": "a45f8245", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "REPO_ROOT: /Users/dellaert/git/GTDynamics\n", + "BUILD_DIR: /Users/dellaert/git/GTDynamics/build\n", + "DATA_DIR: /Users/dellaert/git/GTDynamics/data\n", + "LOG_DIR: /Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook\n" + ] + } + ], + "source": [ + "from __future__ import annotations\n", + "\n", + "import csv\n", + "import datetime as dt\n", + "import os\n", + "import re\n", + "import subprocess\n", + "import time\n", + "from pathlib import Path\n", + "\n", + "from tabulate import tabulate\n", + "import pandas as pd\n", + "import plotly.graph_objects as go\n", + "\n", + "# Update if needed\n", + "REPO_ROOT = Path('/Users/dellaert/git/GTDynamics')\n", + "BUILD_DIR = REPO_ROOT / 'build'\n", + "DATA_DIR = REPO_ROOT / 'data'\n", + "LOG_DIR = BUILD_DIR / 'benchmark_logs_cmopt_notebook'\n", + "STATUS_PATH = LOG_DIR / 'run_status.csv'\n", + "\n", + "DEFAULT_EXPECTED_METHODS = [\n", + " 'Soft Constraint',\n", + " 'Penalty Method',\n", + " 'Augmented Lagrangian',\n", + " 'Constraint Manifold (F)',\n", + " 'Constraint Manifold (I)',\n", + "]\n", + "\n", + "TARGETS = {\n", + " 'arm': {\n", + " 'build_target': 'example_constraint_manifold_arm',\n", + " 'executable': 'examples/example_constraint_manifold/example_constraint_manifold_arm',\n", + " 'benchmark_id': 'arm',\n", + " 'args': ['--skip-cm-f'],\n", + " 'expected_methods': [\n", + " 'Soft Constraint',\n", + " 'Penalty Method',\n", + " 'Augmented Lagrangian',\n", + " 'Constraint Manifold (I)',\n", + " ],\n", + " },\n", + " 'arm_cm_f_only': {\n", + " 'build_target': 'example_constraint_manifold_arm',\n", + " 'executable': 'examples/example_constraint_manifold/example_constraint_manifold_arm',\n", + " 'benchmark_id': 'arm',\n", + " 'run_timeout_s': 180,\n", + " 'args': ['--cm-f-only'],\n", + " 'expected_methods': [\n", + " 'Constraint Manifold (F)',\n", + " ],\n", + " },\n", + " 'connected_poses': {\n", + " 'build_target': 'example_constraint_manifold_connected_poses',\n", + " 'executable': 'examples/example_constraint_manifold/example_constraint_manifold_connected_poses',\n", + " 'benchmark_id': 'connected_poses',\n", + " },\n", + " 'cable_robot': {\n", + " 'build_target': 'example_constraint_manifold_cable_robot',\n", + " 'executable': 'examples/example_constraint_manifold/example_constraint_manifold_cable_robot',\n", + " 'benchmark_id': 'cable_robot',\n", + " },\n", + " 'quadruped': {\n", + " 'build_target': 'example_constraint_manifold_quadruped_mp',\n", + " 'executable': 'examples/example_constraint_manifold/example_constraint_manifold_quadruped_mp',\n", + " 'benchmark_id': 'quadruped',\n", + " 'args': ['--num-steps', '10', '--methods', 'all'],\n", + " },\n", + "}\n", + "\n", + "JOBS = 2\n", + "RUN_TIMEOUT_S = 240\n", + "PRINT_SUBPROCESS_OUTPUT = True\n", + "BENCHMARK_VERBOSE = False\n", + "RETRACTOR_VERBOSE = False\n", + "\n", + "LOG_DIR.mkdir(parents=True, exist_ok=True)\n", + "DATA_DIR.mkdir(parents=True, exist_ok=True)\n", + "print(f'REPO_ROOT: {REPO_ROOT}')\n", + "print(f'BUILD_DIR: {BUILD_DIR}')\n", + "print(f'DATA_DIR: {DATA_DIR}')\n", + "print(f'LOG_DIR: {LOG_DIR}')\n" + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "id": "073284d5", + "metadata": {}, + "outputs": [], + "source": [ + "def normalize_method(method: str) -> str:\n", + " \"\"\"Normalize method labels from C++ (including LaTeX wrappers).\"\"\"\n", + " method = method.strip()\n", + " m = re.fullmatch(r\"\\\\textbf\\{(.+)\\}\", method)\n", + " return m.group(1) if m else method\n", + "\n", + "\n", + "def read_results_csv(path: Path):\n", + " if not path.exists():\n", + " return []\n", + "\n", + " rows = []\n", + " with path.open('r', newline='') as f:\n", + " reader = csv.DictReader(f)\n", + " for row in reader:\n", + " parsed = {\n", + " 'benchmark': row['benchmark'],\n", + " 'method_raw': row['method'],\n", + " 'method': normalize_method(row['method']),\n", + " 'f_dim': int(row['f_dim']),\n", + " 'v_dim': int(row['v_dim']),\n", + " 'time_s': float(row['time_s']),\n", + " 'iters': int(row['iters']),\n", + " 'constraint_l2': float(row['constraint_l2']),\n", + " 'cost': float(row['cost']),\n", + " }\n", + " rows.append(parsed)\n", + " return rows\n", + "\n", + "\n", + "def benchmark_csv_path(benchmark_id: str) -> Path:\n", + " return DATA_DIR / f'{benchmark_id}_benchmark.csv'\n", + "\n", + "\n", + "def read_all_results_csv(targets: dict):\n", + " rows = []\n", + " seen = set()\n", + " for _, target_cfg in targets.items():\n", + " benchmark_id = target_cfg.get('benchmark_id')\n", + " if benchmark_id in seen:\n", + " continue\n", + " seen.add(benchmark_id)\n", + " rows.extend(read_results_csv(benchmark_csv_path(benchmark_id)))\n", + " return rows\n", + "\n", + "\n", + "def write_status_csv(status_rows):\n", + " with STATUS_PATH.open('w', newline='') as f:\n", + " fieldnames = ['benchmark', 'benchmark_id', 'target', 'return_code', 'elapsed_s', 'timestamp_utc', 'log_path', 'benchmark_csv']\n", + " writer = csv.DictWriter(f, fieldnames=fieldnames)\n", + " writer.writeheader()\n", + " writer.writerows(status_rows)\n", + "\n", + "\n", + "def run_one_target(benchmark: str, target_cfg: dict, jobs: int = 2):\n", + " benchmark_id = target_cfg.get('benchmark_id', benchmark)\n", + " benchmark_csv = benchmark_csv_path(benchmark_id)\n", + " run_timeout_s = target_cfg.get('run_timeout_s', RUN_TIMEOUT_S)\n", + " build_target = target_cfg['build_target']\n", + " executable = target_cfg['executable']\n", + " extra_args = target_cfg.get('args', [])\n", + "\n", + " log_path = LOG_DIR / f'{benchmark}.log'\n", + " env = os.environ.copy()\n", + "\n", + " build_cmd = ['make', f'-j{jobs}', build_target]\n", + " run_cmd = [\n", + " str(BUILD_DIR / executable),\n", + " '--benchmark-id', benchmark_id,\n", + " '--benchmark-csv', str(benchmark_csv),\n", + " ]\n", + " if BENCHMARK_VERBOSE:\n", + " run_cmd.append('--verbose-benchmark')\n", + " if RETRACTOR_VERBOSE:\n", + " run_cmd.append('--verbose-retractor')\n", + " run_cmd.extend(extra_args)\n", + " build_cmd_str = ' '.join(build_cmd)\n", + " run_cmd_str = ' '.join(run_cmd)\n", + " print(f'Building: {build_cmd_str} (benchmark={benchmark})')\n", + "\n", + " start = time.perf_counter()\n", + " timed_out = False\n", + " with log_path.open('w') as log_file:\n", + " build_log_pos = log_file.tell()\n", + " build_result = subprocess.run(\n", + " build_cmd,\n", + " cwd=BUILD_DIR,\n", + " env=env,\n", + " text=True,\n", + " stdout=log_file,\n", + " stderr=subprocess.STDOUT,\n", + " )\n", + " if PRINT_SUBPROCESS_OUTPUT:\n", + " log_file.flush()\n", + " with log_path.open('r') as rf:\n", + " rf.seek(build_log_pos)\n", + " for line in rf:\n", + " if 'Built target' in line or 'error' in line.lower():\n", + " print(f'[{benchmark}] {line}', end='')\n", + "\n", + " if build_result.returncode != 0:\n", + " elapsed = time.perf_counter() - start\n", + " print(f' -> build failed, return_code={build_result.returncode}, elapsed={elapsed:.2f}s, log={log_path}')\n", + " return {\n", + " 'benchmark': benchmark,\n", + " 'benchmark_id': benchmark_id,\n", + " 'target': f\"{build_target} -> {executable}\",\n", + " 'return_code': build_result.returncode,\n", + " 'elapsed_s': elapsed,\n", + " 'timestamp_utc': dt.datetime.now(dt.UTC).isoformat(timespec='seconds').replace('+00:00', 'Z'),\n", + " 'log_path': str(log_path),\n", + " 'benchmark_csv': str(benchmark_csv),\n", + " }\n", + "\n", + " print(f'Running: {run_cmd_str} (benchmark={benchmark})')\n", + " try:\n", + " run_log_pos = log_file.tell()\n", + " run_result = subprocess.run(\n", + " run_cmd,\n", + " cwd=BUILD_DIR,\n", + " env=env,\n", + " text=True,\n", + " stdout=log_file,\n", + " stderr=subprocess.STDOUT,\n", + " timeout=run_timeout_s,\n", + " )\n", + " if PRINT_SUBPROCESS_OUTPUT:\n", + " log_file.flush()\n", + " with log_path.open('r') as rf:\n", + " rf.seek(run_log_pos)\n", + " for line in rf:\n", + " stripped = line.strip()\n", + " if (\n", + " stripped.startswith('[BENCH]')\n", + " or stripped.startswith('& ')\n", + " or stripped.startswith('soft constraints:')\n", + " or stripped.startswith('penalty method:')\n", + " or stripped.startswith('augmented lagrangian:')\n", + " or stripped.startswith('constraint manifold basis variables')\n", + " ):\n", + " print(f'[{benchmark}] {line}', end='')\n", + " return_code = run_result.returncode\n", + " except subprocess.TimeoutExpired:\n", + " timed_out = True\n", + " log_file.write('\\n[TIMEOUT]\\n')\n", + " if PRINT_SUBPROCESS_OUTPUT:\n", + " print(f'[TIMEOUT] process exceeded timeout={run_timeout_s}s')\n", + " return_code = 124\n", + "\n", + " elapsed = time.perf_counter() - start\n", + " timeout_suffix = ' (timeout)' if timed_out else ''\n", + " print(f' -> return_code={return_code}, elapsed={elapsed:.2f}s{timeout_suffix}, log={log_path}')\n", + "\n", + " return {\n", + " 'benchmark': benchmark,\n", + " 'benchmark_id': benchmark_id,\n", + " 'target': f\"{build_target} -> {executable}\",\n", + " 'return_code': return_code,\n", + " 'elapsed_s': elapsed,\n", + " 'timestamp_utc': dt.datetime.now(dt.UTC).isoformat(timespec='seconds').replace('+00:00', 'Z'),\n", + " 'log_path': str(log_path),\n", + " 'benchmark_csv': str(benchmark_csv),\n", + " }\n", + "\n", + "\n", + "def run_all_targets(clear_previous: bool = True, jobs: int = 2):\n", + " LOG_DIR.mkdir(parents=True, exist_ok=True)\n", + " if clear_previous:\n", + " for p in LOG_DIR.glob('*.log'):\n", + " p.unlink()\n", + " seen = set()\n", + " for _, target_cfg in TARGETS.items():\n", + " benchmark_id = target_cfg.get('benchmark_id')\n", + " if benchmark_id in seen:\n", + " continue\n", + " seen.add(benchmark_id)\n", + " p = benchmark_csv_path(benchmark_id)\n", + " if p.exists():\n", + " p.unlink()\n", + "\n", + " status_rows = []\n", + " for benchmark, target_cfg in TARGETS.items():\n", + " status_rows.append(run_one_target(benchmark, target_cfg, jobs=jobs))\n", + "\n", + " write_status_csv(status_rows)\n", + " return status_rows\n" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "id": "895bb8c7", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Building: make -j2 example_constraint_manifold_arm (benchmark=arm)\n", + "[arm] [ 95%] Built target gtdynamics\n", + "[arm] [100%] Built target example_constraint_manifold_arm\n", + "Running: /Users/dellaert/git/GTDynamics/build/examples/example_constraint_manifold/example_constraint_manifold_arm --benchmark-id arm --benchmark-csv /Users/dellaert/git/GTDynamics/data/arm_benchmark.csv --skip-cm-f (benchmark=arm)\n", + "[arm] [BENCH] Skipping CM(F); running CM(I) with other methods.\n", + "[arm] soft constraints:\n", + "[arm] [BENCH] Soft Constraint: f_dim=688, v_dim=605, time_s=0.279, iters=205, constraint_l2=8.571e-03, cost=0.430867\n", + "[arm] penalty method:\n", + "[arm] [BENCH] Penalty Method: f_dim=688, v_dim=605, time_s=1.92799, iters=0, constraint_l2=2.670e-05, cost=0.234963\n", + "[arm] augmented lagrangian:\n", + "[arm] [BENCH] Augmented Lagrangian: f_dim=688, v_dim=605, time_s=1.426, iters=0, constraint_l2=1.063e-04, cost=0.233363\n", + "[arm] constraint manifold basis variables (infeasible):\n", + "[arm] [BENCH] \\textbf{Constraint Manifold (I)}: f_dim=146, v_dim=70, time_s=0.006, iters=0, constraint_l2=2.841e-10, cost=594.715\n", + "[arm] & Soft Constraint & $688 \\times 605$ & 0.279 & 205 & 8.57e-03 & 0.43\\\\\n", + "[arm] & Penalty Method & $688 \\times 605$ & 1.928 & 0 & 2.67e-05 & 0.23\\\\\n", + "[arm] & Augmented Lagrangian & $688 \\times 605$ & 1.426 & 0 & 1.06e-04 & 0.23\\\\\n", + "[arm] & \\textbf{Constraint Manifold (I)} & $146 \\times 70$ & 0.006 & 0 & 2.84e-10 & 594.71\\\\\n", + " -> return_code=0, elapsed=4.15s, log=/Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook/arm.log\n", + "Building: make -j2 example_constraint_manifold_arm (benchmark=arm_cm_f_only)\n", + "[arm_cm_f_only] [ 95%] Built target gtdynamics\n", + "[arm_cm_f_only] [100%] Built target example_constraint_manifold_arm\n", + "Running: /Users/dellaert/git/GTDynamics/build/examples/example_constraint_manifold/example_constraint_manifold_arm --benchmark-id arm --benchmark-csv /Users/dellaert/git/GTDynamics/data/arm_benchmark.csv --cm-f-only (benchmark=arm_cm_f_only)\n", + "[arm_cm_f_only] [BENCH] F-only mode enabled.\n", + "[arm_cm_f_only] constraint manifold basis variables (feasible):\n", + "[arm_cm_f_only] [BENCH] \\textbf{Constraint Manifold (F)}: f_dim=146, v_dim=70, time_s=0.004, iters=0, constraint_l2=3.182e-03, cost=594.715\n", + "[arm_cm_f_only] & \\textbf{Constraint Manifold (F)} & $146 \\times 70$ & 0.004 & 0 & 3.18e-03 & 594.71\\\\\n", + " -> return_code=0, elapsed=0.42s, log=/Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook/arm_cm_f_only.log\n", + "Building: make -j2 example_constraint_manifold_connected_poses (benchmark=connected_poses)\n", + "[connected_poses] [ 95%] Built target gtdynamics\n", + "[connected_poses] [100%] Built target example_constraint_manifold_connected_poses\n", + "Running: /Users/dellaert/git/GTDynamics/build/examples/example_constraint_manifold/example_constraint_manifold_connected_poses --benchmark-id connected_poses --benchmark-csv /Users/dellaert/git/GTDynamics/data/connected_poses_benchmark.csv (benchmark=connected_poses)\n", + "[connected_poses] soft constraints:\n", + "[connected_poses] [BENCH] Soft Constraint: f_dim=909, v_dim=606, time_s=0.034, iters=28, constraint_l2=7.525e+01, cost=13.5086\n", + "[connected_poses] penalty method:\n", + "[connected_poses] [BENCH] Penalty Method: f_dim=909, v_dim=606, time_s=0.161914, iters=0, constraint_l2=7.493e+01, cost=5.27731\n", + "[connected_poses] augmented lagrangian:\n", + "[connected_poses] [BENCH] Augmented Lagrangian: f_dim=909, v_dim=606, time_s=0.116, iters=0, constraint_l2=7.493e+01, cost=5.45672\n", + "[connected_poses] constraint manifold basis variables (feasible):\n", + "[connected_poses] [BENCH] \\textbf{Constraint Manifold (F)}: f_dim=606, v_dim=303, time_s=0.094, iters=0, constraint_l2=6.384e+01, cost=9959.82\n", + "[connected_poses] constraint manifold basis variables (infeasible):\n", + "[connected_poses] [BENCH] \\textbf{Constraint Manifold (I)}: f_dim=606, v_dim=303, time_s=0.081, iters=0, constraint_l2=6.384e+01, cost=9961.41\n", + "[connected_poses] & Soft Constraint & $909 \\times 606$ & 0.034 & 28 & 7.52e+01 & 13.51\\\\\n", + "[connected_poses] & Penalty Method & $909 \\times 606$ & 0.1619 & 0 & 7.49e+01 & 5.28\\\\\n", + "[connected_poses] & Augmented Lagrangian & $909 \\times 606$ & 0.116 & 0 & 7.49e+01 & 5.46\\\\\n", + "[connected_poses] & \\textbf{Constraint Manifold (F)} & $606 \\times 303$ & 0.094 & 0 & 6.38e+01 & 9959.82\\\\\n", + "[connected_poses] & \\textbf{Constraint Manifold (I)} & $606 \\times 303$ & 0.081 & 0 & 6.38e+01 & 9961.41\\\\\n", + " -> return_code=0, elapsed=0.93s, log=/Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook/connected_poses.log\n", + "Building: make -j2 example_constraint_manifold_cable_robot (benchmark=cable_robot)\n", + "[cable_robot] [ 95%] Built target gtdynamics\n", + "[cable_robot] [100%] Built target example_constraint_manifold_cable_robot\n", + "Running: /Users/dellaert/git/GTDynamics/build/examples/example_constraint_manifold/example_constraint_manifold_cable_robot --benchmark-id cable_robot --benchmark-csv /Users/dellaert/git/GTDynamics/data/cable_robot_benchmark.csv (benchmark=cable_robot)\n", + "[cable_robot] soft constraints:\n", + "[cable_robot] [BENCH] Soft Constraint: f_dim=140, v_dim=110, time_s=0.125, iters=205, constraint_l2=6.052e-03, cost=0.344925\n", + "[cable_robot] penalty method:\n", + "[cable_robot] [BENCH] Penalty Method: f_dim=140, v_dim=110, time_s=0.609028, iters=0, constraint_l2=4.188e-05, cost=0.239275\n", + "[cable_robot] augmented lagrangian:\n", + "[cable_robot] [BENCH] Augmented Lagrangian: f_dim=140, v_dim=110, time_s=0.444, iters=0, constraint_l2=1.139e-04, cost=0.238587\n", + "[cable_robot] constraint manifold basis variables (feasible):\n", + "[cable_robot] [BENCH] \\textbf{Constraint Manifold (F)}: f_dim=86, v_dim=60, time_s=0.024, iters=0, constraint_l2=1.280e-12, cost=0.233529\n", + "[cable_robot] constraint manifold basis variables (infeasible):\n", + "[cable_robot] [BENCH] \\textbf{Constraint Manifold (I)}: f_dim=86, v_dim=60, time_s=0.018, iters=0, constraint_l2=2.220e-09, cost=0.233527\n", + "[cable_robot] & Soft Constraint & $140 \\times 110$ & 0.125 & 205 & 6.05e-03 & 0.34\\\\\n", + "[cable_robot] & Penalty Method & $140 \\times 110$ & 0.609 & 0 & 4.19e-05 & 0.24\\\\\n", + "[cable_robot] & Augmented Lagrangian & $140 \\times 110$ & 0.444 & 0 & 1.14e-04 & 0.24\\\\\n", + "[cable_robot] & \\textbf{Constraint Manifold (F)} & $86 \\times 60$ & 0.024 & 0 & 1.28e-12 & 0.23\\\\\n", + "[cable_robot] & \\textbf{Constraint Manifold (I)} & $86 \\times 60$ & 0.018 & 0 & 2.22e-09 & 0.23\\\\\n", + " -> return_code=0, elapsed=1.66s, log=/Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook/cable_robot.log\n", + "Building: make -j2 example_constraint_manifold_quadruped_mp (benchmark=quadruped)\n", + "[quadruped] [ 95%] Built target gtdynamics\n", + "[quadruped] [100%] Built target example_constraint_manifold_quadruped_mp\n", + "Running: /Users/dellaert/git/GTDynamics/build/examples/example_constraint_manifold/example_constraint_manifold_quadruped_mp --benchmark-id quadruped --benchmark-csv /Users/dellaert/git/GTDynamics/data/quadruped_benchmark.csv --num-steps 10 --methods all (benchmark=quadruped)\n", + "[quadruped] soft constraints:\n", + "[quadruped] [BENCH] Soft Constraint: f_dim=5066, v_dim=4950, time_s=0.069, iters=7, constraint_l2=1.338e-10, cost=346.627\n", + "[quadruped] penalty method:\n", + "[quadruped] [BENCH] Penalty Method: f_dim=5066, v_dim=4950, time_s=0.955481, iters=0, constraint_l2=6.424e-06, cost=44.8798\n", + "[quadruped] augmented lagrangian:\n", + "[quadruped] [BENCH] Augmented Lagrangian: f_dim=5066, v_dim=4950, time_s=1.025, iters=0, constraint_l2=6.054e-06, cost=44.8975\n", + "[quadruped] constraint manifold basis variables (feasible):\n", + "[quadruped] [BENCH] \\textbf{Constraint Manifold (F)}: f_dim=314, v_dim=198, time_s=1.721, iters=0, constraint_l2=1.027e-03, cost=28.1259\n", + "[quadruped] constraint manifold basis variables (infeasible):\n", + "[quadruped] [BENCH] \\textbf{Constraint Manifold (I)}: f_dim=314, v_dim=198, time_s=0.619, iters=0, constraint_l2=2.770e-07, cost=197.745\n", + "[quadruped] & Soft Constraint & $5066 \\times 4950$ & 0.069 & 7 & 1.34e-10 & 346.63\\\\\n", + "[quadruped] & Penalty Method & $5066 \\times 4950$ & 0.9555 & 0 & 6.42e-06 & 44.88\\\\\n", + "[quadruped] & Augmented Lagrangian & $5066 \\times 4950$ & 1.025 & 0 & 6.05e-06 & 44.90\\\\\n", + "[quadruped] & \\textbf{Constraint Manifold (F)} & $314 \\times 198$ & 1.721 & 0 & 1.03e-03 & 28.13\\\\\n", + "[quadruped] & \\textbf{Constraint Manifold (I)} & $314 \\times 198$ & 0.619 & 0 & 2.77e-07 & 197.74\\\\\n", + " -> return_code=0, elapsed=4.95s, log=/Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook/quadruped.log\n", + "\n", + "| benchmark | benchmark_id | target | return_code | elapsed_s | timestamp_utc | log_path | benchmark_csv |\n", + "|-----------------|-----------------|---------------------------------------------------------------------------------------------------------------------------------|---------------|-------------|----------------------|----------------------------------------------------------------------------------------|-------------------------------------------------------------------|\n", + "| arm | arm | example_constraint_manifold_arm -> examples/example_constraint_manifold/example_constraint_manifold_arm | 0 | 4.153 | 2026-02-08T15:20:12Z | /Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook/arm.log | /Users/dellaert/git/GTDynamics/data/arm_benchmark.csv |\n", + "| arm_cm_f_only | arm | example_constraint_manifold_arm -> examples/example_constraint_manifold/example_constraint_manifold_arm | 0 | 0.425 | 2026-02-08T15:20:12Z | /Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook/arm_cm_f_only.log | /Users/dellaert/git/GTDynamics/data/arm_benchmark.csv |\n", + "| connected_poses | connected_poses | example_constraint_manifold_connected_poses -> examples/example_constraint_manifold/example_constraint_manifold_connected_poses | 0 | 0.930 | 2026-02-08T15:20:13Z | /Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook/connected_poses.log | /Users/dellaert/git/GTDynamics/data/connected_poses_benchmark.csv |\n", + "| cable_robot | cable_robot | example_constraint_manifold_cable_robot -> examples/example_constraint_manifold/example_constraint_manifold_cable_robot | 0 | 1.661 | 2026-02-08T15:20:15Z | /Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook/cable_robot.log | /Users/dellaert/git/GTDynamics/data/cable_robot_benchmark.csv |\n", + "| quadruped | quadruped | example_constraint_manifold_quadruped_mp -> examples/example_constraint_manifold/example_constraint_manifold_quadruped_mp | 0 | 4.955 | 2026-02-08T15:20:20Z | /Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook/quadruped.log | /Users/dellaert/git/GTDynamics/data/quadruped_benchmark.csv |\n", + "\n", + "status CSV: /Users/dellaert/git/GTDynamics/build/benchmark_logs_cmopt_notebook/run_status.csv\n", + "benchmark CSV (arm): /Users/dellaert/git/GTDynamics/data/arm_benchmark.csv\n", + "benchmark CSV (cable_robot): /Users/dellaert/git/GTDynamics/data/cable_robot_benchmark.csv\n", + "benchmark CSV (connected_poses): /Users/dellaert/git/GTDynamics/data/connected_poses_benchmark.csv\n", + "benchmark CSV (quadruped): /Users/dellaert/git/GTDynamics/data/quadruped_benchmark.csv\n" + ] + } + ], + "source": [ + "# Run all benchmarks\n", + "run_status = run_all_targets(clear_previous=True, jobs=JOBS)\n", + "\n", + "print()\n", + "print(tabulate(\n", + " run_status,\n", + " headers='keys',\n", + " tablefmt='github',\n", + " floatfmt='.3f',\n", + "))\n", + "\n", + "print(f'\\nstatus CSV: {STATUS_PATH}')\n", + "for benchmark_id in sorted({cfg.get('benchmark_id') for cfg in TARGETS.values()}):\n", + " print(f'benchmark CSV ({benchmark_id}): {benchmark_csv_path(benchmark_id)}')\n" + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "id": "46b72276", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "| benchmark | method_raw | method | f_dim | v_dim | time_s | iters | constraint_l2 | cost |\n", + "|-----------------|----------------------------------|-------------------------|---------|---------|----------|---------|-----------------|-------------|\n", + "| arm | Augmented Lagrangian | Augmented Lagrangian | 688 | 605 | 1.426 | 0 | 0.00010625 | 0.233363 |\n", + "| arm | \\textbf{Constraint Manifold (F)} | Constraint Manifold (F) | 146 | 70 | 0.004 | 0 | 0.00318196 | 594.715 |\n", + "| arm | \\textbf{Constraint Manifold (I)} | Constraint Manifold (I) | 146 | 70 | 0.006 | 0 | 2.84091e-10 | 594.715 |\n", + "| arm | Penalty Method | Penalty Method | 688 | 605 | 1.92799 | 0 | 2.66998e-05 | 0.234963 |\n", + "| arm | Soft Constraint | Soft Constraint | 688 | 605 | 0.279 | 205 | 0.00857051 | 0.430867 |\n", + "| cable_robot | Augmented Lagrangian | Augmented Lagrangian | 140 | 110 | 0.444 | 0 | 0.000113894 | 0.238587 |\n", + "| cable_robot | \\textbf{Constraint Manifold (F)} | Constraint Manifold (F) | 86 | 60 | 0.024 | 0 | 1.28037e-12 | 0.233529 |\n", + "| cable_robot | \\textbf{Constraint Manifold (I)} | Constraint Manifold (I) | 86 | 60 | 0.018 | 0 | 2.21997e-09 | 0.233527 |\n", + "| cable_robot | Penalty Method | Penalty Method | 140 | 110 | 0.609028 | 0 | 4.18843e-05 | 0.239275 |\n", + "| cable_robot | Soft Constraint | Soft Constraint | 140 | 110 | 0.125 | 205 | 0.00605245 | 0.344925 |\n", + "| connected_poses | Augmented Lagrangian | Augmented Lagrangian | 909 | 606 | 0.116 | 0 | 74.9307 | 5.45672 |\n", + "| connected_poses | \\textbf{Constraint Manifold (F)} | Constraint Manifold (F) | 606 | 303 | 0.094 | 0 | 63.8406 | 9959.82 |\n", + "| connected_poses | \\textbf{Constraint Manifold (I)} | Constraint Manifold (I) | 606 | 303 | 0.081 | 0 | 63.8406 | 9961.41 |\n", + "| connected_poses | Penalty Method | Penalty Method | 909 | 606 | 0.161914 | 0 | 74.9315 | 5.27731 |\n", + "| connected_poses | Soft Constraint | Soft Constraint | 909 | 606 | 0.034 | 28 | 75.2456 | 13.5086 |\n", + "| quadruped | Augmented Lagrangian | Augmented Lagrangian | 5066 | 4950 | 1.025 | 0 | 6.05424e-06 | 44.8975 |\n", + "| quadruped | \\textbf{Constraint Manifold (F)} | Constraint Manifold (F) | 314 | 198 | 1.721 | 0 | 0.00102727 | 28.1259 |\n", + "| quadruped | \\textbf{Constraint Manifold (I)} | Constraint Manifold (I) | 314 | 198 | 0.619 | 0 | 2.77049e-07 | 197.745 |\n", + "| quadruped | Penalty Method | Penalty Method | 5066 | 4950 | 0.955481 | 0 | 6.42358e-06 | 44.8798 |\n", + "| quadruped | Soft Constraint | Soft Constraint | 5066 | 4950 | 0.069 | 7 | 1.33805e-10 | 346.627 |\n", + "\n", + "Method coverage summary:\n", + "| benchmark | methods_found | missing_expected_methods |\n", + "|-----------------|---------------------------------------------------------------------------------------------------------|----------------------------|\n", + "| arm | Augmented Lagrangian, Constraint Manifold (F), Constraint Manifold (I), Penalty Method, Soft Constraint | (none) |\n", + "| cable_robot | Augmented Lagrangian, Constraint Manifold (F), Constraint Manifold (I), Penalty Method, Soft Constraint | (none) |\n", + "| connected_poses | Augmented Lagrangian, Constraint Manifold (F), Constraint Manifold (I), Penalty Method, Soft Constraint | (none) |\n", + "| quadruped | Augmented Lagrangian, Constraint Manifold (F), Constraint Manifold (I), Penalty Method, Soft Constraint | (none) |\n" + ] + } + ], + "source": [ + "rows = read_all_results_csv(TARGETS)\n", + "\n", + "if not rows:\n", + " print('No benchmark rows found in benchmark CSV files.')\n", + "else:\n", + " rows_sorted = sorted(rows, key=lambda r: (r['benchmark'], r['method']))\n", + " print(tabulate(\n", + " rows_sorted,\n", + " headers='keys',\n", + " tablefmt='github',\n", + " floatfmt='.6g',\n", + " ))\n", + "\n", + " present = {}\n", + " for r in rows_sorted:\n", + " present.setdefault(r['benchmark'], set()).add(r['method'])\n", + "\n", + " expected_by_benchmark = {}\n", + " for benchmark, target_cfg in TARGETS.items():\n", + " benchmark_id = target_cfg.get('benchmark_id', benchmark)\n", + " expected = set(target_cfg.get('expected_methods', DEFAULT_EXPECTED_METHODS))\n", + " expected_by_benchmark.setdefault(benchmark_id, set()).update(expected)\n", + "\n", + " summary = []\n", + " for benchmark_id in sorted(expected_by_benchmark.keys()):\n", + " expected_methods = sorted(expected_by_benchmark[benchmark_id])\n", + " got = present.get(benchmark_id, set())\n", + " missing = [m for m in expected_methods if m not in got]\n", + " summary.append({\n", + " 'benchmark': benchmark_id,\n", + " 'methods_found': ', '.join(sorted(got)) if got else '(none)',\n", + " 'missing_expected_methods': ', '.join(missing) if missing else '(none)',\n", + " })\n", + "\n", + " print('\\nMethod coverage summary:')\n", + " print(tabulate(summary, headers='keys', tablefmt='github'))\n" + ] + }, + { + "cell_type": "code", + "execution_count": 23, + "id": "e8f0893b", + "metadata": {}, + "outputs": [ + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "name": "Soft Constraint", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 0.279, + 0.034, + 0.125, + 0.069 + ] + }, + { + "name": "Penalty Method", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 1.927994, + 0.161914, + 0.609028, + 0.955481 + ] + }, + { + "name": "Augmented Lagrangian", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 1.426, + 0.116, + 0.444, + 1.025 + ] + }, + { + "name": "Constraint Manifold (F)", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 0.004, + 0.094, + 0.024, + 1.721 + ] + }, + { + "name": "Constraint Manifold (I)", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 0.006, + 0.081, + 0.018, + 0.619 + ] + } + ], + "layout": { + "barmode": "group", + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "white", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "white", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "#C8D4E3", + "linecolor": "#C8D4E3", + "minorgridcolor": "#C8D4E3", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "#C8D4E3", + "linecolor": "#C8D4E3", + "minorgridcolor": "#C8D4E3", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermap": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermap" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "white", + "showlakes": true, + "showland": true, + "subunitcolor": "#C8D4E3" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "white", + "polar": { + "angularaxis": { + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "" + }, + "bgcolor": "white", + "radialaxis": { + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "white", + "gridcolor": "#DFE8F3", + "gridwidth": 2, + "linecolor": "#EBF0F8", + "showbackground": true, + "ticks": "", + "zerolinecolor": "#EBF0F8" + }, + "yaxis": { + "backgroundcolor": "white", + "gridcolor": "#DFE8F3", + "gridwidth": 2, + "linecolor": "#EBF0F8", + "showbackground": true, + "ticks": "", + "zerolinecolor": "#EBF0F8" + }, + "zaxis": { + "backgroundcolor": "white", + "gridcolor": "#DFE8F3", + "gridwidth": 2, + "linecolor": "#EBF0F8", + "showbackground": true, + "ticks": "", + "zerolinecolor": "#EBF0F8" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "#DFE8F3", + "linecolor": "#A2B1C6", + "ticks": "" + }, + "baxis": { + "gridcolor": "#DFE8F3", + "linecolor": "#A2B1C6", + "ticks": "" + }, + "bgcolor": "white", + "caxis": { + "gridcolor": "#DFE8F3", + "linecolor": "#A2B1C6", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "#EBF0F8", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "#EBF0F8", + "zerolinewidth": 2 + } + } + }, + "title": { + "text": "Runtime Comparison" + }, + "xaxis": { + "title": { + "text": "Benchmark" + } + }, + "yaxis": { + "title": { + "text": "time (s)" + }, + "type": "log" + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "name": "Soft Constraint", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 0.430866861475, + 13.5086495728, + 0.344924848164, + 346.626860998 + ] + }, + { + "name": "Penalty Method", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 0.234963310298, + 5.27731107443, + 0.239275323036, + 44.8798436756 + ] + }, + { + "name": "Augmented Lagrangian", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 0.233362762964, + 5.45671960694, + 0.238587453811, + 44.8975004248 + ] + }, + { + "name": "Constraint Manifold (F)", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 594.714640484, + 9959.81644419, + 0.233528712448, + 28.1258914279 + ] + }, + { + "name": "Constraint Manifold (I)", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 594.714640484, + 9961.40622576, + 0.233527453569, + 197.744544545 + ] + } + ], + "layout": { + "barmode": "group", + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "white", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "white", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "#C8D4E3", + "linecolor": "#C8D4E3", + "minorgridcolor": "#C8D4E3", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "#C8D4E3", + "linecolor": "#C8D4E3", + "minorgridcolor": "#C8D4E3", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermap": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermap" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "white", + "showlakes": true, + "showland": true, + "subunitcolor": "#C8D4E3" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "white", + "polar": { + "angularaxis": { + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "" + }, + "bgcolor": "white", + "radialaxis": { + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "white", + "gridcolor": "#DFE8F3", + "gridwidth": 2, + "linecolor": "#EBF0F8", + "showbackground": true, + "ticks": "", + "zerolinecolor": "#EBF0F8" + }, + "yaxis": { + "backgroundcolor": "white", + "gridcolor": "#DFE8F3", + "gridwidth": 2, + "linecolor": "#EBF0F8", + "showbackground": true, + "ticks": "", + "zerolinecolor": "#EBF0F8" + }, + "zaxis": { + "backgroundcolor": "white", + "gridcolor": "#DFE8F3", + "gridwidth": 2, + "linecolor": "#EBF0F8", + "showbackground": true, + "ticks": "", + "zerolinecolor": "#EBF0F8" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "#DFE8F3", + "linecolor": "#A2B1C6", + "ticks": "" + }, + "baxis": { + "gridcolor": "#DFE8F3", + "linecolor": "#A2B1C6", + "ticks": "" + }, + "bgcolor": "white", + "caxis": { + "gridcolor": "#DFE8F3", + "linecolor": "#A2B1C6", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "#EBF0F8", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "#EBF0F8", + "zerolinewidth": 2 + } + } + }, + "title": { + "text": "Cost Comparison" + }, + "xaxis": { + "title": { + "text": "Benchmark" + } + }, + "yaxis": { + "title": { + "text": "cost" + }, + "type": "log" + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "name": "Soft Constraint", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 0.008570506800417, + 75.24561933366, + 0.006052452139054, + 1.338052994241e-10 + ] + }, + { + "name": "Penalty Method", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 0.0000266998246148, + 74.93153595072, + 0.00004188426181726, + 0.000006423582409842 + ] + }, + { + "name": "Augmented Lagrangian", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 0.0001062504276957, + 74.93068568168, + 0.0001138937867918, + 0.000006054244020671 + ] + }, + { + "name": "Constraint Manifold (F)", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 0.003181955756113, + 63.84058074924, + 1.280371652553e-12, + 0.00102727427726 + ] + }, + { + "name": "Constraint Manifold (I)", + "type": "bar", + "x": [ + "arm", + "connected_poses", + "cable_robot", + "quadruped" + ], + "y": [ + 2.840912772616e-10, + 63.8405783527, + 2.2199728718e-9, + 2.770490515134e-7 + ] + } + ], + "layout": { + "barmode": "group", + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "white", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "white", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "#C8D4E3", + "linecolor": "#C8D4E3", + "minorgridcolor": "#C8D4E3", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "#C8D4E3", + "linecolor": "#C8D4E3", + "minorgridcolor": "#C8D4E3", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermap": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermap" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "white", + "showlakes": true, + "showland": true, + "subunitcolor": "#C8D4E3" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "white", + "polar": { + "angularaxis": { + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "" + }, + "bgcolor": "white", + "radialaxis": { + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "white", + "gridcolor": "#DFE8F3", + "gridwidth": 2, + "linecolor": "#EBF0F8", + "showbackground": true, + "ticks": "", + "zerolinecolor": "#EBF0F8" + }, + "yaxis": { + "backgroundcolor": "white", + "gridcolor": "#DFE8F3", + "gridwidth": 2, + "linecolor": "#EBF0F8", + "showbackground": true, + "ticks": "", + "zerolinecolor": "#EBF0F8" + }, + "zaxis": { + "backgroundcolor": "white", + "gridcolor": "#DFE8F3", + "gridwidth": 2, + "linecolor": "#EBF0F8", + "showbackground": true, + "ticks": "", + "zerolinecolor": "#EBF0F8" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "#DFE8F3", + "linecolor": "#A2B1C6", + "ticks": "" + }, + "baxis": { + "gridcolor": "#DFE8F3", + "linecolor": "#A2B1C6", + "ticks": "" + }, + "bgcolor": "white", + "caxis": { + "gridcolor": "#DFE8F3", + "linecolor": "#A2B1C6", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "#EBF0F8", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "#EBF0F8", + "linecolor": "#EBF0F8", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "#EBF0F8", + "zerolinewidth": 2 + } + } + }, + "title": { + "text": "Constraint Violation Comparison" + }, + "xaxis": { + "title": { + "text": "Benchmark" + } + }, + "yaxis": { + "title": { + "text": "constraint L2" + }, + "type": "log" + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "def make_grouped_bar(rows, metric: str, title: str, y_title: str, log_y: bool = False):\n", + " methods_order = [\n", + " 'Soft Constraint',\n", + " 'Penalty Method',\n", + " 'Augmented Lagrangian',\n", + " 'Constraint Manifold (F)',\n", + " 'Constraint Manifold (I)',\n", + " ]\n", + " benchmarks = []\n", + " seen_benchmarks = set()\n", + " for target_name, target_cfg in TARGETS.items():\n", + " benchmark_name = target_cfg.get('benchmark_id', target_name)\n", + " if benchmark_name not in seen_benchmarks:\n", + " seen_benchmarks.add(benchmark_name)\n", + " benchmarks.append(benchmark_name)\n", + " for row in rows:\n", + " benchmark_name = row['benchmark']\n", + " if benchmark_name not in seen_benchmarks:\n", + " seen_benchmarks.add(benchmark_name)\n", + " benchmarks.append(benchmark_name)\n", + "\n", + " value_map = {}\n", + " for r in rows:\n", + " value_map[(r['benchmark'], r['method'])] = r[metric]\n", + "\n", + " fig = go.Figure()\n", + " for method in methods_order:\n", + " y = []\n", + " for b in benchmarks:\n", + " y.append(value_map.get((b, method), None))\n", + " if any(v is not None for v in y):\n", + " fig.add_trace(go.Bar(name=method, x=benchmarks, y=y))\n", + "\n", + " fig.update_layout(\n", + " title=title,\n", + " barmode='group',\n", + " xaxis_title='Benchmark',\n", + " yaxis_title=y_title,\n", + " template='plotly_white',\n", + " )\n", + " if log_y:\n", + " fig.update_yaxes(type='log')\n", + " return fig\n", + "\n", + "\n", + "if not rows:\n", + " print('No rows to plot.')\n", + "else:\n", + " fig_time = make_grouped_bar(rows, 'time_s', 'Runtime Comparison', 'time (s)', log_y=True)\n", + " fig_cost = make_grouped_bar(rows, 'cost', 'Cost Comparison', 'cost', log_y=True)\n", + " fig_constraint = make_grouped_bar(rows, 'constraint_l2', 'Constraint Violation Comparison', 'constraint L2', log_y=True)\n", + "\n", + " fig_time.show()\n", + " fig_cost.show()\n", + " fig_constraint.show()" + ] + }, + { + "cell_type": "code", + "execution_count": 24, + "id": "0aa41c80", + "metadata": {}, + "outputs": [], + "source": [ + "def tail_log(benchmark: str, n: int = 80):\n", + " p = LOG_DIR / f'{benchmark}.log'\n", + " if not p.exists():\n", + " print(f'No log found: {p}')\n", + " return\n", + " lines = p.read_text(errors='replace').splitlines()\n", + " print(f'--- tail({n}) {p} ---')\n", + " for line in lines[-n:]:\n", + " print(line)\n", + "\n", + "# Example:\n", + "# tail_log('arm', n=120)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/python/requirements.txt b/python/requirements.txt deleted file mode 100644 index f39221cad..000000000 --- a/python/requirements.txt +++ /dev/null @@ -1,2 +0,0 @@ -numpy -gtsam \ No newline at end of file diff --git a/python/templates/gtdynamics.tpl b/python/templates/gtdynamics.tpl index ba56c8945..6ad5015ef 100644 --- a/python/templates/gtdynamics.tpl +++ b/python/templates/gtdynamics.tpl @@ -1,7 +1,5 @@ // Minimal template file for wrapping C++ code. -{include_boost} - #include #include #include @@ -10,19 +8,14 @@ #include #include -#include - #include "gtsam/config.h" #include "gtsam/base/serialization.h" #include "gtsam/base/utilities.h" // for RedirectCout. {includes} -#include {boost_class_export} -{holder_type} - // Preamble for STL classes #include "python/gtdynamics/preamble/{module_name}.h" diff --git a/python/templates/pyproject.toml.in b/python/templates/pyproject.toml.in new file mode 100644 index 000000000..bf6077e0f --- /dev/null +++ b/python/templates/pyproject.toml.in @@ -0,0 +1,49 @@ +[project] +name = "${PROJECT_NAME}" +version = "${PROJECT_VERSION}" +description = "${PROJECT_DESCRIPTION}" +authors = [ + { name = "Varun Agrawal", email = "varunagrawal@gatech.edu" }, + { name="Frank Dellaert", email="dellaert@gatech.edu" } +] +readme = "${PROJECT_SOURCE_DIR}/README.md" +license = "BSD-3-Clause" +keywords=["robotics", "kinematics", "dynamics", "factor graphs"] + +classifiers = [ + "Intended Audience :: Education", + "Intended Audience :: Developers", + "Intended Audience :: Science/Research", + "Operating System :: MacOS", + "Operating System :: Microsoft :: Windows", + "Operating System :: POSIX :: Linux", + "Programming Language :: Python", +] + +requires-python = ">=3.10" +dependencies = [ + "numpy>=2.0.0", + "gtsam>=4.2", +] + +[dependency-groups] +dev = [ + "pytest>=9.0.1", + "pytest-cov>=7.0.0", +] + +[project.urls] +Homepage = "https://github.com/borglab/GTDynamics" +Documentation = "https://github.com/borglab/GTDynamics" +Repository = "https://github.com/borglab/GTDynamics" + +[build-system] +requires = ["hatchling"] +build-backend = "hatchling.build" + +[tool.pytest.ini_options] +addopts = "--cov=gtdynamics --cov-report html --cov-report term" +norecursedirs = [] + +[tool.uv.build-backend] +module-name = "gtdynamics" diff --git a/python/templates/setup.py.in b/python/templates/setup.py.in deleted file mode 100644 index cf397bde4..000000000 --- a/python/templates/setup.py.in +++ /dev/null @@ -1,57 +0,0 @@ -import os -import sys - -try: - from setuptools import setup, find_packages -except ImportError: - from distutils.core import setup, find_packages - -packages = find_packages() - -package_data = { - package: [ - f - for f in os.listdir(package.replace(".", os.path.sep)) - if os.path.splitext(f)[1] in (".so", ".pyd") - ] - for package in packages -} - -dependency_list = open("${PYTHON_REQUIREMENTS_PATH}").read().split('\n') -dependencies = [x for x in dependency_list if x[0] != '#'] - -setup( - name='${PROJECT_NAME}', - description='${PROJECT_DESCRIPTION}', - url='${PROJECT_HOMEPAGE_URL}', - version='${PROJECT_VERSION}', - author="${PROJECT_AUTHOR}", - author_email="${PROJECT_AUTHOR_EMAIL}", - license='Simplified BSD license', - keywords="", - long_description=open("${PROJECT_SOURCE_DIR}/README.md").read(), - long_description_content_type="text/markdown", - python_requires=">=3.6", - # https://pypi.org/pypi?%3Aaction=list_classifiers - classifiers=[ - 'Development Status :: 5 - Production/Stable', - 'Intended Audience :: Education', - 'Intended Audience :: Developers', - 'Intended Audience :: Science/Research', - 'Operating System :: MacOS', - 'Operating System :: Microsoft :: Windows', - 'Operating System :: POSIX', - 'License :: OSI Approved :: BSD License', - 'Programming Language :: Python :: 2', - 'Programming Language :: Python :: 3', - ], - packages=packages, - # Load the built shared object files - package_data=package_data, - include_package_data=True, - test_suite="${PYTHON_TESTS}", - # Ensure that the compiled .so file is properly packaged - zip_safe=False, - platforms="any", - install_requires=dependencies, -) diff --git a/python/tests/test_chain.py b/python/tests/test_chain.py index d5068cd56..3b8972486 100644 --- a/python/tests/test_chain.py +++ b/python/tests/test_chain.py @@ -12,12 +12,13 @@ import unittest from pathlib import Path -import gtdynamics as gtd import numpy as np from gtsam import Point3, Pose3, Rot3, Values from gtsam.utils.test_case import GtsamTestCase from prototype.chain import Chain +import gtdynamics as gtd + def axis(*A): """Make n*1 axis from list""" @@ -36,10 +37,10 @@ def test_one_link(self): # FK at rest self.gtsamAssertEquals(joint1.poe([0]), sMb) - sTb = Pose3(Rot3.Rz(np.pi/2), Point3(0, 5, 0)) + sTb = Pose3(Rot3.Rz(np.pi / 2), Point3(0, 5, 0)) # FK not at rest - q = [np.pi/2] + q = [np.pi / 2] self.gtsamAssertEquals(joint1.poe(q), sTb) J = np.zeros((6, 1)) self.gtsamAssertEquals(joint1.poe(q, J=J), sTb) @@ -56,10 +57,8 @@ def test_three_link(self): sM3, J3 = three_links.spec() expected = Pose3(Rot3(), Point3(15, 0, 0)) self.gtsamAssertEquals(sM3, expected) - expected_J3 = np.array( - [[0, 0, 1, 0, 15, 0], - [0, 0, 1, 0, 10, 0], - [0, 0, 1, 0, 5, 0]]).transpose() + expected_J3 = np.array([[0, 0, 1, 0, 15, 0], [0, 0, 1, 0, 10, 0], + [0, 0, 1, 0, 5, 0]]).transpose() np.testing.assert_allclose(J3, expected_J3) # FK at rest @@ -67,15 +66,13 @@ def test_three_link(self): self.gtsamAssertEquals(three_links.poe([0, 0, 0]), expected) # FK not at rest - q = np.array([0, 0, np.pi/2]) - sT3 = Pose3(Rot3.Rz(np.pi/2), Point3(10, 5, 0)) + q = np.array([0, 0, np.pi / 2]) + sT3 = Pose3(Rot3.Rz(np.pi / 2), Point3(10, 5, 0)) self.gtsamAssertEquals(three_links.poe(q), sT3) expected_J1 = sT3.inverse().Adjoint(axis(0, 0, 1, 0, 0, 0)) np.testing.assert_allclose(expected_J1, [0, 0, 1, 10, 5, 0]) expected_J = np.array( - [expected_J1, - [0, 0, 1, 5, 5, 0], - [0, 0, 1, 0, 5, 0]]).transpose() + [expected_J1, [0, 0, 1, 5, 5, 0], [0, 0, 1, 0, 5, 0]]).transpose() J = np.zeros((6, 3)) self.gtsamAssertEquals(three_links.poe(q, J=J), sT3) np.testing.assert_allclose(J, expected_J) @@ -97,7 +94,7 @@ def setUp(self): # Create chain sub-system self.chain = Chain.from_robot(ROBOT, BASE_NAME) - @ staticmethod + @staticmethod def JointAngles(q: list): """Create Values with joint angles.""" joint_angles = Values() @@ -112,14 +109,11 @@ def test_panda_FK(self): self.assertTrue(ROBOT.link("link0").isFixed()) # Check FK at rest, Conventional FK with GTSAM. - joint_angles = self.JointAngles(np.zeros((7,))) + joint_angles = self.JointAngles(np.zeros((7, ))) fk = ROBOT.forwardKinematics(joint_angles, 0, BASE_NAME) # Use this to print: fk.print("fk", gtd.GTDKeyFormatter) - sR7 = Rot3([ - [1, 0, 0], - [0, -1, 0], - [0, 0, -1] - ]) + sR7 = Rot3([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) + #regression expected_sM7 = Pose3(sR7, Point3(0.0882972, 0.00213401, 0.933844)) actual_sM7 = gtd.Pose(fk, 7) self.gtsamAssertEquals(actual_sM7, expected_sM7, tol=1e-3) @@ -130,14 +124,14 @@ def test_panda_FK(self): # Check Panda twist in end-effector frame self.assertEqual(J7.shape, (6, 7)) - expected_J7 = np.array([ - [0, 0, -1, -0.002, -0.088, 0], - [0, -1, 0, 0.601, 0, 0.088], - [0, 0, -1, -0.002, -0.088, 0], - [0, 1, 0, -0.285, 0, -0.006], - [0, 0, -1, -0.002, -0.088, 0], - [0, 1, 0, 0.099, 0, -0.088], - [0, 0, 1, 0.002, 0, 0]]).transpose() + expected_J7 = np.array([[0, 0, -1, -0.002, -0.088, 0], + [0, -1, 0, 0.601, 0, 0.088], + [0, 0, -1, -0.002, -0.088, 0], + [0, 1, 0, -0.285, 0, -0.006], + [0, 0, -1, -0.002, -0.088, 0], + [0, 1, 0, 0.099, 0, -0.088], + [0, 0, 1, 0.002, 0, 0]]).transpose() + #regression np.testing.assert_allclose(J7, expected_J7, atol=0.001) def check_poe(self, q_list, fTe=None): @@ -170,7 +164,7 @@ def check_poe(self, q_list, fTe=None): q_list[6] += 0.00001 q = np.array(q_list) sT7_plus = self.chain.poe(q, fTe=fTe) - xi = sT7.logmap(sT7_plus)/0.00001 + xi = sT7.logmap(sT7_plus) / 0.00001 np.testing.assert_allclose(poe_J[:, 6], xi, atol=0.01) def test_forward_kinematics_at_rest(self): @@ -184,16 +178,16 @@ def test_forward_kinematics_at_rest_with_offset(self): def test_forward_kinematics_joint0(self): """Test forward kinematics with non-zero joint0 angle.""" - self.check_poe([np.pi/2, 0, 0, 0, 0, 0, 0]) + self.check_poe([np.pi / 2, 0, 0, 0, 0, 0, 0]) def test_forward_kinematics_joint0_with_offset(self): """FK with non-zero joint0, with offset.""" fTe = Pose3(Rot3.Ry(0.1), Point3(1, 2, 3)) - self.check_poe([np.pi/2, 0, 0, 0, 0, 0, 0], fTe) + self.check_poe([np.pi / 2, 0, 0, 0, 0, 0, 0], fTe) def test_forward_kinematics_middle(self): """Test forward kinematics with middle joint rotated.""" - self.check_poe([0, 0, 0, 0, np.pi/2, 0, 0]) + self.check_poe([0, 0, 0, 0, np.pi / 2, 0, 0]) def test_forward_kinematics_random(self): """Test forward kinematics with random configuration.""" diff --git a/python/tests/test_dynamics_graph.py b/python/tests/test_dynamics_graph.py index 5dd438d94..5322fba1b 100644 --- a/python/tests/test_dynamics_graph.py +++ b/python/tests/test_dynamics_graph.py @@ -44,7 +44,7 @@ def test_objective_factors(self): noise6 = gtsam.noiseModel.Unit.Create(6) graph = gtsam.NonlinearFactorGraph() graph.push_back(gtd.LinkObjectives(1, k=777)\ - .pose(gtsam.Pose3(), noise1)\ + .pose(gtsam.Pose3(), noise6)\ .twistAccel(np.zeros(6), noise6)) self.assertEqual(graph.size(), 2) graph.push_back(gtd.JointObjectives(2, 777)\ diff --git a/python/tests/test_factors.py b/python/tests/test_factors.py index 01680f0d6..b46e2b0de 100644 --- a/python/tests/test_factors.py +++ b/python/tests/test_factors.py @@ -12,9 +12,11 @@ import os.path as osp import unittest -import gtdynamics as gtd import gtsam import numpy as np +import numpy.testing as npt + +import gtdynamics as gtd class TestFactors(unittest.TestCase): @@ -22,10 +24,11 @@ class TestFactors(unittest.TestCase): Base class for testing various factors. Provides needed fixtures and common functions. """ + def setUp(self): self.k = 0 - self.wTp_key = gtd.PoseKey(0, self.k).key() - self.wTc_key = gtd.PoseKey(1, self.k).key() + self.wTp_key = gtd.PoseKey(0, self.k) + self.wTc_key = gtd.PoseKey(1, self.k) ROBOT_FILE = osp.join(gtd.SDF_PATH, "test", "simple_rr.sdf") self.robot = gtd.CreateRobotFromFile(str(ROBOT_FILE), "simple_rr_sdf") @@ -33,6 +36,7 @@ def setUp(self): class TestJointMeasurementFactor(TestFactors): """Test suite for various versions of the JointMeasurementFactor.""" + def test_revolute_joint_measurement_factor(self): """Test JointMeasurementFactor for revolute joint.""" factor = gtd.JointMeasurementFactor( @@ -40,3 +44,46 @@ def test_revolute_joint_measurement_factor(self): self.robot.joint("joint_1"), np.pi / 4, self.k) self.assertIsInstance(factor, gtd.JointMeasurementFactor) + + +class TestContactEqualityFactor(TestFactors): + """Test suite for various versions of the ContactEqualityFactor.""" + + def test_print(self): + """Test ContactEqualityFactor print.""" + point_on_link = gtd.PointOnLink(self.robot.link("link_1"), + gtsam.Point3(0, 0, 0)) + factor = gtd.ContactEqualityFactor( + point_on_link, gtsam.noiseModel.Isotropic.Sigma(3, 0.1), 0, 1) + + self.assertIsInstance(factor, gtd.ContactEqualityFactor) + + +class TestPointGoalFactor(TestFactors): + """Test suite for the PointGoalFactor.""" + + def setUp(self): + super().setUp() + self.noise_model = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + + ROBOT_FILE = osp.join(gtd.URDF_PATH, "test", "simple_urdf.urdf") + self.robot = gtd.CreateRobotFromFile(str(ROBOT_FILE), + "simple_urdf_urdf") + + def test_error(self): + """Test RevoluteJointMeasurementFactor.""" + point_com = np.asarray([0, 0, 1]) + goal_point = np.asarray([0, 0, 2]) + factor = gtd.PointGoalFactor(self.wTp_key, self.noise_model, point_com, + goal_point) + + values = gtsam.Values() + values.insert(self.wTp_key, self.robot.link("l1").bMcom()) + + npt.assert_array_almost_equal(np.zeros(3), + factor.unwhitenedError(values)) + + values.clear() + values.insert(self.wTp_key, self.robot.link("l2").bMcom()) + npt.assert_array_almost_equal(np.asarray([0, 0, 2]), + factor.unwhitenedError(values)) diff --git a/python/tests/test_forward_kinematics.py b/python/tests/test_forward_kinematics.py index 8c4572d32..25da50daf 100644 --- a/python/tests/test_forward_kinematics.py +++ b/python/tests/test_forward_kinematics.py @@ -13,12 +13,13 @@ # pylint: disable=no-name-in-module, import-error, no-member from pathlib import Path -import gtdynamics as gtd import gtsam import numpy as np from gtsam.symbol_shorthand import X from gtsam.utils.test_case import GtsamTestCase +import gtdynamics as gtd + class TestRobot(GtsamTestCase): """Tests related to specific robot config.""" @@ -55,7 +56,7 @@ def test_forward_kinematics_factor(self): end_link = self.robot.link(end_link_name) factor = gtd.ForwardKinematicsFactor( X(t), - gtd.PoseKey(end_link.id(), t).key(), # wTleg + gtd.PoseKey(end_link.id(), t), # wTleg self.robot, self.base_name, end_link_name, @@ -69,7 +70,7 @@ def test_forward_kinematics_factor(self): fk = self.robot.forwardKinematics(self.joint_angles, t, self.base_name) end_link_pose = gtd.Pose(fk, end_link.id(), t) - values.insert(gtd.PoseKey(end_link.id(), t).key(), end_link_pose) + values.insert(gtd.PoseKey(end_link.id(), t), end_link_pose) graph = gtsam.NonlinearFactorGraph() graph.push_back(factor) diff --git a/python/tests/test_four_bar.py b/python/tests/test_four_bar.py index 8381e859c..a27ebbc41 100644 --- a/python/tests/test_four_bar.py +++ b/python/tests/test_four_bar.py @@ -13,11 +13,12 @@ import unittest -import gtdynamics as gtd import gtsam import numpy as np from gtsam import Pose3, Rot3 +import gtdynamics as gtd + class TestFourBar(unittest.TestCase): """Create a 4-bar linkage manually and test it.""" @@ -94,11 +95,12 @@ def test_four_bar(self): graph.push_back(prior_graph) # construct init values and solve - init_values = gtd.ZeroValues(robot, 0, 0) + initializer = gtd.Initializer() + init_values = initializer.ZeroValues(robot, 0, 0) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, init_values) result = optimizer.optimize() - a1_key = gtd.JointAccelKey(1, 0).key() + a1_key = gtd.JointAccelKey(1, 0) a1 = result.atDouble(a1_key) self.assertAlmostEqual(a1, 0.125, 5) # regression. Show work! diff --git a/python/tests/test_link.py b/python/tests/test_link.py index 53ff7231d..538844b5c 100644 --- a/python/tests/test_link.py +++ b/python/tests/test_link.py @@ -13,14 +13,16 @@ import os.path as osp import unittest -import gtdynamics as gtd import numpy as np from gtsam import Point3, Pose3, Rot3 from gtsam.utils.test_case import GtsamTestCase +import gtdynamics as gtd + class TestLink(GtsamTestCase): """Tests for the Link class.""" + def setUp(self): """Set up the fixtures.""" # load example robot diff --git a/python/tests/test_panda.py b/python/tests/test_panda.py index d2f95c8f9..227c58ba2 100644 --- a/python/tests/test_panda.py +++ b/python/tests/test_panda.py @@ -9,14 +9,16 @@ * @author Frank Dellaert """ +# pylint: disable=no-name-in-module, import-error, no-member + import unittest from pathlib import Path -# pylint: disable=no-name-in-module, import-error, no-member -import gtdynamics as gtd from gtsam import Point3, Pose3, Rot3, Values from gtsam.utils.test_case import GtsamTestCase +import gtdynamics as gtd + class TestSerial(GtsamTestCase): """Test Franka Panda robot can be read and makes sense.""" @@ -41,10 +43,11 @@ def test_forward_kinematics(self): fk = self.robot.forwardKinematics(joint_angles, 0, self.base_name) # Use this to print: fk.print("fk", gtd.GTDKeyFormatter) sR7 = Rot3([ - [1, 0, 0], + [1, 0, 0], # [0, -1, 0], [0, 0, -1] ]) + # regression expected_sT7 = Pose3(sR7, Point3(0.129315, 0.00227401, 0.88387)) actual_sT7 = gtd.Pose(fk, 7) self.gtsamAssertEquals(actual_sT7, expected_sT7, tol=1e-3) diff --git a/python/tests/test_print.py b/python/tests/test_print.py index 2cc34ebec..959da8e1b 100755 --- a/python/tests/test_print.py +++ b/python/tests/test_print.py @@ -13,29 +13,31 @@ from io import StringIO from unittest.mock import patch -import gtdynamics as gtd import gtsam +import gtdynamics as gtd + class TestPrint(unittest.TestCase): """Test printing of keys.""" + def test_values(self): """Checks that printing Values uses the GTDKeyFormatter instead of gtsam's default""" v = gtd.Values() gtd.InsertJointAngle(v, 0, 1, 2) - self.assertTrue('q(0)1' in v.__repr__()) + self.assertTrue('q(0)1' in repr(v)) def test_nonlinear_factor_graph(self): """Checks that printing NonlinearFactorGraph uses the GTDKeyFormatter""" fg = gtd.NonlinearFactorGraph() fg.push_back( - gtd.MinTorqueFactor( - gtd.TorqueKey(0, 0).key(), gtsam.noiseModel.Unit.Create(1))) - self.assertTrue('T(0)0' in fg.__repr__()) + gtd.MinTorqueFactor(gtd.TorqueKey(0, 0), + gtsam.noiseModel.Unit.Create(1))) + self.assertTrue('T(0)0' in repr(fg)) def test_key_formatter(self): """Tests print method with various key formatters""" - torqueKey = gtd.TorqueKey(0, 0).key() + torqueKey = gtd.TorqueKey(0, 0) factor = gtd.MinTorqueFactor(torqueKey, gtsam.noiseModel.Unit.Create(1)) with patch('sys.stdout', new=StringIO()) as fake_out: @@ -44,13 +46,14 @@ def test_key_formatter(self): self.assertTrue('keys = { T(0)0 }' in fake_out.getvalue()) def myKeyFormatter(key): - return 'this is my key formatter {}'.format(key) + return f'this is my key formatter {key}' with patch('sys.stdout', new=StringIO()) as fake_out: factor.print('factor: ', myKeyFormatter) self.assertTrue('factor: min torque factor' in fake_out.getvalue()) - self.assertTrue('keys = {{ this is my key formatter {} }}'.format( - torqueKey) in fake_out.getvalue()) + self.assertTrue( + f'keys = {{ this is my key formatter {torqueKey} }}' in + fake_out.getvalue()) if __name__ == "__main__": diff --git a/python/tests/test_robot.py b/python/tests/test_robot.py index 590bb186e..8db4b488d 100644 --- a/python/tests/test_robot.py +++ b/python/tests/test_robot.py @@ -13,19 +13,30 @@ import os.path as osp import unittest -import gtdynamics as gtd import numpy as np from gtsam import Point3, Pose3, Rot3 from gtsam.utils.test_case import GtsamTestCase +import gtdynamics as gtd + class TestRobot(GtsamTestCase): """Tests for the Robot class.""" + def setUp(self): """Set up the fixtures.""" # load example robot self.ROBOT_MODEL = osp.join(gtd.URDF_PATH, "a1", "a1.urdf") + def test_fixed_joint(self): + """Test if the fixed joint is parsed correctly.""" + robot = gtd.CreateRobotFromFile(self.ROBOT_MODEL, "", True) + # Try to get the fixed links + self.assertIsNotNone(robot.link("FR_toe")) + self.assertIsNotNone(robot.link("FL_toe")) + self.assertIsNotNone(robot.link("RR_toe")) + self.assertIsNotNone(robot.link("RL_toe")) + def test_forward_kinematics(self): """Test if FK is correct via comparison to a 3rd party library.""" robot = gtd.CreateRobotFromFile(self.ROBOT_MODEL) @@ -58,32 +69,28 @@ def test_forward_kinematics(self): # Dict of link-transforms where the transform is # from the link frame to the link CoM frame. lTcom_adjustments = { - "FL_hip": - Pose3(Rot3(), Point3(-0.003311, 0.000635, 3.1e-05)), - "FL_lower": - Pose3(Rot3(), Point3(0.006435, 0.0, -0.107388)).compose(lowerTcom), - "FL_upper": - Pose3(Rot3(), Point3(-0.003237, -0.022327, -0.027326)), - "FR_hip": - Pose3(Rot3(), Point3(-0.003311, -0.000635, 3.1e-05)), - "FR_lower": - Pose3(Rot3(), Point3(0.006435, 0.0, -0.107388)).compose(lowerTcom), - "FR_upper": - Pose3(Rot3(), Point3(-0.003237, 0.022327, -0.027326)), - "RL_hip": - Pose3(Rot3(), Point3(0.003311, 0.000635, 3.1e-05)), - "RL_lower": - Pose3(Rot3(), Point3(0.006435, 0.0, -0.107388)).compose(lowerTcom), - "RL_upper": - Pose3(Rot3(), Point3(-0.003237, -0.022327, -0.027326)), - "RR_hip": - Pose3(Rot3(), Point3(0.003311, -0.000635, 3.1e-05)), - "RR_lower": - Pose3(Rot3(), Point3(0.006435, 0.0, -0.107388)).compose(lowerTcom), - "RR_upper": - Pose3(Rot3(), Point3(-0.003237, 0.022327, -0.027326)), - "trunk": - Pose3(Rot3(), Point3(0.012731, 0.002186, 0.000515)), + "FL_hip": Pose3(Rot3(), Point3(-0.003311, 0.000635, 3.1e-05)), + "FL_lower": Pose3(Rot3(), Point3(0.006435, 0.0, -0.107388)), + "FL_upper": Pose3(Rot3(), Point3(-0.003237, -0.022327, -0.027326)), + "FL_toe": Pose3(Rot3(), Point3(0, 0, 0)), + "FL_upper_shoulder": Pose3(Rot3(), Point3(0, 0, 0)), + "FR_hip": Pose3(Rot3(), Point3(-0.003311, -0.000635, 3.1e-05)), + "FR_lower": Pose3(Rot3(), Point3(0.006435, 0.0, -0.107388)), + "FR_upper": Pose3(Rot3(), Point3(-0.003237, 0.022327, -0.027326)), + "FR_toe": Pose3(Rot3(), Point3(0, 0, 0)), + "FR_upper_shoulder": Pose3(Rot3(), Point3(0, 0, 0)), + "RL_hip": Pose3(Rot3(), Point3(0.003311, 0.000635, 3.1e-05)), + "RL_lower": Pose3(Rot3(), Point3(0.006435, 0.0, -0.107388)), + "RL_upper": Pose3(Rot3(), Point3(-0.003237, -0.022327, -0.027326)), + "RL_toe": Pose3(Rot3(), Point3(0, 0, 0)), + "RL_upper_shoulder": Pose3(Rot3(), Point3(0, 0, 0)), + "RR_hip": Pose3(Rot3(), Point3(0.003311, -0.000635, 3.1e-05)), + "RR_lower": Pose3(Rot3(), Point3(0.006435, 0.0, -0.107388)), + "RR_upper": Pose3(Rot3(), Point3(-0.003237, 0.022327, -0.027326)), + "RR_toe": Pose3(Rot3(), Point3(0, 0, 0)), + "RR_upper_shoulder": Pose3(Rot3(), Point3(0, 0, 0)), + "trunk": Pose3(Rot3(), Point3(0.012731, 0.002186, 0.000515)), + "imu_link": Pose3() } # True FK poses computed using kinpy @@ -97,6 +104,12 @@ def test_forward_kinematics(self): "FL_upper": Pose3(Rot3.Quaternion(1., 0., 0., 0.), (0.170269, 0.129864, -0.000515)), + "FL_toe": + Pose3(Rot3.Quaternion(1., 0., 0., 0.), + (0.170269, 0.129864, -0.400515)), + "FL_upper_shoulder": + Pose3(Rot3.Quaternion(1., 0., 0., 0.), + (0.170269, 0.125814, -0.000515)), "FR_hip": Pose3(Rot3.Quaternion(1., 0., 0., 0.), (0.170269, -0.049186, -0.000515)), @@ -106,6 +119,13 @@ def test_forward_kinematics(self): "FR_upper": Pose3(Rot3.Quaternion(0.99875026, 0.0, 0.04997917, 0.), (0.170269, -0.134236, -0.000515)), + "FR_toe": + Pose3( + Rot3.Quaternion(0.9987502603949663, 0., 0.049979169270678324, + 0.), (0.130336, -0.134236, -0.398517)), + "FR_upper_shoulder": + Pose3(Rot3.Quaternion(1., 0., 0., 0.), + (0.170269, -0.130186, -0.000515)), "RL_hip": Pose3(Rot3.Quaternion(1., 0., 0., 0.), (-0.195731, 0.044814, -0.000515)), @@ -115,6 +135,12 @@ def test_forward_kinematics(self): "RL_upper": Pose3(Rot3.Quaternion(1., 0., 0., 0.), (-0.195731, 0.129864, -0.000515)), + "RL_toe": + Pose3(Rot3.Quaternion(1., 0., 0., 0.), + (-0.195731, 0.129864, -0.400515)), + "RL_upper_shoulder": + Pose3(Rot3.Quaternion(1., 0., 0., 0.), + (-0.195731, 0.125814, -0.000515)), "RR_hip": Pose3(Rot3.Quaternion(1., 0., 0., 0.), (-0.195731, -0.049186, -0.000515)), @@ -124,19 +150,42 @@ def test_forward_kinematics(self): "RR_upper": Pose3(Rot3.Quaternion(1., 0., 0., 0.), (-0.195731, -0.134236, -0.000515)), + "RR_toe": + Pose3(Rot3.Quaternion(1., 0., 0., 0.), + (-0.195731, -0.134236, -0.400515)), + "RR_upper_shoulder": + Pose3(Rot3.Quaternion(1., 0., 0., 0.), + (-0.195731, -0.130186, -0.000515)), "trunk": Pose3(Rot3.Quaternion(1., 0., 0., 0.), (-0.012731, -0.002186, -0.000515)), + "imu_link": + Pose3(Rot3(), Point3(-0.012731, -0.002186, -0.000515)) } # Test all the links for link in robot.links(): lTcom = lTcom_adjustments[link.name()] + if "_lower" in link.name(): + lTcom = lTcom.compose(lowerTcom) + bTl = bTl_poses[link.name()] expected_bTcom = bTl.compose(lTcom) actual_bTcom = gtd.Pose(fk, link.id(), 0) self.gtsamAssertEquals(actual_bTcom, expected_bTcom, tol=1e-3) + def test_joint_type(self): + """Test if joint.type() works""" + robot = gtd.CreateRobotFromFile(self.ROBOT_MODEL, "", True) + + # Test Revolute joint + joint = robot.joint("FL_hip_joint") + self.assertEqual(joint.type(), gtd.Joint.Type.Revolute) + + # Test Fixed joint + imu_joint = robot.joint("imu_joint") + self.assertEqual(imu_joint.type(), gtd.Joint.Type.Fixed) + if __name__ == "__main__": unittest.main() diff --git a/scripts/CMakeLists.txt b/scripts/CMakeLists.txt deleted file mode 100644 index 1d0bcf72c..000000000 --- a/scripts/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -gtsamAddExamplesGlob("*.cpp" "" "gtdynamics") - diff --git a/scripts/nithya00_constrained_opt_benchmark.cpp b/scripts/nithya00_constrained_opt_benchmark.cpp deleted file mode 100644 index 100da4bad..000000000 --- a/scripts/nithya00_constrained_opt_benchmark.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file nithya_yetong00_constrainedopt_benchmark.cpp - * @brief Benchmark penalty method optimizer and augmented lagrangian optimizer - * on a toy example, and output intermediate results to file. - * @author: Nithya Jayakumar - * @author: Yetong Zhang - */ - -#include -#include - -#include "constrainedExample.h" -#include "gtdynamics/optimizer/AugmentedLagrangianOptimizer.h" -#include "gtdynamics/optimizer/EqualityConstraint.h" -#include "gtdynamics/optimizer/PenaltyMethodOptimizer.h" - -using namespace gtsam; -using namespace gtdynamics; - -int main(int argc, char** argv) { - using namespace constrained_example; - - /// Create a constrained optimization problem with 2 cost factors and 1 - /// constraint. - NonlinearFactorGraph graph; - gtsam::Double_ f1 = x1 + exp(-x2); - gtsam::Double_ f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; - auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); - graph.add(ExpressionFactor(cost_noise, 0., f1)); - graph.add(ExpressionFactor(cost_noise, 0., f2)); - - EqualityConstraints constraints; - double tolerance = 1.0; - gtsam::Double_ g1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); - constraints.push_back(EqualityConstraint::shared_ptr( - new DoubleExpressionEquality(g1, tolerance))); - - /// Create initial values. - Values init_values; - init_values.insert(x1_key, -0.2); - init_values.insert(x2_key, -0.2); - - /// Solve the constraint problem with Penalty Method optimizer. - gtdynamics::PenaltyMethodOptimizer penalty_optimizer; - gtdynamics::ConstrainedOptResult penalty_info; - Values penalty_results = - penalty_optimizer.optimize(graph, constraints, init_values, &penalty_info); - - /// Solve the constraint problem with Augmented Lagrangian optimizer. - gtdynamics::AugmentedLagrangianOptimizer augl_optimizer; - gtdynamics::ConstrainedOptResult augl_info; - Values augl_results = - augl_optimizer.optimize(graph, constraints, init_values, &augl_info); - - /// Function to evaluate constraint violation. - auto evaluate_constraint = [&constraints](const gtsam::Values& values) { - double violation = 0; - for (auto& constraint : constraints) { - violation += (*constraint)(values)(0); - } - return violation; - }; - - /// Function to evaluate cost. - auto evaluate_cost = [&graph](const gtsam::Values& values) { - double cost = graph.error(values); - return cost; - }; - - /// Write results to files for plotting. - std::ofstream penalty_file; - penalty_file.open("penalty_data.txt"); - for (size_t i = 0; i < penalty_info.num_iters.size(); i++) { - penalty_file << penalty_info.num_iters[i] << " " << penalty_info.mu_values[i] - << " " << evaluate_constraint(penalty_info.intermediate_values[i]) - << " " << evaluate_cost(penalty_info.intermediate_values[i]) << "\n"; - } - penalty_file.close(); - - std::ofstream augl_file; - augl_file.open("augl_data.txt"); - for (size_t i = 0; i < augl_info.num_iters.size(); i++) { - augl_file << augl_info.num_iters[i] << " " << augl_info.mu_values[i] << " " - << evaluate_constraint(augl_info.intermediate_values[i]) << " " - << evaluate_cost(augl_info.intermediate_values[i]) << "\n"; - } - augl_file.close(); - return 0; -} \ No newline at end of file diff --git a/tests/ManifoldOptScenarios.h b/tests/ManifoldOptScenarios.h new file mode 100644 index 000000000..1cc69b90c --- /dev/null +++ b/tests/ManifoldOptScenarios.h @@ -0,0 +1,43 @@ +#include +#include +#include + +namespace gtsam { + +namespace so2_scenario { + +double dist_square_func(const double& x1, const double& x2, + gtsam::OptionalJacobian<1, 1> H1 = nullptr, + gtsam::OptionalJacobian<1, 1> H2 = nullptr) { + if (H1) *H1 << 2 * x1; + if (H2) *H2 << 2 * x2; + return x1 * x1 + x2 * x2; +} + +Key x1_key = 1; +Key x2_key = 2; +Double_ x1(x1_key); +Double_ x2(x2_key); +Double_ dist_square(dist_square_func, x1, x2); +Double_ dist_error = dist_square - Double_(1.0); + +std::shared_ptr get_constraints() { + auto constraints = std::make_shared(); + double tolerance = 1e-3; + constraints->emplace_shared>( + dist_error, 0.0, gtsam::Vector1(tolerance)); + return constraints; +} + +std::shared_ptr get_graph(const double& x1_val, + const double& x2_val) { + auto graph = std::make_shared(); + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + graph->addPrior(x1_key, x1_val, model); + graph->addPrior(x2_key, x2_val, model); + return graph; +} + +} // namespace so2_scenario + +} // namespace gtsam diff --git a/tests/constrainedExample.h b/tests/constrainedExample.h index 4ff924e1e..af793362e 100644 --- a/tests/constrainedExample.h +++ b/tests/constrainedExample.h @@ -18,23 +18,17 @@ #include #include #include -#include #include +#include +#include + -namespace gtdynamics { namespace constrained_example { -using gtsam::Double_; -using gtsam::Expression; -using gtsam::Key; -using gtsam::Symbol; -using gtsam::Vector1; -using gtsam::Vector2; -using gtsam::Vector2_; +using namespace gtsam; /// Exponential function e^x. -double exp_func(const double& x, - gtsam::OptionalJacobian<1, 1> H1 = boost::none) { +double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { double result = exp(x); if (H1) H1->setConstant(result); return result; @@ -52,7 +46,7 @@ class PowFunctor { PowFunctor(const double& c) : c_(c) {} double operator()(const double& x, - gtsam::OptionalJacobian<1, 1> H1 = boost::none) const { + gtsam::OptionalJacobian<1, 1> H1 = {}) const { if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); return pow(x, c_); } @@ -77,4 +71,64 @@ Double_ x1(x1_key), x2(x2_key); } // namespace constrained_example -} // namespace gtdynamics +/* ************************************************************************* */ +/** + * Constrained optimization example in L. Vandenberghe slides: + * https://www.seas.ucla.edu/~vandenbe/133B/lectures/nllseq.pdf + * f(x) = 0.5*||x1 + e^(-x2)||^2 + 0.5*||x1^2 + 2*x2 + 1||^2 + * h(x) = x1 + x1^3 + x2 + x2^2 + */ +namespace e_constrained_example { +using namespace constrained_example; +NonlinearFactorGraph GetCost() { + NonlinearFactorGraph graph; + auto f1 = x1 + exp(-x2); + auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; + auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); + graph.add(ExpressionFactor(cost_noise, 0., f1)); + graph.add(ExpressionFactor(cost_noise, 0., f2)); + return graph; +} + +gtsam::NonlinearEqualityConstraints GetConstraints() { + gtsam::NonlinearEqualityConstraints constraints; + Vector sigmas = Vector1(0.1); + auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); + constraints.push_back(gtsam::NonlinearEqualityConstraint::shared_ptr( + new gtsam::ExpressionEqualityConstraint(h1, 0.0, sigmas))); + return constraints; +} + +NonlinearFactorGraph cost = GetCost(); +gtsam::NonlinearEqualityConstraints constraints = GetConstraints(); +} // namespace e_constrained_example + +/* ************************************************************************* */ +/** + * Constrained optimization example with inequality constraints + * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 + * g(x) = 1 - x1^2 - x2^2 + */ +// NOTE(Frank): Commented out until CMCOPt is implemented. +// namespace i_constrained_example { +// using namespace constrained_example; +// NonlinearFactorGraph GetCost() { +// NonlinearFactorGraph graph; +// auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); +// graph.addPrior(x1_key, 1.0, cost_noise); +// graph.addPrior(x2_key, 1.0, cost_noise); +// return graph; +// } + +// gtsam::NonlinearInequalityConstraints GetIConstraints() { +// gtsam::NonlinearInequalityConstraints i_constraints; +// Double_ g1 = Double_(1.0) - x1 * x1 - x2 * x2; +// double tolerance = 0.2; +// i_constraints.emplace_shared(g1, tolerance); +// return i_constraints; +// } + +// NonlinearFactorGraph cost = GetCost(); +// gtsam::NonlinearEqualityConstraints e_constraints; +// gtsam::NonlinearInequalityConstraints i_constraints = GetIConstraints(); +// } // namespace i_constrained_example diff --git a/tests/gtsamConstrainedExample.h b/tests/gtsamConstrainedExample.h new file mode 100644 index 000000000..5d355f4e2 --- /dev/null +++ b/tests/gtsamConstrainedExample.h @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file gtsamConstrainedExample.h + * @brief Simple constrained optimization example using GTSAM constraints. + */ + +#pragma once + +#include +#include +#include + +namespace constrained_example { + +using namespace gtsam; + +/// Exponential function e^x. +inline double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { + double result = exp(x); + if (H1) H1->setConstant(result); + return result; +} + +/// Exponential expression e^x. +inline Double_ exp(const Double_& x) { return Double_(exp_func, x); } + +/// Pow functor used for pow function. +class PowFunctor { + private: + double c_; + + public: + explicit PowFunctor(const double& c) : c_(c) {} + + double operator()(const double& x, + gtsam::OptionalJacobian<1, 1> H1 = {}) const { + if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); + return pow(x, c_); + } +}; + +/// Pow function. +inline Double_ pow(const Double_& x, const double& c) { + PowFunctor pow_functor(c); + return Double_(pow_functor, x); +} + +/// Plus between Double expression and double. +inline Double_ operator+(const Double_& x, const double& d) { + return x + Double_(d); +} + +/// Negative sign operator. +inline Double_ operator-(const Double_& x) { return Double_(0.0) - x; } + +/// Keys for creating expressions. +inline Symbol x1_key('x', 1); +inline Symbol x2_key('x', 2); +inline Double_ x1(x1_key), x2(x2_key); + +} // namespace constrained_example + +namespace constrained_example1 { +using namespace constrained_example; + +inline NonlinearFactorGraph Cost() { + NonlinearFactorGraph graph; + auto f1 = x1 + exp(-x2); + auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; + auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); + graph.add(ExpressionFactor(cost_noise, 0., f1)); + graph.add(ExpressionFactor(cost_noise, 0., f2)); + return graph; +} + +inline NonlinearEqualityConstraints EqConstraints() { + NonlinearEqualityConstraints constraints; + auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); + constraints.emplace_shared>( + h1, 0.0, Vector1(1.0)); + return constraints; +} + +inline Values InitValues() { + Values values; + values.insert(x1_key, -0.2); + values.insert(x2_key, -0.2); + return values; +} + +inline Values OptimalValues() { + Values values; + values.insert(x1_key, 0.0); + values.insert(x2_key, 0.0); + return values; +} + +inline NonlinearFactorGraph costs = Cost(); +inline NonlinearEqualityConstraints eqConstraints = EqConstraints(); +inline NonlinearInequalityConstraints ineqConstraints; +inline Values init_values = InitValues(); +inline ConstrainedOptProblem problem(costs, eqConstraints, ineqConstraints); +inline Values optimal_values = OptimalValues(); + +} // namespace constrained_example1 diff --git a/tests/make_joint.h b/tests/make_joint.h index 818963d73..3f5cc53ea 100644 --- a/tests/make_joint.h +++ b/tests/make_joint.h @@ -13,12 +13,20 @@ #pragma once -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/universal_robot/HelicalJoint.h" +#include +#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 = boost::make_shared(Link(1, name, mass, inertia, bMcom, bMl)); - auto l2 = boost::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 boost::make_shared( - 1, "j1", bMj, l1, l2, jScrewAxis, joint_params); + 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/testAugmentedLagrangianOptimizer.cpp b/tests/testAugmentedLagrangianOptimizer.cpp index 6bab81567..1cef22cf1 100644 --- a/tests/testAugmentedLagrangianOptimizer.cpp +++ b/tests/testAugmentedLagrangianOptimizer.cpp @@ -13,51 +13,65 @@ */ #include +#include +#include #include "constrainedExample.h" -#include "gtdynamics/optimizer/AugmentedLagrangianOptimizer.h" -#include "gtdynamics/optimizer/EqualityConstraint.h" -using namespace gtdynamics; using namespace gtsam; -using gtsam::assert_equal; -using std::map; -using std::string; -TEST(AugmentedLagrangianOptimizer, ConstrainedExample) { - using namespace constrained_example; - /// Create a constrained optimization problem with 2 cost factors and 1 - /// constraint. - NonlinearFactorGraph graph; - auto f1 = x1 + exp(-x2); - auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; - auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); - graph.add(ExpressionFactor(cost_noise, 0., f1)); - graph.add(ExpressionFactor(cost_noise, 0., f2)); +/* ************************************************************************* */ +// NOTE(Frank): Commented out until CMCOPt is implemented. +// TEST(LagrangeDualFunction, IConstrainedExample) { +// using namespace i_constrained_example; +// { +// double mu_e = 0.2; +// double mu_i = 0.3; +// std::vector lambda_e; +// std::vector lambda_i{2.0}; +// double d = 1e-6; +// NonlinearFactorGraph dual_graph = +// AugmentedLagrangianOptimizer::LagrangeDualFunction( +// cost, e_constraints, i_constraints, mu_e, mu_i, lambda_e, lambda_i, +// d); - EqualityConstraints constraints; - double tolerance = 1.0; - auto g1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); - constraints.push_back(EqualityConstraint::shared_ptr( - new DoubleExpressionEquality(g1, tolerance))); +// Values values; +// values.insert(x1_key, -1.0); +// values.insert(x2_key, -1.0); +// double g = -1 / 0.2; +// double expected_error1 = cost.error(values) - 2.0 * g + +// 0.5 * mu_i * pow(g, 2) + 0.5 * d * pow(g, 2) + +// pow(2.0, 2) / (2 * d); +// EXPECT_DOUBLES_EQUAL(expected_error1, dual_graph.error(values), 1e-9); +// } +// } - /// Create initial values. - Values init_values; - init_values.insert(x1_key, -0.2); - init_values.insert(x2_key, -0.2); +/* ************************************************************************* */ +// NOTE(Frank): Commented out until CMCOPt is implemented. +// TEST(AugmentedLagrangianOptimizer, IEConstrainedExample) { +// using namespace i_constrained_example; - /// Solve the constraint problem with Augmented Lagrangian optimizer. - gtdynamics::AugmentedLagrangianOptimizer optimizer; - Values results = optimizer.optimize(graph, constraints, init_values); +// /// Create initial values. +// Values init_values; +// init_values.insert(x1_key, 0.0); +// init_values.insert(x2_key, 0.0); - /// Check the result is correct within tolerance. - Values gt_results; - gt_results.insert(x1_key, 0.0); - gt_results.insert(x2_key, 0.0); - double tol = 1e-4; - EXPECT(assert_equal(gt_results, results, tol)); -} +// /// Solve the constraint problem with Augmented Lagrangian optimizer. +// gtsam::AugmentedLagrangianOptimizer optimizer; +// Values results = +// optimizer.optimize(cost, e_constraints, i_constraints, init_values); + +// /// Check the result is correct within tolerance. +// Values gt_results; +// gt_results.insert(x1_key, sqrt(2) / 2); +// gt_results.insert(x2_key, sqrt(2) / 2); +// double tol = 1e-4; +// EXPECT( +// assert_equal(gt_results.atDouble(x1_key), results.atDouble(x1_key), tol)); +// EXPECT( +// assert_equal(gt_results.atDouble(x2_key), results.atDouble(x2_key), tol)); +// } int main() { TestResult tr; diff --git a/tests/testChain.cpp b/tests/testChain.cpp index 1743c1eef..05b9ee718 100644 --- a/tests/testChain.cpp +++ b/tests/testChain.cpp @@ -12,10 +12,19 @@ */ #include +#include +#include +#include +#include +#include +#include +#include #include +#include +#include #include - -#include "gtdynamics/dynamics/Chain.h" +#include +#include using namespace gtdynamics; using gtsam::assert_equal; @@ -23,6 +32,11 @@ using gtsam::Matrix; using gtsam::Point3; using gtsam::Pose3; using gtsam::Rot3; +using gtsam::Vector3; +using gtsam::Vector6; +using std::placeholders::_1; +using std::placeholders::_2; +using std::placeholders::_3; // Test Chain class functionality with a three-joint chain - compose method TEST(Chain, ThreeLinksComposeMethod) { @@ -74,7 +88,7 @@ TEST(Chain, ThreeLinksPoeRest) { // Check poe for FK at rest (with jacobian) Matrix J0; - POE = composed.poe(joint_angles, boost::none, J0); + POE = composed.poe(joint_angles, {}, J0); EXPECT(assert_equal(POE, expected, 1e-6)); EXPECT(assert_equal(J0, expected_J, 1e-6)); } @@ -103,7 +117,7 @@ TEST(Chain, ThreeLinksPoeNotRest) { Vector joint_angles1(3); joint_angles1 << 0, 0, M_PI / 2; Matrix J1; - Pose3 POE1 = composed.poe(joint_angles1, boost::none, J1); + Pose3 POE1 = composed.poe(joint_angles1, {}, J1); EXPECT(assert_equal(POE1, expected_not_rest, 1e-6)); EXPECT(assert_equal(J1, expected_J1, 1e-6)); } @@ -130,6 +144,1350 @@ TEST(Chain, InitChain) { THROWS_EXCEPTION(Chain(sMb, screwAxis5)); } +// Test Chain Constraint +TEST(Chain, ChainConstraint) { + // Get three link robot + Robot robot = CreateRobotFromFile( + kSdfPath + std::string("test/simple_rrr.sdf"), "simple_rrr_sdf"); + + // Create a single link chain using the robot joints + std::vector chains; + for (auto&& joint : robot.joints()) { + Chain single_link_chain(joint->pMc(), joint->cScrewAxis()); + chains.emplace_back(single_link_chain); + } + + // Compose chains + Chain composed = Chain::compose(chains); + Pose3 expected_sMb = Pose3(Rot3(), Point3(0, 0, 1.6)); + Matrix expected_axes(6, 3); + expected_axes << 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0.9, 0.3, 0, 0, 0, 0, 0, 0; + EXPECT(assert_equal(composed.sMb(), expected_sMb, 1e-6)); + EXPECT(assert_equal(composed.axes(), expected_axes, 1e-6)); + + // Get key for wrench at joint 1 on link 0 at time 0 + const gtsam::Key wrench_key = gtdynamics::WrenchKey(0, 1, 0); + + // Get expression for chain constraint using 3 link chain at time 0 + auto expression = composed.ChainConstraint3(robot.joints(), wrench_key, 0); + + // Create initial values. + gtsam::Values init_values; + for (auto&& joint : robot.joints()) { + InsertJointAngle(&init_values, joint->id(), 0, 0.0); + InsertTorque(&init_values, joint->id(), 0, 0.0); + } + + // Set initial values for wrench + gtsam::Vector wrench(6); + wrench << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + InsertWrench(&init_values, 0, 1, 0, wrench); + + // Set tolerance + Vector3 tolerance(0.1, 0.1, 0.1); + + // Create equality constraint + gtsam::ExpressionEqualityConstraint constraint( + expression, gtsam::Vector3::Zero(), tolerance); + Vector3 expected_values(-1, -0.3, 0.3); // regression + Vector values = constraint.unwhitenedError(init_values); + EXPECT(assert_equal(values, expected_values, 1e-6)); + + // Create Factor same as in Optimizer for SOFT_CONSTRAINT + auto factor = constraint.penaltyFactor(1.0); + + // Check error + auto error = factor->unwhitenedError(init_values); + EXPECT(assert_equal(error, expected_values, 1e-6)); +} + +// Test Chain Constraint Jacobians - MakeVector3 +TEST(Chain, MakeVector3Jacobians) { + Matrix J0, J1, J2; + MakeVector3(17.0, 18.0, 19.0, J0, {}, {}); + MakeVector3(0.0, 0.0, 207.34567, {}, J1, {}); + MakeVector3(-9.0, -18.0, -1.0, {}, {}, J2); + + // binded function for numerical derivative + auto f = std::bind(MakeVector3, _1, _2, _3, nullptr, nullptr, nullptr); + + auto numericalH0 = + gtsam::numericalDerivative31(f, 17.0, + 18.0, 19.0); + + auto numericalH1 = + gtsam::numericalDerivative32(f, 0.0, 0.0, + 207.34567); + + auto numericalH2 = + gtsam::numericalDerivative33( + f, -9.0, -18.0, -1.0); + + EXPECT(assert_equal(numericalH0, J0)); + EXPECT(assert_equal(numericalH1, J1)); + EXPECT(assert_equal(numericalH2, J2)); +} + +Matrix get_angles_test_cases() { + Matrix angles_cases(10, 3); + angles_cases << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.5, 0.5, M_PI / 2, 0.0, + 0.0, 0.0, M_PI / 8, 0.0, 0.0, 0.0, M_PI / 14, M_PI / 14, M_PI / 7, 0.0, + M_PI / 14, 0.0, M_PI / 7, 0.0, M_PI / 14, M_PI / 7, 0.0, M_PI / 14, + M_PI / 7; + return angles_cases; +} + +Matrix get_torques_test_cases() { + Matrix torques_cases(10, 3); + torques_cases << 0.0, 0.0, 0.0, 100.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0; + return torques_cases; +} + +Matrix get_wrench_test_cases() { + Matrix wrench_cases(10, 6); + wrench_cases << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, + 0.0, 0.0, 1.0; + return wrench_cases; +} + +// Test Chain Constraint Jacobians - DynamicalEquality3 - H_angles - first type +// of chain +TEST(Chain, DynamicalEquality3_H_angles_chain1) { + // Initialize pose and screw axis for chain instantiation + Pose3 sMb = Pose3(Rot3(), Point3(1, 1, 1)); + Matrix screwAxis(6, 1); + screwAxis << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0; + + // Instantiate chains and create a vector + Chain joint1(sMb, screwAxis), joint2(sMb, screwAxis), joint3(sMb, screwAxis); + std::vector chains{joint1, joint2, joint3}; + + // Compose chains + Chain composed = Chain::compose(chains); + + Vector3 angles, torques; + gtsam::Vector wrench(6); + Matrix J1, J; + + // binded function for numerical derivative + auto f = std::bind(&Chain::DynamicalEquality3, composed, _1, _2, _3, nullptr, + nullptr, nullptr); + + // lambda function to get numerical derivative + auto num_derivative = [&](Vector6 wrench, Vector3 angles, Vector3 torques) { + return gtsam::numericalDerivative32( + f, wrench, angles, torques); + }; + + // lambda function to get the Jacobian + auto get_jacobian = [&](Vector6 wrench, Vector3 angles, Vector3 torques) { + composed.DynamicalEquality3(wrench, angles, torques, {}, J1, {}); + return J1; + }; + + Matrix anglesMat = get_angles_test_cases(); + Matrix torquesMat = get_torques_test_cases(); + Matrix wrenchMat = get_wrench_test_cases(); + + for (int i = 0; i < anglesMat.rows(); ++i) { + angles = anglesMat.row(i); + torques = torquesMat.row(i); + wrench = wrenchMat.row(i); + + auto numericalH_case = num_derivative(wrench, angles, torques); + + J = get_jacobian(wrench, angles, torques); + + EXPECT(assert_equal(J, numericalH_case, 1e-5)); + } +} + +// Test Chain Constraint Jacobians -AdjointWrenchEquality3 - H_angles - first type +// of chain +TEST(Chain, AdjointWrenchEquality3_H_angles_chain1) { + // Initialize pose and screw axis for chain instantiation + Pose3 sMb = Pose3(Rot3(), Point3(1, 1, 1)); + Matrix screwAxis(6, 1); + screwAxis << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0; + + // Instantiate chains and create a vector + Chain joint1(sMb, screwAxis), joint2(sMb, screwAxis), joint3(sMb, screwAxis); + std::vector chains{joint1, joint2, joint3}; + + // Compose chains + Chain composed = Chain::compose(chains); + + Vector3 angles, torques; + gtsam::Vector wrench_body(6); + Matrix J1, J; + + // binded function for numerical derivative + auto f = std::bind(&Chain::AdjointWrenchEquality3, composed, _1, _2, + nullptr, nullptr); + + // lambda function to get numerical derivative + auto num_derivative = [&](Vector3 angles, Vector6 wrench_body) { + return gtsam::numericalDerivative21( + f, angles, wrench_body); + }; + + // lambda function to get the Jacobian + auto get_jacobian = [&](Vector3 angles, Vector6 wrench_body) { + composed.AdjointWrenchEquality3(angles, wrench_body, J1, nullptr); + return J1; + }; + + Matrix anglesMat = get_angles_test_cases(); + Matrix torquesMat = get_torques_test_cases(); + Matrix wrenchMat = get_wrench_test_cases(); + + for (int i = 0; i < anglesMat.rows(); ++i) { + angles = anglesMat.row(i); + wrench_body = wrenchMat.row(i); + + auto numericalH_case = num_derivative(angles, wrench_body); + + J = get_jacobian(angles, wrench_body); + + EXPECT(assert_equal(J, numericalH_case, 1e-5)); + } +} + +// Test Chain Constraint Jacobians -AdjointWrenchEquality3 - H_angles - second type +// of chain +TEST(Chain, AdjointWrenchEquality3_H_angles_chain2) { + // Initialize pose and screw axis for chain instantiation + Pose3 sMb = Pose3(Rot3(), Point3(17.0, -3.0, 99.5)); + Matrix screwAxis(6, 1); + double one_over_sqrt_3 = 1 / sqrt(3); + screwAxis << one_over_sqrt_3, one_over_sqrt_3, one_over_sqrt_3, 1.5, 1.5, + 0.0; // size of omega should be 1 + + // Instantiate chains and create a vector + Chain joint1(sMb, screwAxis), joint2(sMb, screwAxis), joint3(sMb, screwAxis); + std::vector chains{joint1, joint2, joint3}; + + // Compose chains + Chain composed = Chain::compose(chains); + + Vector3 angles, torques; + gtsam::Vector wrench_body(6); + Matrix J1, J; + + // binded function for numerical derivative + auto f = std::bind(&Chain::AdjointWrenchEquality3, composed, _1, _2, + nullptr, nullptr); + + // lambda function to get numerical derivative + auto num_derivative = [&](Vector3 angles, Vector6 wrench_body) { + return gtsam::numericalDerivative21( + f, angles, wrench_body); + }; + + // lambda function to get the Jacobian + auto get_jacobian = [&](Vector3 angles, Vector6 wrench_body) { + composed.AdjointWrenchEquality3(angles, wrench_body, J1, nullptr); + return J1; + }; + + Matrix anglesMat = get_angles_test_cases(); + Matrix torquesMat = get_torques_test_cases(); + Matrix wrenchMat = get_wrench_test_cases(); + + for (int i = 0; i < anglesMat.rows(); ++i) { + angles = anglesMat.row(i); + wrench_body = wrenchMat.row(i); + + auto numericalH_case = num_derivative(angles, wrench_body); + + J = get_jacobian(angles, wrench_body); + + EXPECT(assert_equal(J, numericalH_case, 1e-5)); + } +} + +// Test Chain Constraint Jacobians -AdjointWrenchEquality3 - H_angles - third type +// of chain +TEST(Chain, AdjointWrenchEquality3_H_angles_chain3) { + // Initialize pose and screw axis for chain instantiation + Pose3 sMb = Pose3(Rot3(), Point3(0, 3, 8)); + Matrix screwAxis0(6, 1), screwAxis1(6, 1), screwAxis2(6, 1); + screwAxis0 << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0; + screwAxis1 << 0.0, 1.0, 0.0, 3.0, 0.0, 0.0; + screwAxis2 << 1.0, 0.0, 0.0, 0.0, 0.0, 29.0; + + // Instantiate chains and create a vector + Chain joint1(sMb, screwAxis0), joint2(sMb, screwAxis1), + joint3(sMb, screwAxis2); + std::vector chains{joint1, joint2, joint3}; + + // Compose chains + Chain composed = Chain::compose(chains); + + Vector3 angles, torques; + gtsam::Vector wrench_body(6); + Matrix J1, J; + + // binded function for numerical derivative + auto f = std::bind(&Chain::AdjointWrenchEquality3, composed, _1, _2, + nullptr, nullptr); + + // lambda function to get numerical derivative + auto num_derivative = [&](Vector3 angles, Vector6 wrench_body) { + return gtsam::numericalDerivative21( + f, angles, wrench_body); + }; + + // lambda function to get the Jacobian + auto get_jacobian = [&](Vector3 angles, Vector6 wrench_body) { + composed.AdjointWrenchEquality3(angles, wrench_body, J1, nullptr); + return J1; + }; + + Matrix anglesMat = get_angles_test_cases(); + Matrix torquesMat = get_torques_test_cases(); + Matrix wrenchMat = get_wrench_test_cases(); + + for (int i = 0; i < anglesMat.rows(); ++i) { + angles = anglesMat.row(i); + wrench_body = wrenchMat.row(i); + + auto numericalH_case = num_derivative(angles, wrench_body); + + J = get_jacobian(angles, wrench_body); + + EXPECT(assert_equal(J, numericalH_case, 1e-5)); + } +} + +// Test Chain Constraint Jacobians -PoeEquality3 - H_angles - first type +// of chain +TEST(Chain, PoeEquality3_H_angles_chain1) { + // Initialize pose and screw axis for chain instantiation + Pose3 sMb = Pose3(Rot3(), Point3(1, 1, 1)); + Matrix screwAxis(6, 1); + screwAxis << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0; + + // Instantiate chains and create a vector + Chain joint1(sMb, screwAxis), joint2(sMb, screwAxis), joint3(sMb, screwAxis); + std::vector chains{joint1, joint2, joint3}; + + // Compose chains + Chain composed = Chain::compose(chains); + + Vector3 angles, torques; + gtsam::Vector wrench_body(6); + Matrix J1, J; + + // binded function for numerical derivative + auto f = std::bind(&Chain::PoeEquality3, composed, _1, nullptr); + + // lambda function to get numerical derivative + auto num_derivative = [&](Vector3 angles) { + return gtsam::numericalDerivative11( + f, angles); + }; + + // lambda function to get the Jacobian + auto get_jacobian = [&](Vector3 angles) { + composed.PoeEquality3(angles, J1); + return J1; + }; + + Matrix anglesMat = get_angles_test_cases(); + + for (int i = 0; i < anglesMat.rows(); ++i) { + angles = anglesMat.row(i); + + auto numericalH_case = num_derivative(angles); + + J = get_jacobian(angles); + + EXPECT(assert_equal(J, numericalH_case, 1e-5)); + } +} + +// Test Chain Constraint Jacobians -PoeEquality3 - H_angles - second type +// of chain +TEST(Chain, PoeEquality3_H_angles_chain2) { + // Initialize pose and screw axis for chain instantiation + Pose3 sMb = Pose3(Rot3(), Point3(17.0, -3.0, 99.5)); + Matrix screwAxis(6, 1); + double one_over_sqrt_3 = 1 / sqrt(3); + screwAxis << one_over_sqrt_3, one_over_sqrt_3, one_over_sqrt_3, 1.5, 1.5, + 0.0; // size of omega should be 1 + + // Instantiate chains and create a vector + Chain joint1(sMb, screwAxis), joint2(sMb, screwAxis), joint3(sMb, screwAxis); + std::vector chains{joint1, joint2, joint3}; + + // Compose chains + Chain composed = Chain::compose(chains); + + Vector3 angles, torques; + gtsam::Vector wrench_body(6); + Matrix J1, J; + + // binded function for numerical derivative + auto f = std::bind(&Chain::PoeEquality3, composed, _1, nullptr); + + // lambda function to get numerical derivative + auto num_derivative = [&](Vector3 angles) { + return gtsam::numericalDerivative11( + f, angles); + }; + + // lambda function to get the Jacobian + auto get_jacobian = [&](Vector3 angles) { + composed.PoeEquality3(angles, J1); + return J1; + }; + + Matrix anglesMat = get_angles_test_cases(); + + for (int i = 0; i < anglesMat.rows(); ++i) { + angles = anglesMat.row(i); + + auto numericalH_case = num_derivative(angles); + + J = get_jacobian(angles); + + EXPECT(assert_equal(J, numericalH_case, 1e-5)); + } +} + +// Test Chain Constraint Jacobians -PoeEquality3 - H_angles - third type +// of chain +TEST(Chain, PoeEquality3_H_angles_chain3) { + // Initialize pose and screw axis for chain instantiation + Pose3 sMb = Pose3(Rot3(), Point3(0, 3, 8)); + Matrix screwAxis0(6, 1), screwAxis1(6, 1), screwAxis2(6, 1); + screwAxis0 << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0; + screwAxis1 << 0.0, 1.0, 0.0, 3.0, 0.0, 0.0; + screwAxis2 << 1.0, 0.0, 0.0, 0.0, 0.0, 29.0; + + // Instantiate chains and create a vector + Chain joint1(sMb, screwAxis0), joint2(sMb, screwAxis1), + joint3(sMb, screwAxis2); + std::vector chains{joint1, joint2, joint3}; + + // Compose chains + Chain composed = Chain::compose(chains); + + Vector3 angles, torques; + gtsam::Vector wrench_body(6); + Matrix J1, J; + + // binded function for numerical derivative + auto f = std::bind(&Chain::PoeEquality3, composed, _1, nullptr); + + // lambda function to get numerical derivative + auto num_derivative = [&](Vector3 angles) { + return gtsam::numericalDerivative11( + f, angles); + }; + + // lambda function to get the Jacobian + auto get_jacobian = [&](Vector3 angles) { + composed.PoeEquality3(angles, J1); + return J1; + }; + + Matrix anglesMat = get_angles_test_cases(); + + for (int i = 0; i < anglesMat.rows(); ++i) { + angles = anglesMat.row(i); + + auto numericalH_case = num_derivative(angles); + + J = get_jacobian(angles); + + EXPECT(assert_equal(J, numericalH_case, 1e-5)); + } +} + +// Test Chain Constraint Jacobians - DynamicalEquality3 - H_angles - second type +// of chain +TEST(Chain, DynamicalEquality3_H_angles_chain2) { + // Initialize pose and screw axis for chain instantiation + Pose3 sMb = Pose3(Rot3(), Point3(17.0, -3.0, 99.5)); + Matrix screwAxis(6, 1); + double one_over_sqrt_3 = 1 / sqrt(3); + screwAxis << one_over_sqrt_3, one_over_sqrt_3, one_over_sqrt_3, 1.5, 1.5, + 0.0; // size of omega should be 1 + + // Instantiate chains and create a vector + Chain joint1(sMb, screwAxis), joint2(sMb, screwAxis), joint3(sMb, screwAxis); + std::vector chains{joint1, joint2, joint3}; + + // Compose chains + Chain composed = Chain::compose(chains); + + Vector3 angles, torques; + gtsam::Vector wrench(6); + Matrix J1, J; + + // binded function for numerical derivative + auto f = std::bind(&Chain::DynamicalEquality3, composed, _1, _2, _3, nullptr, + nullptr, nullptr); + + // lambda function to get numerical derivative + auto num_derivative = [&](Vector6 wrench, Vector3 angles, Vector3 torques) { + return gtsam::numericalDerivative32( + f, wrench, angles, torques); + }; + + // lambda function to get the Jacobian + auto get_jacobian = [&](Vector6 wrench, Vector3 angles, Vector3 torques) { + composed.DynamicalEquality3(wrench, angles, torques, {}, J1, {}); + return J1; + }; + + Matrix anglesMat = get_angles_test_cases(); + Matrix torquesMat = get_torques_test_cases(); + Matrix wrenchMat = get_wrench_test_cases(); + + for (int i = 0; i < anglesMat.rows(); ++i) { + angles = anglesMat.row(i); + torques = torquesMat.row(i); + wrench = wrenchMat.row(i); + + auto numericalH_case = num_derivative(wrench, angles, torques); + + J = get_jacobian(wrench, angles, torques); + + EXPECT(assert_equal(J, numericalH_case, 1e-5)); + } +} + +// Test Chain Constraint Jacobians - DynamicalEquality3 - H_angles - third type +// of chain +TEST(Chain, DynamicalEquality3_H_angles_chain3) { + // Initialize pose and screw axis for chain instantiation + Pose3 sMb = Pose3(Rot3(), Point3(0, 3, 8)); + Matrix screwAxis0(6, 1), screwAxis1(6, 1), screwAxis2(6, 1); + screwAxis0 << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0; + screwAxis1 << 0.0, 1.0, 0.0, 3.0, 0.0, 0.0; + screwAxis2 << 1.0, 0.0, 0.0, 0.0, 0.0, 29.0; + + // Instantiate chains and create a vector + Chain joint1(sMb, screwAxis0), joint2(sMb, screwAxis1), + joint3(sMb, screwAxis2); + std::vector chains{joint1, joint2, joint3}; + + // Compose chains + Chain composed = Chain::compose(chains); + + Vector3 angles, torques; + gtsam::Vector wrench(6); + Matrix J1, J; + + // binded function for numerical derivative + auto f = std::bind(&Chain::DynamicalEquality3, composed, _1, _2, _3, nullptr, + nullptr, nullptr); + + // lambda function to get numerical derivative + auto num_derivative = [&](Vector6 wrench, Vector3 angles, Vector3 torques) { + return gtsam::numericalDerivative32( + f, wrench, angles, torques); + }; + + // lambda function to get the Jacobian + auto get_jacobian = [&](Vector6 wrench, Vector3 angles, Vector3 torques) { + composed.DynamicalEquality3(wrench, angles, torques, {}, J1, {}); + return J1; + }; + + Matrix anglesMat = get_angles_test_cases(); + Matrix torquesMat = get_torques_test_cases(); + Matrix wrenchMat = get_wrench_test_cases(); + + for (int i = 0; i < anglesMat.rows(); ++i) { + angles = anglesMat.row(i); + torques = torquesMat.row(i); + wrench = wrenchMat.row(i); + + auto numericalH_case = num_derivative(wrench, angles, torques); + + J = get_jacobian(wrench, angles, torques); + + EXPECT(assert_equal(J, numericalH_case, 1e-5)); + } +} + +// Test Chain Constraint Jacobians - DynamicalEquality3 - H_wrench +TEST(Chain, DynamicalEquality3_H_wrench) { + // Initialize pose and screw axis for chain instantiation + Pose3 sMb = Pose3(Rot3(), Point3(0, 3, 8)); + Matrix screwAxis0(6, 1), screwAxis1(6, 1), screwAxis2(6, 1); + screwAxis0 << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0; + screwAxis1 << 0.0, 1.0, 0.0, 3.0, 0.0, 0.0; + screwAxis2 << 1.0, 0.0, 0.0, 0.0, 0.0, 29.0; + + // Instantiate chains and create a vector + Chain joint1(sMb, screwAxis0), joint2(sMb, screwAxis1), + joint3(sMb, screwAxis2); + std::vector chains{joint1, joint2, joint3}; + + // Compose chains + Chain composed = Chain::compose(chains); + + Vector3 angles, torques; + gtsam::Vector wrench(6); + Matrix J0, J; + + // binded function for numerical derivative + auto f = std::bind(&Chain::DynamicalEquality3, composed, _1, _2, _3, nullptr, + nullptr, nullptr); + + // lambda function to get numerical derivative + auto num_derivative = [&](Vector6 wrench, Vector3 angles, Vector3 torques) { + return gtsam::numericalDerivative31( + f, wrench, angles, torques); + }; + + // lambda function to get the Jacobian + auto get_jacobian = [&](Vector6 wrench, Vector3 angles, Vector3 torques) { + composed.DynamicalEquality3(wrench, angles, torques, J0, {}, {}); + return J0; + }; + + Matrix anglesMat = get_angles_test_cases(); + Matrix torquesMat = get_torques_test_cases(); + Matrix wrenchMat = get_wrench_test_cases(); + + for (int i = 0; i < anglesMat.rows(); ++i) { + angles = anglesMat.row(i); + torques = torquesMat.row(i); + wrench = wrenchMat.row(i); + + auto numericalH_case = num_derivative(wrench, angles, torques); + + J = get_jacobian(wrench, angles, torques); + + EXPECT(assert_equal(J, numericalH_case, 1e-5)); + } +} + +// Test Chain Constraint Jacobians - DynamicalEquality3 - H_torques +TEST(Chain, DynamicalEquality3_H_torques) { + // Initialize pose and screw axis for chain instantiation + Pose3 sMb = Pose3(Rot3(), Point3(0, 3, 8)); + Matrix screwAxis0(6, 1), screwAxis1(6, 1), screwAxis2(6, 1); + screwAxis0 << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0; + screwAxis1 << 0.0, 1.0, 0.0, 3.0, 0.0, 0.0; + screwAxis2 << 1.0, 0.0, 0.0, 0.0, 0.0, 29.0; + + // Instantiate chains and create a vector + Chain joint1(sMb, screwAxis0), joint2(sMb, screwAxis1), + joint3(sMb, screwAxis2); + std::vector chains{joint1, joint2, joint3}; + + // Compose chains + Chain composed = Chain::compose(chains); + + Vector3 angles, torques; + gtsam::Vector wrench(6); + Matrix J2, J; + + // binded function for numerical derivative + auto f = std::bind(&Chain::DynamicalEquality3, composed, _1, _2, _3, nullptr, + nullptr, nullptr); + + // lambda function to get numerical derivative + auto num_derivative = [&](Vector6 wrench, Vector3 angles, Vector3 torques) { + return gtsam::numericalDerivative33( + f, wrench, angles, torques); + }; + + // lambda function to get the Jacobian + auto get_jacobian = [&](Vector6 wrench, Vector3 angles, Vector3 torques) { + composed.DynamicalEquality3(wrench, angles, torques, {}, {}, J2); + return J2; + }; + + Matrix anglesMat = get_angles_test_cases(); + Matrix torquesMat = get_torques_test_cases(); + Matrix wrenchMat = get_wrench_test_cases(); + + for (int i = 0; i < anglesMat.rows(); ++i) { + angles = anglesMat.row(i); + torques = torquesMat.row(i); + wrench = wrenchMat.row(i); + + auto numericalH_case = num_derivative(wrench, angles, torques); + + J = get_jacobian(wrench, angles, torques); + + EXPECT(assert_equal(J, numericalH_case, 1e-5)); + } +} + +// Test Chain Constraint Factor Jacobians +TEST(Chain, ChainConstraintFactorJacobians) { + // Get three link robot + Robot robot = CreateRobotFromFile( + kSdfPath + std::string("test/simple_rrr.sdf"), "simple_rrr_sdf"); + + // Create a single link chain using the robot joints + std::vector chains; + for (auto&& joint : robot.joints()) { + Chain single_link_chain(joint->pMc(), joint->cScrewAxis()); + chains.emplace_back(single_link_chain); + } + + // Compose chains + Chain composed = Chain::compose(chains); + Pose3 expected_sMb = Pose3(Rot3(), Point3(0, 0, 1.6)); + Matrix expected_axes(6, 3); + expected_axes << 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0.9, 0.3, 0, 0, 0, 0, 0, 0; + EXPECT(assert_equal(composed.sMb(), expected_sMb, 1e-6)); + EXPECT(assert_equal(composed.axes(), expected_axes, 1e-6)); + + // Get key for wrench at joint 1 on link 0 at time 0 + const gtsam::Key wrench_key = gtdynamics::WrenchKey(0, 1, 0); + + // Get expression for chain constraint using 3 link chain at time 0 + auto expression = composed.ChainConstraint3(robot.joints(), wrench_key, 0); + + // Create initial values. + gtsam::Values init_values; + for (auto&& joint : robot.joints()) { + InsertJointAngle(&init_values, joint->id(), 0, 0.0); + InsertTorque(&init_values, joint->id(), 0, 0.0); + } + + // Set initial values for wrench + gtsam::Vector wrench(6); + wrench << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + InsertWrench(&init_values, 0, 1, 0, wrench); + + // Set tolerance + Vector3 tolerance(0.1, 0.1, 0.1); + + // Create equality constraint + gtsam::ExpressionEqualityConstraint constraint( + expression, gtsam::Vector3::Zero(), tolerance); + Vector3 expected_values(-1, -0.3, 0.3); // regression + Vector values = constraint.unwhitenedError(init_values); + EXPECT(assert_equal(values, expected_values, 1e-6)); + + // Create Factor same as in Optimizer for SOFT_CONSTRAINT + auto factor = constraint.penaltyFactor(1.0); + + // Check Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(*factor, init_values, 1e-7, 1e-3); +} + +TEST(Chain, A1QuadOneLegCompare) { + // This test checks equality between torque calculation with and without + // chains. This assumes one static leg of the a1 quadruped with zero mass for + // the links besides the trunk. + + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + // initialize joints of FL leg + JointSharedPtr j0, j1, j2; + + for (auto&& joint : robot.joints()) { + if (joint->name().find("FL") != std::string::npos) { + if (joint->name().find("lower") != std::string::npos) { + j2 = joint; + } + if (joint->name().find("upper") != std::string::npos) { + j1 = joint; + } + if (joint->name().find("hip") != std::string::npos) { + j0 = joint; + } + } + } + + // Calculate all relevant relative poses. + Pose3 M_T_H = j0->pMc(); + Pose3 M_H_T = M_T_H.inverse(); + Pose3 M_H_U = j1->pMc(); + Pose3 M_U_H = M_H_U.inverse(); + Pose3 M_U_L = j2->pMc(); + Pose3 M_L_U = M_U_L.inverse(); + Pose3 M_T_L = M_T_H * M_H_U * M_U_L; + Pose3 M_L_T = M_T_L.inverse(); + + // Set Gravity Wrench + Matrix gravity(1, 6); + gravity << 0.0, 0.0, 0.0, 0.0, 0.0, -10.0; + + // Calculate all wrenches and torques without chains. + Matrix F_2_L = -gravity * M_T_L.AdjointMap(); + Matrix F_1_U = F_2_L * M_L_U.AdjointMap(); + Matrix F_0_H = F_1_U * M_U_H.AdjointMap(); + + // joint 0 + Matrix tau0 = F_0_H * j0->cScrewAxis(); + EXPECT(assert_equal(tau0(0, 0), -0.448145, 1e-6)); // regression + + // joint 1 + Matrix tau1 = F_1_U * j1->cScrewAxis(); + EXPECT(assert_equal(tau1(0, 0), 1.70272, 1e-5)); // regression + + // joint 2 + Matrix tau2 = F_2_L * j2->cScrewAxis(); + EXPECT(assert_equal(tau2(0, 0), 1.70272, 1e-5)); // regression + + // Calculate all wrenches and torques with chains. + Chain chain1(M_T_H, j0->cScrewAxis()); + Chain chain2(M_H_U, j1->cScrewAxis()); + Chain chain3(M_U_L, j2->cScrewAxis()); + + std::vector chains{chain1, chain2, chain3}; + + // Compose Chains + Chain composed = Chain::compose(chains); + + // test for same values + Matrix torques = F_2_L * composed.axes(); + EXPECT(assert_equal(torques(0, 0), tau0(0, 0), 1e-6)); + EXPECT(assert_equal(torques(0, 1), tau1(0, 0), 1e-5)); + EXPECT(assert_equal(torques(0, 2), tau2(0, 0), 1e-5)); +} + +gtsam::Values OldGraphOneLeg() { + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + // Get trunk with one leg, only the trunk has mass + for (auto&& link : robot.links()) { + if (link->id() > 3) { + robot.removeLink(link); + } else if (link->name().find("trunk") == std::string::npos) { + link->setMass(0.0); + link->setInertia(gtsam::Matrix3::Zero()); + } else { + link->setMass(1.0); + link->setInertia(gtsam::Matrix3::Identity()); + } + } + + // Create Contact Point + std::vector lower_feet = {robot.link("FL_lower")}; + const Point3 contact_in_com(0, 0, -0.07); + auto stationary = + std::make_shared(lower_feet, contact_in_com); + auto contact_points = stationary->contactPoints(); + + gtsam::Vector3 gravity(0, 0, -10.0); + + OptimizerSetting opt(1e-4, 1e-4, 1e-4, 1e-4); + DynamicsGraph graph_builder(opt, gravity); + + gtsam::Vector6 wrench_zero = gtsam::Z_6x1; + + gtsam::NonlinearFactorGraph graph; + + // Need the following to use the wrench factor (Coriolis, Generalized + // Momentum, Gravity) + auto bp_cost_model(gtsam::noiseModel::Isotropic::Sigma(6, 1e-30)); + graph.addPrior(PoseKey(0, 0), robot.link("trunk")->bMcom(), bp_cost_model); + graph.addPrior(TwistKey(0, 0), wrench_zero, bp_cost_model); + graph.addPrior(TwistAccelKey(0, 0), wrench_zero, bp_cost_model); + + // Build dynamics factors + //graph.add(graph_builder.qFactors(robot, 0));//, contact_points)); + graph.add(graph_builder.dynamicsFactors(robot, 0, contact_points, 1.0)); + + // Initialize joint kinematics/dynamics to 0. + gtsam::Values init_vals; + for (auto&& joint : robot.joints()) { + int j = joint->id(); + InsertWrench(&init_vals, joint->parent()->id(), j, 0, wrench_zero); + InsertWrench(&init_vals, joint->child()->id(), j, 0, wrench_zero); + InsertTorque(&init_vals, j, 0, 0.0); + InsertJointAngle(&init_vals, j, 0, 0.0); + } + for (auto&& link : robot.links()) { + //graph.addPrior(PoseKey(link->id(), 0), link->bMcom(), bp_cost_model); + InsertTwist(&init_vals, link->id(), 0, wrench_zero); + InsertTwistAccel(&init_vals, link->id(), 0, wrench_zero); + InsertPose(&init_vals, link->id(), 0, link->bMcom()); + } + init_vals.insert(ContactWrenchKey(3, 0, 0), wrench_zero); + + // Constraint angles to zero + Optimizer optimizer; + gtsam::NonlinearEqualityConstraints eqs; + auto cost_model = gtsam::noiseModel::Unit::Create(1); + for (auto&& joint : robot.joints()) { + const int joint_id = joint->id(); + gtsam::Double_ angle(JointAngleKey(joint_id, 0)); + //eqs.emplace_shared>(angle, 0.0, 1e-1); + } + + /// Solve the constraint problem with LM optimizer. + gtsam::Values results = optimizer.optimize(graph, eqs, init_vals); + + return results; +} + +gtsam::Values NewGraphOneLeg() { + // This Graph uses chain constraints for one leg of the a1 robot, a wrench + // constraint on the trunk and zero angles at the joints constraints (robot + // standing still) + + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + // Get joint and composed chains for each leg + auto chain_joints = ChainDynamicsGraph::getChainJoints(robot); + auto composed_chains = ChainDynamicsGraph::getComposedChains(chain_joints); + + // Initialize Constraints + gtsam::NonlinearEqualityConstraints constraints; + + // Set Gravity Wrench + gtsam::Vector6 gravity; + gravity << 0.0, 0.0, 0.0, 0.0, 0.0, -10.0; + gtsam::Vector6_ gravity_wrench(gravity); + + // Create expression for wrench constraint on trunk + gtsam::Vector6_ trunk_wrench_constraint = gravity_wrench; + + // Set tolerances + double dynamicsTolerance = 6.2*(1e-5); + gtsam::Vector3 contact_tolerance = Vector3::Ones() * dynamicsTolerance; + gtsam::Vector6 wrench_tolerance = Vector6::Ones() * dynamicsTolerance; + + // Get key for wrench at hip joint with id 0 + const gtsam::Key wrench_key_0_T = gtdynamics::WrenchKey(0, 0, 0); + + // create expression for the wrench + gtsam::Vector6_ wrench_0_T(wrench_key_0_T); + + // add wrench to trunk constraint + trunk_wrench_constraint += wrench_0_T; + + // Add trunk wrench constraint to constraints + constraints.emplace_shared< + gtsam::ExpressionEqualityConstraint>( + trunk_wrench_constraint, gtsam::Vector6::Zero(), wrench_tolerance); + + // Get expression for chain on the leg + gtsam::Vector6_ wrench_end_effector = + composed_chains[0].AdjointWrenchConstraint3(chain_joints[0], wrench_key_0_T, 0); + + // Create contact constraint + Point3 contact_in_com(0.0, 0.0, -0.07); + gtsam::Vector3_ contact_constraint = ContactDynamicsMomentConstraint( + wrench_end_effector, gtsam::Pose3(gtsam::Rot3(), (-1) * contact_in_com)); + constraints.emplace_shared< + gtsam::ExpressionEqualityConstraint>( + contact_constraint, gtsam::Vector3::Zero(), contact_tolerance); + + // Create initial values. + gtsam::Values init_values; + for (auto&& joint : robot.joints()) { + if (joint->id() > 2) continue; + InsertJointAngle(&init_values, joint->id(), 0, 0.0); + } + gtsam::Vector6 wrench_zero = gtsam::Z_6x1; + init_values.insert(gtdynamics::WrenchKey(0, 0, 0), wrench_zero); + + /// Solve the constraint problem with LM optimizer. + gtsam::NonlinearFactorGraph graph; + Optimizer optimizer; + gtsam::Values results = optimizer.optimize(graph, constraints, init_values); + return results; +} + +TEST(Chain, oneLegCompareGraphsA1) { + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + // Get joint and composed chains for each leg + auto chain_joints = ChainDynamicsGraph::getChainJoints(robot); + + // calculate both ways + gtsam::Values new_graph_results = NewGraphOneLeg(); + gtsam::Values old_graph_results = OldGraphOneLeg(); + + // Get torque and angle results and compare + for (int i = 0; i < 3; ++i) { + double new_angle = + new_graph_results.at(gtdynamics::JointAngleKey(i)); + double old_angle = + old_graph_results.at(gtdynamics::JointAngleKey(i)); + EXPECT(assert_equal(new_angle, old_angle, 1e-4)); + } + + // Get new angles + double new_angle0 = + new_graph_results.at(gtdynamics::JointAngleKey(0)); + double new_angle1 = + new_graph_results.at(gtdynamics::JointAngleKey(1)); + double new_angle2 = + new_graph_results.at(gtdynamics::JointAngleKey(2)); + + // Get wrench keys + const gtsam::Key wrench_key_0_T = gtdynamics::WrenchKey(0, 0, 0); + const gtsam::Key contact_wrench_key = gtdynamics::ContactWrenchKey(3, 0, 0); + + // Get result wrench for hip joints and compare + gtsam::Vector6 wrench_new_0_T = + new_graph_results.at(wrench_key_0_T); + gtsam::Vector6 wrench_old_0_T = + old_graph_results.at(wrench_key_0_T); + EXPECT(assert_equal(wrench_new_0_T, wrench_old_0_T, 1e-10)); + + // Get result wrench for lower link and compare + Pose3 trunkThip = chain_joints[0][0]->parentTchild(new_angle0); + Pose3 hipTupper = chain_joints[0][1]->parentTchild(new_angle1); + Pose3 upperTlower = chain_joints[0][2]->parentTchild(new_angle2); + Pose3 trunkTlower = trunkThip * hipTupper * upperTlower; + gtsam::Vector6 wrench_new_3_L = + wrench_new_0_T.transpose() * trunkTlower.AdjointMap(); + gtsam::Vector6 wrench_old_3_L = + old_graph_results.at(contact_wrench_key); + EXPECT(assert_equal(wrench_new_3_L, wrench_old_3_L, 1e-3)); + + // calculate wrench on ground and compare + Point3 contact_in_com(0.0, 0.0, -0.07); + Pose3 M_L_G = Pose3(Rot3(), contact_in_com); + gtsam::Vector6 wrench_new_3_G = + wrench_new_3_L.transpose() * M_L_G.AdjointMap(); + gtsam::Vector6 wrench_old_3_G = + wrench_old_3_L.transpose() * M_L_G.AdjointMap(); + EXPECT(assert_equal(wrench_new_3_G, wrench_old_3_G, 1e-3)); + + // check torque zero on ground wrench + gtsam::Matrix36 H_contact_wrench; + H_contact_wrench << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0; + gtsam::Vector3 zero_torque{0.0, 0.0, 0.0}; + gtsam::Vector3 contact_torque_new = H_contact_wrench * wrench_new_3_G; + gtsam::Vector3 contact_torque_old = H_contact_wrench * wrench_old_3_G; + EXPECT(assert_equal(contact_torque_new, zero_torque, 1e-4)); + EXPECT(assert_equal(contact_torque_old, zero_torque, 1e-4)); +} + +gtsam::Values OldGraphFourLegs() { + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + // set masses and inertias + for (auto&& link : robot.links()) { + if (link->name().find("trunk") == std::string::npos) { + link->setMass(0.0); + link->setInertia(gtsam::Matrix3::Zero()); + } + } + + // Create Contact Point + std::vector lower_feet = { + robot.link("FL_lower"), robot.link("FR_lower"), robot.link("RL_lower"), + robot.link("RR_lower")}; + const Point3 contact_in_com(0, 0, -0.07); + auto stationary = + std::make_shared(lower_feet, contact_in_com); + auto contact_points = stationary->contactPoints(); + + gtsam::Vector3 gravity(0, 0, -10.0); + + OptimizerSetting opt(5e-5, 5e-5, 5e-5, 5e-5); + DynamicsGraph graph_builder(opt, gravity); + + gtsam::Vector6 wrench_zero = gtsam::Z_6x1; + + gtsam::NonlinearFactorGraph graph; + + // Need the following to use the wrench factor (Coriolis, Generalized + // Momentum, Gravity) + auto constrained_model = gtsam::noiseModel::Constrained::All(6); + auto bp_cost_model(gtsam::noiseModel::Isotropic::Sigma(6, 1e-5)); + graph.addPrior(PoseKey(0, 0), robot.link("trunk")->bMcom(), constrained_model); + graph.addPrior(TwistKey(0, 0), wrench_zero, constrained_model); + graph.addPrior(TwistAccelKey(0, 0), wrench_zero, constrained_model); + //graph.addPrior(WrenchKey(0,0,0), wrench_zero, bp_cost_model); + graph.addPrior(WrenchKey(0,3,0), wrench_zero, constrained_model); + graph.addPrior(WrenchKey(0,6,0), wrench_zero, constrained_model); + graph.addPrior(WrenchKey(0,9,0), wrench_zero, constrained_model); + graph.addPrior(JointAngleKey(1,0), 0.44, gtsam::noiseModel::Isotropic::Sigma(1, 5e-5)); + graph.addPrior(JointAngleKey(2,0), 0.0, gtsam::noiseModel::Isotropic::Sigma(1, 5e-5)); + + // Build dynamics factors + //graph.add(graph_builder.qFactors(robot, 0));//, contact_points)); + graph.add(graph_builder.dynamicsFactors(robot, 0, contact_points, 1.0)); + + // Initialize joint kinematics/dynamics to 0. + gtsam::Values init_vals; + for (auto&& joint : robot.joints()) { + const int j = joint->id(); + InsertWrench(&init_vals, joint->parent()->id(), j, 0, wrench_zero); + InsertWrench(&init_vals, joint->child()->id(), j, 0, wrench_zero); + InsertTorque(&init_vals, j, 0, 0.0); + InsertJointAngle(&init_vals, j, 0, 0.0); + //graph.addPrior(JointAngleKey(joint->id(),0), 0.0, gtsam::noiseModel::Isotropic::Sigma(1, 5e-5)); + } + for (auto&& link : robot.links()) { + InsertTwist(&init_vals, link->id(), 0, wrench_zero); + InsertTwistAccel(&init_vals, link->id(), 0, wrench_zero); + InsertPose(&init_vals, link->id(), 0, link->bMcom()); + //graph.addPrior(PoseKey(link->id(), 0), link->bMcom(), bp_cost_model); + //graph.addPrior(TwistKey(link->id(), 0), wrench_zero, bp_cost_model); + //graph.addPrior(TwistAccelKey(link->id(), 0), wrench_zero, bp_cost_model); + } + init_vals.insert(ContactWrenchKey(3, 0, 0), wrench_zero); + init_vals.insert(ContactWrenchKey(6, 0, 0), wrench_zero); + init_vals.insert(ContactWrenchKey(9, 0, 0), wrench_zero); + init_vals.insert(ContactWrenchKey(12, 0, 0), wrench_zero); + + /// Solve the constraint problem with LM optimizer. + OptimizationParameters params; + auto optimizer = Optimizer(params); + gtsam::NonlinearEqualityConstraints constraints; + gtsam::Values results = optimizer.optimize(graph, constraints, init_vals); + + return results; +} + +gtsam::Values NewGraphFourLegs() { + // This Graph uses chain constraints for four legs of the a1 robot, a wrench + // constraint on the trunk and zero angles and torques at the joints + // constraints (robot standing still) + + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + std::vector lower_feet = { + robot.link("FL_lower"), robot.link("FR_lower"), robot.link("RL_lower"), + robot.link("RR_lower")}; + const Point3 contact_in_com(0, 0, -0.07); + auto stationary = + std::make_shared(lower_feet, contact_in_com); + auto contact_points = stationary->contactPoints(); + + gtsam::NonlinearFactorGraph graph; + gtsam::Vector3 gravity(0, 0, -10.0); + + OptimizerSetting opt(5e-5, 5e-5, 5e-5, 5e-5); + //ChainDynamicsGraph chain_graph(robot, opt, 1*(1e-4), 6*(1e-5), 10.08*(1e-5), gravity); + ChainDynamicsGraph chain_graph(robot, opt, gravity); + + gtsam::Vector6 wrench_zero = gtsam::Z_6x1; + + auto constrained_model = gtsam::noiseModel::Constrained::All(6); + auto bp_cost_model(gtsam::noiseModel::Isotropic::Sigma(6, 1e-5)); + graph.addPrior(PoseKey(0, 0), robot.link("trunk")->bMcom(), constrained_model); + graph.addPrior(TwistKey(0, 0), wrench_zero, constrained_model); + graph.addPrior(TwistAccelKey(0, 0), wrench_zero, constrained_model); + //graph.addPrior(WrenchKey(0,0,0), wrench_zero, bp_cost_model); + graph.addPrior(WrenchKey(0,3,0), wrench_zero, constrained_model); + graph.addPrior(WrenchKey(0,6,0), wrench_zero, constrained_model); + graph.addPrior(WrenchKey(0,9,0), wrench_zero, constrained_model); + graph.addPrior(JointAngleKey(1,0), 0.44, gtsam::noiseModel::Isotropic::Sigma(1, 5e-5)); + graph.addPrior(JointAngleKey(2,0), 0.0, gtsam::noiseModel::Isotropic::Sigma(1, 5e-5)); + + // Build dynamics factors + //graph.add(chain_graph.qFactors(robot, 0));//, contact_points)); + graph.add(chain_graph.dynamicsFactors(robot, 0, contact_points, 1.0)); + + // Create initial values. + gtsam::Values init_values; + for (auto&& joint : robot.joints()) { + InsertJointAngle(&init_values, joint->id(), 0, 0.0); + //graph.addPrior(JointAngleKey(joint->id(),0), 0.0, gtsam::noiseModel::Isotropic::Sigma(1, 5e-5)); + //InsertTorque(&init_values, joint->id(), 0, 0.0); + } + + for (int i = 0; i < 4; ++i) { + init_values.insert(gtdynamics::WrenchKey(0, 3 * i, 0), wrench_zero); + } + + InsertPose(&init_values, 0, 0, robot.link("trunk")->bMcom()); + InsertTwist(&init_values, 0, 0, wrench_zero); + InsertTwistAccel(&init_values, 0, 0, wrench_zero); + + /// Solve the constraint problem with LM optimizer. + OptimizationParameters params; + auto optimizer = Optimizer(params); + gtsam::Values results = optimizer.optimize(graph, init_values); + return results; +} + +TEST(Chain, fourLegsCompareGraphsA1) { + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + // Get joint and composed chains for each leg + auto chain_joints = ChainDynamicsGraph::getChainJoints(robot); + + // calculate both ways + gtsam::Values new_graph_results = NewGraphFourLegs(); + gtsam::Values old_graph_results = OldGraphFourLegs(); + + // Get torque and angle results and compare + for (int i = 0; i < 12; ++i) { + double new_angle = + new_graph_results.at(gtdynamics::JointAngleKey(i)); + double old_angle = + old_graph_results.at(gtdynamics::JointAngleKey(i)); + EXPECT(assert_equal(new_angle, old_angle, 1e-3)); + } + + int hip_joint_id = 0; + int hip_link_id = 1; + for (int i = 0; i < 4; ++i) { + // Get new angles + double new_angle0 = + new_graph_results.at(gtdynamics::JointAngleKey(hip_joint_id)); + double new_angle1 = new_graph_results.at( + gtdynamics::JointAngleKey(hip_joint_id + 1)); + double new_angle2 = new_graph_results.at( + gtdynamics::JointAngleKey(hip_joint_id + 2)); + + // Get wrench keys + const gtsam::Key wrench_key_0_T = gtdynamics::WrenchKey(0, hip_joint_id, 0); + const gtsam::Key contact_wrench_key = + gtdynamics::ContactWrenchKey(hip_link_id + 2, 0, 0); + + // Get result wrench for hip joints and compare + gtsam::Vector6 wrench_new_0_T = + new_graph_results.at(wrench_key_0_T); + gtsam::Vector6 wrench_old_0_T = + old_graph_results.at(wrench_key_0_T); + EXPECT(assert_equal(wrench_new_0_T, wrench_old_0_T, 1e-3)); + + // Get result wrench for lower link and compare + Pose3 trunkThip = chain_joints[i][0]->parentTchild(new_angle0); + Pose3 hipTupper = chain_joints[i][1]->parentTchild(new_angle1); + Pose3 upperTlower = chain_joints[i][2]->parentTchild(new_angle2); + Pose3 trunkTlower = trunkThip * hipTupper * upperTlower; + gtsam::Vector6 wrench_new_3_L = + wrench_new_0_T.transpose() * trunkTlower.AdjointMap(); + gtsam::Vector6 wrench_old_3_L = + old_graph_results.at(contact_wrench_key); + EXPECT(assert_equal(wrench_new_3_L, wrench_old_3_L, 5e-2)); + + // calculate wrench on ground and compare + Point3 contact_in_com(0.0, 0.0, -0.07); + Pose3 M_L_G = Pose3(Rot3(), contact_in_com); + gtsam::Vector6 wrench_new_3_G = + wrench_new_3_L.transpose() * M_L_G.AdjointMap(); + gtsam::Vector6 wrench_old_3_G = + wrench_old_3_L.transpose() * M_L_G.AdjointMap(); + EXPECT(assert_equal(wrench_new_3_G, wrench_old_3_G, 5e-2)); + + // check torque zero on ground wrench + gtsam::Matrix36 H_contact_wrench; + H_contact_wrench << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0; + gtsam::Vector3 zero_torque{0.0, 0.0, 0.0}; + gtsam::Vector3 contact_torque_new = H_contact_wrench * wrench_new_3_G; + gtsam::Vector3 contact_torque_old = H_contact_wrench * wrench_old_3_G; + EXPECT(assert_equal(contact_torque_new, zero_torque, 1e-3)); + EXPECT(assert_equal(contact_torque_old, zero_torque, 1e-3)); + hip_joint_id += 3; + hip_link_id += 3; + } +} + +TEST(Chain,A1forwardKinematicsWithPOE) { + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + OptimizerSetting opt; + ChainDynamicsGraph chain_graph(robot, opt); + auto chain_joints = chain_graph.getChainJoints(robot); + auto composed_chains = chain_graph.getComposedChains(chain_joints); + + Vector3 test_angles( 0.0, 0.0, 0.0); + Pose3 chain_ee_pose = composed_chains[0].poe(test_angles, {}, nullptr); + + gtsam::Values test_values; + InsertJointAngle(&test_values, 0, 0, 0.0); + InsertJointAngle(&test_values, 1, 0, 0.0); + InsertJointAngle(&test_values, 2, 0, 0.0); + gtsam::Values expected = robot.forwardKinematics(test_values, 0, robot.link("trunk")->name()); + auto fk_ee_pose = Pose(expected, 3, 0); + EXPECT(assert_equal(chain_ee_pose, fk_ee_pose, 1e-6)); +} + +TEST(Chain, fourLegsCompareQfactorsA1) { + + auto robot = + CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1"); + + std::vector lower_feet = { + robot.link("FL_lower"), robot.link("FR_lower"), robot.link("RL_lower"), + robot.link("RR_lower")}; + const Point3 contact_in_com(0, 0, -0.07); + auto stationary = + std::make_shared(lower_feet, contact_in_com); + auto contact_points = stationary->contactPoints(); + + gtsam::NonlinearFactorGraph graph_DG, graph_CDG; + gtsam::Vector3 gravity(0, 0, -10.0); + + OptimizerSetting opt(1e-3); + opt.p_cost_model = gtsam::noiseModel::Constrained::All(6); + ChainDynamicsGraph chain_graph(robot, opt, gravity); + DynamicsGraph dynamics_graph(opt, gravity ); + + auto bp_cost_model(gtsam::noiseModel::Isotropic::Sigma(6, 1e-6)); + auto p_cost_model(gtsam::noiseModel::Isotropic::Sigma(6, 1e-3)); + auto cp_cost_model(gtsam::noiseModel::Isotropic::Sigma(1, 1e-3)); + auto cp_cost_model_angs(gtsam::noiseModel::Isotropic::Sigma(1, 5e-1)); + + graph_DG.addPrior(PoseKey(0, 0), robot.links()[0]->bMcom(), bp_cost_model); + graph_CDG.addPrior(PoseKey(0, 0), robot.links()[0]->bMcom(), bp_cost_model); + for (auto&& joint : robot.joints()){ + const int id = joint->id(); + graph_DG.addPrior(JointAngleKey(id, 0), 0.0, cp_cost_model_angs); + graph_CDG.addPrior(JointAngleKey(id, 0), 0.0, cp_cost_model_angs); + } + + graph_CDG.add(chain_graph.qFactors(robot, 0, contact_points)); + graph_DG.add(dynamics_graph.qFactors(robot, 0, contact_points)); + + // Create initial values. + gtsam::Values init_values_DG, init_values_CDG; + for (auto&& joint : robot.joints()) { + InsertJointAngle(&init_values_DG, joint->id(), 0, 0.0); + InsertJointAngle(&init_values_CDG, joint->id(), 0, 0.0); + } + + for (auto&& link : robot.links()) { + const int i = link->id(); + InsertPose(&init_values_DG, i, 0, link->bMcom()); + if (i==0 || i==3 || i==6 || i==9 || i==12) + InsertPose(&init_values_CDG, i, 0, link->bMcom()); + } + + /// Solve the constraint problem with LM optimizer. + gtsam::LevenbergMarquardtParams params; + //params.setVerbosityLM("SUMMARY"); + params.setlambdaInitial(1e10); + params.setlambdaLowerBound(1e-7); + params.setlambdaUpperBound(1e10); + params.setAbsoluteErrorTol(1e-3); + gtsam::LevenbergMarquardtOptimizer optimizer1(graph_DG, init_values_DG, params); + gtsam::LevenbergMarquardtOptimizer optimizer2(graph_CDG, init_values_CDG, params); + + gtsam::Values results_DG = optimizer1.optimize(); + gtsam::Values results_CDG = optimizer2.optimize(); + + for (int i = 0; i < 12; ++i) { + double angle_DG = + results_DG.at(gtdynamics::JointAngleKey(i)); + double angle_CDG = + results_CDG.at(gtdynamics::JointAngleKey(i)); + EXPECT(assert_equal(angle_DG, angle_CDG, 1e-3)); + } +} + + int main() { TestResult tr; return TestRegistry::runAllTests(tr); diff --git a/tests/testCollocationFactor.cpp b/tests/testCollocationFactor.cpp index dad902bb3..cb2ac80f3 100644 --- a/tests/testCollocationFactor.cpp +++ b/tests/testCollocationFactor.cpp @@ -12,6 +12,8 @@ */ #include +#include +#include #include #include #include @@ -23,9 +25,6 @@ #include -#include "gtdynamics/factors/CollocationFactors.h" -#include "gtdynamics/universal_robot/RobotModels.h" - using namespace gtdynamics; using gtsam::assert_equal, gtsam::Pose3, gtsam::Vector6, gtsam::Rot3; @@ -50,8 +49,8 @@ TEST(PoseTwistFunctor, error) { // Create factor EulerPoseCollocationFactor factor(example::pose_p_key, example::pose_c_key, - example::twist_p_key, example::dt_key, - example::cost_model); + example::twist_p_key, example::dt_key, + example::cost_model); // call evaluateError auto actual_errors = factor.evaluateError(pose_p, pose_c, twist, dt); @@ -83,8 +82,8 @@ TEST(RandomData, EulerPose) { // Create factor EulerPoseCollocationFactor factor(example::pose_p_key, example::pose_c_key, - example::twist_p_key, example::dt_key, - example::cost_model); + example::twist_p_key, example::dt_key, + example::cost_model); // call evaluateError auto actual_errors = factor.evaluateError(pose_p, pose_c, twist, dt); @@ -114,9 +113,9 @@ TEST(RandomData, TrapezoidalPose) { Pose3 pose_c = pose_p * Pose3::Expmap(twistdt); // Create factor - TrapezoidalPoseCollocationFactor factor(example::pose_p_key, example::pose_c_key, - example::twist_p_key, example::twist_c_key, - example::dt_key, example::cost_model); + TrapezoidalPoseCollocationFactor factor( + example::pose_p_key, example::pose_c_key, example::twist_p_key, + example::twist_c_key, example::dt_key, example::cost_model); // call evaluateError auto actual_errors = @@ -147,8 +146,8 @@ TEST(RandomData, EulerTwist) { // Create factor EulerTwistCollocationFactor factor(example::twist_p_key, example::twist_c_key, - example::accel_p_key, example::dt_key, - example::cost_model); + example::accel_p_key, example::dt_key, + example::cost_model); // call evaluateError auto actual_errors = factor.evaluateError(twist_p, twist_c, accel_p, dt); @@ -177,9 +176,9 @@ TEST(RandomData, TrapezoidalTwist) { double dt = 0.1; // Create factor - TrapezoidalTwistCollocationFactor factor(example::twist_p_key, example::twist_c_key, - example::accel_p_key, example::accel_c_key, - example::dt_key, example::cost_model); + TrapezoidalTwistCollocationFactor factor( + example::twist_p_key, example::twist_c_key, example::accel_p_key, + example::accel_c_key, example::dt_key, example::cost_model); // call evaluateError auto actual_errors = diff --git a/tests/testConstBiasFactor.cpp b/tests/testConstBiasFactor.cpp new file mode 100644 index 000000000..797fb3d17 --- /dev/null +++ b/tests/testConstBiasFactor.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testBiasedFactor.cpp + * @brief test const bias factor. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +TEST(BiasedFactor, pose) { + // Keys. + Key x1_key = 1; + Key x2_key = 2; + + // Base Factor. + auto noise = noiseModel::Unit::Create(3); + auto base_factor = std::make_shared>( + x1_key, x2_key, Point3(0, 0, 1), noise); + + // Create manifold values for testing. + Values values; + values.insert(x1_key, Point3(0, 0, 0)); + values.insert(x2_key, Point3(1, 0, 1)); + + // Bias + Vector bias = (Vector(3) << 1, 1, 0.1).finished(); + + // Test constructor. + gtdynamics::BiasedFactor const_bias_factor(base_factor, bias); + + // Check error. + Vector expected_error1 = (Vector(3) << 2, 1, 0.1).finished(); + EXPECT( + assert_equal(expected_error1, const_bias_factor.unwhitenedError(values))); + + // Check jacobian computation is correct. + EXPECT_CORRECT_FACTOR_JACOBIANS(const_bias_factor, values, 1e-7, 1e-5); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testConstVarFactor.cpp b/tests/testConstVarFactor.cpp new file mode 100644 index 000000000..d4b8e12af --- /dev/null +++ b/tests/testConstVarFactor.cpp @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testConstVarFactor.cpp + * @brief test const variable factor. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +TEST(ConstVarFactor, errorAndJacobian) { + // Keys. + Key x1_key = 1; + Key x2_key = 2; + + // Construct original factor. + auto noise = noiseModel::Unit::Create(6); + auto base_factor = std::make_shared>( + x1_key, x2_key, Pose3(Rot3(), Point3(1, 0, 0)), noise); + + // Fixed variables. + KeySet fixed_keys; + fixed_keys.insert(x1_key); + Values fixed_values; + fixed_values.insert(x1_key, Pose3(Rot3(), Point3(0, 0, 0))); + + // Test constructor. + gtdynamics::ConstVarFactor const_var_factor(base_factor, fixed_keys); + const_var_factor.setFixedValues(fixed_values); + EXPECT(const_var_factor.checkActive()); + + // Construct values for testing. + Values values; + values.insert(x2_key, Pose3(Rot3(), Point3(1, 0, 0))); + + // Check error. + Vector expected_error1 = Vector::Zero(6); + EXPECT( + assert_equal(expected_error1, const_var_factor.unwhitenedError(values))); + + // Check jacobian computation is correct. + EXPECT_CORRECT_FACTOR_JACOBIANS(const_var_factor, values, 1e-7, 1e-5); + + // A factor with all variables fixed should not be active. + fixed_keys.insert(x2_key); + gtdynamics::ConstVarFactor const_var_factor_all_fixed(base_factor, fixed_keys); + EXPECT(!const_var_factor_all_fixed.checkActive()); +} + +TEST(ConstVarGraph, error) { + // Keys. + Key x1_key = 1; + Key x2_key = 2; + + // Construct original factor graph. + auto noise = noiseModel::Unit::Create(6); + NonlinearFactorGraph graph; + graph.emplace_shared>(x1_key, x2_key, Pose3(Rot3(), Point3(1, 0, 0)), noise); + graph.addPrior(x1_key, Pose3(Rot3(), Point3(0, 0, 0))); + graph.addPrior(x2_key, Pose3(Rot3(), Point3(0, 0, 0))); + + // fixed keys + KeySet fixed_keys; + fixed_keys.insert(x1_key); + Values fixed_values; + fixed_values.insert(x1_key, Pose3(Rot3(), Point3(0, 0, 0))); + + // Construct const var graph with fixed keys. + NonlinearFactorGraph new_graph; + gtdynamics::ConstVarFactors const_var_factors; + std::tie(new_graph, const_var_factors) = gtdynamics::ConstVarGraph(graph, fixed_keys); + EXPECT_LONGS_EQUAL(2, new_graph.size()); + EXPECT_LONGS_EQUAL(1, const_var_factors.size()); + + // Construct const var graph with fixed values. + new_graph = gtdynamics::ConstVarGraph(graph, fixed_values); + EXPECT_LONGS_EQUAL(2, new_graph.size()); + Values values; + values.insert(x2_key, Pose3(Rot3(), Point3(0.5, 0, 0))); + EXPECT_DOUBLES_EQUAL(0.25, new_graph.error(values), 1e-5); +} + + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testConstraintManifold.cpp b/tests/testConstraintManifold.cpp new file mode 100644 index 000000000..8275a181b --- /dev/null +++ b/tests/testConstraintManifold.cpp @@ -0,0 +1,484 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testConstraintManifold.cpp + * @brief Test constraint manifold. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace gtsam; +using namespace gtdynamics; + +/** Simple example Pose3 with between constraints. */ +TEST_UNSAFE(ConstraintManifold, connected_poses) { + Key x1_key = 1; + Key x2_key = 2; + Key x3_key = 3; + + // Constraints. + auto constraints = std::make_shared(); + auto noise = noiseModel::Unit::Create(6); + auto factor12 = std::make_shared>( + x1_key, x2_key, Pose3(Rot3(), Point3(0, 0, 1)), noise); + auto factor23 = std::make_shared>( + x2_key, x3_key, Pose3(Rot3(), Point3(0, 1, 0)), noise); + constraints->emplace_shared(factor12); + constraints->emplace_shared(factor23); + + // Create manifold values for testing. + Values cm_base_values; + cm_base_values.insert(x1_key, Pose3(Rot3(), Point3(0, 0, 0))); + cm_base_values.insert(x2_key, Pose3(Rot3(), Point3(0, 0, 1))); + cm_base_values.insert(x3_key, Pose3(Rot3(), Point3(0, 1, 1))); + + // Create constraint manifold with various tspacebasis and retractors + BasisKeyFunc basis_key_func = [=](const KeyVector& keys) -> KeyVector { + return KeyVector{x3_key}; + }; + std::vector basis_creators{ + std::make_shared(), + std::make_shared(basis_key_func)}; + std::vector retractor_creators{ + std::make_shared(), + std::make_shared(basis_key_func)}; + + for (const auto& basis_creator : basis_creators) { + for (const auto& retractor_creator : retractor_creators) { + auto params = std::make_shared(); + params->basis_creator = basis_creator; + params->retractor_creator = retractor_creator; + ConstraintManifold manifold(constraints, cm_base_values, params, true); + + // Check recover + Values values; + Matrix H_recover_x2, H_recover_x3; + EXPECT(assert_equal(Pose3(Rot3(), Point3(0, 0, 1)), + manifold.recover(x2_key, H_recover_x2))); + EXPECT(assert_equal(Pose3(Rot3(), Point3(0, 1, 1)), + manifold.recover(x3_key, H_recover_x3))); + + // Check recover jacobian + std::function x2_recover_func = + std::bind(&ConstraintManifold::recover, std::placeholders::_1, + x2_key, nullptr); + EXPECT(assert_equal(numericalDerivative11( + x2_recover_func, manifold), + H_recover_x2)); + + std::function x3_recover_func = + std::bind(&ConstraintManifold::recover, std::placeholders::_1, + x3_key, nullptr); + EXPECT(assert_equal(numericalDerivative11( + x3_recover_func, manifold), + H_recover_x3)); + + // check retract + Vector xi = (Vector(6) << 0, 0, 0, 0, 0, 1).finished(); + auto new_cm = manifold.retract(xi); + } + } +} + +/** Minimal benchmark-method sequence regression for connected poses. + * Runs Soft -> Penalty -> Augmented Lagrangian -> CM on a small problem. + */ +TEST(ConstraintManifold, benchmark_sequence_connected_poses_minimal) { + const Key x1Key = 1; + const Key x2Key = 2; + const Key x3Key = 3; + + auto constraints = gtsam::NonlinearEqualityConstraints(); + const auto equalityNoise = noiseModel::Unit::Create(6); + auto factor12 = std::make_shared>( + x1Key, x2Key, Pose3(Rot3(), Point3(0, 0, 1)), equalityNoise); + auto factor23 = std::make_shared>( + x2Key, x3Key, Pose3(Rot3(), Point3(0, 1, 0)), equalityNoise); + constraints.emplace_shared(factor12); + constraints.emplace_shared(factor23); + + NonlinearFactorGraph costs; + const auto priorNoise = noiseModel::Isotropic::Sigma(6, 1.0); + costs.addPrior(x1Key, Pose3(Rot3(), Point3(0, 0, 0)), priorNoise); + costs.addPrior(x3Key, Pose3(Rot3(), Point3(0, 1, 1)), priorNoise); + + Values initValues; + initValues.insert(x1Key, Pose3(Rot3(), Point3(0.1, 0.0, 0.0))); + initValues.insert(x2Key, Pose3(Rot3(), Point3(0.0, 0.0, 0.0))); + initValues.insert(x3Key, Pose3(Rot3(), Point3(0.0, 0.0, 0.0))); + + const auto createProblem = [=]() { + return EConsOptProblem(costs, constraints, initValues); + }; + + ConstrainedOptBenchmark::Options runOptions; + runOptions.id = "test_connected_poses_minimal"; + runOptions.csvPath = "/tmp/test_connected_poses_minimal_benchmark.csv"; + runOptions.methods = {ConstrainedOptBenchmark::Method::SOFT, + ConstrainedOptBenchmark::Method::PENALTY, + ConstrainedOptBenchmark::Method::AUGMENTED_LAGRANGIAN, + ConstrainedOptBenchmark::Method::CM_F, + ConstrainedOptBenchmark::Method::CM_I}; + runOptions.softMu = 1.0; + runOptions.constraintUnitScale = 1.0; + + ConstrainedOptBenchmark runner(runOptions); + runner.setProblemFactory(createProblem); + + std::map resultsByMethod; + runner.setResultCallback( + [&](ConstrainedOptBenchmark::Method method, const Values& result) { + resultsByMethod[method] = result; + }); + + bool success = true; + std::string errorMessage; + std::ostringstream latexOs; + + try { + runner.run(latexOs); + } catch (const std::exception& e) { + success = false; + errorMessage = e.what(); + } + + if (!success) { + std::cout << "benchmark sequence failure: " << errorMessage << std::endl; + } + EXPECT(success); + if (success) { + EXPECT(resultsByMethod.count(ConstrainedOptBenchmark::Method::SOFT) == 1); + EXPECT(resultsByMethod.count(ConstrainedOptBenchmark::Method::PENALTY) == + 1); + EXPECT(resultsByMethod.count( + ConstrainedOptBenchmark::Method::AUGMENTED_LAGRANGIAN) == 1); + EXPECT(resultsByMethod.count(ConstrainedOptBenchmark::Method::CM_F) == 1); + EXPECT(resultsByMethod.count(ConstrainedOptBenchmark::Method::CM_I) == 1); + + const auto problem = createProblem(); + EXPECT(problem.evaluateEConstraintViolationL2Norm(resultsByMethod.at( + ConstrainedOptBenchmark::Method::CM_I)) < 1e-3); + } +} + +/** Multi-component benchmark regression to stress key-collision risks. + * Uses two disconnected constrained components and one unconstrained variable, + * and runs CM twice to catch state/key reuse issues. + */ +TEST(ConstraintManifold, benchmark_sequence_multi_component_no_key_collision) { + const Key x1Key = 1, x2Key = 2, x3Key = 3; + const Key y1Key = 4, y2Key = 5, y3Key = 6; + const Key uKey = 7; // unconstrained variable + + auto constraints = gtsam::NonlinearEqualityConstraints(); + const auto equalityNoise = noiseModel::Unit::Create(6); + constraints.emplace_shared( + std::make_shared>( + x1Key, x2Key, Pose3(Rot3(), Point3(0, 0, 1)), equalityNoise)); + constraints.emplace_shared( + std::make_shared>( + x2Key, x3Key, Pose3(Rot3(), Point3(0, 1, 0)), equalityNoise)); + constraints.emplace_shared( + std::make_shared>( + y1Key, y2Key, Pose3(Rot3(), Point3(1, 0, 0)), equalityNoise)); + constraints.emplace_shared( + std::make_shared>( + y2Key, y3Key, Pose3(Rot3(), Point3(0, -1, 0)), equalityNoise)); + + NonlinearFactorGraph costs; + const auto posePriorNoise = noiseModel::Isotropic::Sigma(6, 1.0); + const auto scalarPriorNoise = noiseModel::Isotropic::Sigma(1, 1.0); + costs.addPrior(x1Key, Pose3(Rot3(), Point3(0, 0, 0)), posePriorNoise); + costs.addPrior(x3Key, Pose3(Rot3(), Point3(0, 1, 1)), posePriorNoise); + costs.addPrior(y1Key, Pose3(Rot3(), Point3(2, 0, 0)), posePriorNoise); + costs.addPrior(y3Key, Pose3(Rot3(), Point3(2, -1, 0)), posePriorNoise); + costs.addPrior(uKey, 0.5, scalarPriorNoise); + + Values initValues; + initValues.insert(x1Key, Pose3(Rot3(), Point3(0.3, 0.0, 0.0))); + initValues.insert(x2Key, Pose3(Rot3(), Point3(0.0, 0.0, 0.0))); + initValues.insert(x3Key, Pose3(Rot3(), Point3(0.0, 0.0, 0.0))); + initValues.insert(y1Key, Pose3(Rot3(), Point3(2.2, 0.1, 0.0))); + initValues.insert(y2Key, Pose3(Rot3(), Point3(0.0, 0.0, 0.0))); + initValues.insert(y3Key, Pose3(Rot3(), Point3(0.0, 0.0, 0.0))); + initValues.insert(uKey, -1.0); + + const auto createProblem = [=]() { + return EConsOptProblem(costs, constraints, initValues); + }; + + ConstrainedOptBenchmark::Options runOptions; + runOptions.id = "test_multi_component"; + runOptions.csvPath = "/tmp/test_multi_component_benchmark.csv"; + runOptions.methods = {ConstrainedOptBenchmark::Method::SOFT, + ConstrainedOptBenchmark::Method::PENALTY, + ConstrainedOptBenchmark::Method::AUGMENTED_LAGRANGIAN, + ConstrainedOptBenchmark::Method::CM_F, + ConstrainedOptBenchmark::Method::CM_I}; + runOptions.softMu = 1.0; + runOptions.constraintUnitScale = 1.0; + + bool success = true; + std::string errorMessage; + std::map firstRunResults; + std::map secondRunResults; + std::ostringstream latexOs; + + try { + ConstrainedOptBenchmark firstRunner(runOptions); + firstRunner.setProblemFactory(createProblem); + firstRunner.setResultCallback( + [&](ConstrainedOptBenchmark::Method method, const Values& result) { + firstRunResults[method] = result; + }); + firstRunner.run(latexOs); + + ConstrainedOptBenchmark::Options secondRunOptions = runOptions; + secondRunOptions.methods = {ConstrainedOptBenchmark::Method::CM_I}; + ConstrainedOptBenchmark secondRunner(secondRunOptions); + secondRunner.setProblemFactory(createProblem); + secondRunner.setResultCallback( + [&](ConstrainedOptBenchmark::Method method, const Values& result) { + secondRunResults[method] = result; + }); + secondRunner.run(latexOs); + } catch (const std::exception& e) { + success = false; + errorMessage = e.what(); + } + + if (!success) { + std::cout << "multi-component benchmark sequence failure: " << errorMessage + << std::endl; + } + EXPECT(success); + if (success) { + EXPECT(firstRunResults.count(ConstrainedOptBenchmark::Method::CM_I) == 1); + EXPECT(secondRunResults.count(ConstrainedOptBenchmark::Method::CM_I) == 1); + const auto problem = createProblem(); + EXPECT(problem.evaluateEConstraintViolationL2Norm(firstRunResults.at( + ConstrainedOptBenchmark::Method::CM_I)) < 1e-3); + EXPECT(problem.evaluateEConstraintViolationL2Norm(secondRunResults.at( + ConstrainedOptBenchmark::Method::CM_I)) < 1e-3); + EXPECT( + assert_equal(0.5, + secondRunResults.at(ConstrainedOptBenchmark::Method::CM_I) + .atDouble(uKey), + 1e-3)); + } +} + +/** Regression: connected-poses benchmark has many disconnected constraint + * components, and each manifold key should be stable and unique. + */ +TEST(ConstraintManifold, connected_poses_many_components_manifold_keys_stable) { + using gtsam::symbol_shorthand::A; + using gtsam::symbol_shorthand::B; + + constexpr size_t kNumSteps = 40; + auto constraints = gtsam::NonlinearEqualityConstraints(); + auto costs = NonlinearFactorGraph(); + auto initValues = Values(); + + auto equalityNoise = noiseModel::Unit::Create(3); + auto priorNoise = noiseModel::Isotropic::Sigma(3, 1.0); + auto odoNoise = noiseModel::Isotropic::Sigma(3, 1.0); + + initValues.insert(A(0), Pose2(0.0, 0.0, 0.0)); + initValues.insert(B(0), Pose2(0.0, 1.0, 0.0)); + costs.addPrior(A(0), Pose2(0.0, 0.0, 0.0), priorNoise); + costs.addPrior(B(0), Pose2(0.0, 1.0, 0.0), priorNoise); + + for (size_t k = 0; k <= kNumSteps; ++k) { + constraints.emplace_shared( + std::make_shared>(A(k), B(k), Pose2(0.0, 1.0, 0.0), + equalityNoise)); + if (k > 0) { + initValues.insert(A(k), Pose2(0.0, 0.0, 0.0)); + initValues.insert(B(k), Pose2(0.0, 1.0, 0.0)); + costs.emplace_shared>( + A(k - 1), A(k), Pose2(0.1, 0.0, 0.0), odoNoise); + costs.emplace_shared>( + B(k - 1), B(k), Pose2(0.1, 0.0, 0.0), odoNoise); + } + } + + EConsOptProblem problem(costs, constraints, initValues); + auto moptParams = ConstrainedOptBenchmark::DefaultMoptParams(); + LevenbergMarquardtParams lmParams; + NonlinearMOptimizer optimizer(moptParams, lmParams); + + bool success = true; + std::string errorMessage; + ManifoldOptProblem moptProblem; + try { + moptProblem = optimizer.initializeMoptProblem( + problem.costs(), problem.constraints(), problem.initValues()); + } catch (const std::exception& e) { + success = false; + errorMessage = e.what(); + } + + if (!success) { + std::cout << "connected-poses manifold-key regression failure: " + << errorMessage << std::endl; + } + EXPECT(success); + if (success) { + EXPECT(moptProblem.components_.size() == (kNumSteps + 1)); + EXPECT(moptProblem.manifold_keys_.size() == (kNumSteps + 1)); + for (size_t k = 0; k <= kNumSteps; ++k) { + EXPECT(moptProblem.manifold_keys_.find(A(k)) != + moptProblem.manifold_keys_.end()); + } + } +} + +/** Regression for SV CM mode: fully constrained components should not fail + * basis-key initialization. + */ +TEST(ConstraintManifold, sv_mode_fully_constrained_component_no_throw) { + const Key x1Key = 1; + const Key x2Key = 2; + const auto noise = noiseModel::Unit::Create(6); + + NonlinearFactorGraph constraintsGraph; + constraintsGraph.emplace_shared>( + x1Key, x2Key, Pose3(Rot3(), Point3(0, 0, 1)), noise); + constraintsGraph.addPrior(x1Key, Pose3(Rot3(), Point3(0, 0, 0)), + noise); + constraintsGraph.addPrior(x2Key, Pose3(Rot3(), Point3(0, 0, 1)), + noise); + auto constraints = + gtsam::NonlinearEqualityConstraints::FromCostGraph(constraintsGraph); + + NonlinearFactorGraph costs; + Values initValues; + initValues.insert(x1Key, Pose3(Rot3(), Point3(0.1, 0.0, 0.0))); + initValues.insert(x2Key, Pose3(Rot3(), Point3(0.0, 0.2, 0.8))); + EConsOptProblem problem(costs, constraints, initValues); + + BasisKeyFunc basisKeyFunc = [](const KeyVector& keys) { return keys; }; + auto moptParams = ConstrainedOptBenchmark::DefaultMoptParamsSV(basisKeyFunc); + LevenbergMarquardtParams lmParams; + NonlinearMOptimizer optimizer(moptParams, lmParams); + + bool success = true; + std::string errorMessage; + ManifoldOptProblem moptProblem; + try { + moptProblem = optimizer.initializeMoptProblem( + problem.costs(), problem.constraints(), problem.initValues()); + } catch (const std::exception& e) { + success = false; + errorMessage = e.what(); + } + + if (!success) { + std::cout << "SV fully-constrained regression failure: " << errorMessage + << std::endl; + } + EXPECT(success); + if (success) { + EXPECT(moptProblem.components_.size() == 1); + EXPECT(moptProblem.manifold_keys_.size() == 0); + EXPECT(moptProblem.fixed_manifolds_.size() == 1); + } +} + +/** Dynamics manifold for cart-pole robot. */ +TEST(ConstraintManifold_retract, cart_pole_dynamics) { + // cart-pole robot setting + auto robot = CreateRobotFromFile(kUrdfPath + std::string("cart_pole.urdf")) + .fixLink("l0"); + int j0_id = robot.joint("j0")->id(), j1_id = robot.joint("j1")->id(); + const gtsam::Vector3 gravity(0, 0, -10); + OptimizerSetting opt; + auto graph_builder = DynamicsGraph(opt, gravity); + + // constraints graph + NonlinearFactorGraph constraints_graph; + constraints_graph.add(graph_builder.dynamicsFactorGraph(robot, 0)); + + // initial values + Initializer initializer; + Values values0 = initializer.ZeroValues(robot, 0, 0.0); + Values known_values; + for (const auto& joint : robot.joints()) { + InsertJointAngle(&known_values, joint->id(), + JointAngle(values0, joint->id())); + InsertJointVel(&known_values, joint->id(), JointVel(values0, joint->id())); + InsertTorque(&known_values, joint->id(), 0.0); + } + for (const auto& link : robot.links()) { + InsertPose(&known_values, link->id(), Pose(values0, link->id())); + InsertTwist(&known_values, link->id(), Twist(values0, link->id())); + } + values0 = graph_builder.linearSolveFD(robot, 0, known_values); + Values init_values = values0; + + // basis keys + KeyVector basis_keys; + basis_keys.push_back(JointAngleKey(j0_id, 0)); + basis_keys.push_back(JointAngleKey(j1_id, 0)); + basis_keys.push_back(JointVelKey(j0_id, 0)); + basis_keys.push_back(JointVelKey(j1_id, 0)); + basis_keys.push_back(JointAccelKey(j0_id, 0)); + basis_keys.push_back(JointAccelKey(j1_id, 0)); + BasisKeyFunc basis_key_func = [=](const KeyVector& keys) -> KeyVector { + return basis_keys; + }; + + // constraint manifold + auto constraints = std::make_shared( + gtsam::NonlinearEqualityConstraints::FromCostGraph(constraints_graph)); + auto cc_params = std::make_shared(); + cc_params->retractor_creator = + std::make_shared(basis_key_func); + cc_params->basis_creator = + std::make_shared(basis_key_func); + auto cm = ConstraintManifold(constraints, init_values, cc_params, true); + + // retract + Vector xi = (Vector(6) << 1, 0, 0, 0, 0, 0).finished(); + auto new_cm = cm.retract(xi); + // Check basis variables shall get the exact update. + EXPECT(assert_equal(1., new_cm.recover(JointAngleKey(j0_id, 0)))); + EXPECT(assert_equal(0., new_cm.recover(JointAngleKey(j1_id, 0)))); + EXPECT(assert_equal(0., new_cm.recover(JointVelKey(j0_id, 0)))); + EXPECT(assert_equal(0., new_cm.recover(JointVelKey(j1_id, 0)))); + EXPECT(assert_equal(0., new_cm.recover(JointAccelKey(j0_id, 0)))); + EXPECT(assert_equal(0., new_cm.recover(JointAccelKey(j1_id, 0)))); + + // Check that all constraints shall be satisfied after retraction. + EXPECT(assert_equal(0., constraints->violationNorm(new_cm.values()))); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testContactDynamicsFrictionConeFactor.cpp b/tests/testContactDynamicsFrictionConeFactor.cpp index de29d1f8d..a1ada346f 100644 --- a/tests/testContactDynamicsFrictionConeFactor.cpp +++ b/tests/testContactDynamicsFrictionConeFactor.cpp @@ -12,6 +12,7 @@ */ #include +#include #include #include #include @@ -25,8 +26,6 @@ #include -#include "gtdynamics/factors/ContactDynamicsFrictionConeFactor.h" - using gtsam::LabeledSymbol; using gtsam::Point3; using gtsam::Pose3; @@ -76,8 +75,8 @@ TEST(ContactDynamicsFrictionConeFactor, error) { (gtsam::Vector(6) << 0, 0, 0, 1, 0, 0).finished()); EXPECT_CORRECT_FACTOR_JACOBIANS( factor, values_simple, - 1e-7, // Step used when computing numerical derivative jacobians. - 1e-3); // Tolerance. + 1e-7, // Step used when computing numerical derivative jacobians. + 1e-3); // Tolerance. // Link angled with contact wrench angled relative to ground. EXPECT(assert_equal( @@ -97,8 +96,8 @@ TEST(ContactDynamicsFrictionConeFactor, error) { (gtsam::Vector(6) << 0, 0, 0, 0, 0, 1).finished()); EXPECT_CORRECT_FACTOR_JACOBIANS( factor, values_b, - 1e-7, // Step used when computing numerical derivative jacobians. - 1e-3); // Tolerance. + 1e-7, // Step used when computing numerical derivative jacobians. + 1e-3); // Tolerance. gtsam::Values values_c; values_c.insert(pose_key, @@ -108,8 +107,8 @@ TEST(ContactDynamicsFrictionConeFactor, error) { (gtsam::Vector(6) << 0, 0, 0, 0, 0, 1).finished()); EXPECT_CORRECT_FACTOR_JACOBIANS( factor, values_c, - 1e-7, // Step used when computing numerical derivative jacobians. - 1e-3); // Tolerance. + 1e-7, // Step used when computing numerical derivative jacobians. + 1e-3); // Tolerance. } /** diff --git a/tests/testContactDynamicsMomentFactor.cpp b/tests/testContactDynamicsMomentFactor.cpp index 0697fd9e7..9389fcf5b 100644 --- a/tests/testContactDynamicsMomentFactor.cpp +++ b/tests/testContactDynamicsMomentFactor.cpp @@ -12,6 +12,8 @@ */ #include +#include +#include #include #include #include @@ -24,9 +26,6 @@ #include -#include "gtdynamics/factors/ContactDynamicsMomentFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" - using namespace gtdynamics; using gtsam::assert_equal; diff --git a/tests/testContactEqualityFactor.cpp b/tests/testContactEqualityFactor.cpp index dacd3175e..a98756fb7 100644 --- a/tests/testContactEqualityFactor.cpp +++ b/tests/testContactEqualityFactor.cpp @@ -12,6 +12,9 @@ */ #include +#include +#include +#include #include #include #include @@ -23,10 +26,6 @@ #include -#include "gtdynamics/factors/ContactEqualityFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/values.h" - using namespace gtdynamics; using namespace gtsam; using gtsam::assert_equal; @@ -41,9 +40,9 @@ TEST(ContactEqualityFactor, Constructor) { } TEST(ContactEqualityFactor, Error) { - auto end_link = robot.links()[2]; - + LinkSharedPtr end_link = robot.links()[0]; PointOnLink point_on_link{end_link, Point3(0, 0, 1)}; + ContactEqualityFactor factor(point_on_link, kModel, 0, 1); // Hand computed value from SDF file @@ -59,7 +58,7 @@ TEST(ContactEqualityFactor, Error) { } TEST(ContactEqualityFactor, Jacobians) { - auto end_link = robot.links()[2]; + auto end_link = robot.links()[0]; PointOnLink point_on_link{end_link, Point3(0, 0, 1)}; ContactEqualityFactor factor(point_on_link, kModel, 0, 1); @@ -79,7 +78,7 @@ TEST(ContactEqualityFactor, ArbitraryTime) { CreateRobotFromFile(kUrdfPath + std::string("test/simple_urdf.urdf")); auto end_link = robot.link("l2"); - size_t k1 = 81, k2 = k1+3; + size_t k1 = 81, k2 = k1 + 3; PointOnLink point_on_link{end_link, Point3(0, 0, 0.777)}; ContactEqualityFactor factor(point_on_link, kModel, k1, k2); diff --git a/tests/testContactHeightFactor.cpp b/tests/testContactHeightFactor.cpp index bea78a28d..2aab7660f 100644 --- a/tests/testContactHeightFactor.cpp +++ b/tests/testContactHeightFactor.cpp @@ -12,6 +12,7 @@ */ #include +#include #include #include #include @@ -25,8 +26,6 @@ #include -#include "gtdynamics/factors/ContactHeightFactor.h" - using namespace gtdynamics; using gtsam::assert_equal; using gtsam::Vector3, gtsam::Vector1, gtsam::Rot3, gtsam::Pose3, gtsam::Point3; diff --git a/tests/testContactKinematicsAccelFactor.cpp b/tests/testContactKinematicsAccelFactor.cpp index 3f14bc037..89ce8aef7 100644 --- a/tests/testContactKinematicsAccelFactor.cpp +++ b/tests/testContactKinematicsAccelFactor.cpp @@ -12,6 +12,8 @@ */ #include +#include +#include #include #include #include @@ -24,9 +26,6 @@ #include -#include "gtdynamics/factors/ContactKinematicsAccelFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" - using namespace gtdynamics; using gtsam::assert_equal; diff --git a/tests/testContactKinematicsTwistFactor.cpp b/tests/testContactKinematicsTwistFactor.cpp index 2c563569c..0b27adf2f 100644 --- a/tests/testContactKinematicsTwistFactor.cpp +++ b/tests/testContactKinematicsTwistFactor.cpp @@ -12,6 +12,8 @@ */ #include +#include +#include #include #include #include @@ -24,9 +26,6 @@ #include -#include "gtdynamics/factors/ContactKinematicsTwistFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" - using namespace gtdynamics; using gtsam::assert_equal; @@ -72,7 +71,8 @@ TEST(ContactKinematicsTwistFactor, error) { (gtsam::Vector(6) << 1, 0, 0, 0, 0, 0).finished(); gtsam::Values values_twist_angular; values_twist_angular.insert(twist_key, link_twist_angular); - EXPECT(assert_equal(factor.unwhitenedError(values_twist_angular).norm(), 1.0)); + EXPECT( + assert_equal(factor.unwhitenedError(values_twist_angular).norm(), 1.0)); // A link with both angular velocity and linear velocity at the CoM should // have a linear velocity at the contact point (unless they cancel each other @@ -81,8 +81,9 @@ TEST(ContactKinematicsTwistFactor, error) { (gtsam::Vector(6) << 2, 0, 0, 0, 0, 4).finished(); gtsam::Values values_twist_angular_linear; values_twist_angular_linear.insert(twist_key, link_twist_angular_linear); - EXPECT(assert_equal(factor.unwhitenedError(values_twist_angular_linear).norm(), - std::sqrt(std::pow(2, 2) + std::pow(4, 2)))); + EXPECT( + assert_equal(factor.unwhitenedError(values_twist_angular_linear).norm(), + std::sqrt(std::pow(2, 2) + std::pow(4, 2)))); // Make sure linearization is correct gtsam::Values values; diff --git a/tests/testContactPointFactor.cpp b/tests/testContactPointFactor.cpp index 6bda73479..8e001b172 100644 --- a/tests/testContactPointFactor.cpp +++ b/tests/testContactPointFactor.cpp @@ -12,6 +12,9 @@ */ #include +#include +#include +#include #include #include #include @@ -23,15 +26,12 @@ #include -#include "gtdynamics/factors/ContactPointFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/values.h" - using namespace gtdynamics; using namespace gtsam; using gtsam::assert_equal; -auto kModel = noiseModel::Isotropic::Sigma(3, 0.1); +auto kPointModel = noiseModel::Isotropic::Sigma(3, 0.1); +auto kPoseModel = noiseModel::Isotropic::Sigma(6, 0.1); auto robot = simple_rr::getRobot(); @@ -39,10 +39,10 @@ TEST(ContactPointFactor, Constructor) { Key link_pose_key = gtdynamics::PoseKey(0, 0), point_key = gtdynamics::PoseKey(1, 0); Point3 lPc(0, 0, 1); - ContactPointFactor(link_pose_key, point_key, kModel, lPc); + ContactPointFactor(link_pose_key, point_key, kPointModel, lPc); PointOnLink point_on_link(robot.links()[0], lPc); - ContactPointFactor(point_on_link, point_key, kModel, 10); + ContactPointFactor(point_on_link, point_key, kPointModel, 10); } TEST(ContactPointFactor, Error) { @@ -50,17 +50,17 @@ TEST(ContactPointFactor, Error) { PointOnLink point_on_link{end_link, Point3(0, 0, 1)}; Key point_key = gtdynamics::PoseKey(1, 0); - Point3 wPc(0, 0, 1); + Point3 wPc(0, 0, 1.1); - ContactPointFactor factor(point_on_link, point_key, kModel, 0); + ContactPointFactor factor(point_on_link, point_key, kPointModel, 0); Vector error = factor.evaluateError(point_on_link.link->bMcom(), wPc); - EXPECT(assert_equal(Vector3(0,0,-0.1), error, 1e-9)); + EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); // Check error when contact point is not consistent - Point3 wPc2(1, 1, 2); + Point3 wPc2(1, 1, 2.1); error = factor.evaluateError(point_on_link.link->bMcom(), wPc2); - EXPECT(assert_equal(Vector3(1.0,1.0,0.9), error, 1e-9)); + EXPECT(assert_equal(Vector3::Ones(), error, 1e-9)); } TEST(ContactPointFactor, Jacobians) { @@ -70,7 +70,7 @@ TEST(ContactPointFactor, Jacobians) { Key point_key = gtdynamics::PoseKey(1, 0); Point3 wPc(0, 0, 1); - ContactPointFactor factor(point_on_link, point_key, kModel, 0); + ContactPointFactor factor(point_on_link, point_key, kPointModel, 0); Values values; InsertPose(&values, end_link->id(), 0, point_on_link.link->bMcom()); @@ -80,6 +80,109 @@ TEST(ContactPointFactor, Jacobians) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } +TEST(FixedContactPointFactor, Error) { + LinkSharedPtr end_link = robot.links()[0]; + Point3 contact_in_com(0, 0, 1); + PointOnLink point_on_link{end_link, contact_in_com}; + + Key link_pose_key = gtdynamics::PoseKey(0, 0); + Point3 wPc(0, 0, 1); + + FixedContactPointFactor factor(link_pose_key, kPointModel, wPc, contact_in_com); + Vector error = factor.evaluateError(point_on_link.link->bMcom()); + EXPECT(assert_equal(Vector3(0, 0, -0.1), error, 1e-9)); + + // Check error when contact point is not consistent + Point3 wPc2(1, 1, 2); + FixedContactPointFactor factor2(link_pose_key, kPointModel, wPc2, contact_in_com); + error = factor2.evaluateError(point_on_link.link->bMcom()); + EXPECT(assert_equal(Vector3(1.0, 1.0, 0.9), error, 1e-9)); +} + +TEST(FixedContactPointFactor, Jacobians) { + auto end_link = robot.links()[0]; + Key link_pose_key = gtdynamics::PoseKey(0, 0); + Point3 contact_in_com(0, 0, 1); + PointOnLink point_on_link{end_link, contact_in_com}; + + Point3 wPc(0, 0, 1); + + FixedContactPointFactor factor(link_pose_key, kPointModel, wPc, contact_in_com); + + Values values; + InsertPose(&values, end_link->id(), 0, point_on_link.link->bMcom()); + + // Check Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +TEST(ContactPoseFactor, Constructor) { + Key link_pose_key = gtdynamics::PoseKey(0, 0), + point_key = gtdynamics::PoseKey(1, 0); + Point3 wPc(0, 0, 1); + Pose3 linkTcontact(Rot3(), wPc); + ContactPoseFactor(link_pose_key, point_key, kPoseModel, linkTcontact); + + PointOnLink point_on_link(robot.links()[0], wPc); + ContactPoseFactor(point_on_link, point_key, kPoseModel, 10); +} + +TEST(ContactPoseFactor, Error) { + LinkSharedPtr end_link = robot.links()[0]; + Point3 wPc(0, 0, 1); + PointOnLink point_on_link{end_link, wPc}; + + Key point_key = gtdynamics::PoseKey(1, 0); + + Pose3 wTcontact(Rot3(), wPc); + + ContactPoseFactor factor(point_on_link, point_key, kPoseModel, 0); + + Vector error = factor.evaluateError(point_on_link.link->bMcom(), wTcontact); + Vector6 expected; + expected << 0, 0, 0, 0, 0, -0.1; + EXPECT(assert_equal(expected, error, 1e-9)); + + // Check error when contact point is translated + Point3 wPc2(1, 1, 2); + Pose3 wTcontact2(Rot3(), wPc2); + error = factor.evaluateError(point_on_link.link->bMcom(), wTcontact2); + Vector6 expected2; + expected2 << 0, 0, 0, 1.0, 1.0, 0.9; + EXPECT(assert_equal(expected2, error, 1e-9)); + + // Check error when contact point is rotated + Pose3 wTcontact3(Rot3::RzRyRx(0, 2, 0), wPc); + error = factor.evaluateError(point_on_link.link->bMcom(), wTcontact3); + // regression + Vector6 expected3; + expected3 << 0, 2, -0, 0.1, 0, -0.0642092616; + EXPECT(assert_equal(expected3, error, 1e-9)); +} + +TEST(ContactPoseFactor, Jacobians) { + auto end_link = robot.links()[0]; + Point3 wPc(0, 0, 1); + PointOnLink point_on_link{end_link, wPc}; + + Key point_key = gtdynamics::PoseKey(1, 0); + Pose3 wTc(Rot3(), wPc); + + ContactPoseFactor factor(point_on_link, point_key, kPoseModel, 0); + + Values values; + InsertPose(&values, end_link->id(), 0, point_on_link.link->bMcom()); + values.insert(point_key, wTc); + + // Check Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + + // Check Jacobians with non-identity rotation + Pose3 wTc2(Rot3::RzRyRx(1, 2, 3), wPc); + values.update(point_key, wTc2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); diff --git a/tests/testDynamics.cpp b/tests/testDynamics.cpp index 1e69f458e..0718ae80a 100644 --- a/tests/testDynamics.cpp +++ b/tests/testDynamics.cpp @@ -12,14 +12,12 @@ */ #include +#include +#include #include -#include #include -#include "gtdynamics/dynamics/Dynamics.h" -#include "gtdynamics/universal_robot/RobotModels.h" - using namespace gtdynamics; using namespace gtsam; @@ -39,7 +37,7 @@ TEST(Dynamics, Coriolis) { gtsam::Pose3::adjointTranspose(twist, inertia * twist); EXPECT(assert_equal(expected, Coriolis(inertia, twist, actualH), 1e-6)); Matrix6 numericalH = numericalDerivative11( - boost::bind(&Coriolis, inertia, _1, boost::none), twist); + std::bind(&Coriolis, inertia, std::placeholders::_1, nullptr), twist); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } diff --git a/tests/testDynamicsGraph.cpp b/tests/testDynamicsGraph.cpp index 074a0c4bd..ba6f62968 100644 --- a/tests/testDynamicsGraph.cpp +++ b/tests/testDynamicsGraph.cpp @@ -12,6 +12,14 @@ */ #include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -25,15 +33,6 @@ #include -#include "gtdynamics/dynamics/DynamicsGraph.h" -#include "gtdynamics/factors/MinTorqueFactor.h" -#include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/universal_robot/sdf.h" -#include "gtdynamics/utils/initialize_solution_utils.h" -#include "gtdynamics/utils/utils.h" -#include "gtdynamics/utils/values.h" - using namespace gtdynamics; int DEBUG_SIMPLE_OPTIMIZATION_EXAMPLE = 0; @@ -120,8 +119,8 @@ TEST(dynamicsFactorGraph_FD, simple_urdf_eq_mass) { graph.addPrior(TwistKey(i, t), gtsam::Z_6x1, graph_builder.opt().bv_cost_model); } - - gtsam::GaussNewtonOptimizer optimizer(graph, ZeroValues(robot, t)); + Initializer initializer; + gtsam::GaussNewtonOptimizer optimizer(graph, initializer.ZeroValues(robot, t)); Values result = optimizer.optimize(); gtsam::Vector actual_qAccel = DynamicsGraph::jointAccels(robot, result, t); @@ -161,7 +160,8 @@ TEST(dynamicsFactorGraph_FD, four_bar_linkage_pure) { auto graph = graph_builder.dynamicsFactorGraph(robot, 0); graph.add(prior_factors); - Values init_values = ZeroValues(robot, 0); + Initializer initializer; + Values init_values = initializer.ZeroValues(robot, 0); // test the four bar linkage FD in the free-floating scenario gtsam::GaussNewtonOptimizer optimizer(graph, init_values); @@ -204,7 +204,8 @@ TEST(dynamicsFactorGraph_FD, jumping_robot) { graph.add(graph_builder.forwardDynamicsPriors(robot, 0, known_values)); // test jumping robot FD - gtsam::GaussNewtonOptimizer optimizer(graph, ZeroValues(robot, 0)); + Initializer initializer; + gtsam::GaussNewtonOptimizer optimizer(graph, initializer.ZeroValues(robot, 0)); Values result = optimizer.optimize(); // check acceleration @@ -236,17 +237,14 @@ TEST(collocationFactors, simple_urdf) { int j = robot.joints()[0]->id(); NonlinearFactorGraph prior_factors; - prior_factors.add( - PriorFactor(JointAngleKey(j, t), 1, - graph_builder.opt().prior_q_cost_model)); + prior_factors.add(PriorFactor( + JointAngleKey(j, t), 1, graph_builder.opt().prior_q_cost_model)); prior_factors.add(PriorFactor( JointVelKey(j, t), 1, graph_builder.opt().prior_qv_cost_model)); - prior_factors.add( - PriorFactor(JointAccelKey(j, t), 1, - graph_builder.opt().prior_qa_cost_model)); - prior_factors.add( - PriorFactor(JointAccelKey(j, t + 1), 2, - graph_builder.opt().prior_qa_cost_model)); + prior_factors.add(PriorFactor( + JointAccelKey(j, t), 1, graph_builder.opt().prior_qa_cost_model)); + prior_factors.add(PriorFactor( + JointAccelKey(j, t + 1), 2, graph_builder.opt().prior_qa_cost_model)); Values init_values; InsertJointAngle(&init_values, j, t, 0.0); @@ -331,7 +329,8 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { } } - Values init_values = ZeroValuesTrajectory(robot, num_steps); + Initializer initializer; + Values init_values = initializer.ZeroValuesTrajectory(robot, num_steps); // test Euler auto euler_graph = graph_builder.trajectoryFG(robot, num_steps, dt, @@ -377,7 +376,7 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { graph_builder.opt().time_cost_model)); mp_prior_graph.add(PriorFactor(PhaseKey(1), dt1, graph_builder.opt().time_cost_model)); - init_values = ZeroValuesTrajectory(robot, num_steps, 2); + init_values = initializer.ZeroValuesTrajectory(robot, num_steps, 2); // multi-phase Euler NonlinearFactorGraph mp_euler_graph = graph_builder.multiPhaseTrajectoryFG( @@ -454,14 +453,14 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rr) { gtsam::noiseModel::Isotropic::Sigma(1, 1))); // Set initial values. - Values init_values = ZeroValues(robot, 0, 0.0, contact_points); + Initializer initializer; + Values init_values = initializer.ZeroValues(robot, 0, 0.0, contact_points); // Optimize! gtsam::GaussNewtonOptimizer optimizer(graph, init_values); Values results = optimizer.optimize(); // std::cout << "Error: " << graph.error(results) << std::endl; - auto contact_wrench_key = ContactWrenchKey(l0->id(), 0, 0); gtsam::Vector contact_wrench_optimized = results.at(contact_wrench_key); @@ -501,11 +500,9 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_biped) { auto body = biped.link("body"); prior_factors.addPrior(PoseKey(body->id(), 0), body->bMcom(), graph_builder.opt().bp_cost_model); - prior_factors.addPrior(TwistKey(body->id(), 0), - gtsam::Z_6x1, + prior_factors.addPrior(TwistKey(body->id(), 0), gtsam::Z_6x1, graph_builder.opt().bv_cost_model); - prior_factors.addPrior(TwistAccelKey(body->id(), 0), - gtsam::Z_6x1, + prior_factors.addPrior(TwistAccelKey(body->id(), 0), gtsam::Z_6x1, graph_builder.opt().ba_cost_model); graph.add(prior_factors); @@ -515,7 +512,8 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_biped) { gtsam::noiseModel::Isotropic::Sigma(1, 1))); // Set initial values. - Values init_values = ZeroValues(biped, 0, 0.0, contact_points); + Initializer initializer; + Values init_values = initializer.ZeroValues(biped, 0, 0.0, contact_points); // Regression on graph and values size. EXPECT_LONGS_EQUAL(74, graph.size()); @@ -587,7 +585,8 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rrr) { gtsam::noiseModel::Isotropic::Sigma(1, 0.1))); // Set initial values. - Values init_values = ZeroValues(robot, 0, 0.0, contact_points); + Initializer initializer; + Values init_values = initializer.ZeroValues(robot, 0, 0.0, contact_points); // Optimize! gtsam::GaussNewtonOptimizer optimizer(graph, init_values); diff --git a/tests/testDynamicsSymbol.cpp b/tests/testDynamicsSymbol.cpp index b96e6aa57..360b804aa 100644 --- a/tests/testDynamicsSymbol.cpp +++ b/tests/testDynamicsSymbol.cpp @@ -84,6 +84,31 @@ TEST(DynamicsSymbol, SimpleSymbol) { EXPECT_LONGS_EQUAL((long)key, (long)(Key)DynamicsSymbol(key)); } +TEST(DynamicsSymbol, Level) { + Key q_key = DynamicsSymbol::JointSymbol("q", 0, 0); + Key v_key = DynamicsSymbol::JointSymbol("v", 0, 0); + Key a_key = DynamicsSymbol::JointSymbol("a", 0, 0); + + EXPECT(IsQLevel(q_key)); + EXPECT(!IsQLevel(v_key)); + EXPECT(!IsQLevel(a_key)); + + EXPECT(IsVLevel(v_key)); + EXPECT(!IsVLevel(q_key)); + EXPECT(!IsVLevel(a_key)); + + std::vector keys = {q_key, v_key, a_key}; + EXPECT_LONGS_EQUAL(2, IdentifyLevel(keys)); + EXPECT_LONGS_EQUAL(1, IdentifyLevel(std::vector{q_key, v_key})); + EXPECT_LONGS_EQUAL(0, IdentifyLevel(std::vector{q_key})); + + KeySet q_keys, v_keys, ad_keys; + ClassifyKeysByLevel(keys, q_keys, v_keys, ad_keys); + EXPECT(q_keys.exists(q_key)); + EXPECT(v_keys.exists(v_key)); + EXPECT(ad_keys.exists(a_key)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testEqualityConstraint.cpp b/tests/testEqualityConstraint.cpp deleted file mode 100644 index c4029b182..000000000 --- a/tests/testEqualityConstraint.cpp +++ /dev/null @@ -1,169 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file testEqualityConstraintFactor.cpp - * @brief Test Equality Constraint Factor. - * @author: Yetong Zhang - */ - -#include -#include -#include -#include -#include - -#include "constrainedExample.h" -#include "gtdynamics/factors/PoseFactor.h" -#include "gtdynamics/optimizer/EqualityConstraint.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "make_joint.h" - -using namespace gtdynamics; -using namespace gtsam; -using constrained_example::x1_key, constrained_example::x2_key; -using constrained_example::x1, constrained_example::x2; -using constrained_example::pow; - -// Test methods of DoubleExpressionEquality. -TEST(EqualityConstraint, DoubleExpressionEquality) { - // create constraint from double expression - // g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides - double tolerance = 0.1; - auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2);; - auto constraint = DoubleExpressionEquality(g, tolerance); - - // Create 2 sets of values for testing. - Values values1, values2; - values1.insert(x1_key, 0.0); - values1.insert(x2_key, 0.0); - values2.insert(x1_key, 1.0); - values2.insert(x2_key, 1.0); - - // Check that values1 are feasible. - EXPECT(constraint.feasible(values1)); - - // Check that violation evaluates as 0 at values1. - EXPECT(assert_equal(Vector::Zero(1), constraint(values1))); - EXPECT(assert_equal(Vector::Zero(1), - constraint.toleranceScaledViolation(values1))); - - // Check that values2 are indeed deemed infeasible. - EXPECT(!constraint.feasible(values2)); - - // Check constraint violation is indeed g(x) at values2. - EXPECT(assert_equal(Vector::Constant(1, 4.0), constraint(values2))); - - // Check scaled violation is indeed g(x)/tolerance at values2. - EXPECT(assert_equal(Vector::Constant(1, 40.0), - constraint.toleranceScaledViolation(values2))); - - // Check dimension is 1 for scalar g. - EXPECT(constraint.dim() == 1); - - // Generate factor representing the term in merit function. - double mu = 4; - Vector bias = Vector::Constant(1, 0.5); - auto merit_factor = constraint.createFactor(mu, bias); - - // Check that noise model sigma == tolerance/sqrt(mu). - auto expected_noise = noiseModel::Isotropic::Sigma(1, tolerance / sqrt(mu)); - EXPECT(expected_noise->equals(*merit_factor->noiseModel())); - - // Check that error is equal to 0.5*mu * (g(x)+bias)^2/tolerance^2. - double expected_error1 = 50; // 0.5 * 4 * ||0 + 0.5||_(0.1^2)^2 - EXPECT(assert_equal(expected_error1, merit_factor->error(values1))); - double expected_error2 = 4050; // 0.5 * 4 * ||4 + 0.5||_(0.1^2)^2 - EXPECT(assert_equal(expected_error2, merit_factor->error(values2))); - - // Check jacobian computation is correct. - EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5); - EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); -} - -// Test methods of VectorExpressionEquality. -TEST(EqualityConstraint, VectorExpressionEquality) { - // g(v1, v2) = v1 + v2, our own example. - Vector2_ x1_vec_expr(x1_key); - Vector2_ x2_vec_expr(x2_key); - auto g = x1_vec_expr + x2_vec_expr; - auto tolerance = Vector2(0.1, 0.5); - auto constraint = VectorExpressionEquality<2>(g, tolerance); - - // Create 2 sets of values for testing. - Values values1, values2; - values1.insert(x1_key, Vector2(1, 1)); - values1.insert(x2_key, Vector2(-1, -1)); - values2.insert(x1_key, Vector2(1, 1)); - values2.insert(x2_key, Vector2(1, 1)); - - // Check that values1 are feasible. - EXPECT(constraint.feasible(values1)); - - // Check that violation evaluates as 0 at values1. - auto expected_violation1 = (Vector(2) << 0, 0).finished(); - EXPECT(assert_equal(expected_violation1, constraint(values1))); - auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished(); - EXPECT(assert_equal(expected_scaled_violation1, - constraint.toleranceScaledViolation(values1))); - - // Check that values2 are indeed deemed infeasible. - EXPECT(!constraint.feasible(values2)); - - // Check constraint violation is indeed g(x) at values2. - auto expected_violation2 = (Vector(2) << 2, 2).finished(); - EXPECT(assert_equal(expected_violation2, constraint(values2))); - - // Check scaled violation is indeed g(x)/tolerance at values2. - auto expected_scaled_violation2 = (Vector(2) << 20, 4).finished(); - EXPECT(assert_equal(expected_scaled_violation2, - constraint.toleranceScaledViolation(values2))); - - // Check dim is the dimension of the vector. - EXPECT(constraint.dim() == 2); - - // Generate factor representing the term in merit function. - double mu = 4; - Vector bias = (Vector(2) << 1, 0.5).finished(); - auto merit_factor = constraint.createFactor(mu, bias); - - // Check that noise model sigma == tolerance/sqrt(mu). - auto expected_noise = noiseModel::Diagonal::Sigmas(tolerance / sqrt(mu)); - EXPECT(expected_noise->equals(*merit_factor->noiseModel())); - - // Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(tolerance^2). - double expected_error1 = 202; // 0.5 * 4 * ||[1, 0.5]||_([0.1,0.5]^2)^2 - EXPECT(assert_equal(expected_error1, merit_factor->error(values1))); - double expected_error2 = 1850; // 0.5 * 4 * ||[3, 2.5]||_([0.1,0.5]^2)^2 - EXPECT(assert_equal(expected_error2, merit_factor->error(values2))); - - // Check jacobian computation is correct. - EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5); - EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); -} - -TEST(EqualityConstraint, Container) { - EqualityConstraints constraints; - - double tolerance1 = 0.1; - auto g1 = x1 + pow(x1, 3) + x2 + pow(x2, 2);; - - Vector2_ x1_vec_expr(x1_key); - Vector2_ x2_vec_expr(x2_key); - auto g2 = x1_vec_expr + x2_vec_expr; - auto tolerance2 = Vector2(0.1, 0.5); - - constraints.emplace_shared(g1, tolerance1); - constraints.emplace_shared>(g2, tolerance2); - - EXPECT_LONGS_EQUAL(2, constraints.size()); -} - -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} diff --git a/tests/testFixedJoint.cpp b/tests/testFixedJoint.cpp new file mode 100644 index 000000000..8a7b5ff1b --- /dev/null +++ b/tests/testFixedJoint.cpp @@ -0,0 +1,50 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testFixedJoint.cpp + * @brief Test FixedJoint class. + * @author Varun Agrawal + */ + +#include +#include +#include + +#include "gtdynamics/universal_robot/FixedJoint.h" +#include "gtdynamics/universal_robot/Link.h" +#include "gtdynamics/universal_robot/RobotModels.h" +#include "gtdynamics/universal_robot/sdf.h" +#include "gtdynamics/utils/utils.h" + +using namespace gtdynamics; + +using gtsam::assert_equal; +using gtsam::Point3; +using gtsam::Pose3; +using gtsam::Rot3; + +/** + * Construct a Fixed joint. + */ +TEST(FixedJoint, Constructor) { + auto robot = simple_urdf::getRobot(); + auto l1 = robot.link("l1"); + auto l2 = robot.link("l2"); + FixedJoint j(0, "j1", gtsam::Pose3(), l1, l2); +} + +TEST(FixedJoint, A1) { + Robot a1 = CreateRobotFromFile(kUrdfPath + std::string("a1/a1.urdf"), "", true); + std::string joint_name = "FR_toe_fixed"; + EXPECT(a1.joint("FR_toe_fixed")->name() == "FR_toe_fixed"); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testForwardKinematicsFactor.cpp b/tests/testForwardKinematicsFactor.cpp index 40cd4e184..0e78eeccb 100644 --- a/tests/testForwardKinematicsFactor.cpp +++ b/tests/testForwardKinematicsFactor.cpp @@ -12,6 +12,9 @@ */ #include +#include +#include +#include #include #include #include @@ -23,17 +26,12 @@ #include -#include "gtdynamics/factors/ForwardKinematicsFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/values.h" - using namespace gtdynamics; using namespace gtsam; using gtsam::assert_equal; const size_t i1 = 0, i2 = 2; -const Key key1 = gtdynamics::PoseKey(i1), - key2 = gtdynamics::PoseKey(i2); +const Key key1 = gtdynamics::PoseKey(i1), key2 = gtdynamics::PoseKey(i2); auto kModel = noiseModel::Isotropic::Sigma(6, 0.1); diff --git a/tests/testGeneralPriorFactor.cpp b/tests/testGeneralPriorFactor.cpp new file mode 100644 index 000000000..9a37068cb --- /dev/null +++ b/tests/testGeneralPriorFactor.cpp @@ -0,0 +1,67 @@ +/** + * @file testGeneralPriorFactor.cpp + * @brief Test GeneralPriorFactor + * @author Frank Dellaert + * @date Nov 4, 2014 + */ + +#include +#include +#include +#include + +using namespace std; +using namespace std::placeholders; +using namespace gtsam; +using namespace gtdynamics; + + +// /* ************************************************************************* */ +// TEST(GeneralPriorFactor, ConstructorPose3) { +// Key x_key = 1; +// Values values; +// values.insert(x_key, Pose3(Rot3::Rx(0.5), Point3(1, 2, 3))); + +// SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0); +// GeneralPriorFactor factor(x_key, values.at(x_key), model); + +// // Check Error and Jacobians +// Vector6 expected_error = Vector::Zero(6); +// EXPECT(assert_equal(expected_error, +// factor.unwhitenedError(values), 1e-6)); +// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + +// Values values1; +// values1.insert(x_key, Pose3(Rot3::Rx(0.6), Point3(1, 3, 3))); +// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values1, 1e-7, 1e-5); +// } + +/* ************************************************************************* */ +TEST(GeneralPriorFactor, ConstructorPoint3) { + Key x_key = 1; + Values values; + values.insert(x_key, Point3(1, 2, 3)); + + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + GeneralPriorFactor factor(x_key, values.at(x_key), model); + + // Check Error and Jacobians + Vector3 expected_error = Vector::Zero(3); + EXPECT(assert_equal(expected_error, + factor.unwhitenedError(values), 1e-6)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + + Values values1; + values1.insert(x_key, Point3(1, 3, 3)); + Vector3 expected_error1 = (Vector(3) << 0, 1, 0).finished(); + EXPECT(assert_equal(expected_error1, + factor.unwhitenedError(values1), 1e-6)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values1, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/tests/testGraphUtils.cpp b/tests/testGraphUtils.cpp new file mode 100644 index 000000000..43a7022ed --- /dev/null +++ b/tests/testGraphUtils.cpp @@ -0,0 +1,60 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testGraphUtils.cpp + * @brief Test GraphUtils. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include + +using namespace gtdynamics; +using namespace gtsam; + +TEST(GraphUtils, GraphDim) { + NonlinearFactorGraph graph; + graph.add(PriorFactor(0, 0.0, noiseModel::Isotropic::Sigma(1, 1.0))); + graph.add(PriorFactor(1, Vector2(0, 0), noiseModel::Isotropic::Sigma(2, 1.0))); + + EXPECT_LONGS_EQUAL(3, GraphDim(graph)); +} + +TEST(GraphUtils, IndexSet) { + IndexSet indices; + indices.insert(1); + indices.insert(5); + + EXPECT(indices.exists(1)); + EXPECT(!indices.exists(2)); +} + +TEST(GraphUtils, IndexSetMap) { + IndexSetMap index_map; + Key key = 10; + index_map.addIndex(key, 1); + index_map.addIndex(key, 2); + + EXPECT(index_map.exists(key)); + EXPECT(index_map.at(key).exists(1)); + EXPECT(index_map.at(key).exists(2)); + EXPECT(!index_map.at(key).exists(3)); + + IndexSetMap index_map2; + index_map2.addIndex(key, 3); + index_map.mergeWith(index_map2); + EXPECT(index_map.at(key).exists(3)); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testHelicalJoint.cpp b/tests/testHelicalJoint.cpp index 4b66251ee..2ec6c35bf 100644 --- a/tests/testHelicalJoint.cpp +++ b/tests/testHelicalJoint.cpp @@ -12,17 +12,16 @@ */ #include +#include +#include +#include +#include +#include +#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/sdf.h" -#include "gtdynamics/utils/utils.h" - using namespace gtdynamics; using gtsam::assert_equal; using gtsam::Point3; @@ -44,60 +43,60 @@ TEST(Joint, params_constructor) { parameters.scalar_limits.value_upper_limit = 1.57; parameters.scalar_limits.value_limit_threshold = 0; - auto j1 = boost::make_shared( - 123, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, - gtsam::Vector3(1, 0, 0), 0.5, parameters); + auto j1 = HelicalJoint(123, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, + gtsam::Vector3(1, 0, 0), 0.5, parameters); // name - EXPECT(assert_equal(j1->name(), "j1")); + EXPECT(assert_equal(j1.name(), "j1")); // ID - EXPECT(123 == j1->id()); + EXPECT(123 == j1.id()); // joint effort type - EXPECT(j1->parameters().effort_type == JointEffortType::Actuated); + EXPECT(j1.parameters().effort_type == JointEffortType::Actuated); // other link - EXPECT(j1->otherLink(l2) == l1); - EXPECT(j1->otherLink(l1) == l2); + EXPECT(j1.otherLink(l2) == l1); + EXPECT(j1.otherLink(l1) == l2); // screw axis gtsam::Vector6 screw_axis_l1, screw_axis_l2; screw_axis_l1 << -1, 0, 0, -0.5 / 2 / M_PI, -1, 0; // parent frame screw_axis_l2 << 1, 0, 0, 0.5 / 2 / M_PI, -1, 0; // child frame - EXPECT(assert_equal(screw_axis_l2, j1->screwAxis(l2))); - EXPECT(assert_equal(screw_axis_l1, j1->screwAxis(l1))); + EXPECT(assert_equal(screw_axis_l2, j1.screwAxis(l2))); + EXPECT(assert_equal(screw_axis_l1, j1.screwAxis(l1))); // rest transform Pose3 T_12comRest(Rot3::Rx(0), Point3(0, 0, 2)); Pose3 T_21comRest(Rot3::Rx(0), Point3(0, 0, -2)); - EXPECT(assert_equal(T_12comRest, j1->relativePoseOf(l2, 0.0))); - EXPECT(assert_equal(T_21comRest, j1->relativePoseOf(l1, 0.0))); + EXPECT(assert_equal(T_12comRest, j1.relativePoseOf(l2, 0.0))); + EXPECT(assert_equal(T_21comRest, j1.relativePoseOf(l1, 0.0))); // transform to (rotating -pi/2) Pose3 T_12com(Rot3::Rx(-M_PI / 2), Point3(-0.125, 1, 1)); Pose3 T_21com(Rot3::Rx(M_PI / 2), Point3(0.125, 1, -1)); - EXPECT(assert_equal(T_12com, j1->relativePoseOf(l2, -M_PI / 2))); - EXPECT(assert_equal(T_21com, j1->relativePoseOf(l1, -M_PI / 2))); + EXPECT(assert_equal(T_12com, j1.relativePoseOf(l2, -M_PI / 2))); + EXPECT(assert_equal(T_21com, j1.relativePoseOf(l1, -M_PI / 2))); // links - auto links = j1->links(); + auto links = j1.links(); EXPECT(links[0] == l1); EXPECT(links[1] == l2); // parent & child link - EXPECT(j1->parent() == l1); - EXPECT(j1->child() == l2); + EXPECT(j1.parent() == l1); + EXPECT(j1.child() == l2); // joint limit EXPECT(assert_equal(parameters.scalar_limits.value_lower_limit, - j1->parameters().scalar_limits.value_lower_limit)); + j1.parameters().scalar_limits.value_lower_limit)); EXPECT(assert_equal(parameters.scalar_limits.value_upper_limit, - j1->parameters().scalar_limits.value_upper_limit)); + j1.parameters().scalar_limits.value_upper_limit)); EXPECT(assert_equal(parameters.scalar_limits.value_limit_threshold, - j1->parameters().scalar_limits.value_limit_threshold)); + j1.parameters().scalar_limits.value_limit_threshold)); } +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION BOOST_CLASS_EXPORT(gtdynamics::HelicalJoint) TEST(HelicalJoint, Serialization) { @@ -107,7 +106,7 @@ TEST(HelicalJoint, Serialization) { JointParams parameters; - auto j1 = boost::make_shared( + auto j1 = std::make_shared( 123, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, gtsam::Vector3(1, 0, 0), 0.5, parameters); @@ -116,6 +115,7 @@ TEST(HelicalJoint, Serialization) { EXPECT(equalsDereferencedXML(j1)); EXPECT(equalsDereferencedBinary(j1)); } +#endif int main() { TestResult tr; diff --git a/tests/testInitializeSolutionUtils.cpp b/tests/testInitializer.cpp similarity index 89% rename from tests/testInitializeSolutionUtils.cpp rename to tests/testInitializer.cpp index 9403e9e38..ef9fb5363 100644 --- a/tests/testInitializeSolutionUtils.cpp +++ b/tests/testInitializer.cpp @@ -12,6 +12,10 @@ */ #include +#include +#include +#include +#include #include #include #include @@ -20,11 +24,6 @@ #include #include -#include "gtdynamics/dynamics/DynamicsGraph.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/universal_robot/sdf.h" -#include "gtdynamics/utils/initialize_solution_utils.h" - using namespace gtdynamics; using gtsam::assert_equal; using gtsam::Point3; @@ -44,9 +43,9 @@ TEST(InitializeSolutionUtils, Interpolation) { // We will interpolate from 0->10s, in 1 second increments. double T_i = 0, T_f = 10, dt = 1; - gtsam::Values init_vals = - InitializeSolutionInterpolation(robot, "link_0", wTb_i, wTb_f, T_i, T_f, - dt); + Initializer initializer; + gtsam::Values init_vals = initializer.InitializeSolutionInterpolation( + robot, "link_0", wTb_i, wTb_f, T_i, T_f, dt); int n_steps_final = static_cast(std::round(T_f / dt)); @@ -63,8 +62,7 @@ TEST(InitializeSolutionUtils, Interpolation) { // Check penultimate pose. EXPECT( assert_equal(Pose3(wTb_i.rotation().slerp(0.9, wRb_f), - Point3(0.794193007439, 1.03129011851, - 0.961521708273)), + Point3(0.794193007439, 1.03129011851, 0.961521708273)), Pose(init_vals, id, n_steps_final - 1))); // Check end pose. @@ -83,7 +81,8 @@ TEST(InitializeSolutionUtils, InitializeSolutionInterpolationMultiPhase) { std::vector ts = {5, 10}; double dt = 1; - gtsam::Values init_vals = InitializeSolutionInterpolationMultiPhase( + Initializer initializer; + gtsam::Values init_vals = initializer.InitializeSolutionInterpolationMultiPhase( robot, "l1", wTb_i, wTb_t, ts, dt); Pose3 pose = Pose(init_vals, l1->id()); @@ -122,7 +121,8 @@ TEST(InitializeSolutionUtils, InitializePosesAndJoints) { gtsam::Sampler sampler(sampler_noise_model); std::vector wTl_dt; - auto actual = InitializePosesAndJoints(robot, wTb_i, wTb_t, l2->name(), t_i, + Initializer initializer; + auto actual = initializer.InitializePosesAndJoints(robot, wTb_i, wTb_t, l2->name(), t_i, timesteps, dt, sampler, &wTl_dt); gtsam::Values expected; InsertPose(&expected, 0, Pose3(Rot3(), Point3(0, 0, 1))); @@ -178,7 +178,8 @@ TEST(InitializeSolutionUtils, InverseKinematics) { * | | l1 :( * ¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯|¯¯¯¯¯¯¯¯¯¯¯¯¯ */ - gtsam::Values init_vals = InitializeSolutionInverseKinematics( + Initializer initializer; + gtsam::Values init_vals = initializer.InitializeSolutionInverseKinematics( robot, l2->name(), wTb_i, wTb_t, ts, dt, kNoiseSigma, contact_points); EXPECT(assert_equal(wTb_i, Pose(init_vals, l2->id()), 1e-3)); @@ -213,7 +214,8 @@ TEST(InitializeSolutionUtils, ZeroValues) { Pose3 oTc_l1(Rot3(), Point3(0, 0, -1.0)); PointOnLinks contact_points = {{l1, oTc_l1.translation()}}; - gtsam::Values init_vals = ZeroValues(robot, 0, 0.0, contact_points); + Initializer initializer; + gtsam::Values init_vals = initializer.ZeroValues(robot, 0, 0.0, contact_points); Pose3 pose; double joint_angle; @@ -240,8 +242,9 @@ TEST(InitializeSolutionUtils, ZeroValuesTrajectory) { Pose3 oTc_l1(Rot3(), Point3(0, 0, -1.0)); PointOnLinks contact_points = {{l1, oTc_l1.translation()}}; + Initializer initializer; gtsam::Values init_vals = - ZeroValuesTrajectory(robot, 100, -1, 0.0, contact_points); + initializer.ZeroValuesTrajectory(robot, 100, -1, 0.0, contact_points); double joint_angle; for (size_t t = 0; t <= 100; t++) { @@ -285,15 +288,16 @@ TEST(InitializeSolutionUtils, MultiPhaseInverseKinematicsTrajectory) { ts.push_back(3 * steps_per_phase); // Initial values for transition graphs. + gtdynamics::Initializer initializer; std::vector transition_graph_init; transition_graph_init.push_back( - gtdynamics::ZeroValues(robot, 1 * steps_per_phase, kNoiseSigma, p0)); + initializer.ZeroValues(robot, 1 * steps_per_phase, kNoiseSigma, p0)); transition_graph_init.push_back( - gtdynamics::ZeroValues(robot, 2 * steps_per_phase, kNoiseSigma, p0)); + initializer.ZeroValues(robot, 2 * steps_per_phase, kNoiseSigma, p0)); double dt = 1.0; - gtsam::Values init_vals = MultiPhaseInverseKinematicsTrajectory( + gtsam::Values init_vals = initializer.MultiPhaseInverseKinematicsTrajectory( robot, l2->name(), phase_steps, wTb_i, wTb_t, ts, transition_graph_init, dt, kNoiseSigma, phase_contact_points); diff --git a/tests/testJointMeasurementFactor.cpp b/tests/testJointMeasurementFactor.cpp index 62ff85a4f..5eaddba3c 100644 --- a/tests/testJointMeasurementFactor.cpp +++ b/tests/testJointMeasurementFactor.cpp @@ -12,6 +12,10 @@ */ #include +#include +#include +#include +#include #include #include #include @@ -23,11 +27,6 @@ #include -#include "gtdynamics/factors/JointMeasurementFactor.h" -#include "gtdynamics/universal_robot/RevoluteJoint.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/values.h" - using namespace gtdynamics; using namespace gtsam; using gtsam::assert_equal; diff --git a/tests/testKinematicsConstraints.cpp b/tests/testKinematicsConstraints.cpp new file mode 100644 index 000000000..923ccf54a --- /dev/null +++ b/tests/testKinematicsConstraints.cpp @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testKinematicsConstraints.cpp + * @brief Isolate evaluation of kinematics constraints. + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include "contactGoalsExample.h" + +using namespace gtdynamics; + +namespace { + +bool AllViolationsFinite(const gtsam::NonlinearEqualityConstraints& constraints, + const gtsam::Values& values) { + for (size_t i = 0; i < constraints.size(); ++i) { + gtsam::NonlinearEqualityConstraints single; + single.push_back(constraints.at(i)); + const double violation = single.violationNorm(values); + if (!std::isfinite(violation)) { + return false; + } + } + return true; +} + +} // namespace + +TEST(KinematicsConstraints, SliceViolation) { + using namespace contact_goals_example; + + const size_t k = 777; + const Slice slice(k); + + KinematicsParameters parameters; + Kinematics kinematics(parameters); + + auto constraints = kinematics.constraints(slice, robot); + auto goal_constraints = kinematics.pointGoalConstraints(slice, contact_goals); + for (const auto& constraint : goal_constraints) { + constraints.push_back(constraint); + } + + auto values = kinematics.initialValues(slice, robot, 0.0); + + EXPECT(AllViolationsFinite(constraints, values)); +} + +TEST(KinematicsConstraints, IntervalViolation) { + using namespace contact_goals_example; + + const size_t num_time_steps = 5; + const Interval interval(0, num_time_steps - 1); + + KinematicsParameters parameters; + Kinematics kinematics(parameters); + + auto constraints = kinematics.constraints(interval, robot); + auto goal_constraints = + kinematics.pointGoalConstraints(interval, contact_goals); + for (const auto& constraint : goal_constraints) { + constraints.push_back(constraint); + } + + auto values = kinematics.initialValues(interval, robot, 0.0); + + EXPECT(AllViolationsFinite(constraints, values)); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testKinematicsInterval.cpp b/tests/testKinematicsInterval.cpp index 5dac4b734..971592949 100644 --- a/tests/testKinematicsInterval.cpp +++ b/tests/testKinematicsInterval.cpp @@ -52,7 +52,7 @@ TEST(Interval, InverseKinematics) { auto result = kinematics.inverse(interval, robot, contact_goals); // Check that goals are achieved - constexpr double tol = 1e-5; + constexpr double tol = 1e-4; for (const ContactGoal& goal : contact_goals) { for (size_t k = 0; k < num_time_steps; k++) { EXPECT(goal.satisfied(result, k, tol)); @@ -77,8 +77,8 @@ TEST(Interval, Interpolate) { // Create a kinematic trajectory over timesteps 5, 6, 7, 8, 9 that // interpolates between goal configurations at timesteps 5 and 9. - gtsam::Values result = - kinematics.interpolate(Interval(5, 9), robot, contact_goals, contact_goals2); + gtsam::Values result = kinematics.interpolate(Interval(5, 9), robot, + contact_goals, contact_goals2); EXPECT(result.exists(PoseKey(0, 5))); EXPECT(result.exists(PoseKey(0, 9))); EXPECT(assert_equal(Pose(result1, 0, 5), Pose(result, 0, 5))); diff --git a/tests/testKinematicsPhase.cpp b/tests/testKinematicsPhase.cpp index 2ed8755ed..94aa021e2 100644 --- a/tests/testKinematicsPhase.cpp +++ b/tests/testKinematicsPhase.cpp @@ -31,7 +31,8 @@ TEST(Phase, InverseKinematics) { constexpr size_t num_time_steps = 5; const std::vector link_vec = {LH, RF}; - auto constraint = boost::make_shared(link_vec , contact_in_com); + auto constraint = + std::make_shared(link_vec, contact_in_com); Phase phase0(0, num_time_steps, constraint); // TODO(frank): test methods producing constraints. diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index 9c9a4cfc7..b256fb227 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -77,7 +77,7 @@ TEST(Slice, InverseKinematics) { // GTD_PRINT(*factor); // Check that goals are achieved - constexpr double tol = 1e-5; + constexpr double tol = 1e-4; for (const ContactGoal& goal : contact_goals) { EXPECT(goal.satisfied(result, k, tol)); } diff --git a/tests/testLink.cpp b/tests/testLink.cpp index b1f67614b..42cea8312 100644 --- a/tests/testLink.cpp +++ b/tests/testLink.cpp @@ -12,24 +12,21 @@ */ #include +#include +#include +#include +#include #include #include #include #include -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/universal_robot/RevoluteJoint.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/utils.h" - using namespace gtdynamics; using gtsam::assert_equal; 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(), @@ -70,7 +67,7 @@ TEST(Link, NumJoints) { EXPECT_LONGS_EQUAL(1, l1->numJoints()); - auto j2 = boost::make_shared( + auto j2 = std::make_shared( 123, "j2", Pose3(Rot3(), Point3(0, 0.5, 2)), l1, l2, gtsam::Vector3(1, 0, 0), JointParams()); @@ -78,6 +75,10 @@ TEST(Link, NumJoints) { EXPECT_LONGS_EQUAL(2, l1->numJoints()); } +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + +using namespace gtsam::serializationTestHelpers; + // Declaration needed for serialization of derived class. BOOST_CLASS_EXPORT(gtdynamics::RevoluteJoint) @@ -95,6 +96,18 @@ TEST(Link, Serialization) { EXPECT(equalsDereferencedXML(l1)); EXPECT(equalsDereferencedBinary(l1)); } +#endif + +TEST(Link, Print) { + Link link(1, "l1", 100.0, gtsam::Vector3(3, 2, 1).asDiagonal(), + Pose3(Rot3(), Point3(0, 0, 1)), Pose3()); + + std::string expected = + "l1, id=1:\n" + " com pose: 0 -0 0, 0 0 1\n" + " link pose: 0 -0 0, 0 0 0\n"; + EXPECT(gtsam::assert_print_equal(expected, link)); +} int main() { TestResult tr; diff --git a/tests/testManifoldOptimizer_rot2.cpp b/tests/testManifoldOptimizer_rot2.cpp new file mode 100644 index 000000000..4468321fa --- /dev/null +++ b/tests/testManifoldOptimizer_rot2.cpp @@ -0,0 +1,338 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testManifoldOpt_so2.cpp + * @brief Test manifold optimizer with SO(2) manifold. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "ManifoldOptScenarios.h" + +namespace gtsam { + +/** + * Manually defined Rot2 manifold that performs retraction with projection. + */ +class Rot2Projection { + /** we store cos(theta) and sin(theta) */ + double c_, s_; + + /** normalize to make sure cos and sin form unit vector */ + Rot2Projection& normalize() { + double scale = c_ * c_ + s_ * s_; + if (std::abs(scale - 1.0) > 1e-10) { + scale = 1 / sqrt(scale); + c_ *= scale; + s_ *= scale; + } + return *this; + } + + /** private constructor from cos/sin */ + inline Rot2Projection(double c, double s) : c_(c), s_(s) {} + + Point2 tangent() const { return Point2(-s_, c_); } + + /** return 2*2 rotation matrix */ + Matrix2 matrix() const { + Matrix2 rvalue_; + rvalue_ << c_, -s_, s_, c_; + return rvalue_; + } + + /** return 2*2 transpose (inverse) rotation matrix */ + Matrix2 transpose() const { + Matrix2 rvalue_; + rvalue_ << c_, s_, -s_, c_; + return rvalue_; + } + + public: + enum { dimension = 1 }; + + /// @name Constructors and named constructors + /// @{ + + /** default constructor, zero rotation */ + Rot2Projection() : c_(1.0), s_(0.0) {} + + /** copy constructor */ + Rot2Projection(const Rot2Projection& r) : Rot2Projection(r.c_, r.s_) {} + + /** copy assignment operator */ + Rot2Projection& operator=(const Rot2Projection& r) { + if (this != &r) { + c_ = r.c_; + s_ = r.s_; + } + return *this; + } + + /// Constructor from angle in radians == exponential map at identity + Rot2Projection(double theta) : c_(cos(theta)), s_(sin(theta)) {} + + /// Named constructor from cos(theta),sin(theta) pair, will *not* normalize! + static Rot2Projection fromCosSin(double c, double s) { + Rot2Projection R(c, s); + return R.normalize(); + } + + /** print */ + void print(const std::string& s = "theta") const { + std::cout << s << ": " << theta() << std::endl; + } + + /** equals with an tolerance */ + bool equals(const Rot2Projection& R, double tol = 1e-9) const { + return std::abs(c_ - R.c_) <= tol && std::abs(s_ - R.s_) <= tol; + } + + /// retraction with optional derivatives. + Rot2Projection retract(const gtsam::Vector1& v, // TODO: use xi + gtsam::OptionalJacobian<1, 1> H1 = nullptr, + gtsam::OptionalJacobian<1, 1> H2 = nullptr) const { + if (H1) H1->setZero(); + if (H2) H2->setZero(); + + Point2 tangent_v = v(0) * tangent(); + Rot2Projection R(c_ + tangent_v.x(), s_ + tangent_v.y()); + return R.normalize(); + } + + /// localCoordinates with optional derivatives. + gtsam::Vector1 localCoordinates( + const Rot2Projection& g, gtsam::OptionalJacobian<1, 1> H1 = nullptr, + gtsam::OptionalJacobian<1, 1> H2 = nullptr) const { + if (H1) H1->setZero(); + if (H2) H2->setZero(); + return Vector1(0); + } + + /** + * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ + */ + Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = nullptr, + OptionalJacobian<2, 2> H2 = nullptr) const { + const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y()); + if (H1) *H1 << -q.y(), q.x(); + if (H2) *H2 = matrix(); + return q; + } + + /** + * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ + */ + Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = nullptr, + OptionalJacobian<2, 2> H2 = nullptr) const { + const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y()); + if (H1) *H1 << q.y(), -q.x(); + if (H2) *H2 = transpose(); + return q; + } + + /** return cos */ + inline double c() const { return c_; } + + /** return sin */ + inline double s() const { return s_; } + + /** return angle (RADIANS) */ + double theta() const { return ::atan2(s_, c_); } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(c_); + ar& BOOST_SERIALIZATION_NVP(s_); + } +#endif +}; + +// Specialize Rot2Projection traits to use a Retract/Local +template <> +struct traits : gtsam::internal::Manifold {}; + +} // namespace gtsam + +using namespace gtsam; +using namespace gtdynamics; + +/** Check creation of manifold optimization problem. */ +TEST(ManifoldOptProblem, SO2) { + using namespace so2_scenario; + auto costs = get_graph(-2, 0); + auto constraints = get_constraints(); + + Values init_values; + init_values.insert(x1_key, 0.8); + init_values.insert(x2_key, 0.6); + + LevenbergMarquardtParams nopt_params; + ManifoldOptimizerParameters mopt_params; + NonlinearMOptimizer optimizer(mopt_params, nopt_params); + auto mopt_problem = + optimizer.initializeMoptProblem(*costs, *constraints, init_values); + + EXPECT_LONGS_EQUAL(1, mopt_problem.components_.size()); + EXPECT_LONGS_EQUAL(0, mopt_problem.unconstrained_keys_.size()); + EXPECT_LONGS_EQUAL(1, mopt_problem.manifold_keys_.size()); + EXPECT_LONGS_EQUAL(1, mopt_problem.values_.size()); + EXPECT_LONGS_EQUAL(2, mopt_problem.graph_.size()); + EXPECT_LONGS_EQUAL(2, mopt_problem.problemDimension().first); + EXPECT_LONGS_EQUAL(1, mopt_problem.problemDimension().second); +} + +/** Optimization using Rot2 manifold. */ +TEST(ManifoldOptimization, SO2) { + Key rot_key = 1; + + Expression pt_unit(Point2(1, 0)); + Expression rot(rot_key); + Expression rotated_pt(&Rot2::rotate, rot, pt_unit); + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); + NonlinearFactorGraph graph; + Point2 goal_pt(-2, 0); + graph.addExpressionFactor(noise, goal_pt, rotated_pt); + + Values init_values; + init_values.insert(rot_key, Rot2::fromCosSin(0.8, 0.6)); + + LevenbergMarquardtParams params; + params.minModelFidelity = 0.5; + // params.setVerbosityLM("SUMMARY"); + LevenbergMarquardtOptimizer optimizer(graph, init_values, params); + auto result = optimizer.optimize(); + // result.print(); + + Rot2 expected_rot = Rot2::fromCosSin(-1, 0.0); + EXPECT(assert_equal(expected_rot, result.at(rot_key), 1e-4)); +} + +/** Optimization using Type1 manifold optimizer. */ +TEST(NonlinearMOptimizer, SO2) { + using namespace so2_scenario; + auto costs = get_graph(-2, 0); + auto constraints = get_constraints(); + + Values init_values; + init_values.insert(x1_key, 0.8); + init_values.insert(x2_key, 0.6); + + LevenbergMarquardtParams nopt_params; + nopt_params.minModelFidelity = 0.5; + // nopt_params.setVerbosityLM("SUMMARY"); + ManifoldOptimizerParameters mopt_params; + NonlinearMOptimizer optimizer(mopt_params, nopt_params); + auto result = optimizer.optimize(*costs, *constraints, init_values); + // result.print(); + + EXPECT(assert_equal(-1.0, result.atDouble(x1_key), 1e-5)); + EXPECT(assert_equal(0.0, result.atDouble(x2_key), 1e-5)); +} + +/** Optimization using Type1 manifold optimizer, infeasible. */ +TEST(NonlinearMOptimizer_infeasible, SO2) { + using namespace so2_scenario; + auto costs = get_graph(-2, 0); + auto constraints = get_constraints(); + + Values init_values; + init_values.insert(x1_key, 0.5); + init_values.insert(x2_key, 0.5); + + LevenbergMarquardtParams nopt_params; + nopt_params.minModelFidelity = 0.5; + // nopt_params.setVerbosityLM("SUMMARY"); + ManifoldOptimizerParameters mopt_params; + mopt_params.cc_params->retractor_creator->params()->lm_params.setMaxIterations(4); + NonlinearMOptimizer optimizer(mopt_params, nopt_params); + auto result = optimizer.optimize(*costs, *constraints, init_values); + // result.print(); + + // TODO: check why this does not converge as good as Type2. + EXPECT(assert_equal(-1.0, result.atDouble(x1_key), 1e-3)); + EXPECT(assert_equal(0.0, result.atDouble(x2_key), 1e-3)); +} + +/** For infeasible methods, all 3 methods shall generate the same Gauss-Newton + * iterations. */ +TEST(ManifoldOptimizer, GaussNewtonEquality) { + // Problem setting. + using namespace so2_scenario; + double init_x = 0.8; + double init_y = 0.6; + double goal_x = -2; + double goal_y = 0; + + // rot2 manifold problem + Key rot_key = 1; + Expression pt_unit(Point2(1, 0)); + Expression rot(rot_key); + Expression rotated_pt(&Rot2Projection::rotate, rot, pt_unit); + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); + NonlinearFactorGraph graph_rot2; + Point2 goal_pt(goal_x, goal_y); + graph_rot2.addExpressionFactor(noise, goal_pt, rotated_pt); + + Values init_values_rot2; + init_values_rot2.insert(rot_key, Rot2Projection::fromCosSin(0.8, 0.6)); + + // constraint manifold problem (type1 and type2) + auto costs_cm = get_graph(goal_x, goal_y); + auto constraints_cm = get_constraints(); + + Values init_values_cm; + init_values_cm.insert(x1_key, init_x); + init_values_cm.insert(x2_key, init_y); + + // optimization stages + GaussNewtonParams nopt_params; + GaussNewtonOptimizer optimizer_m(graph_rot2, init_values_rot2, nopt_params); + ManifoldOptimizerParameters mopt_params; + NonlinearMOptimizer optimizer_type1(mopt_params, nopt_params); + auto mopt_problem = optimizer_type1.initializeMoptProblem( + *costs_cm, *constraints_cm, init_values_cm); + auto mopt_noptimizer = + optimizer_type1.constructNonlinearOptimizer(mopt_problem); + + // check values at each iteration + for (size_t i = 0; i < 2; i++) { + // run one iteration + optimizer_m.iterate(); + mopt_noptimizer->iterate(); + // retrieve values + Values values_m = optimizer_m.values(); + Values values_type1 = + optimizer_type1.baseValues(mopt_problem, mopt_noptimizer->values()); + // check values are the same + auto expected_rot = values_m.at(rot_key); + double expected_x = expected_rot.c(); + double expected_y = expected_rot.s(); + double type1_x = values_type1.atDouble(x1_key); + double type1_y = values_type1.atDouble(x2_key); + double tol = 1e-5; + EXPECT_DOUBLES_EQUAL(expected_x, type1_x, tol); + EXPECT_DOUBLES_EQUAL(expected_y, type1_y, tol); + } +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testMinTorqueFactor.cpp b/tests/testMinTorqueFactor.cpp index 01c0f5ae4..1a3e57cd2 100644 --- a/tests/testMinTorqueFactor.cpp +++ b/tests/testMinTorqueFactor.cpp @@ -12,6 +12,7 @@ */ #include +#include #include #include #include @@ -23,8 +24,6 @@ #include -#include "gtdynamics/factors/MinTorqueFactor.h" - using namespace gtdynamics; using gtsam::assert_equal; diff --git a/tests/testMultiJacobian.cpp b/tests/testMultiJacobian.cpp new file mode 100644 index 000000000..56a28f264 --- /dev/null +++ b/tests/testMultiJacobian.cpp @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testMultiJacobian.cpp + * @brief Test jacobian w.r.t. multiple variables. + * @author Yetong Zhang + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtdynamics; + +// Test add and mult operators for MultiJacobian +TEST(MultiJacobian, Add_Mult) { + Key x1 = 1; + Key x2 = 2; + Key x3 = 3; + MultiJacobian jac1, jac2; + Matrix H1_x1 = (Matrix(2, 2) << 1, 2, 3, 4).finished(); + Matrix H1_x2 = (Matrix(2, 1) << 3, 2).finished(); + Matrix H2_x1 = (Matrix(2, 2) << 2, 2, 3, 3).finished(); + Matrix H2_x3 = (Matrix(2, 2) << 4, 3, 2, 1).finished(); + jac1.addJacobian(x1, H1_x1); + jac1.addJacobian(x2, H1_x2); + jac2.addJacobian(x1, H2_x1); + jac2.addJacobian(x3, H2_x3); + + VectorValues expected_row; + expected_row.insert(x1, Vector2(1, 2)); + expected_row.insert(x2, Vector1(3)); + EXPECT(assert_equal(expected_row, jac1.row(0))); + + MultiJacobian expected_sum; + expected_sum.insert({x1, (Matrix(2, 2) << 3, 4, 6, 7).finished()}); + expected_sum.insert({x2, H1_x2}); + expected_sum.insert({x3, H2_x3}); + EXPECT(expected_sum.equals(jac1 + jac2)); + + MultiJacobian jac12_sum = jac1; + jac12_sum += jac2; + EXPECT(expected_sum.equals(jac12_sum)); + + Matrix m = I_2x2 * 2; + MultiJacobian expected_mult1; + expected_mult1.insert({x1, (Matrix(2, 2) << 2, 4, 6, 8).finished()}); + expected_mult1.insert({x2, (Matrix(2, 1) << 6, 4).finished()}); + EXPECT(expected_mult1.equals(m * jac1)); + + MultiJacobian jac12_stack = MultiJacobian::VerticalStack(jac1, jac2); + MultiJacobian expected_stack12; + expected_stack12.insert( + {x1, (Matrix(4, 2) << 1, 2, 3, 4, 2, 2, 3, 3).finished()}); + expected_stack12.insert({x2, (Matrix(4, 1) << 3, 2, 0, 0).finished()}); + expected_stack12.insert( + {x3, (Matrix(4, 2) << 0, 0, 0, 0, 4, 3, 2, 1).finished()}); + EXPECT(expected_stack12.equals(jac12_stack)); +} + + +// Test add and mult operators for MultiJacobian +TEST(MultiJacobians, Mult) { + Key x1 = 1; + Key x2 = 2; + Key x3 = 3; + Key x4 = 4; + + MultiJacobian jac1_x1, jac1_x2, jac1_x3, jac1_x4; + jac1_x1.insert({x1, I_2x2}); + jac1_x2.insert({x2, I_2x2}); + jac1_x3.insert({x3, I_2x2}); + jac1_x4.insert({x1, (Matrix(1,2)<<1,2).finished()}); + jac1_x4.insert({x2, (Matrix(1,2)<<2,1).finished()}); + jac1_x4.insert({x3, (Matrix(1,2)<<1,-1).finished()}); + MultiJacobians jacs1{{x1, jac1_x1}, {x2, jac1_x2}, {x3, jac1_x3}, {x4, jac1_x4}}; + + MultiJacobian jac2_x1, jac2_x2, jac2_x3; + jac2_x1.insert({x1, I_2x2}); + jac2_x2.insert({x2, I_2x2}); + jac2_x3.insert({x1, 2*I_2x2}); + jac2_x3.insert({x2, -1*I_2x2}); + MultiJacobians jacs2{{x1, jac2_x1}, {x2, jac2_x2}, {x3, jac2_x3}}; + + MultiJacobian expected_jac_x1, expected_jac_x2, expected_jac_x3, expected_jac_x4; + expected_jac_x1.insert({x1, I_2x2}); + expected_jac_x2.insert({x2, I_2x2}); + expected_jac_x3.insert({x1, 2*I_2x2}); + expected_jac_x3.insert({x2, -1*I_2x2}); + expected_jac_x4.insert({x1, (Matrix(1,2)<<1+2,2-2).finished()}); + expected_jac_x4.insert({x2, (Matrix(1,2)<<2-1,1+1).finished()}); + + MultiJacobians jacs_mult = JacobiansMultiply(jacs1, jacs2); + EXPECT(expected_jac_x1.equals(jacs_mult.at(x1))); + EXPECT(expected_jac_x2.equals(jacs_mult.at(x2))); + EXPECT(expected_jac_x3.equals(jacs_mult.at(x3))); + EXPECT(expected_jac_x4.equals(jacs_mult.at(x4))); +} + +/// Test computing jacobians from a bayes net. +TEST(MultiJacobian, ComputeBayesNetJacobian) { + /// Construct a bayes net + Key x1 = 1; + Key x2 = 2; + Key x3 = 3; + Key x4 = 4; + Key x5 = 5; + GaussianFactorGraph graph; + auto model1 = noiseModel::Isotropic::Sigma(1, 1.0); + auto model2 = noiseModel::Isotropic::Sigma(2, 1.0); + graph.add(JacobianFactor(x1, I_1x1, x2, I_1x1, x3, -I_1x1, Vector1(0), model1)); + Matrix21 H_3, H_4; + H_3 << 1, 0; + H_4 << 0, 1; + graph.add(JacobianFactor(x3, H_3, x4, H_4, x5, -I_2x2, Vector2(0, 0), model2)); + + Ordering ordering; + ordering.push_back(x5); + ordering.push_back(x3); + auto elim_result = graph.eliminatePartialSequential(ordering); + auto bayes_net = elim_result.first; + + + MultiJacobians jacobians; + KeyVector basis_keys{x1, x2, x4}; + std::map var_dim; + var_dim.emplace(x1, 1); + var_dim.emplace(x2, 1); + var_dim.emplace(x4, 1); + ComputeBayesNetJacobian(*bayes_net, basis_keys, var_dim, jacobians); + + MultiJacobian jacobian_x3, jacobian_x5; + jacobian_x3.addJacobian(x1, I_1x1); + jacobian_x3.addJacobian(x2, I_1x1); + jacobian_x5.addJacobian(x1, H_3); + jacobian_x5.addJacobian(x2, H_3); + jacobian_x5.addJacobian(x4, H_4); + EXPECT(jacobians.at(x1).equals(MultiJacobian::Identity(x1, 1))); + EXPECT(jacobians.at(x2).equals(MultiJacobian::Identity(x2, 1))); + EXPECT(jacobians.at(x4).equals(MultiJacobian::Identity(x4, 1))); + EXPECT(jacobians.at(x3).equals(jacobian_x3)); + EXPECT(jacobians.at(x5).equals(jacobian_x5)); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testMutableLMOptimizer.cpp b/tests/testMutableLMOptimizer.cpp new file mode 100644 index 000000000..0f82a99db --- /dev/null +++ b/tests/testMutableLMOptimizer.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testTspaceBasis.cpp + * @brief Test tagent space basis for constraint manifold. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtdynamics; + +/** Simple example Pose3 with between constraints. */ +TEST(MutableLMOptimizer, optimize) { + MutableLMOptimizer optimizer; + + Key x1_key = 1; + Key x2_key = 2; + NonlinearFactorGraph graph; + auto noise = noiseModel::Unit::Create(6); + graph.addPrior(x1_key, Pose3(Rot3(), Point3(0, 0, 0)), noise); + graph.emplace_shared>( + x1_key, x2_key, Pose3(Rot3(), Point3(0, 0, 1)), noise); + + Values values; + values.insert(x1_key, Pose3(Rot3(), Point3(0, 0, 0))); + values.insert(x2_key, Pose3(Rot3(), Point3(0, 0, 0))); + + optimizer.setGraph(graph); + optimizer.setValues(values); + auto result = optimizer.optimize(); + + Values expected_result; + expected_result.insert(x1_key, Pose3(Rot3(), Point3(0, 0, 0))); + expected_result.insert(x2_key, Pose3(Rot3(), Point3(0, 0, 1))); + EXPECT(assert_equal(expected_result, result)); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testObjectiveFactors.cpp b/tests/testObjectiveFactors.cpp index 5134fc32b..6f8fd05bc 100644 --- a/tests/testObjectiveFactors.cpp +++ b/tests/testObjectiveFactors.cpp @@ -100,7 +100,7 @@ TEST(Phase, AddGoals) { StanceTrajectory(stance_point, num_stance_steps), id, k); EXPECT_LONGS_EQUAL(num_stance_steps, factors.size()); - auto f = boost::dynamic_pointer_cast(factors.back()); + auto f = std::dynamic_pointer_cast(factors.back()); EXPECT(assert_equal(stance_point, f->goalPoint(), 1e-5)); // Check that prediction error is zero. @@ -117,12 +117,12 @@ TEST(Phase, AddGoals) { EXPECT_LONGS_EQUAL(num_swing_steps, swing_factors.size()); // Last goal point should have moved just in front of stance_point - auto g = boost::dynamic_pointer_cast(swing_factors.front()); + auto g = std::dynamic_pointer_cast(swing_factors.front()); EXPECT(assert_equal(stance_point + Point3(0.01, 0, 0.035588), g->goalPoint(), 1e-5)); // Last goal point should have moved just shy of stance_point + step - auto h = boost::dynamic_pointer_cast(swing_factors.back()); + auto h = std::dynamic_pointer_cast(swing_factors.back()); EXPECT(assert_equal(stance_point + step + Point3(-0.01, 0, 0.055228), h->goalPoint(), 1e-5)); } diff --git a/tests/testPenaltyMethodOptimizer.cpp b/tests/testPenaltyMethodOptimizer.cpp index 5923e233e..b69a56372 100644 --- a/tests/testPenaltyMethodOptimizer.cpp +++ b/tests/testPenaltyMethodOptimizer.cpp @@ -12,17 +12,16 @@ */ #include +#include +#include -#include "constrainedExample.h" -#include "gtdynamics/optimizer/PenaltyMethodOptimizer.h" +#include "gtsamConstrainedExample.h" -using namespace gtdynamics; using namespace gtsam; -using gtsam::assert_equal; -using std::map; -using std::string; -TEST(PenaltyMethodOptimizer, ConstrainedExample) { +TEST(PenaltyOptimizer, ConstrainedExample) { + using namespace constrained_example1; + using namespace constrained_example; /// Create a constrained optimization problem with 2 cost factors and 1 @@ -34,20 +33,19 @@ TEST(PenaltyMethodOptimizer, ConstrainedExample) { graph.add(ExpressionFactor(cost_noise, 0., f1)); graph.add(ExpressionFactor(cost_noise, 0., f2)); - EqualityConstraints constraints; - double tolerance = 1.0; + Vector sigmas = Vector1(0.1); auto g1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); - constraints.push_back(EqualityConstraint::shared_ptr( - new DoubleExpressionEquality(g1, tolerance))); + graph.push_back(gtsam::NonlinearEqualityConstraint::shared_ptr( + new gtsam::ExpressionEqualityConstraint(g1, 0.0, sigmas))); /// Create initial values. - Values init_values; - init_values.insert(x1_key, -0.2); - init_values.insert(x2_key, -0.2); + Values initialValues; + initialValues.insert(x1_key, -0.2); + initialValues.insert(x2_key, -0.2); /// Solve the constraint problem with Penalty method optimizer. - gtdynamics::PenaltyMethodOptimizer optimizer; - Values results = optimizer.optimize(graph, constraints, init_values); + gtsam::PenaltyOptimizer optimizer(graph, initialValues); + Values results = optimizer.optimize(); /// Check the result is correct within tolerance. Values gt_results; diff --git a/tests/testPhase.cpp b/tests/testPhase.cpp index 1ac3e1950..3b4a262c8 100644 --- a/tests/testPhase.cpp +++ b/tests/testPhase.cpp @@ -12,14 +12,14 @@ */ #include +#include +#include +#include +#include +#include #include -#include "gtdynamics/dynamics/DynamicsGraph.h" -#include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/universal_robot/sdf.h" -#include "gtdynamics/utils/Phase.h" -#include "gtdynamics/utils/Trajectory.h" #include "walkCycleExample.h" using namespace gtdynamics; @@ -32,8 +32,10 @@ TEST(Phase, All) { using namespace walk_cycle_example; Phase phase1(0, 2, phase_1); - - auto phase1_foot_constraint = boost::static_pointer_cast(phase1.constraintSpec()); + + auto phase1_foot_constraint = + std::static_pointer_cast( + phase1.constraintSpec()); Point3 cp = phase1_foot_constraint->contactPoint("tarsus_3_L3"); EXPECT(assert_equal(contact_in_com, cp)); @@ -62,13 +64,15 @@ TEST(Phase, All) { Point3 goal(1, 2, 3); const size_t k_start = 777; ContactPointGoals cp_goals = {{"tarsus_2_L2", goal}, - {"tarsus_1_L1", goal}, - {"tarsus_3_L3", goal}, - {"tarsus_4_L4", goal}, - {"tarsus_5_R4", goal}}; - gtsam::NonlinearFactorGraph factors = phase1_foot_constraint->contactPointObjectives( - walk_cycle.contactPoints(), step, cost_model, k_start, cp_goals, 2); - auto new_goals = phase1_foot_constraint->updateContactPointGoals(walk_cycle.contactPoints(), step, cp_goals); + {"tarsus_1_L1", goal}, + {"tarsus_3_L3", goal}, + {"tarsus_4_L4", goal}, + {"tarsus_5_R4", goal}}; + gtsam::NonlinearFactorGraph factors = + phase1_foot_constraint->contactPointObjectives( + walk_cycle.contactPoints(), step, cost_model, k_start, cp_goals, 2); + auto new_goals = phase1_foot_constraint->updateContactPointGoals( + walk_cycle.contactPoints(), step, cp_goals); EXPECT_LONGS_EQUAL(num_time_steps * 5, factors.size()); EXPECT(assert_equal(goal, new_goals["tarsus_2_L2"])); diff --git a/tests/testPointGoalFactor.cpp b/tests/testPointGoalFactor.cpp index f6a2b288c..d0991fead 100644 --- a/tests/testPointGoalFactor.cpp +++ b/tests/testPointGoalFactor.cpp @@ -12,6 +12,8 @@ */ #include +#include +#include #include #include #include @@ -21,9 +23,6 @@ #include #include -#include "gtdynamics/factors/PointGoalFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" - using namespace gtdynamics; using gtsam::assert_equal; using gtsam::LabeledSymbol; diff --git a/tests/testPoseFactor.cpp b/tests/testPoseFactor.cpp index bf0db19a7..05381061a 100644 --- a/tests/testPoseFactor.cpp +++ b/tests/testPoseFactor.cpp @@ -12,6 +12,8 @@ */ #include +#include +#include #include #include #include @@ -23,8 +25,6 @@ #include -#include "gtdynamics/factors/PoseFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" #include "make_joint.h" using namespace gtdynamics; @@ -40,8 +40,7 @@ using gtsam::noiseModel::Gaussian; namespace example { // nosie model Gaussian::shared_ptr cost_model = Gaussian::Covariance(gtsam::I_6x6); -gtsam::Key wTp_key = PoseKey(1), wTc_key = PoseKey(2), - q_key = JointAngleKey(1); +gtsam::Key wTp_key = PoseKey(1), wTc_key = PoseKey(2), q_key = JointAngleKey(1); } // namespace example // Test twist factor for stationary case @@ -50,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, @@ -78,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); @@ -112,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); @@ -130,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/testPreintegratedContactFactors.cpp b/tests/testPreintegratedContactFactors.cpp index 12e79c4b9..ad6fb67d1 100644 --- a/tests/testPreintegratedContactFactors.cpp +++ b/tests/testPreintegratedContactFactors.cpp @@ -12,6 +12,7 @@ */ #include +#include #include #include #include @@ -24,8 +25,6 @@ #include -#include "gtdynamics/factors/PreintegratedContactFactors.h" - using namespace gtdynamics; using namespace gtsam; using gtdynamics::PoseKey; diff --git a/tests/testPrismaticJoint.cpp b/tests/testPrismaticJoint.cpp index d86e4d62a..be9d2059a 100644 --- a/tests/testPrismaticJoint.cpp +++ b/tests/testPrismaticJoint.cpp @@ -12,16 +12,15 @@ */ #include +#include +#include +#include +#include +#include #include #include #include -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/universal_robot/PrismaticJoint.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/universal_robot/sdf.h" -#include "gtdynamics/utils/utils.h" - using namespace gtdynamics; using gtsam::assert_equal; using gtsam::Point3; @@ -45,63 +44,61 @@ TEST(Joint, params_constructor_prismatic) { 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); - - // get shared ptr - EXPECT(j1->shared() == j1); + auto j1 = + PrismaticJoint(1, "j1", Pose3(Rot3::Rx(1.5707963268), Point3(0, 0, 2)), + l1, l2, j1_axis, parameters); // get, set ID - EXPECT(j1->id() == 1); + EXPECT(j1.id() == 1); // name - EXPECT(assert_equal(j1->name(), "j1")); + EXPECT(assert_equal(j1.name(), "j1")); // joint effort type - EXPECT(j1->parameters().effort_type == JointEffortType::Actuated); + EXPECT(j1.parameters().effort_type == JointEffortType::Actuated); // other link - EXPECT(j1->otherLink(l2) == l1); - EXPECT(j1->otherLink(l1) == l2); + EXPECT(j1.otherLink(l2) == l1); + EXPECT(j1.otherLink(l1) == l2); // rest transform Pose3 T_12comRest(Rot3::Rx(1.5707963268), Point3(0, -1, 1)); Pose3 T_21comRest(Rot3::Rx(-1.5707963268), Point3(0, -1, -1)); - EXPECT(assert_equal(T_12comRest, j1->relativePoseOf(l2, 0.0), 1e-5)); - EXPECT(assert_equal(T_21comRest, j1->relativePoseOf(l1, 0.0), 1e-5)); + EXPECT(assert_equal(T_12comRest, j1.relativePoseOf(l2, 0.0), 1e-5)); + EXPECT(assert_equal(T_21comRest, j1.relativePoseOf(l1, 0.0), 1e-5)); // transform to (translating +1) Pose3 T_12com(Rot3::Rx(1.5707963268), Point3(0, -2, 1)); Pose3 T_21com(Rot3::Rx(-1.5707963268), Point3(0, -1, -2)); - EXPECT(assert_equal(T_12com, j1->relativePoseOf(l2, 1), 1e-5)); - EXPECT(assert_equal(T_21com, j1->relativePoseOf(l1, 1), 1e-5)); + EXPECT(assert_equal(T_12com, j1.relativePoseOf(l2, 1), 1e-5)); + EXPECT(assert_equal(T_21com, j1.relativePoseOf(l1, 1), 1e-5)); // screw axis gtsam::Vector6 screw_axis_l1, screw_axis_l2; screw_axis_l1 << 0, 0, 0, 0, 1, 0; screw_axis_l2 << 0, 0, 0, 0, 0, 1; - EXPECT(assert_equal(screw_axis_l1, j1->screwAxis(l1))); - EXPECT(assert_equal(screw_axis_l2, j1->screwAxis(l2), 1e-5)); + EXPECT(assert_equal(screw_axis_l1, j1.screwAxis(l1))); + EXPECT(assert_equal(screw_axis_l2, j1.screwAxis(l2), 1e-5)); // links - auto links = j1->links(); + auto links = j1.links(); EXPECT(links[0] == l1); EXPECT(links[1] == l2); // parent & child link - EXPECT(j1->parent() == l1); - EXPECT(j1->child() == l2); + EXPECT(j1.parent() == l1); + EXPECT(j1.child() == l2); // joint limit EXPECT(assert_equal(parameters.scalar_limits.value_lower_limit, - j1->parameters().scalar_limits.value_lower_limit)); + j1.parameters().scalar_limits.value_lower_limit)); EXPECT(assert_equal(parameters.scalar_limits.value_upper_limit, - j1->parameters().scalar_limits.value_upper_limit)); + j1.parameters().scalar_limits.value_upper_limit)); EXPECT(assert_equal(parameters.scalar_limits.value_limit_threshold, - j1->parameters().scalar_limits.value_limit_threshold)); + j1.parameters().scalar_limits.value_limit_threshold)); } +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION BOOST_CLASS_EXPORT(gtdynamics::PrismaticJoint) TEST(PrismaticJoint, Serialization) { @@ -112,7 +109,7 @@ TEST(PrismaticJoint, Serialization) { JointParams parameters; const gtsam::Vector3 j1_axis(0, 0, 1); - auto j1 = boost::make_shared( + auto j1 = std::make_shared( 1, "j1", Pose3(Rot3::Rx(1.5707963268), Point3(0, 0, 2)), l1, l2, j1_axis, parameters); @@ -121,6 +118,7 @@ TEST(PrismaticJoint, Serialization) { EXPECT(equalsDereferencedXML(j1)); EXPECT(equalsDereferencedBinary(j1)); } +#endif int main() { TestResult tr; diff --git a/tests/testRetractor.cpp b/tests/testRetractor.cpp new file mode 100644 index 000000000..2823e4483 --- /dev/null +++ b/tests/testRetractor.cpp @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testRetractor.cpp + * @brief Test retractor for constraint manifold. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gtdynamics/cmopt/TspaceBasis.h" + +using namespace gtsam; +using namespace gtdynamics; + +/** Simple example Pose3 with between constraints. */ +TEST(TspaceBasis, connected_poses) { + Key x1_key = 1; + Key x2_key = 2; + Key x3_key = 3; + + // Constraints. + auto constraints = std::make_shared(); + auto noise = noiseModel::Unit::Create(6); + auto factor12 = std::make_shared>( + x1_key, x2_key, Pose3(Rot3(), Point3(0, 0, 1)), noise); + auto factor23 = std::make_shared>( + x2_key, x3_key, Pose3(Rot3(), Point3(0, 0, 1)), noise); + constraints->emplace_shared(factor12); + constraints->emplace_shared(factor23); + + // Create manifold values for testing. + Values base_values; + base_values.insert(x1_key, Pose3(Rot3(), Point3(0, 0, -1))); + base_values.insert(x2_key, Pose3(Rot3(), Point3(0, 0, 1))); + base_values.insert(x3_key, Pose3(Rot3(), Point3(0, 0, 3))); + + + // Construct retractor. + auto params_uopt = std::make_shared(); + auto params_proj = std::make_shared(); + auto params_fix_vars = std::make_shared(); + params_fix_vars->use_basis_keys = true; + KeyVector basis_keys{x3_key}; + UoptRetractor retractor_uopt(constraints, params_uopt); + ProjRetractor retractor_proj(constraints, params_proj); + BasisRetractor retractor_basis(constraints, params_fix_vars, basis_keys); + + Values values_uopt = retractor_uopt.retractConstraints(base_values); + Values expected_uopt; + expected_uopt.insert(x1_key, Pose3(Rot3(), Point3(0, 0, 0))); + expected_uopt.insert(x2_key, Pose3(Rot3(), Point3(0, 0, 1))); + expected_uopt.insert(x3_key, Pose3(Rot3(), Point3(0, 0, 2))); + EXPECT(assert_equal(expected_uopt, values_uopt)); + + Values values_proj = retractor_proj.retractConstraints(base_values); + Values expected_proj; + expected_proj.insert(x1_key, Pose3(Rot3(), Point3(0, 0, 0))); + expected_proj.insert(x2_key, Pose3(Rot3(), Point3(0, 0, 1))); + expected_proj.insert(x3_key, Pose3(Rot3(), Point3(0, 0, 2))); + EXPECT(assert_equal(expected_proj, values_proj, 1e-4)); + + Values values_basis = retractor_basis.retractConstraints(base_values); + Values expected_basis; + expected_basis.insert(x1_key, Pose3(Rot3(), Point3(0, 0, 1))); + expected_basis.insert(x2_key, Pose3(Rot3(), Point3(0, 0, 2))); + expected_basis.insert(x3_key, Pose3(Rot3(), Point3(0, 0, 3))); + EXPECT(assert_equal(expected_basis, values_basis)); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testRevoluteJoint.cpp b/tests/testRevoluteJoint.cpp index eb8cad545..53c865bfe 100644 --- a/tests/testRevoluteJoint.cpp +++ b/tests/testRevoluteJoint.cpp @@ -12,18 +12,17 @@ */ #include +#include +#include +#include +#include +#include +#include #include #include #include #include -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/universal_robot/RevoluteJoint.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/universal_robot/sdf.h" -#include "gtdynamics/utils/utils.h" -#include "gtdynamics/utils/values.h" - using gtsam::assert_equal; using gtsam::Matrix; using gtsam::Matrix61; @@ -212,6 +211,7 @@ TEST(RevoluteJoint, ParentTchild) { EXPECT(assert_equal(expected_pTc, pTc, 1e-4)); } +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION BOOST_CLASS_EXPORT(gtdynamics::RevoluteJoint) TEST(RevoluteJoint, Serialization) { @@ -222,6 +222,7 @@ TEST(RevoluteJoint, Serialization) { EXPECT(equalsXML(j1)); EXPECT(equalsBinary(j1)); } +#endif int main() { TestResult tr; diff --git a/tests/testRobot.cpp b/tests/testRobot.cpp index a0bdad6b3..740fa556d 100644 --- a/tests/testRobot.cpp +++ b/tests/testRobot.cpp @@ -14,21 +14,18 @@ #include #include +#include +#include +#include +#include +#include +#include +#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" -#include "gtdynamics/utils/utils.h" - using namespace gtdynamics; using gtsam::assert_equal; using gtsam::Point3; @@ -104,8 +101,8 @@ TEST(Robot, ForwardKinematics) { Values results = robot.forwardKinematics(values); // The CoM frames at rest are: - Pose3 T_wl1_rest(Rot3::identity(), Point3(0, 0, 1)); - Pose3 T_wl2_rest(Rot3::identity(), Point3(0, 0, 3)); + Pose3 T_wl1_rest(Rot3::Identity(), Point3(0, 0, 1)); + Pose3 T_wl2_rest(Rot3::Identity(), Point3(0, 0, 3)); // At rest, all the twists are 0: Vector6 V_l1_rest, V_l2_rest; V_l1_rest << 0, 0, 0, 0, 0, 0; @@ -123,7 +120,7 @@ TEST(Robot, ForwardKinematics) { Values results2 = robot.forwardKinematics(values2); - Pose3 T_wl1_move(Rot3::identity(), Point3(0, 0, 1)); // link1 stays put + Pose3 T_wl1_move(Rot3::Identity(), Point3(0, 0, 1)); // link1 stays put // link2 is rotated by 90 degrees and now points along -Y axis. Pose3 T_wl2_move(Rot3::Rx(M_PI_2), Point3(0, -1, 2)); Vector6 V_l1_move, V_l2_move; @@ -164,10 +161,10 @@ TEST(Robot, ForwardKinematicsRPR) { robot = robot.fixLink("link_0"); Values fk_results = robot.forwardKinematics(values); - Pose3 T_wl0_rest(Rot3::identity(), Point3(0, 0, 0.1)); - Pose3 T_wl1_rest(Rot3::identity(), Point3(0, 0, 0.5)); - Pose3 T_wl2_rest(Rot3::identity(), Point3(0, 0, 1.1)); - Pose3 T_wl3_rest(Rot3::identity(), Point3(0, 0, 1.7)); + Pose3 T_wl0_rest(Rot3::Identity(), Point3(0, 0, 0.1)); + Pose3 T_wl1_rest(Rot3::Identity(), Point3(0, 0, 0.5)); + Pose3 T_wl2_rest(Rot3::Identity(), Point3(0, 0, 1.1)); + Pose3 T_wl3_rest(Rot3::Identity(), Point3(0, 0, 1.7)); Vector6 V_l0_rest, V_l1_rest, V_l2_rest, V_l3_rest; V_l0_rest << 0, 0, 0, 0, 0, 0; V_l1_rest << 0, 0, 0, 0, 0, 0; @@ -191,7 +188,7 @@ TEST(Robot, ForwardKinematicsRPR) { fk_results = robot.forwardKinematics(values2); - Pose3 T_wl0_move(Rot3::identity(), Point3(0, 0, 0.1)); + Pose3 T_wl0_move(Rot3::Identity(), Point3(0, 0, 0.1)); Pose3 T_wl1_move(Rot3::Ry(M_PI_2), Point3(0.3, 0, 0.2)); Pose3 T_wl2_move(Rot3::Ry(M_PI_2), Point3(1.4, 0, 0.2)); Pose3 T_wl3_move(Rot3::Ry(M_PI_2), Point3(2.0, 0, 0.2)); @@ -237,6 +234,54 @@ TEST(ForwardKinematics, FourBar) { THROWS_EXCEPTION(robot.forwardKinematics(wrong_vels)); } +TEST(ForwardKinematics, A1) { + Robot robot = + CreateRobotFromFile(kUrdfPath + std::string("a1/a1.urdf"), "", true); + robot = robot.fixLink("trunk"); + + Values values; + Values fk_results = robot.forwardKinematics(values); + // 21 joint angles, 21, joint velocities, 22 link poses, 22 link twists + EXPECT_LONGS_EQUAL(86, fk_results.size()); + + Values joint_angles; + // Sanity check that 0 joint angles gives us the same pose as from the URDF + std::vector angles = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + size_t jidx = 0; + for (auto&& joint : robot.joints()) { + InsertJointAngle(&joint_angles, joint->id(), 0, angles[jidx]); + jidx += 1; + } + + // This is the toe joint we wish to test. + size_t joint_id = 20; + + fk_results = robot.forwardKinematics(joint_angles, 0, std::string("trunk")); + EXPECT(assert_equal(Pose3(Rot3(), Point3(-0.183, -0.13205, -0.4)), + Pose(fk_results, joint_id, 0))); + + // Joint angles from A1 simulation. + angles = { + 0.000174304, 0, 0.923033, -1.83381, 0, 0.000172539, + 0, 0.924125, -1.83302, 0, 0.000137167, 0, + 0.878277, -1.85284, 0, 0.000140037, 0, 0.877832, + -1.852, 0, 0}; + + jidx = 0; + joint_angles.clear(); + for (auto&& joint : robot.joints()) { + InsertJointAngle(&joint_angles, joint->id(), 0, angles[jidx]); + jidx += 1; + } + fk_results = robot.forwardKinematics(joint_angles, 0, std::string("trunk")); + // regression + EXPECT(assert_equal( + Pose3(Rot3(0.638821, 0, 0.769356, 0, 1, 0, -0.769356, 0, 0.638821), + Point3(-0.336871, -0.13205, -0.327764)), + Pose(fk_results, 20, 0), 1e-6)); +} + TEST(Robot, Equality) { Robot robot1 = CreateRobotFromFile( kSdfPath + std::string("test/four_bar_linkage_pure.sdf")); @@ -255,6 +300,8 @@ TEST(Robot, Equality) { EXPECT(!robot1.equals(robot2)); } +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + // Declaration needed for serialization of derived class. BOOST_CLASS_EXPORT(gtdynamics::RevoluteJoint) BOOST_CLASS_EXPORT(gtdynamics::HelicalJoint) @@ -269,6 +316,7 @@ TEST(Robot, Serialization) { EXPECT(equalsXML(robot)); EXPECT(equalsBinary(robot)); } +#endif int main() { TestResult tr; diff --git a/tests/testSdf.cpp b/tests/testSdf.cpp index 69449d548..16bb7e0cd 100644 --- a/tests/testSdf.cpp +++ b/tests/testSdf.cpp @@ -24,6 +24,8 @@ #include #include +#include + using namespace gtdynamics; using gtsam::assert_equal; using gtsam::Point3; @@ -54,6 +56,18 @@ TEST(Sdf, load_and_parse_urdf_file) { EXPECT(assert_equal(3, simple_urdf.LinkByName("l2")->Inertial().Moi()(2, 2))); } +TEST(Sdf, load_vision60_urdf_contact_goal_links) { + const auto robot = + CreateRobotFromFile(kUrdfPath + std::string("vision60.urdf")); + + const std::array link_names = {"lower1", "lower0", "lower2", + "lower3"}; + for (const auto& name : link_names) { + const auto link = robot.link(name); + EXPECT(assert_equal(name, link->name())); + } +} + TEST(Sdf, load_and_parse_sdf_file) { auto simple_sdf = GetSdf(kSdfPath + std::string("test/simple.sdf")); @@ -80,10 +94,10 @@ TEST(Sdf, load_and_parse_sdf_world_file) { EXPECT(assert_equal(0.03, l1.Inertial().Moi()(2, 2))); } -TEST(Sdf, Pose3FromIgnition) { - ignition::math::Pose3d pose_to_parse(-1, 1, -1, M_PI / 2, 0, -M_PI); +TEST(Sdf, Pose3FromGazebo) { + gz::math::Pose3d pose_to_parse(-1, 1, -1, M_PI / 2, 0, -M_PI); - gtsam::Pose3 parsed_pose = Pose3FromIgnition(pose_to_parse); + gtsam::Pose3 parsed_pose = Pose3FromGazebo(pose_to_parse); EXPECT(assert_equal(gtsam::Pose3(gtsam::Rot3::RzRyRx(M_PI / 2, 0, -M_PI), gtsam::Point3(-1, 1, -1)), @@ -186,8 +200,8 @@ TEST(Urdf, ConstructorLink) { const gtsam::Vector3 j1_axis = GetSdfAxis(*simple_urdf.JointByName("j1")); // Test constructor. - auto j1 = boost::make_shared(1, "j1", bTj, l1, l2, j1_axis, - j1_parameters); + auto j1 = std::make_shared(1, "j1", bTj, l1, l2, j1_axis, + j1_parameters); // get shared ptr EXPECT(l1->shared() == l1); @@ -254,62 +268,58 @@ TEST(Sdf, urdf_constructor_revolute) { const gtsam::Vector3 j1_axis = GetSdfAxis(*simple_urdf.JointByName("j1")); // Test constructor. - auto j1 = boost::make_shared(1, "j1", bMj1, l1, l2, j1_axis, - j1_parameters); - - // get shared ptr - EXPECT(j1->shared() == j1); + auto j1 = RevoluteJoint(1, "j1", bMj1, l1, l2, j1_axis, j1_parameters); // get ID - EXPECT(j1->id() == 1); + EXPECT(j1.id() == 1); // name - EXPECT(assert_equal(j1->name(), "j1")); + EXPECT(assert_equal(j1.name(), "j1")); // joint type - EXPECT(j1->type() == Joint::Type::Revolute); + EXPECT(j1.type() == Joint::Type::Revolute); // joint effort type - EXPECT(j1->parameters().effort_type == JointEffortType::Actuated); + EXPECT(j1.parameters().effort_type == JointEffortType::Actuated); // other link - EXPECT(j1->otherLink(l2) == l1); - EXPECT(j1->otherLink(l1) == l2); + EXPECT(j1.otherLink(l2) == l1); + EXPECT(j1.otherLink(l1) == l2); // rest transform Pose3 M_12(Rot3::Rx(0), Point3(0, 0, 2)); Pose3 M_21(Rot3::Rx(0), Point3(0, 0, -2)); - EXPECT(assert_equal(M_12, j1->relativePoseOf(l2, 0.0))); - EXPECT(assert_equal(M_21, j1->relativePoseOf(l1, 0.0))); + EXPECT(assert_equal(M_12, j1.relativePoseOf(l2, 0.0))); + EXPECT(assert_equal(M_21, j1.relativePoseOf(l1, 0.0))); // transform to (rotating -pi/2) Pose3 T_12(Rot3::Rx(-M_PI / 2), Point3(0, 1, 1)); Pose3 T_21(Rot3::Rx(M_PI / 2), Point3(0, 1, -1)); - EXPECT(assert_equal(T_12, j1->relativePoseOf(l2, -M_PI / 2))); - EXPECT(assert_equal(T_21, j1->relativePoseOf(l1, -M_PI / 2))); + EXPECT(assert_equal(T_12, j1.relativePoseOf(l2, -M_PI / 2))); + EXPECT(assert_equal(T_21, j1.relativePoseOf(l1, -M_PI / 2))); // screw axis gtsam::Vector6 screw_axis_l1, screw_axis_l2; screw_axis_l1 << -1, 0, 0, 0, -1, 0; screw_axis_l2 << 1, 0, 0, 0, -1, 0; - EXPECT(assert_equal(screw_axis_l1, j1->screwAxis(l1))); - EXPECT(assert_equal(screw_axis_l2, j1->screwAxis(l2))); + EXPECT(assert_equal(screw_axis_l1, j1.screwAxis(l1))); + EXPECT(assert_equal(screw_axis_l2, j1.screwAxis(l2))); // links - auto links = j1->links(); + auto links = j1.links(); EXPECT(links[0] == l1); EXPECT(links[1] == l2); // parent & child link - EXPECT(j1->parent() == l1); - EXPECT(j1->child() == l2); + EXPECT(j1.parent() == l1); + EXPECT(j1.child() == l2); // joint limit - EXPECT(assert_equal(-1.57, j1->parameters().scalar_limits.value_lower_limit)); - EXPECT(assert_equal(1.57, j1->parameters().scalar_limits.value_upper_limit)); + EXPECT(assert_equal(-1.57, j1.parameters().scalar_limits.value_lower_limit)); + EXPECT(assert_equal(1.57, j1.parameters().scalar_limits.value_upper_limit)); EXPECT( - assert_equal(1e-9, j1->parameters().scalar_limits.value_limit_threshold)); + assert_equal(1e-9, j1.parameters().scalar_limits.value_limit_threshold)); } // Construct a Revolute Joint from SDF and ensure all values are as expected. @@ -333,24 +343,23 @@ TEST(Sdf, sdf_constructor_revolute) { // constructor for j1 JointParams j1_parameters; j1_parameters.effort_type = JointEffortType::Actuated; - auto j1 = boost::make_shared(1, "joint_1", bMj1, l0, l1, - j1_axis, j1_parameters); + auto j1 = RevoluteJoint(1, "joint_1", bMj1, l0, l1, j1_axis, j1_parameters); // check screw axis gtsam::Vector6 screw_axis_j1_l0, screw_axis_j1_l1; screw_axis_j1_l0 << 0, 0, -1, 0, 0, 0; screw_axis_j1_l1 << 0, 0, 1, 0, 0, 0; - EXPECT(assert_equal(screw_axis_j1_l0, j1->screwAxis(l0))); - EXPECT(assert_equal(screw_axis_j1_l1, j1->screwAxis(l1))); + EXPECT(assert_equal(screw_axis_j1_l0, j1.screwAxis(l0))); + EXPECT(assert_equal(screw_axis_j1_l1, j1.screwAxis(l1))); // Check transform from l0 com to l1 com at rest and at various angles. - Pose3 M_01(Rot3::identity(), Point3(0, 0, 0.4)); + Pose3 M_01(Rot3::Identity(), Point3(0, 0, 0.4)); Pose3 T_01_neg(Rot3::Rz(-M_PI / 2), Point3(0, 0, 0.4)); Pose3 T_01_pos(Rot3::Rz(M_PI / 2), Point3(0, 0, 0.4)); - EXPECT(assert_equal(M_01, j1->relativePoseOf(l1, 0.0))); - EXPECT(assert_equal(T_01_neg, j1->relativePoseOf(l1, -M_PI / 2))); - EXPECT(assert_equal(T_01_pos, j1->relativePoseOf(l1, M_PI / 2))); + EXPECT(assert_equal(M_01, j1.relativePoseOf(l1, 0.0))); + EXPECT(assert_equal(T_01_neg, j1.relativePoseOf(l1, -M_PI / 2))); + EXPECT(assert_equal(T_01_pos, j1.relativePoseOf(l1, M_PI / 2))); // constructor for j2 JointParams j2_parameters; @@ -361,8 +370,8 @@ TEST(Sdf, sdf_constructor_revolute) { const gtsam::Vector3 j2_axis = GetSdfAxis(*model.JointByName("joint_2")); - auto j2 = boost::make_shared(2, "joint_2", bMj2, l1, l2, - j2_axis, j2_parameters); + auto j2 = std::make_shared(2, "joint_2", bMj2, l1, l2, j2_axis, + j2_parameters); // check screw axis gtsam::Vector6 screw_axis_j2_l1, screw_axis_j2_l2; @@ -372,7 +381,7 @@ TEST(Sdf, sdf_constructor_revolute) { EXPECT(assert_equal(screw_axis_j2_l2, j2->screwAxis(l2))); // Check transform from l1 com to l2 com at rest and at various angles. - Pose3 M_12(Rot3::identity(), Point3(0, 0, 0.6)); + Pose3 M_12(Rot3::Identity(), Point3(0, 0, 0.6)); Pose3 T_12_pi_2(Rot3::Ry(M_PI / 2), Point3(0.3, 0.0, 0.3)); Pose3 T_12_pi_4(Rot3::Ry(M_PI / 4), Point3(0.2121, 0.0, 0.5121)); @@ -405,13 +414,12 @@ TEST(Sdf, limit_params) { GetJointFrame(*model.JointByName("j1"), sdf_link_l1, sdf_link_l2); const gtsam::Vector3 j1_axis = GetSdfAxis(*model.JointByName("j1")); - auto j1 = boost::make_shared(1, "j1", j1_bTj, l1, l2, j1_axis, - j1_parameters); + auto j1 = RevoluteJoint(1, "j1", j1_bTj, l1, l2, j1_axis, j1_parameters); - EXPECT(assert_equal(-1.57, j1->parameters().scalar_limits.value_lower_limit)); - EXPECT(assert_equal(1.57, j1->parameters().scalar_limits.value_upper_limit)); + EXPECT(assert_equal(-1.57, j1.parameters().scalar_limits.value_lower_limit)); + EXPECT(assert_equal(1.57, j1.parameters().scalar_limits.value_upper_limit)); EXPECT( - assert_equal(1e-9, j1->parameters().scalar_limits.value_limit_threshold)); + assert_equal(1e-9, j1.parameters().scalar_limits.value_limit_threshold)); // Check revolute joint limits parsed correctly for a robot with no limits. auto model2 = @@ -432,14 +440,18 @@ TEST(Sdf, limit_params) { const gtsam::Vector3 joint_1_axis = GetSdfAxis(*model2.JointByName("joint_1")); - auto joint_1 = boost::make_shared(1, "joint_1", joint_1_bTj, - link_0, link_1, joint_1_axis, - joint_1_parameters); + auto joint_1 = + std::make_shared(1, "joint_1", joint_1_bTj, link_0, link_1, + joint_1_axis, joint_1_parameters); + + // Check the lower limit, expect negative infinity + EXPECT(std::isinf(joint_1->parameters().scalar_limits.value_lower_limit)); + EXPECT(joint_1->parameters().scalar_limits.value_lower_limit < 0); + + // Check the upper limit, expect positive infinity + EXPECT(std::isinf(joint_1->parameters().scalar_limits.value_upper_limit)); + EXPECT(joint_1->parameters().scalar_limits.value_upper_limit > 0); - EXPECT(assert_equal(-1e16, - joint_1->parameters().scalar_limits.value_lower_limit)); - EXPECT(assert_equal(1e16, - joint_1->parameters().scalar_limits.value_upper_limit)); EXPECT(assert_equal( 1e-9, joint_1->parameters().scalar_limits.value_limit_threshold)); } @@ -466,61 +478,57 @@ TEST(Sdf, urdf_constructor_prismatic) { const gtsam::Vector3 j1_axis = GetSdfAxis(*simple_urdf.JointByName("j1")); // Test constructor. - auto j1 = boost::make_shared(1, "j1", bTj, l1, l2, j1_axis, - j1_parameters); - - // get shared ptr - EXPECT(j1->shared() == j1); + auto j1 = PrismaticJoint(1, "j1", bTj, l1, l2, j1_axis, j1_parameters); // get ID - EXPECT(j1->id() == 1); + EXPECT(j1.id() == 1); // name - EXPECT(assert_equal(j1->name(), "j1")); + EXPECT(assert_equal(j1.name(), "j1")); // joint type - EXPECT(j1->type() == Joint::Type::Prismatic); + EXPECT(j1.type() == Joint::Type::Prismatic); // joint effort type - EXPECT(j1->parameters().effort_type == JointEffortType::Actuated); + EXPECT(j1.parameters().effort_type == JointEffortType::Actuated); // other link - EXPECT(j1->otherLink(l2) == l1); - EXPECT(j1->otherLink(l1) == l2); + EXPECT(j1.otherLink(l2) == l1); + EXPECT(j1.otherLink(l1) == l2); // rest transform Pose3 M_12(Rot3::Rx(1.5707963268), Point3(0, -1, 1)); Pose3 M_21(Rot3::Rx(-1.5707963268), Point3(0, -1, -1)); - EXPECT(assert_equal(M_12, j1->relativePoseOf(l2, 0), 1e-5)); - EXPECT(assert_equal(M_21, j1->relativePoseOf(l1, 0), 1e-5)); + EXPECT(assert_equal(M_12, j1.relativePoseOf(l2, 0), 1e-5)); + EXPECT(assert_equal(M_21, j1.relativePoseOf(l1, 0), 1e-5)); // transform to (translating +1) Pose3 T_12(Rot3::Rx(1.5707963268), Point3(0, -2, 1)); Pose3 T_21(Rot3::Rx(-1.5707963268), Point3(0, -1, -2)); - EXPECT(assert_equal(T_12, j1->relativePoseOf(l2, 1), 1e-5)); - EXPECT(assert_equal(T_21, j1->relativePoseOf(l1, 1), 1e-5)); + EXPECT(assert_equal(T_12, j1.relativePoseOf(l2, 1), 1e-5)); + EXPECT(assert_equal(T_21, j1.relativePoseOf(l1, 1), 1e-5)); // screw axis gtsam::Vector6 screw_axis_l1, screw_axis_l2; screw_axis_l1 << 0, 0, 0, 0, 1, 0; screw_axis_l2 << 0, 0, 0, 0, 0, 1; - EXPECT(assert_equal(screw_axis_l1, j1->screwAxis(l1), 1e-5)); - EXPECT(assert_equal(screw_axis_l2, j1->screwAxis(l2))); + EXPECT(assert_equal(screw_axis_l1, j1.screwAxis(l1), 1e-5)); + EXPECT(assert_equal(screw_axis_l2, j1.screwAxis(l2))); // links - auto links = j1->links(); + auto links = j1.links(); EXPECT(links[0] == l1); EXPECT(links[1] == l2); // parent & child link - EXPECT(j1->parent() == l1); - EXPECT(j1->child() == l2); + EXPECT(j1.parent() == l1); + EXPECT(j1.child() == l2); // joint limit - EXPECT(assert_equal(0, j1->parameters().scalar_limits.value_lower_limit)); - EXPECT(assert_equal(2, j1->parameters().scalar_limits.value_upper_limit)); + EXPECT(assert_equal(0, j1.parameters().scalar_limits.value_lower_limit)); + EXPECT(assert_equal(2, j1.parameters().scalar_limits.value_upper_limit)); EXPECT( - assert_equal(1e-9, j1->parameters().scalar_limits.value_limit_threshold)); + assert_equal(1e-9, j1.parameters().scalar_limits.value_limit_threshold)); } /** @@ -543,26 +551,26 @@ TEST(Sdf, sdf_constructor_screw) { j1_parameters.effort_type = JointEffortType::Actuated; const gtsam::Vector3 j1_axis = GetSdfAxis(*model.JointByName("joint_1")); - auto j1 = boost::make_shared( - 1, "joint_1", bTj, l0, l1, j1_axis, - model.JointByName("joint_1")->ThreadPitch(), j1_parameters); + auto j1 = + HelicalJoint(1, "joint_1", bTj, l0, l1, j1_axis, + model.JointByName("joint_1")->ThreadPitch(), j1_parameters); // expected values for screw about z axis // check screw axis gtsam::Vector6 screw_axis_j1_l0, screw_axis_j1_l1; screw_axis_j1_l0 << 0, 0, -1, 0, 0, -0.5 / 2 / M_PI; // parent frame screw_axis_j1_l1 << 0, 0, 1, 0, 0, 0.5 / 2 / M_PI; // child frame - EXPECT(assert_equal(screw_axis_j1_l0, j1->screwAxis(l0))); - EXPECT(assert_equal(screw_axis_j1_l1, j1->screwAxis(l1))); + EXPECT(assert_equal(screw_axis_j1_l0, j1.screwAxis(l0))); + EXPECT(assert_equal(screw_axis_j1_l1, j1.screwAxis(l1))); // Check transform from l0 com to l1 com at rest and at various angles. - Pose3 M_01(Rot3::identity(), Point3(0, 0, 0.4)); + Pose3 M_01(Rot3::Identity(), Point3(0, 0, 0.4)); Pose3 T_01_neg(Rot3::Rz(-M_PI / 2), Point3(0, 0, 0.4 - 0.125)); Pose3 T_01_pos(Rot3::Rz(M_PI / 2), Point3(0, 0, 0.4 + 0.125)); - EXPECT(assert_equal(M_01, j1->relativePoseOf(l1, 0))); - EXPECT(assert_equal(T_01_neg, j1->relativePoseOf(l1, -M_PI / 2))); - EXPECT(assert_equal(T_01_pos, j1->relativePoseOf(l1, M_PI / 2))); + EXPECT(assert_equal(M_01, j1.relativePoseOf(l1, 0))); + EXPECT(assert_equal(T_01_neg, j1.relativePoseOf(l1, -M_PI / 2))); + EXPECT(assert_equal(T_01_pos, j1.relativePoseOf(l1, M_PI / 2))); } // Initialize a Robot with "models/urdfs/test/simple_urdf.urdf" and make sure @@ -582,8 +590,7 @@ TEST(Robot, simple_urdf) { GetJointFrame(*simple_urdf.JointByName("j1"), sdf_link_l1, sdf_link_l2); const gtsam::Vector3 j1_axis = GetSdfAxis(*simple_urdf.JointByName("j1")); - auto j1 = boost::make_shared(1, "j1", bTj, l1, l2, j1_axis, - j1_parameters); + auto j1 = RevoluteJoint(1, "j1", bTj, l1, l2, j1_axis, j1_parameters); // Initialize Robot instance. auto simple_robot = @@ -609,9 +616,9 @@ TEST(Robot, simple_urdf) { // Check transforms between link CoM frames. EXPECT(assert_equal(gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0, 0, -2)), - j1->relativePoseOf(j1->parent(), 0))); + j1.relativePoseOf(j1.parent(), 0))); EXPECT(assert_equal(gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0, 0, 2)), - j1->relativePoseOf(j1->child(), 0))); + j1.relativePoseOf(j1.child(), 0))); // Check link frames and com frames are correct EXPECT(assert_equal(Pose3(Rot3(), Point3(0, 0, 1)), l1->bMcom())); @@ -648,6 +655,16 @@ TEST(Sdf, sdf_constructor) { l1.inertia())); } +TEST(Sdf, PreserveFixedJoint) { + // // Get the A1 robot with fixed joints lumped together. + Robot a1 = CreateRobotFromFile(kUrdfPath + std::string("a1/a1.urdf")); + EXPECT_LONGS_EQUAL(12, a1.numJoints()); + + Robot a1_fixed_joints = + CreateRobotFromFile(kUrdfPath + std::string("a1/a1.urdf"), "", true); + EXPECT_LONGS_EQUAL(21, a1_fixed_joints.numJoints()); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); diff --git a/tests/testSimulator.cpp b/tests/testSimulator.cpp index afae88737..5561f7146 100644 --- a/tests/testSimulator.cpp +++ b/tests/testSimulator.cpp @@ -12,6 +12,10 @@ */ #include +#include +#include +#include +#include #include #include #include @@ -19,11 +23,6 @@ #include -#include "gtdynamics/dynamics/Simulator.h" -#include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/utils.h" - using namespace gtdynamics; TEST(Simulate, simple_urdf) { diff --git a/tests/testSpiderWalking.cpp b/tests/testSpiderWalking.cpp index 6c1ae8803..240322a8e 100644 --- a/tests/testSpiderWalking.cpp +++ b/tests/testSpiderWalking.cpp @@ -51,9 +51,9 @@ Trajectory getTrajectory(const Robot &robot, size_t repeat) { // Create three different FootContactConstraintSpecs, one for all the feet on the // ground, one with even feet on the ground, one with odd feet in contact.. const Point3 contact_in_com(0, 0.19, 0); - auto stationary = boost::make_shared(all_feet, contact_in_com); - auto odd = boost::make_shared(odd_feet, contact_in_com); - auto even = boost::make_shared(even_feet, contact_in_com); + auto stationary = std::make_shared(all_feet, contact_in_com); + auto odd = std::make_shared(odd_feet, contact_in_com); + auto even = std::make_shared(even_feet, contact_in_com); FootContactVector states = {stationary, even, stationary, odd}; std::vector phase_lengths = {1,2,1,2}; @@ -149,8 +149,9 @@ TEST(testSpiderWalking, WholeEnchilada) { // Initialize solution. double gaussian_noise = 0.0; + Initializer initializer; Values init_vals = - trajectory.multiPhaseInitialValues(robot, gaussian_noise, desired_dt); + trajectory.multiPhaseInitialValues(robot, initializer, gaussian_noise, desired_dt); EXPECT_LONGS_EQUAL(3847, init_vals.size()); // Compare error for all factors with expected values in file. diff --git a/tests/testStaticWrenchFactor.cpp b/tests/testStaticWrenchFactor.cpp index 2e5f19ea0..1d8aaf814 100644 --- a/tests/testStaticWrenchFactor.cpp +++ b/tests/testStaticWrenchFactor.cpp @@ -12,6 +12,9 @@ */ #include +#include +#include +#include #include #include #include @@ -23,10 +26,6 @@ #include -#include "gtdynamics/statics/StaticWrenchFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/values.h" - using namespace gtdynamics; using namespace gtsam; diff --git a/tests/testStatics.cpp b/tests/testStatics.cpp index 6dcc1c94b..ae19eca1f 100644 --- a/tests/testStatics.cpp +++ b/tests/testStatics.cpp @@ -12,14 +12,12 @@ */ #include +#include +#include #include -#include #include -#include "gtdynamics/statics/Statics.h" -#include "gtdynamics/universal_robot/RobotModels.h" - using namespace gtdynamics; using namespace gtsam; constexpr double kTol = 1e-6; @@ -39,7 +37,8 @@ TEST(Statics, GravityWrench1) { EXPECT(assert_equal((Vector(6) << 0, 0, 0, 0, 0, -100 * g).finished(), GravityWrench(gravity, mass, wTcom, actualH), kTol)); Matrix6 numericalH = numericalDerivative11( - boost::bind(&GravityWrench, gravity, mass, _1, boost::none), wTcom); + std::bind(&GravityWrench, gravity, mass, std::placeholders::_1, nullptr), + wTcom); EXPECT(assert_equal(numericalH, actualH, kTol)); } @@ -51,7 +50,8 @@ TEST(Statics, GravityWrench2) { EXPECT(assert_equal((Vector(6) << 0, 0, 0, 0, -15 * g, 0).finished(), GravityWrench(gravity, mass, wTcom, actualH), kTol)); Matrix6 numericalH = numericalDerivative11( - boost::bind(&GravityWrench, gravity, mass, _1, boost::none), wTcom); + std::bind(&GravityWrench, gravity, mass, std::placeholders::_1, nullptr), + wTcom); EXPECT(assert_equal(numericalH, actualH, kTol)); } @@ -61,7 +61,7 @@ TEST(Statics, ResultantWrench) { wrenches[1] << 6, 5, 4, 3, 2, 1; std::vector actualH(2); EXPECT(assert_equal((Vector(6) << 7, 7, 7, 7, 7, 7).finished(), - ResultantWrench(wrenches, actualH), kTol)); + ResultantWrench(wrenches, &actualH), kTol)); Matrix expected(6, 6); expected.setIdentity(); EXPECT(assert_equal(expected, actualH[0], kTol)); diff --git a/tests/testStaticsSlice.cpp b/tests/testStaticsSlice.cpp index 7c5cba315..b6f7a2d95 100644 --- a/tests/testStaticsSlice.cpp +++ b/tests/testStaticsSlice.cpp @@ -12,10 +12,10 @@ */ #include +#include +#include #include "contactGoalsExample.h" -#include "gtdynamics/statics/Statics.h" -#include "gtdynamics/universal_robot/RevoluteJoint.h" using namespace gtdynamics; using namespace gtsam; @@ -44,8 +44,8 @@ TEST(Statics, OneMovingLink) { const Pose3 bMcom(Rot3(), Point3(L / 2, 0, 0)); const auto I3 = Matrix3::Identity(); // inertia auto base = - boost::make_shared(0, "base", 1e10, I3, Pose3(), Pose3(), true); - auto link = boost::make_shared(1, "link", 1.0, I3, bMcom, Pose3()); + std::make_shared(0, "base", 1e10, I3, Pose3(), Pose3(), true); + auto link = std::make_shared(1, "link", 1.0, I3, bMcom, Pose3()); // Create joint constexpr unsigned char id = 22; @@ -53,7 +53,7 @@ TEST(Statics, OneMovingLink) { const Pose3 bMj; const Vector3 axis(0, 0, 1); auto joint = - boost::make_shared(id, "joint1", bMj, base, link, axis); + std::make_shared(id, "joint1", bMj, base, link, axis); // Create mechanism. // TODO(frank): specifying name is redundant and failure prone! @@ -117,16 +117,13 @@ TEST(Statics, Quadruped) { EXPECT_LONGS_EQUAL(37, graph.size()); // Test initialization - auto values = statics.initialValues(slice, robot); + auto values = statics.initialValues(slice, robot, 0.0); EXPECT_LONGS_EQUAL(36, values.size()); // Solve for wrenches, with known kinematics auto result = statics.solve(slice, robot, ik_solution); EXPECT_LONGS_EQUAL(61, result.size()); - // TODO(Varun) Issue #233 - // Regression - result.print("", GTDKeyFormatter); - // EXPECT_DOUBLES_EQUAL(0.0670426, Torque(result, 0, k), 1e-5); + EXPECT_DOUBLES_EQUAL(0.054764, Torque(result, 0, k), 1e-5); // Optimize kinematics while minimizing torque auto minimal = statics.minimizeTorques(slice, robot); diff --git a/tests/testSubstituteFactor.cpp b/tests/testSubstituteFactor.cpp new file mode 100644 index 000000000..f83ff31af --- /dev/null +++ b/tests/testSubstituteFactor.cpp @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testSubstituteFactor.cpp + * @brief test substitute factor. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtdynamics; + +TEST(SubstituteFactor, pose) { + // Keys. + Key x1_key = 1; + Key x2_key = 2; + Key x3_key = 3; + Key x4_key = 4; + Key cm_key = 2; + + // Constraints. + auto constraints = std::make_shared(); + auto noise = noiseModel::Unit::Create(3); + auto factor12 = std::make_shared>( + x1_key, x2_key, Point3(0, 0, 1), noise); + auto factor23 = std::make_shared>( + x2_key, x3_key, Point3(0, 1, 0), noise); + constraints->emplace_shared(factor12); + constraints->emplace_shared(factor23); + + // Create manifold values for testing. + Values cm_base_values; + cm_base_values.insert(x1_key, Point3(0, 0, 0)); + cm_base_values.insert(x2_key, Point3(0, 0, 1)); + cm_base_values.insert(x3_key, Point3(0, 1, 1)); + + // Construct manifold. + ConstraintManifold manifold(constraints, cm_base_values); + + // Construct cost factor. + auto cost_factor1 = std::make_shared>( + x3_key, x4_key, Point3(1, 0, 0), noise); + auto cost_factor2 = std::make_shared>( + x1_key, x3_key, Point3(1, 0, 0), noise); + + // Replacement map. + std::map replacement_map; + replacement_map[x1_key] = cm_key; + replacement_map[x2_key] = cm_key; + replacement_map[x3_key] = cm_key; + + // Test constructor. + SubstituteFactor subs_factor1(cost_factor1, replacement_map); + SubstituteFactor subs_factor2(cost_factor2, replacement_map); + + // Test keys. + EXPECT(subs_factor1.isReplaced(x3_key)); + EXPECT(!subs_factor1.isReplaced(x4_key)); + EXPECT(subs_factor2.isReplaced(x1_key)); + EXPECT(subs_factor2.isReplaced(x3_key)); + + // Construct values for testing. + Values values; + values.insert(cm_key, manifold); + values.insert(x4_key, Point3(2, 2, 2)); + + // Check error. + Vector expected_error1 = (Vector(3) << 1, 1, 1).finished(); + EXPECT(assert_equal(expected_error1, subs_factor1.unwhitenedError(values))); + + Vector expected_error2 = (Vector(3) << -1, 1, 1).finished(); + EXPECT(assert_equal(expected_error2, subs_factor2.unwhitenedError(values))); + + // Check jacobian computation is correct. + EXPECT_CORRECT_FACTOR_JACOBIANS(subs_factor1, values, 1e-7, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(subs_factor2, values, 1e-7, 1e-5); +} + +TEST(SubstituteFactor, fully_constrained_manifold) { + // Keys. + Key x1_key = 1; + Key x2_key = 2; + Key x3_key = 3; + Key x4_key = 4; + Key cm_key = 2; + + // Constraints. + auto constraints = std::make_shared(); + auto noise = noiseModel::Unit::Create(3); + auto factor1 = + std::make_shared>(x1_key, Point3(0, 0, 0), noise); + auto factor12 = std::make_shared>( + x1_key, x2_key, Point3(0, 0, 1), noise); + auto factor23 = std::make_shared>( + x2_key, x3_key, Point3(0, 1, 0), noise); + constraints->emplace_shared(factor1); + constraints->emplace_shared(factor12); + constraints->emplace_shared(factor23); + + // Create manifold values for testing. + Values cm_base_values; + cm_base_values.insert(x1_key, Point3(0, 0, 0)); + cm_base_values.insert(x2_key, Point3(0, 0, 1)); + cm_base_values.insert(x3_key, Point3(0, 1, 1)); + + // Construct manifold. + ConstraintManifold manifold(constraints, cm_base_values); + + // Construct cost factor. + auto cost_factor1 = std::make_shared>( + x3_key, x4_key, Point3(1, 0, 0), noise); + auto cost_factor2 = std::make_shared>( + x1_key, x3_key, Point3(1, 0, 0), noise); + + // Replacement map. + std::map replacement_map; + replacement_map[x1_key] = cm_key; + replacement_map[x2_key] = cm_key; + replacement_map[x3_key] = cm_key; + Values fc_manifolds; + fc_manifolds.insert(cm_key, manifold); + + // Test constructor. + SubstituteFactor subs_factor1(cost_factor1, replacement_map, fc_manifolds); + SubstituteFactor subs_factor2(cost_factor2, replacement_map, fc_manifolds); + EXPECT(!subs_factor2.checkActive()); + + // Construct values for testing. + Values values; + values.insert(x4_key, Point3(2, 2, 2)); + + // Check error. + Vector expected_error1 = (Vector(3) << 1, 1, 1).finished(); + EXPECT(assert_equal(expected_error1, subs_factor1.unwhitenedError(values))); + + // Check jacobian computation is correct. + EXPECT_CORRECT_FACTOR_JACOBIANS(subs_factor1, values, 1e-7, 1e-5); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testTimer.cpp b/tests/testTimer.cpp new file mode 100644 index 000000000..e1ea807f9 --- /dev/null +++ b/tests/testTimer.cpp @@ -0,0 +1,37 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testTimer.cpp + * @brief Test Timer. + * @author Yetong Zhang + */ + +#include +#include + +#include +#include + +using namespace gtsam; +using namespace gtdynamics; + +TEST(Timer, Timer) { + Timer timer; + timer.start(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + timer.stop(); + + EXPECT(timer.seconds() >= 0.01); + EXPECT(timer.milliSeconds() >= 10.0); + EXPECT(timer.microSeconds() >= 10000.0); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testTorqueFactor.cpp b/tests/testTorqueFactor.cpp index bd0231f7e..245701e6d 100644 --- a/tests/testTorqueFactor.cpp +++ b/tests/testTorqueFactor.cpp @@ -12,6 +12,9 @@ */ #include +#include +#include +#include #include #include #include @@ -23,9 +26,6 @@ #include -#include "gtdynamics/factors/TorqueFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/values.h" #include "make_joint.h" using namespace gtdynamics; @@ -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/testTrajectory.cpp b/tests/testTrajectory.cpp index 9270f66ad..0a363fc8d 100644 --- a/tests/testTrajectory.cpp +++ b/tests/testTrajectory.cpp @@ -12,14 +12,14 @@ */ #include +#include +#include +#include +#include +#include +#include +#include -#include "gtdynamics/dynamics/DynamicsGraph.h" -#include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/universal_robot/sdf.h" -#include "gtdynamics/utils/DynamicsSymbol.h" -#include "gtdynamics/utils/Phase.h" -#include "gtdynamics/utils/Trajectory.h" -#include "gtdynamics/utils/WalkCycle.h" #include "walkCycleExample.h" using namespace gtsam; @@ -38,7 +38,7 @@ TEST(Trajectory, error) { // Initialize Trajectory size_t repeat = 3; using namespace walk_cycle_example; - auto trajectory = Trajectory(walk_cycle, repeat); + Trajectory trajectory(walk_cycle, repeat); // test phase method EXPECT_LONGS_EQUAL(2, trajectory.phase(0).numTimeSteps()); @@ -66,13 +66,14 @@ TEST(Trajectory, error) { auto cp_goals = walk_cycle.initContactPointGoal(robot, 0); EXPECT_LONGS_EQUAL(5, cp_goals.size()); - //regression + // regression EXPECT(gtsam::assert_equal(gtsam::Point3(-0.926417, 1.19512, 0.000151302), cp_goals["tarsus_2_L2"], 1e-5)); double gaussian_noise = 1e-5; + Initializer initializer; vector transition_graph_init = - trajectory.transitionPhaseInitialValues(robot, gaussian_noise); + trajectory.transitionPhaseInitialValues(robot, initializer, gaussian_noise); EXPECT_LONGS_EQUAL(5, transition_graph_init.size()); gtsam::Vector3 gravity(0, 0, -9.8); @@ -95,7 +96,7 @@ TEST(Trajectory, error) { EXPECT_LONGS_EQUAL(4298, graph.size()); EXPECT_LONGS_EQUAL(4712, graph.keys().size()); - Values init_vals = trajectory.multiPhaseInitialValues(robot, 1e-5, 1. / 240); + Values init_vals = trajectory.multiPhaseInitialValues(robot, initializer, 1e-5, 1. / 240); EXPECT_LONGS_EQUAL(4712, init_vals.size()); // Test objectives for contact links. @@ -106,9 +107,10 @@ TEST(Trajectory, error) { const size_t expected = repeat * ((2 + 3) * 5); EXPECT_LONGS_EQUAL(expected, contact_link_objectives.size()); // // regression - // auto last_factor = boost::dynamic_pointer_cast( + // auto last_factor = std::dynamic_pointer_cast( // contact_link_objectives.back()); - // EXPECT(gtsam::assert_equal(gtsam::Point3(-0.190001, -0.300151, 0.000151302), + // EXPECT(gtsam::assert_equal(gtsam::Point3(-0.190001, -0.300151, + // 0.000151302), // last_factor->goalPoint(), 1e-5)); // Test boundary conditions. diff --git a/tests/testTspaceBasis.cpp b/tests/testTspaceBasis.cpp new file mode 100644 index 000000000..2bf0f919e --- /dev/null +++ b/tests/testTspaceBasis.cpp @@ -0,0 +1,190 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testTspaceBasis.cpp + * @brief Test tagent space basis for constraint manifold. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtdynamics; + +/** Simple example Pose3 with between constraints. */ +TEST(TspaceBasis, connected_poses) { + Key x1_key = 1; + Key x2_key = 2; + Key x3_key = 3; + + // Constraints. + auto constraints = std::make_shared(); + auto noise = noiseModel::Unit::Create(6); + auto factor12 = std::make_shared>( + x1_key, x2_key, Pose3(Rot3(), Point3(0, 0, 1)), noise); + auto factor23 = std::make_shared>( + x2_key, x3_key, Pose3(Rot3(), Point3(0, 1, 0)), noise); + constraints->emplace_shared(factor12); + constraints->emplace_shared(factor23); + + // Create manifold values for testing. + Values cm_base_values; + cm_base_values.insert(x1_key, Pose3(Rot3(), Point3(0, 0, 0))); + cm_base_values.insert(x2_key, Pose3(Rot3(), Point3(0, 0, 1))); + cm_base_values.insert(x3_key, Pose3(Rot3(), Point3(0, 1, 1))); + + // Construct basis. + KeyVector basis_keys{x3_key}; + auto basis_params = std::make_shared(); + auto basis_m = std::make_shared(constraints, cm_base_values, + basis_params); + auto basis_e = std::make_shared(constraints, cm_base_values, + basis_params, basis_keys); + auto sparse_creator = OrthonormalBasisCreator::CreateSparse(); + auto basis_sm = sparse_creator->create(constraints, cm_base_values); + + auto linear_graph = constraints->penaltyGraph().linearize(cm_base_values); + std::vector basis_vec{basis_m, basis_e, basis_sm}; + + // Check dimension. + for (const auto& basis : basis_vec) { + EXPECT_LONGS_EQUAL(6, basis->dim()); + } + + // Check tagent vector properties + for (int i = 0; i < 6; i++) { + Vector xi1 = Vector::Zero(6); + xi1(i) = 1; + Vector xi10 = xi1 * 10; + for (const auto& basis : basis_vec) { + // check null space property + auto delta1 = basis->computeTangentVector(xi1); + EXPECT_DOUBLES_EQUAL(0.0, linear_graph->error(delta1), 1e-9); + // check linear space property + auto delta10 = basis->computeTangentVector(xi10); + EXPECT(assert_equal(delta10, 10 * delta1)); + // check construct xi from tangent vector + auto recover_xi1 = basis->computeXi(delta1); + EXPECT(assert_equal(xi1, recover_xi1)); + } + } + + // Check linear space property + Vector xi1 = Vector::Zero(6); + Vector xi2 = Vector::Zero(6); + xi1(1) = 1; + xi2(2) = 1; + for (const auto& basis : basis_vec) { + // check null space property + auto delta1 = basis->computeTangentVector(xi1); + auto delta2 = basis->computeTangentVector(xi2); + auto delta12 = basis->computeTangentVector(xi1 + xi2); + EXPECT(assert_equal(delta12, delta1 + delta2)); + } + + // recoverJacobian will be checked in ConstraintManifold tests. + + // Create new values for testing. + Values new_cm_base_values; + new_cm_base_values.insert(x1_key, Pose3(Rot3(), Point3(1, 1, 1))); + new_cm_base_values.insert(x2_key, Pose3(Rot3(), Point3(1, 1, 2))); + new_cm_base_values.insert(x3_key, Pose3(Rot3(), Point3(1, 2, 2))); + + // Check createWithNewValues. + for (const auto& basis : basis_vec) { + auto new_basis = basis->createWithNewValues(new_cm_base_values); + for (int i = 0; i < 6; i++) { + Vector xi1 = Vector::Zero(6); + xi1(i) = 1; + // check null space property + auto delta1 = basis->computeTangentVector(xi1); + EXPECT_DOUBLES_EQUAL(0.0, linear_graph->error(delta1), 1e-9); + } + } +} + +/** Simple example Pose3 with between constraints. */ +TEST(TspaceBasis, linear_system) { + Key x1_key = 1; + Key x2_key = 2; + Key x3_key = 3; + Key x4_key = 4; + Double_ x1(x1_key); + Double_ x2(x2_key); + Double_ x3(x3_key); + Double_ x4(x4_key); + double tol = 1.0; + auto constraints = std::make_shared(); + constraints->emplace_shared>( + x1 + x2 + x3, 0.0, (Vector(1) << tol).finished()); + constraints->emplace_shared>(x1 + x2 + 2 * x3 + x4, + 0.0, (Vector(1) << tol).finished()); + + Values values; + values.insertDouble(x1_key, 0.0); + values.insertDouble(x2_key, 0.0); + values.insertDouble(x3_key, 0.0); + values.insertDouble(x4_key, 0.0); + KeyVector basis_keys{x1_key, x2_key}; + auto basis_params = std::make_shared(); + // auto basis_m = std::make_shared(basis_params, cc, values); + auto basis_e = + std::make_shared(constraints, values, basis_params, basis_keys); + auto basis_m = std::make_shared(constraints, values, basis_params); + auto sparse_creator = OrthonormalBasisCreator::CreateSparse(); + auto basis_sm = sparse_creator->create(constraints, values); + + // Construct new basis by adding additional constraints + gtsam::NonlinearEqualityConstraints new_constraints; + new_constraints.emplace_shared>( + x2, 0.0, (Vector(1) << tol).finished()); + auto new_basis_e = basis_e->createWithAdditionalConstraints(new_constraints, values); + auto new_basis_m = basis_m->createWithAdditionalConstraints(new_constraints, values); + auto new_basis_sm = basis_sm->createWithAdditionalConstraints(new_constraints, values); + NonlinearFactorGraph merit_graph = constraints->penaltyGraph(); + merit_graph.add(new_constraints.penaltyGraph()); + auto linear_graph = merit_graph.linearize(values); + for (const auto& vector: new_basis_e->basisVectors()) { + EXPECT(assert_equal(0.0, linear_graph->error(vector))); + } + for (const auto& vector: new_basis_m->basisVectors()) { + EXPECT(assert_equal(0.0, linear_graph->error(vector))); + } + for (const auto& vector: new_basis_sm->basisVectors()) { + EXPECT(assert_equal(0.0, linear_graph->error(vector))); + } + + Matrix expected_H_x1 = I_1x1; + Matrix expected_H_x2 = I_1x1*0; + Matrix expected_H_x3 = I_1x1 * -1; + Matrix expected_H_x4 = I_1x1; + EXPECT(assert_equal(expected_H_x1, new_basis_e->recoverJacobian(x1_key))); + EXPECT(assert_equal(expected_H_x2, new_basis_e->recoverJacobian(x2_key))); + EXPECT(assert_equal(expected_H_x3, new_basis_e->recoverJacobian(x3_key))); + EXPECT(assert_equal(expected_H_x4, new_basis_e->recoverJacobian(x4_key))); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testTwistAccelFactor.cpp b/tests/testTwistAccelFactor.cpp index 8fec423b9..1b34d198a 100644 --- a/tests/testTwistAccelFactor.cpp +++ b/tests/testTwistAccelFactor.cpp @@ -12,6 +12,8 @@ */ #include +#include +#include #include #include #include @@ -24,8 +26,6 @@ #include #include -#include "gtdynamics/factors/TwistAccelFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" #include "make_joint.h" using namespace gtdynamics; @@ -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 ecacc1ce7..ffa653af0 100644 --- a/tests/testTwistFactor.cpp +++ b/tests/testTwistFactor.cpp @@ -11,9 +11,9 @@ * @author Frank Dellaert and Mandy Xie */ -#include "make_joint.h" - #include +#include +#include #include #include #include @@ -25,8 +25,7 @@ #include -#include "gtdynamics/factors/TwistFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" +#include "make_joint.h" using namespace gtdynamics; using gtsam::assert_equal; @@ -34,9 +33,8 @@ using gtsam::assert_equal; namespace example { gtsam::noiseModel::Gaussian::shared_ptr cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::I_6x6); -gtsam::Key twist_p_key = TwistKey(1), - twist_c_key = TwistKey(2), qKey = JointAngleKey(1), - qVelKey = JointVelKey(1); +gtsam::Key twist_p_key = TwistKey(1), twist_c_key = TwistKey(2), + qKey = JointAngleKey(1), qVelKey = JointVelKey(1); } // namespace example // Test twist factor for stationary case @@ -46,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/testUniversalRobotChanges.cpp b/tests/testUniversalRobotChanges.cpp new file mode 100644 index 000000000..3e80cfd9a --- /dev/null +++ b/tests/testUniversalRobotChanges.cpp @@ -0,0 +1,101 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testUniversalRobotChanges.cpp + * @brief Test new features in Universal Robot classes. + * @author Yetong Zhang + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace gtdynamics; +using gtsam::assert_equal; +using gtsam::Point3; +using gtsam::Pose3; +using gtsam::Rot3; + +TEST(Link, RenameReassign) { + Link l1(1, "l1", 100.0, gtsam::Vector3(3, 2, 1).asDiagonal(), + Pose3(Rot3(), Point3(0, 0, 1)), Pose3()); + + // Test Rename + l1.rename("link1"); + EXPECT(assert_equal("link1", l1.name())); + + // Test Reassign + l1.reassign(2); + EXPECT_LONGS_EQUAL(2, l1.id()); +} + +TEST(Joint, RenameReassign) { + auto robot = simple_urdf::getRobot(); + auto l1 = robot.link("l1"); + auto l2 = robot.link("l2"); + RevoluteJoint j1(1, "j1", Pose3(Rot3(), Point3(0, 0.5, 2)), l1, l2, + gtsam::Vector3(1, 0, 0), JointParams()); + + // Test Rename + j1.rename("joint1"); + EXPECT(assert_equal("joint1", j1.name())); + + // Test Reassign + j1.reassign(2); + EXPECT_LONGS_EQUAL(2, j1.id()); +} + +TEST(Robot, RenameReassign) { + Robot robot = simple_urdf::getRobot(); + + // Test Rename Links + std::map link_name_map = {{"l1", "link1"}, + {"l2", "link2"}}; + robot.renameLinks(link_name_map); + EXPECT(robot.link("link1")); + EXPECT(robot.link("link2")); + THROWS_EXCEPTION(robot.link("l1")); + THROWS_EXCEPTION(robot.link("l2")); + + // Test Rename Joints + std::map joint_name_map = {{"j1", "joint1"}}; + robot.renameJoints(joint_name_map); + EXPECT(robot.joint("joint1")); + THROWS_EXCEPTION(robot.joint("j1")); + + // Test Reassign Links + std::vector ordered_link_names = {"link2", "link1"}; + robot.reassignLinks(ordered_link_names); + EXPECT_LONGS_EQUAL(0, robot.link("link2")->id()); + EXPECT_LONGS_EQUAL(1, robot.link("link1")->id()); + + // Test Reassign Joints + std::vector ordered_joint_names = {"joint1"}; + robot.reassignJoints(ordered_joint_names); + EXPECT_LONGS_EQUAL(0, robot.joint("joint1")->id()); + + // Test Ordered Links + auto links = robot.orderedLinks(); + EXPECT_LONGS_EQUAL(2, links.size()); + EXPECT(assert_equal("link2", links[0]->name())); + EXPECT(assert_equal("link1", links[1]->name())); + + // Test Ordered Joints + auto joints = robot.orderedJoints(); + EXPECT_LONGS_EQUAL(1, joints.size()); + EXPECT(assert_equal("joint1", joints[0]->name())); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testUtils.cpp b/tests/testUtils.cpp index 20532fd52..e2cf0d26d 100644 --- a/tests/testUtils.cpp +++ b/tests/testUtils.cpp @@ -12,6 +12,7 @@ */ #include +#include #include #include #include @@ -19,11 +20,9 @@ #include #include -#include +#include #include -#include "gtdynamics/utils/utils.h" - using namespace gtdynamics; using gtsam::assert_equal; @@ -63,6 +62,35 @@ TEST(utils, calcQ) { EXPECT(assert_equal(expected_Q, actual_Q, 1e-6)); } +TEST(utils, Derivatives) { + using namespace gtsam; + Point3 p(1, 2, 3); + Matrix H_p; + EXPECT_DOUBLES_EQUAL(3.0, point3_z(p, H_p), 1e-9); + EXPECT(assert_equal((Matrix(1, 3) << 0, 0, 1).finished(), H_p)); + + double x1 = 10.0, x2 = 2.0; + Matrix H1, H2; + EXPECT_DOUBLES_EQUAL(5.0, double_division(x1, x2, H1, H2), 1e-9); + EXPECT(assert_equal((Matrix(1, 1) << 0.5).finished(), H1)); + EXPECT(assert_equal((Matrix(1, 1) << -2.5).finished(), H2)); + + double x = 2.0; + Matrix H; + EXPECT_DOUBLES_EQUAL(0.5, reciprocal(x, H), 1e-9); + EXPECT(assert_equal((Matrix(1, 1) << -0.25).finished(), H)); + + EXPECT_DOUBLES_EQUAL(1.0, clip_by_one(0.5, H), 1e-9); + EXPECT(assert_equal((Matrix(1, 1) << 0.0).finished(), H)); + EXPECT_DOUBLES_EQUAL(2.0, clip_by_one(2.0, H), 1e-9); + EXPECT(assert_equal((Matrix(1, 1) << 1.0).finished(), H)); + + Matrix H1s, H2s; + EXPECT(assert_equal(Vector2(10.0, 2.0), double_stack(x1, x2, H1s, H2s))); + EXPECT(assert_equal((Matrix(2, 1) << 1, 0).finished(), H1s)); + EXPECT(assert_equal((Matrix(2, 1) << 0, 1).finished(), H2s)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); diff --git a/tests/testValues.cpp b/tests/testValues.cpp index 4812829b4..85af4e8b3 100644 --- a/tests/testValues.cpp +++ b/tests/testValues.cpp @@ -14,11 +14,10 @@ #include #include +#include #include #include -#include "gtdynamics/utils/values.h" - using namespace gtdynamics; using gtsam::assert_equal; @@ -35,6 +34,20 @@ TEST(Values, at) { CHECK_EXCEPTION(TwistAccel(values, 7), KeyDoesNotExist); } +TEST(Values, DynamicsValuesFromPrev) { + gtsam::Values values; + InsertJointAngle(&values, 0, 0, 1.0); + InsertJointVel(&values, 0, 0, 2.0); + + gtsam::Values next_values = DynamicsValuesFromPrev(values); + EXPECT(assert_equal(1.0, JointAngle(next_values, 0, 1))); + EXPECT(assert_equal(2.0, JointVel(next_values, 0, 1))); + + gtsam::Values jump_values = DynamicsValuesFromPrev(values, 5); + EXPECT(assert_equal(1.0, JointAngle(jump_values, 0, 5))); + EXPECT(assert_equal(2.0, JointVel(jump_values, 0, 5))); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); diff --git a/tests/testWalkCycle.cpp b/tests/testWalkCycle.cpp index 17a8c5f4e..5a4477516 100644 --- a/tests/testWalkCycle.cpp +++ b/tests/testWalkCycle.cpp @@ -12,13 +12,13 @@ */ #include +#include +#include +#include +#include +#include +#include -#include "gtdynamics/dynamics/DynamicsGraph.h" -#include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/universal_robot/sdf.h" -#include "gtdynamics/utils/Phase.h" -#include "gtdynamics/utils/WalkCycle.h" -#include "gtdynamics/utils/Trajectory.h" #include "walkCycleExample.h" using namespace gtdynamics; @@ -70,18 +70,23 @@ TEST(WalkCycle, objectives) { constexpr size_t num_time_steps = 5; const Point3 contact_in_com(0.14, 0, 0); - std::vector phase_0_links = {robot.link("lower1"), robot.link("lower2")}; - std::vector phase_1_links = {robot.link("lower0"), robot.link("lower3")}; + std::vector phase_0_links = {robot.link("lower1"), + robot.link("lower2")}; + std::vector phase_1_links = {robot.link("lower0"), + robot.link("lower3")}; + + auto phase0 = std::make_shared(phase_0_links, + contact_in_com); + auto phase1 = std::make_shared(phase_1_links, + contact_in_com); - auto phase0 = boost::make_shared(phase_0_links, contact_in_com); - auto phase1 = boost::make_shared(phase_1_links, contact_in_com); - FootContactVector states = {phase0, phase1}; std::vector phase_lengths = {num_time_steps, num_time_steps}; - auto walk_cycle = WalkCycle({phase0, phase1}, {num_time_steps, num_time_steps}); + auto walk_cycle = + WalkCycle({phase0, phase1}, {num_time_steps, num_time_steps}); - //check Phase swing links function + // check Phase swing links function auto swing_links0 = walk_cycle.getPhaseSwingLinks(0); auto swing_links1 = walk_cycle.getPhaseSwingLinks(1); EXPECT_LONGS_EQUAL(swing_links0.size(), 2); diff --git a/tests/testWrenchEquivalenceFactor.cpp b/tests/testWrenchEquivalenceFactor.cpp index e432a11fc..0d4c7698a 100644 --- a/tests/testWrenchEquivalenceFactor.cpp +++ b/tests/testWrenchEquivalenceFactor.cpp @@ -12,6 +12,8 @@ */ #include +#include +#include #include #include #include @@ -24,8 +26,6 @@ #include #include -#include "gtdynamics/factors/WrenchEquivalenceFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" #include "make_joint.h" using namespace gtdynamics; @@ -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. @@ -64,8 +64,7 @@ TEST(WrenchEquivalenceFactor, error_1) { values.insert(example::wrench_j_key, wrench_j); values.insert(example::wrench_k_key, wrench_k); values.insert(example::qKey, q); - Vector6 expected_errors, - actual_errors = factor->unwhitenedError(values); + Vector6 expected_errors, actual_errors = factor->unwhitenedError(values); expected_errors << 0, 0, 0, 0, 0, 0; EXPECT(assert_equal(expected_errors, actual_errors, 1e-6)); @@ -80,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. @@ -92,11 +91,10 @@ TEST(WrenchEquivalenceFactor, error_2) { values.insert(example::wrench_j_key, wrench_j); values.insert(example::wrench_k_key, wrench_k); values.insert(example::qKey, q); - Vector6 expected_errors, - actual_errors = factor->unwhitenedError(values); + Vector6 expected_errors, actual_errors = factor->unwhitenedError(values); expected_errors << 0, 0, 0, 0, 0, 0; EXPECT(assert_equal(expected_errors, actual_errors, 1e-6)); - + // Make sure linearization is correct. double diffDelta = 1e-7; @@ -109,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. @@ -121,8 +119,7 @@ TEST(WrenchEquivalenceFactor, error_3) { values.insert(example::wrench_j_key, wrench_j); values.insert(example::wrench_k_key, wrench_k); values.insert(example::qKey, q); - Vector6 expected_errors, - actual_errors = factor->unwhitenedError(values); + Vector6 expected_errors, actual_errors = factor->unwhitenedError(values); expected_errors << 0, 0, 0, 0, 0, 0; EXPECT(assert_equal(expected_errors, actual_errors, 1e-6)); diff --git a/tests/testWrenchFactor.cpp b/tests/testWrenchFactor.cpp index 0a2cd1597..044c3aa83 100644 --- a/tests/testWrenchFactor.cpp +++ b/tests/testWrenchFactor.cpp @@ -12,6 +12,9 @@ */ #include +#include +#include +#include #include #include #include @@ -23,10 +26,6 @@ #include -#include "gtdynamics/factors/WrenchFactor.h" -#include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/utils/values.h" - using namespace gtdynamics; using namespace gtsam; @@ -51,9 +50,9 @@ TEST(WrenchFactor, Case1) { // Create all factors int id = 0; const double M = example::inertia(3, 3); - auto factor = WrenchFactor(example::cost_model, example::link, - {WrenchKey(id, 1), WrenchKey(id, 2)}, - 0, example::gravity); + auto factor = + WrenchFactor(example::cost_model, example::link, + {WrenchKey(id, 1), WrenchKey(id, 2)}, 0, example::gravity); Values x; InsertTwist(&x, id, (Vector(6) << 0, 0, 0, 0, 0, 0).finished()); InsertTwistAccel(&x, id, @@ -74,9 +73,10 @@ TEST(WrenchFactor, Case2) { // Create all factors int id = 0; const double M = example::inertia(3, 3); - auto factor = WrenchFactor(example::cost_model, example::link, - {WrenchKey(id, 1), WrenchKey(id, 2), WrenchKey(id, 3)}, - 0, example::gravity); + auto factor = + WrenchFactor(example::cost_model, example::link, + {WrenchKey(id, 1), WrenchKey(id, 2), WrenchKey(id, 3)}, 0, + example::gravity); Values x; InsertTwist(&x, id, (Vector(6) << 0, 0, 0, 0, 0, 0).finished()); InsertTwistAccel(&x, id, (Vector(6) << 0, 0, 0, 0, 0, 0).finished()); @@ -99,8 +99,7 @@ TEST(WrenchFactor, NonzeroTwistCase) { const double M = example::inertia(3, 3); // gravity set to zero in this case auto factor = WrenchFactor(example::cost_model, example::link, - {WrenchKey(id, 1), WrenchKey(id, 2)}, - 0); + {WrenchKey(id, 1), WrenchKey(id, 2)}, 0); Values x; InsertTwist(&x, id, (Vector(6) << 0, 0, 1, 0, 1, 0).finished()); diff --git a/tests/testWrenchPlanarFactor.cpp b/tests/testWrenchPlanarFactor.cpp index 0d9b86a99..e283f4d46 100644 --- a/tests/testWrenchPlanarFactor.cpp +++ b/tests/testWrenchPlanarFactor.cpp @@ -12,6 +12,7 @@ */ #include +#include #include #include #include @@ -23,7 +24,6 @@ #include -#include "gtdynamics/factors/WrenchPlanarFactor.h" #include "make_joint.h" using namespace gtdynamics; @@ -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 diff --git a/tests/walkCycleExample.h b/tests/walkCycleExample.h index c460f8c5d..948459d1c 100644 --- a/tests/walkCycleExample.h +++ b/tests/walkCycleExample.h @@ -19,7 +19,7 @@ namespace gtdynamics { namespace walk_cycle_example { Robot robot = - CreateRobotFromFile(kSdfPath + std::string("spider.sdf"), "spider"); + CreateRobotFromFile(kSdfPath + std::string("spider.sdf"), "spider"); // First phase constexpr size_t num_time_steps = 2; @@ -28,19 +28,21 @@ const std::vector links_1 = {robot.link("tarsus_1_L1"), robot.link("tarsus_2_L2"), robot.link("tarsus_3_L3")}; -const auto phase_1 = boost::make_shared(links_1, contact_in_com); +const auto phase_1 = + std::make_shared(links_1, contact_in_com); // Second phase constexpr size_t num_time_steps_2 = 3; -const std::vector links_2 = {robot.link("tarsus_2_L2"), - robot.link("tarsus_3_L3"), - robot.link("tarsus_4_L4"), - robot.link("tarsus_5_R4")}; +const std::vector links_2 = { + robot.link("tarsus_2_L2"), robot.link("tarsus_3_L3"), + robot.link("tarsus_4_L4"), robot.link("tarsus_5_R4")}; -const auto phase_2 = boost::make_shared(links_2, contact_in_com); +const auto phase_2 = + std::make_shared(links_2, contact_in_com); // Initialize walk cycle -const WalkCycle walk_cycle({phase_1, phase_2}, {num_time_steps, num_time_steps_2}); +const WalkCycle walk_cycle({phase_1, phase_2}, + {num_time_steps, num_time_steps_2}); } // namespace walk_cycle_example } // namespace gtdynamics \ No newline at end of file From 669accd0fcb33aea5aa26fe010c37d0897411d41 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 9 Feb 2026 17:09:29 +0000 Subject: [PATCH 14/17] Initial plan From 8174ea9f2a0ade56c94df699b245983f16f7e358 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 9 Feb 2026 17:34:34 +0000 Subject: [PATCH 15/17] Address review comments: add documentation for use_fk, pose_goals, and joint_priors parameters Co-authored-by: dellaert <10515273+dellaert@users.noreply.github.com> --- gtdynamics/kinematics/Kinematics.h | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 5ce89567e..7db8be54c 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -109,7 +109,7 @@ struct PoseGoal { double tol = 1e-9) const; }; -///< Map of time to PoseGoal +///< Map of time step k to PoseGoal for that time step using PoseGoals = std::map; /// Noise models etc specific to Kinematics class @@ -183,8 +183,8 @@ class Kinematics : public Optimizer { /** * @fn Create a pose prior for a given link for each given pose. * @param context Slice or Interval instance. - * @param pose_goals an object of PoseGoals: map of time k to desired pose at - * that time, which will be used as mean of the prior + * @param pose_goals an object of PoseGoals: map of time step k to desired pose + * at that time step, which will be used as mean of the prior * @returns graph with pose goal factors. */ template @@ -195,8 +195,10 @@ class Kinematics : public Optimizer { * @fn Factors that minimize joint angles. * @param context Slice or Interval instance. * @param robot Robot specification from URDF/SDF. - * @param joint_priors Values where the mean of the priors is specified. The - * default is an empty Values, meaning that the means will default to 0. + * @param joint_priors Values where the mean of the priors is specified. Uses + * gtsam::Values to support different prior means for different time steps in + * intervals/trajectories. The default is an empty Values, meaning that the + * means will default to 0. * @returns graph with prior factors on joint angles. */ template @@ -226,8 +228,12 @@ class Kinematics : public Optimizer { * * @param context Slice or Interval instance. * @param robot Robot specification from URDF/SDF. - * @param gaussian_noise time step to check (default 0.1). - * @param initial_joints initial values for joints + * @param gaussian_noise Gaussian noise standard deviation for initialization + * (default 0.0). + * @param initial_joints Initial values for joints. + * @param use_fk If true, use forward kinematics to initialize poses based on + * joint angles. If false, initialize poses near rest configuration with + * Gaussian noise (default false). * @returns values with identity poses and zero joint angles. */ template @@ -256,8 +262,9 @@ class Kinematics : public Optimizer { * @param context Slice or Interval instance * @param robot Robot specification from URDF/SDF * @param pose_goals goals for EE poses - * @param joint_priors optional argument to put priors centered on given - * values. If empty, the priors will be centered on the origin + * @param joint_priors Optional argument to put priors centered on given + * values. Uses gtsam::Values to support different prior means for different + * time steps. If empty, the priors will be centered on the origin. * @return values with poses and joint angles */ template From ec44ccfac4518613fc6766a727918c1416d49881 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 9 Feb 2026 17:35:11 +0000 Subject: [PATCH 16/17] Complete review comment fixes Co-authored-by: dellaert <10515273+dellaert@users.noreply.github.com> --- _codeql_detected_source_root | 1 + 1 file changed, 1 insertion(+) create mode 120000 _codeql_detected_source_root diff --git a/_codeql_detected_source_root b/_codeql_detected_source_root new file mode 120000 index 000000000..945c9b46d --- /dev/null +++ b/_codeql_detected_source_root @@ -0,0 +1 @@ +. \ No newline at end of file From 74d29945220028bda3cfb29c8e19a02e91ea9bd8 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 9 Feb 2026 17:35:27 +0000 Subject: [PATCH 17/17] Clean up: remove codeql symlink and add to gitignore Co-authored-by: dellaert <10515273+dellaert@users.noreply.github.com> --- .gitignore | 1 + _codeql_detected_source_root | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) delete mode 120000 _codeql_detected_source_root diff --git a/.gitignore b/.gitignore index dbd6de3b0..d9ee594ae 100644 --- a/.gitignore +++ b/.gitignore @@ -121,3 +121,4 @@ scripts/gerry00_rpr_viz.gif traj.csv results/ +_codeql_detected_source_root diff --git a/_codeql_detected_source_root b/_codeql_detected_source_root deleted file mode 120000 index 945c9b46d..000000000 --- a/_codeql_detected_source_root +++ /dev/null @@ -1 +0,0 @@ -. \ No newline at end of file