From c9da1be59ad84ced616178a610a8c7b05e2f731f Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 10 Jun 2026 20:24:46 -0400 Subject: [PATCH 01/18] test of workflow --- .github/workflows/linux-ci.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index c1767f7bc..d7914c1a3 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -3,6 +3,9 @@ name: Linux CI on: pull_request: workflow_dispatch: + schedule: + - cron: '27 0 * * 4' # Thursday 00:33 UTC (one-time test) + # 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. From 7f45a868e1670249098200dc7f2d0c9c9be4f853 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 10 Jun 2026 20:29:07 -0400 Subject: [PATCH 02/18] another attempt --- .github/workflows/linux-ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index d7914c1a3..1a2af1096 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -4,7 +4,7 @@ on: pull_request: workflow_dispatch: schedule: - - cron: '27 0 * * 4' # Thursday 00:33 UTC (one-time test) + - cron: '30 0 * * 4' # Thursday 00:33 UTC (one-time test) # Cancels any in-progress workflow runs for the same PR when a new push is made, From f299e726be11d99c76faf0145e304576315e0112 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 10 Jun 2026 20:32:14 -0400 Subject: [PATCH 03/18] attempt 2 --- .github/workflows/linux-ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index 1a2af1096..d85498640 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -4,7 +4,7 @@ on: pull_request: workflow_dispatch: schedule: - - cron: '30 0 * * 4' # Thursday 00:33 UTC (one-time test) + - cron: '33 0 * * 4' # Thursday 00:33 UTC (one-time test) # Cancels any in-progress workflow runs for the same PR when a new push is made, From febfe38222f52a6056bd03ce6bf941c2a898a15e Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 10 Jun 2026 20:35:15 -0400 Subject: [PATCH 04/18] cron update --- .github/workflows/linux-ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index d85498640..89e128792 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -4,7 +4,7 @@ on: pull_request: workflow_dispatch: schedule: - - cron: '33 0 * * 4' # Thursday 00:33 UTC (one-time test) + - cron: '55 0 * * 4' # Thursday 00:33 UTC (one-time test) # Cancels any in-progress workflow runs for the same PR when a new push is made, From 2042bb4f762428cce86aa80768edecf6d2338948 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 10 Jun 2026 20:48:06 -0400 Subject: [PATCH 05/18] attempt 3 --- .github/workflows/linux-ci.yml | 2 +- .github/workflows/macos-ci.yml | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index 89e128792..51aa5b1b2 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -4,7 +4,7 @@ on: pull_request: workflow_dispatch: schedule: - - cron: '55 0 * * 4' # Thursday 00:33 UTC (one-time test) + - cron: '49 0 * * 4' # Thursday 00:33 UTC (one-time test) # Cancels any in-progress workflow runs for the same PR when a new push is made, diff --git a/.github/workflows/macos-ci.yml b/.github/workflows/macos-ci.yml index c5921e8b8..96cac596d 100644 --- a/.github/workflows/macos-ci.yml +++ b/.github/workflows/macos-ci.yml @@ -1,6 +1,11 @@ name: macOS CI -on: [pull_request] +on: + pull_request: + workflow_dispatch: + schedule: + - cron: '49 0 * * 4' # Thursday 00:33 UTC (one-time test) + # 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. From c19792f9bec1865ae18e6bb41cf2a72fb28f3475 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 10 Jun 2026 20:51:54 -0400 Subject: [PATCH 06/18] attempt 4 --- .github/workflows/linux-ci.yml | 2 +- .github/workflows/macos-ci.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index 51aa5b1b2..ad9119f07 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -4,7 +4,7 @@ on: pull_request: workflow_dispatch: schedule: - - cron: '49 0 * * 4' # Thursday 00:33 UTC (one-time test) + - cron: '5 1 * * 4' # Thursday 00:33 UTC (one-time test) # Cancels any in-progress workflow runs for the same PR when a new push is made, diff --git a/.github/workflows/macos-ci.yml b/.github/workflows/macos-ci.yml index 96cac596d..5b08145da 100644 --- a/.github/workflows/macos-ci.yml +++ b/.github/workflows/macos-ci.yml @@ -4,7 +4,7 @@ on: pull_request: workflow_dispatch: schedule: - - cron: '49 0 * * 4' # Thursday 00:33 UTC (one-time test) + - cron: '5 1 * * 4' # Thursday 00:33 UTC (one-time test) # Cancels any in-progress workflow runs for the same PR when a new push is made, From 8dcaa22fbef14f225da1d81e1c529ebf8ac4eec5 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Thu, 11 Jun 2026 10:44:33 -0400 Subject: [PATCH 07/18] re-added manual dispatch --- .github/workflows/linux-ci.yml | 2 -- .github/workflows/macos-ci.yml | 2 -- 2 files changed, 4 deletions(-) diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index ad9119f07..4e557de20 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -3,8 +3,6 @@ name: Linux CI on: pull_request: workflow_dispatch: - schedule: - - cron: '5 1 * * 4' # Thursday 00:33 UTC (one-time test) # Cancels any in-progress workflow runs for the same PR when a new push is made, diff --git a/.github/workflows/macos-ci.yml b/.github/workflows/macos-ci.yml index 5b08145da..939b499af 100644 --- a/.github/workflows/macos-ci.yml +++ b/.github/workflows/macos-ci.yml @@ -3,8 +3,6 @@ name: macOS CI on: pull_request: workflow_dispatch: - schedule: - - cron: '5 1 * * 4' # Thursday 00:33 UTC (one-time test) # Cancels any in-progress workflow runs for the same PR when a new push is made, From 44031e0ad3464ca083287908c6e6cea87a99a176 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 30 Jun 2026 12:03:50 -0400 Subject: [PATCH 08/18] made joint sigmas directly adjustable --- gtdynamics.i | 3 +- gtdynamics/kinematics/KinematicsParameters.h | 19 ++++++-- gtdynamics/kinematics/KinematicsSlice.cpp | 10 ++-- .../kinematics/tests/testKinematicsSlice.cpp | 48 +++++++++++++++++++ 4 files changed, 73 insertions(+), 7 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index 53e0fc11a..3b8a4c3fc 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -358,7 +358,7 @@ class PoseGoal { class KinematicsParameters : gtdynamics::OptimizationParameters { gtsam::SharedNoiseModel p_cost_model; gtsam::SharedNoiseModel g_cost_model; - gtsam::SharedNoiseModel prior_q_cost_model; + std::map prior_q_cost_model; gtsam::SharedNoiseModel bp_cost_model; gtsam::SharedNoiseModel cp_cost_model; gtsam::SharedNoiseModel bv_cost_model; @@ -374,6 +374,7 @@ class KinematicsParameters : gtdynamics::OptimizationParameters { double bv_cost_model_sigma = 1e-4, double v_cost_model_sigma = 1e-4, double cv_cost_model_sigma = 1e-2); + void setJointPriorSigma(string joint_name, double sigma); }; class Kinematics { diff --git a/gtdynamics/kinematics/KinematicsParameters.h b/gtdynamics/kinematics/KinematicsParameters.h index dfa80f587..981e05ae3 100644 --- a/gtdynamics/kinematics/KinematicsParameters.h +++ b/gtdynamics/kinematics/KinematicsParameters.h @@ -15,6 +15,9 @@ #include #include +#include +#include + namespace gtdynamics { /// Noise models etc specific to Kinematics class @@ -22,13 +25,18 @@ struct KinematicsParameters : public OptimizationParameters { using Isotropic = gtsam::noiseModel::Isotropic; gtsam::SharedNoiseModel p_cost_model, // pose factor g_cost_model, // goal point - prior_q_cost_model, // joint angle prior factor bp_cost_model, // fixed-link pose prior factor cp_cost_model, // contact-height factor bv_cost_model, // fixed-link twist prior factor v_cost_model, // twist factor cv_cost_model; // contact-twist factor + /// Joint-angle prior noise models, keyed by joint name. The "" key holds the + /// default applied to any joint not explicitly listed; override an individual + /// joint by inserting its name with a different (e.g. larger) sigma so that + /// joint is freer to move toward the goal. + std::map prior_q_cost_model; + KinematicsParameters() : KinematicsParameters(1e-4, 1e-2, 0.5, 1e-4, 1e-2, 1e-4, 1e-4, 1e-2) {} @@ -44,12 +52,12 @@ struct KinematicsParameters : public OptimizationParameters { const gtsam::SharedNoiseModel& cv_cost_model = Isotropic::Sigma(3, 1e-2)) : p_cost_model(p_cost_model), g_cost_model(g_cost_model), - prior_q_cost_model(prior_q_cost_model), bp_cost_model(bp_cost_model), cp_cost_model(cp_cost_model), bv_cost_model(bv_cost_model), v_cost_model(v_cost_model), - cv_cost_model(cv_cost_model) {} + cv_cost_model(cv_cost_model), + prior_q_cost_model({{"", prior_q_cost_model}}) {} // TODO(yetong): replace noise model with tolerance. KinematicsParameters(double p_cost_model_sigma, @@ -68,6 +76,11 @@ struct KinematicsParameters : public OptimizationParameters { Isotropic::Sigma(6, bv_cost_model_sigma), Isotropic::Sigma(6, v_cost_model_sigma), Isotropic::Sigma(3, cv_cost_model_sigma)) {} + + /// Override the prior sigma for one joint by name ("" = default). + void setJointPriorSigma(const std::string& joint_name, double sigma) { + prior_q_cost_model[joint_name] = Isotropic::Sigma(1, sigma); + } }; } // namespace gtdynamics diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 33c03bd71..a7021cc9c 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -248,7 +248,7 @@ gtsam::NonlinearEqualityConstraints Kinematics::jointAngleConstraints( const gtsam::Values& joint_targets) const { gtsam::NonlinearEqualityConstraints constraints; - const gtsam::Vector1 tolerance(p_.prior_q_cost_model->sigmas()(0)); + const gtsam::Vector1 tolerance(p_.prior_q_cost_model.at("")->sigmas()(0)); for (auto&& joint : robot.joints()) { const gtsam::Key key = JointAngleKey(joint->id(), slice.k); if (!joint_targets.exists(key)) { @@ -302,11 +302,15 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( const Slice& slice, const Robot& robot, const Values& mean) const { NonlinearFactorGraph graph; - // Minimize the joint angles. + // Per-joint prior model from p_.prior_q_cost_model by name, else "" default. for (auto&& joint : robot.joints()) { const gtsam::Key key = JointAngleKey(joint->id(), slice.k); + auto it = p_.prior_q_cost_model.find(joint->name()); + const gtsam::SharedNoiseModel cost_model = + (it != p_.prior_q_cost_model.end()) ? it->second + : p_.prior_q_cost_model.at(""); graph.addPrior(key, (mean.exists(key) ? mean.at(key) : 0.0), - p_.prior_q_cost_model); + cost_model); } return graph; diff --git a/gtdynamics/kinematics/tests/testKinematicsSlice.cpp b/gtdynamics/kinematics/tests/testKinematicsSlice.cpp index d53207d54..1e7f6548f 100644 --- a/gtdynamics/kinematics/tests/testKinematicsSlice.cpp +++ b/gtdynamics/kinematics/tests/testKinematicsSlice.cpp @@ -422,6 +422,54 @@ TEST(Slice, BarLabPoseConstraintIK) { result.at(PoseKey(base_link->id(), k)), tol)); } +// Higher gantry prior sigmas let the gantry absorb more of the motion to goal. +TEST(Slice, BarLabGantryPriorSigma) { + using gtsam::Pose3; + using gtsam::Values; + + const Robot robot = + CreateRobotFromFile(kUrdfPath + std::string("bar_lab.urdf")) + .fixLink("columns"); + auto ee_link = robot.link("robot1_link_6"); + const auto ee_id = ee_link->id(); + const std::vector gantry_joints = { + "bridge1_joint_EA_X", "robot1_joint_EA_Y", "robot1_joint_EA_Z"}; + + // Reachable goal from a config that uses the gantry, so there is redundancy. + Kinematics goal_kinematics; + Values fk = goal_kinematics.initialValues(kSlice, robot, 0.0, + barLabConfig(robot), true); + const Pose3 wTcom_goal = fk.at(PoseKey(ee_id, k)); + PoseGoals pose_goals = {{k, PoseGoal(ee_link, Pose3(), wTcom_goal)}}; + + // Solve IK with the given gantry prior sigma; pose goal is a hard constraint. + auto solve = [&](double gantry_sigma) { + KinematicsParameters params; + params.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + for (const auto& name : gantry_joints) + params.setJointPriorSigma(name, gantry_sigma); + return Kinematics(params).inverse(kSlice, robot, pose_goals, gtsam::Values(), + /*pose_goals_as_constraints=*/true); + }; + + // Total absolute travel of the gantry joints from their zero prior mean. + auto gantry_travel = [&](const Values& result) { + double travel = 0.0; + for (const auto& name : gantry_joints) + travel += std::fabs( + result.at(JointAngleKey(robot.joint(name)->id(), k))); + return travel; + }; + + const Values tight = solve(0.1); + const Values loose = solve(10.0); + + // Both reach the goal pose; the looser gantry priors move the gantry more. + EXPECT(assert_equal(wTcom_goal, tight.at(PoseKey(ee_id, k)), 1e-3)); + EXPECT(assert_equal(wTcom_goal, loose.at(PoseKey(ee_id, k)), 1e-3)); + EXPECT(gantry_travel(tight) < gantry_travel(loose)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 00166f2ee4db872c27e77e593bddd4ee501bfbe6 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 30 Jun 2026 12:44:01 -0400 Subject: [PATCH 09/18] fixed c++ tests --- gtdynamics.i | 2 +- gtdynamics/kinematics/KinematicsParameters.h | 17 ++++++++--------- gtdynamics/kinematics/KinematicsSlice.cpp | 10 +++++----- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index 3b8a4c3fc..50d06939c 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -358,7 +358,7 @@ class PoseGoal { class KinematicsParameters : gtdynamics::OptimizationParameters { gtsam::SharedNoiseModel p_cost_model; gtsam::SharedNoiseModel g_cost_model; - std::map prior_q_cost_model; + gtsam::SharedNoiseModel prior_q_cost_model; gtsam::SharedNoiseModel bp_cost_model; gtsam::SharedNoiseModel cp_cost_model; gtsam::SharedNoiseModel bv_cost_model; diff --git a/gtdynamics/kinematics/KinematicsParameters.h b/gtdynamics/kinematics/KinematicsParameters.h index 981e05ae3..fef1c2564 100644 --- a/gtdynamics/kinematics/KinematicsParameters.h +++ b/gtdynamics/kinematics/KinematicsParameters.h @@ -25,17 +25,16 @@ struct KinematicsParameters : public OptimizationParameters { using Isotropic = gtsam::noiseModel::Isotropic; gtsam::SharedNoiseModel p_cost_model, // pose factor g_cost_model, // goal point + prior_q_cost_model, // joint angle prior factor (default) bp_cost_model, // fixed-link pose prior factor cp_cost_model, // contact-height factor bv_cost_model, // fixed-link twist prior factor v_cost_model, // twist factor cv_cost_model; // contact-twist factor - /// Joint-angle prior noise models, keyed by joint name. The "" key holds the - /// default applied to any joint not explicitly listed; override an individual - /// joint by inserting its name with a different (e.g. larger) sigma so that - /// joint is freer to move toward the goal. - std::map prior_q_cost_model; + /// Per-joint joint-angle prior models keyed by joint name; overrides + /// prior_q_cost_model for listed joints (used by kinematics IK only). + std::map joint_prior_overrides; KinematicsParameters() : KinematicsParameters(1e-4, 1e-2, 0.5, 1e-4, 1e-2, 1e-4, 1e-4, 1e-2) {} @@ -52,12 +51,12 @@ struct KinematicsParameters : public OptimizationParameters { const gtsam::SharedNoiseModel& cv_cost_model = Isotropic::Sigma(3, 1e-2)) : p_cost_model(p_cost_model), g_cost_model(g_cost_model), + prior_q_cost_model(prior_q_cost_model), bp_cost_model(bp_cost_model), cp_cost_model(cp_cost_model), bv_cost_model(bv_cost_model), v_cost_model(v_cost_model), - cv_cost_model(cv_cost_model), - prior_q_cost_model({{"", prior_q_cost_model}}) {} + cv_cost_model(cv_cost_model) {} // TODO(yetong): replace noise model with tolerance. KinematicsParameters(double p_cost_model_sigma, @@ -77,9 +76,9 @@ struct KinematicsParameters : public OptimizationParameters { Isotropic::Sigma(6, v_cost_model_sigma), Isotropic::Sigma(3, cv_cost_model_sigma)) {} - /// Override the prior sigma for one joint by name ("" = default). + /// Override the joint-angle prior sigma for one joint by name. void setJointPriorSigma(const std::string& joint_name, double sigma) { - prior_q_cost_model[joint_name] = Isotropic::Sigma(1, sigma); + joint_prior_overrides[joint_name] = Isotropic::Sigma(1, sigma); } }; diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index a7021cc9c..4145db503 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -248,7 +248,7 @@ gtsam::NonlinearEqualityConstraints Kinematics::jointAngleConstraints( const gtsam::Values& joint_targets) const { gtsam::NonlinearEqualityConstraints constraints; - const gtsam::Vector1 tolerance(p_.prior_q_cost_model.at("")->sigmas()(0)); + const gtsam::Vector1 tolerance(p_.prior_q_cost_model->sigmas()(0)); for (auto&& joint : robot.joints()) { const gtsam::Key key = JointAngleKey(joint->id(), slice.k); if (!joint_targets.exists(key)) { @@ -302,13 +302,13 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( const Slice& slice, const Robot& robot, const Values& mean) const { NonlinearFactorGraph graph; - // Per-joint prior model from p_.prior_q_cost_model by name, else "" default. + // Per-joint override from p_.joint_prior_overrides, else prior_q_cost_model. for (auto&& joint : robot.joints()) { const gtsam::Key key = JointAngleKey(joint->id(), slice.k); - auto it = p_.prior_q_cost_model.find(joint->name()); + auto it = p_.joint_prior_overrides.find(joint->name()); const gtsam::SharedNoiseModel cost_model = - (it != p_.prior_q_cost_model.end()) ? it->second - : p_.prior_q_cost_model.at(""); + (it != p_.joint_prior_overrides.end()) ? it->second + : p_.prior_q_cost_model; graph.addPrior(key, (mean.exists(key) ? mean.at(key) : 0.0), cost_model); } From 7282f3338151d44280779f63a72d1b07a6cc0016 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 30 Jun 2026 12:55:30 -0400 Subject: [PATCH 10/18] added python unit test --- python/tests/test_forward_inv_kinematics.py | 38 +++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/python/tests/test_forward_inv_kinematics.py b/python/tests/test_forward_inv_kinematics.py index cf8669306..94b629bb5 100644 --- a/python/tests/test_forward_inv_kinematics.py +++ b/python/tests/test_forward_inv_kinematics.py @@ -146,6 +146,44 @@ def test_forward_inv_kinematics(self): got = gtd.JointAngle(result, j.id(), k) self.assertAlmostEqual(got, self.angle_rad[j.name()], delta=1e-2) + def test_gantry_prior_sigma(self): + """Looser gantry prior sigmas let the gantry absorb more of the motion.""" + k = self.k + # Fix the gantry base so the gantry joints lie in the chain to the EE. + robot = gtd.CreateRobotFromFile( + str(Path(gtd.URDF_PATH) / "bar_lab.urdf")).fixLink("columns") + link6 = robot.link("robot1_link_6") + gantry_joints = ["bridge1_joint_EA_X", "robot1_joint_EA_Y", + "robot1_joint_EA_Z"] + + # Reachable goal from a config that uses the gantry (so it is redundant). + config = { + "bridge1_joint_EA_X": 2.0, "robot1_joint_EA_Y": 1.5, + "robot1_joint_EA_Z": 0.5, "robot1_joint_1": 0.3, + "robot1_joint_2": 0.4, "robot1_joint_3": -0.5, + "robot1_joint_4": 0.2, "robot1_joint_5": 0.6, "robot1_joint_6": -0.3, + } + q = gtsam.Values() + for j in robot.joints(): + gtd.InsertJointAngle(q, j.id(), k, config.get(j.name(), 0.0)) + fk = robot.forwardKinematics(q, k, "columns") + pose_goals = {k: gtd.PoseGoal(link6, gtsam.Pose3(), + gtd.Pose(fk, link6.id(), k))} + slice0 = gtd.Slice(k) + + # Total gantry travel from the zero prior mean for a given gantry sigma. + def gantry_travel(gantry_sigma): + params = gtd.KinematicsParameters() + for name in gantry_joints: + params.setJointPriorSigma(name, gantry_sigma) + result = gtd.Kinematics(params).inverse( + slice0, robot, pose_goals, gtsam.Values(), True) + return sum(abs(gtd.JointAngle(result, robot.joint(n).id(), k)) + for n in gantry_joints) + + # Looser gantry priors -> the gantry moves more to reach the goal. + self.assertLess(gantry_travel(0.1), gantry_travel(10.0)) + From 3e584e6d64ce1b42aacc8ee365e3bdbdac9a6b8e Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 30 Jun 2026 12:59:56 -0400 Subject: [PATCH 11/18] removed old stuff that shouldn't be here --- .github/workflows/linux-ci.yml | 1 - .github/workflows/macos-ci.yml | 5 +---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index d47e4166c..3193711a7 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -2,7 +2,6 @@ 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: diff --git a/.github/workflows/macos-ci.yml b/.github/workflows/macos-ci.yml index 26f0a18cb..332dc47e8 100644 --- a/.github/workflows/macos-ci.yml +++ b/.github/workflows/macos-ci.yml @@ -1,9 +1,6 @@ name: macOS CI -on: - pull_request: - workflow_dispatch: - +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. From 50a96ca814af3ac2dbf23cf2bd917d4d5bb09153 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 30 Jun 2026 16:11:20 -0400 Subject: [PATCH 12/18] added way to override joint limits --- gtdynamics.i | 1 + gtdynamics/kinematics/KinematicsParameters.h | 9 +++++++++ gtdynamics/kinematics/KinematicsSlice.cpp | 18 ++++++++++++------ 3 files changed, 22 insertions(+), 6 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index 50d06939c..becd8ee25 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -375,6 +375,7 @@ class KinematicsParameters : gtdynamics::OptimizationParameters { double v_cost_model_sigma = 1e-4, double cv_cost_model_sigma = 1e-2); void setJointPriorSigma(string joint_name, double sigma); + void setJointLimit(string joint_name, double lower, double upper); }; class Kinematics { diff --git a/gtdynamics/kinematics/KinematicsParameters.h b/gtdynamics/kinematics/KinematicsParameters.h index fef1c2564..9b23536e8 100644 --- a/gtdynamics/kinematics/KinematicsParameters.h +++ b/gtdynamics/kinematics/KinematicsParameters.h @@ -17,6 +17,7 @@ #include #include +#include namespace gtdynamics { @@ -36,6 +37,9 @@ struct KinematicsParameters : public OptimizationParameters { /// prior_q_cost_model for listed joints (used by kinematics IK only). std::map joint_prior_overrides; + /// Per-joint {lower, upper} limit overrides by joint name (IK only). + std::map> joint_limit_overrides; + KinematicsParameters() : KinematicsParameters(1e-4, 1e-2, 0.5, 1e-4, 1e-2, 1e-4, 1e-4, 1e-2) {} @@ -80,6 +84,11 @@ struct KinematicsParameters : public OptimizationParameters { void setJointPriorSigma(const std::string& joint_name, double sigma) { joint_prior_overrides[joint_name] = Isotropic::Sigma(1, sigma); } + + /// Override the joint-angle limits for one joint by name. + void setJointLimit(const std::string& joint_name, double lower, double upper) { + joint_limit_overrides[joint_name] = {lower, upper}; + } }; } // namespace gtdynamics diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 4145db503..04350903d 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -321,12 +321,18 @@ 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), - 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)); + // Override URDF limits for joints listed in p_.joint_limit_overrides. + auto it = p_.joint_limit_overrides.find(joint->name()); + const auto& limits = joint->parameters().scalar_limits; + const double lower = + (it != p_.joint_limit_overrides.end()) ? it->second.first + : limits.value_lower_limit; + const double upper = + (it != p_.joint_limit_overrides.end()) ? it->second.second + : limits.value_upper_limit; + graph.add(JointLimitFactor(JointAngleKey(joint->id(), slice.k), + gtsam::noiseModel::Isotropic::Sigma(1, 0.001), + lower, upper, 0.04)); } return graph; } From b0e22fbd15929f3bda767bac3f8be4fb5de74610 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 1 Jul 2026 11:25:47 -0400 Subject: [PATCH 13/18] cleaned out string usage, now jid should be referenced directly --- gtdynamics.i | 4 ++-- gtdynamics/kinematics/KinematicsParameters.h | 20 +++++++++---------- gtdynamics/kinematics/KinematicsSlice.cpp | 4 ++-- .../kinematics/tests/testKinematicsSlice.cpp | 2 +- python/tests/test_forward_inv_kinematics.py | 2 +- 5 files changed, 16 insertions(+), 16 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index becd8ee25..09c95401e 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -374,8 +374,8 @@ class KinematicsParameters : gtdynamics::OptimizationParameters { double bv_cost_model_sigma = 1e-4, double v_cost_model_sigma = 1e-4, double cv_cost_model_sigma = 1e-2); - void setJointPriorSigma(string joint_name, double sigma); - void setJointLimit(string joint_name, double lower, double upper); + void setJointPriorSigma(int joint_id, double sigma); + void setJointLimit(int joint_id, double lower, double upper); }; class Kinematics { diff --git a/gtdynamics/kinematics/KinematicsParameters.h b/gtdynamics/kinematics/KinematicsParameters.h index 9b23536e8..4e24c18d7 100644 --- a/gtdynamics/kinematics/KinematicsParameters.h +++ b/gtdynamics/kinematics/KinematicsParameters.h @@ -33,12 +33,12 @@ struct KinematicsParameters : public OptimizationParameters { v_cost_model, // twist factor cv_cost_model; // contact-twist factor - /// Per-joint joint-angle prior models keyed by joint name; overrides + /// Per-joint joint-angle prior models keyed by joint id; overrides /// prior_q_cost_model for listed joints (used by kinematics IK only). - std::map joint_prior_overrides; + std::map joint_prior_overrides; - /// Per-joint {lower, upper} limit overrides by joint name (IK only). - std::map> joint_limit_overrides; + /// Per-joint {lower, upper} limit overrides by joint id (IK only). + std::map> joint_limit_overrides; KinematicsParameters() : KinematicsParameters(1e-4, 1e-2, 0.5, 1e-4, 1e-2, 1e-4, 1e-4, 1e-2) {} @@ -80,14 +80,14 @@ struct KinematicsParameters : public OptimizationParameters { Isotropic::Sigma(6, v_cost_model_sigma), Isotropic::Sigma(3, cv_cost_model_sigma)) {} - /// Override the joint-angle prior sigma for one joint by name. - void setJointPriorSigma(const std::string& joint_name, double sigma) { - joint_prior_overrides[joint_name] = Isotropic::Sigma(1, sigma); + /// Override the joint-angle prior sigma for one joint by id. + void setJointPriorSigma(int joint_id, double sigma) { + joint_prior_overrides[joint_id] = Isotropic::Sigma(1, sigma); } - /// Override the joint-angle limits for one joint by name. - void setJointLimit(const std::string& joint_name, double lower, double upper) { - joint_limit_overrides[joint_name] = {lower, upper}; + /// Override the joint-angle limits for one joint by id. + void setJointLimit(int joint_id, double lower, double upper) { + joint_limit_overrides[joint_id] = {lower, upper}; } }; diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 04350903d..0959153fc 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -305,7 +305,7 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( // Per-joint override from p_.joint_prior_overrides, else prior_q_cost_model. for (auto&& joint : robot.joints()) { const gtsam::Key key = JointAngleKey(joint->id(), slice.k); - auto it = p_.joint_prior_overrides.find(joint->name()); + auto it = p_.joint_prior_overrides.find(joint->id()); const gtsam::SharedNoiseModel cost_model = (it != p_.joint_prior_overrides.end()) ? it->second : p_.prior_q_cost_model; @@ -322,7 +322,7 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( NonlinearFactorGraph graph; for (auto&& joint : robot.joints()) { // Override URDF limits for joints listed in p_.joint_limit_overrides. - auto it = p_.joint_limit_overrides.find(joint->name()); + auto it = p_.joint_limit_overrides.find(joint->id()); const auto& limits = joint->parameters().scalar_limits; const double lower = (it != p_.joint_limit_overrides.end()) ? it->second.first diff --git a/gtdynamics/kinematics/tests/testKinematicsSlice.cpp b/gtdynamics/kinematics/tests/testKinematicsSlice.cpp index 1e7f6548f..180a977b8 100644 --- a/gtdynamics/kinematics/tests/testKinematicsSlice.cpp +++ b/gtdynamics/kinematics/tests/testKinematicsSlice.cpp @@ -447,7 +447,7 @@ TEST(Slice, BarLabGantryPriorSigma) { KinematicsParameters params; params.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; for (const auto& name : gantry_joints) - params.setJointPriorSigma(name, gantry_sigma); + params.setJointPriorSigma(robot.joint(name)->id(), gantry_sigma); return Kinematics(params).inverse(kSlice, robot, pose_goals, gtsam::Values(), /*pose_goals_as_constraints=*/true); }; diff --git a/python/tests/test_forward_inv_kinematics.py b/python/tests/test_forward_inv_kinematics.py index 94b629bb5..a69296e39 100644 --- a/python/tests/test_forward_inv_kinematics.py +++ b/python/tests/test_forward_inv_kinematics.py @@ -175,7 +175,7 @@ def test_gantry_prior_sigma(self): def gantry_travel(gantry_sigma): params = gtd.KinematicsParameters() for name in gantry_joints: - params.setJointPriorSigma(name, gantry_sigma) + params.setJointPriorSigma(robot.joint(name).id(), gantry_sigma) result = gtd.Kinematics(params).inverse( slice0, robot, pose_goals, gtsam.Values(), True) return sum(abs(gtd.JointAngle(result, robot.joint(n).id(), k)) From 50f6906d9bfd9548885d57ebf2dc2593af04e083 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 1 Jul 2026 12:01:18 -0400 Subject: [PATCH 14/18] direct key references --- gtdynamics.i | 4 ++-- gtdynamics/kinematics/KinematicsParameters.h | 21 ++++++++++--------- gtdynamics/kinematics/KinematicsSlice.cpp | 4 ++-- .../kinematics/tests/testKinematicsSlice.cpp | 2 +- python/tests/test_forward_inv_kinematics.py | 2 +- 5 files changed, 17 insertions(+), 16 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index 09c95401e..df428896d 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -374,8 +374,8 @@ class KinematicsParameters : gtdynamics::OptimizationParameters { double bv_cost_model_sigma = 1e-4, double v_cost_model_sigma = 1e-4, double cv_cost_model_sigma = 1e-2); - void setJointPriorSigma(int joint_id, double sigma); - void setJointLimit(int joint_id, double lower, double upper); + void setJointPriorSigma(gtsam::Key joint_key, double sigma); + void setJointLimit(gtsam::Key joint_key, double lower, double upper); }; class Kinematics { diff --git a/gtdynamics/kinematics/KinematicsParameters.h b/gtdynamics/kinematics/KinematicsParameters.h index 4e24c18d7..d3d41d32c 100644 --- a/gtdynamics/kinematics/KinematicsParameters.h +++ b/gtdynamics/kinematics/KinematicsParameters.h @@ -13,6 +13,7 @@ #pragma once #include +#include #include #include @@ -33,12 +34,12 @@ struct KinematicsParameters : public OptimizationParameters { v_cost_model, // twist factor cv_cost_model; // contact-twist factor - /// Per-joint joint-angle prior models keyed by joint id; overrides + /// Per-joint joint-angle prior models keyed by joint key; overrides /// prior_q_cost_model for listed joints (used by kinematics IK only). - std::map joint_prior_overrides; + std::map joint_prior_overrides; - /// Per-joint {lower, upper} limit overrides by joint id (IK only). - std::map> joint_limit_overrides; + /// Per-joint {lower, upper} limit overrides by joint key (IK only). + std::map> joint_limit_overrides; KinematicsParameters() : KinematicsParameters(1e-4, 1e-2, 0.5, 1e-4, 1e-2, 1e-4, 1e-4, 1e-2) {} @@ -80,14 +81,14 @@ struct KinematicsParameters : public OptimizationParameters { Isotropic::Sigma(6, v_cost_model_sigma), Isotropic::Sigma(3, cv_cost_model_sigma)) {} - /// Override the joint-angle prior sigma for one joint by id. - void setJointPriorSigma(int joint_id, double sigma) { - joint_prior_overrides[joint_id] = Isotropic::Sigma(1, sigma); + /// Override the joint-angle prior sigma for one joint by key. + void setJointPriorSigma(gtsam::Key joint_key, double sigma) { + joint_prior_overrides[joint_key] = Isotropic::Sigma(1, sigma); } - /// Override the joint-angle limits for one joint by id. - void setJointLimit(int joint_id, double lower, double upper) { - joint_limit_overrides[joint_id] = {lower, upper}; + /// Override the joint-angle limits for one joint by key. + void setJointLimit(gtsam::Key joint_key, double lower, double upper) { + joint_limit_overrides[joint_key] = {lower, upper}; } }; diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 0959153fc..dfeeb84c9 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -305,7 +305,7 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( // Per-joint override from p_.joint_prior_overrides, else prior_q_cost_model. for (auto&& joint : robot.joints()) { const gtsam::Key key = JointAngleKey(joint->id(), slice.k); - auto it = p_.joint_prior_overrides.find(joint->id()); + auto it = p_.joint_prior_overrides.find(joint->key()); const gtsam::SharedNoiseModel cost_model = (it != p_.joint_prior_overrides.end()) ? it->second : p_.prior_q_cost_model; @@ -322,7 +322,7 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( NonlinearFactorGraph graph; for (auto&& joint : robot.joints()) { // Override URDF limits for joints listed in p_.joint_limit_overrides. - auto it = p_.joint_limit_overrides.find(joint->id()); + auto it = p_.joint_limit_overrides.find(joint->key()); const auto& limits = joint->parameters().scalar_limits; const double lower = (it != p_.joint_limit_overrides.end()) ? it->second.first diff --git a/gtdynamics/kinematics/tests/testKinematicsSlice.cpp b/gtdynamics/kinematics/tests/testKinematicsSlice.cpp index 180a977b8..4fe3642ef 100644 --- a/gtdynamics/kinematics/tests/testKinematicsSlice.cpp +++ b/gtdynamics/kinematics/tests/testKinematicsSlice.cpp @@ -447,7 +447,7 @@ TEST(Slice, BarLabGantryPriorSigma) { KinematicsParameters params; params.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; for (const auto& name : gantry_joints) - params.setJointPriorSigma(robot.joint(name)->id(), gantry_sigma); + params.setJointPriorSigma(robot.joint(name)->key(), gantry_sigma); return Kinematics(params).inverse(kSlice, robot, pose_goals, gtsam::Values(), /*pose_goals_as_constraints=*/true); }; diff --git a/python/tests/test_forward_inv_kinematics.py b/python/tests/test_forward_inv_kinematics.py index a69296e39..57baff0cb 100644 --- a/python/tests/test_forward_inv_kinematics.py +++ b/python/tests/test_forward_inv_kinematics.py @@ -175,7 +175,7 @@ def test_gantry_prior_sigma(self): def gantry_travel(gantry_sigma): params = gtd.KinematicsParameters() for name in gantry_joints: - params.setJointPriorSigma(robot.joint(name).id(), gantry_sigma) + params.setJointPriorSigma(robot.joint(name).key(), gantry_sigma) result = gtd.Kinematics(params).inverse( slice0, robot, pose_goals, gtsam.Values(), True) return sum(abs(gtd.JointAngle(result, robot.joint(n).id(), k)) From 3b11930163f18dc484a2e68b872867f4a357a1cc Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 1 Jul 2026 13:31:52 -0400 Subject: [PATCH 15/18] initial fix to initialization issue --- examples/example_a1_walking/main.cpp | 8 ++++---- examples/example_a1_walking/main_combined.cpp | 6 +++--- .../example_full_kinodynamic_balancing/main.cpp | 8 ++++---- examples/example_spider_walking/main.cpp | 2 +- .../scripts/alejandro_yetong_01_id_four_bar.cpp | 2 +- gtdynamics/dynamics/ChainDynamicsGraph.cpp | 2 +- gtdynamics/dynamics/DynamicsGraph.cpp | 2 +- gtdynamics/dynamics/DynamicsSlice.cpp | 2 +- gtdynamics/dynamics/tests/testChain.cpp | 8 ++++---- gtdynamics/dynamics/tests/testDynamicsGraph.cpp | 14 +++++++------- .../kinematics/tests/testKinematicsSlice.cpp | 2 +- gtdynamics/kinematics/tests/testPoseFactor.cpp | 2 +- .../mechanics/tests/testWrenchPlanarFactor.cpp | 2 +- gtdynamics/statics/Statics.cpp | 2 +- gtdynamics/universal_robot/Link.cpp | 2 +- gtdynamics/utils/Initializer.cpp | 4 ++-- gtdynamics/utils/Trajectory.cpp | 2 +- tests/testObjectiveFactors.cpp | 6 +++--- tests/testSpiderWalking.cpp | 2 +- 19 files changed, 39 insertions(+), 39 deletions(-) diff --git a/examples/example_a1_walking/main.cpp b/examples/example_a1_walking/main.cpp index 91c1ffb73..29a107c3b 100644 --- a/examples/example_a1_walking/main.cpp +++ b/examples/example_a1_walking/main.cpp @@ -108,7 +108,7 @@ int massGraph() { 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))); + .twist(gtsam::Vector6::Zero(), Isotropic::Sigma(6, 5e-2))); } // Add link and joint boundary conditions to FG. @@ -262,7 +262,7 @@ int oldGraph() { 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))); + .twist(gtsam::Vector6::Zero(), Isotropic::Sigma(6, 5e-2))); } // Add link and joint boundary conditions to FG. @@ -418,7 +418,7 @@ int newGraph() { 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))); + .twist(gtsam::Vector6::Zero(), 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))); @@ -435,7 +435,7 @@ int newGraph() { 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))); + .twist(gtsam::Vector6::Zero(), Isotropic::Sigma(6, 5e-2))); } // Constrain all Phase keys to have duration of 1 /240. diff --git a/examples/example_a1_walking/main_combined.cpp b/examples/example_a1_walking/main_combined.cpp index debdddac9..3f6c6487d 100644 --- a/examples/example_a1_walking/main_combined.cpp +++ b/examples/example_a1_walking/main_combined.cpp @@ -144,8 +144,8 @@ int CombinedRun(bool add_mass_to_body) { 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))); + .twist(gtsam::Vector6::Zero(), Isotropic::Sigma(6, 5e-5)) + .twistAccel(gtsam::Vector6::Zero(), Isotropic::Sigma(6, 5e-5))); } // Add prior on A1 lower joint angles @@ -210,7 +210,7 @@ int CombinedRun(bool add_mass_to_body) { if (i == 0) boundary_objectives_CDG.add(LinkObjectives(i, 0) .pose(link->bMcom(), dynamics_model_6) - .twist(gtsam::Z_6x1, dynamics_model_6)); + .twist(gtsam::Vector6::Zero(), 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)); diff --git a/examples/example_full_kinodynamic_balancing/main.cpp b/examples/example_full_kinodynamic_balancing/main.cpp index 6abfd74fd..f7b287b18 100644 --- a/examples/example_full_kinodynamic_balancing/main.cpp +++ b/examples/example_full_kinodynamic_balancing/main.cpp @@ -58,10 +58,10 @@ int main(int argc, char** argv) { // Specify boundary conditions for base and joints. gtsam::Pose3 base_pose_init = vision60.link("body")->bMcom(); - gtsam::Vector6 base_twist_init = gtsam::Z_6x1, - base_twist_final = gtsam::Z_6x1, - base_accel_init = gtsam::Z_6x1, - base_accel_final = gtsam::Z_6x1; + gtsam::Vector6 base_twist_init = gtsam::Vector6::Zero(), + base_twist_final = gtsam::Vector6::Zero(), + base_accel_init = gtsam::Vector6::Zero(), + base_accel_final = gtsam::Vector6::Zero(); gtsam::Vector joint_angles_init = gtsam::Vector::Zero(12), joint_vels_init = gtsam::Vector::Zero(12), joint_accels_init = gtsam::Vector::Zero(12), diff --git a/examples/example_spider_walking/main.cpp b/examples/example_spider_walking/main.cpp index fb0053ec8..30c149623 100644 --- a/examples/example_spider_walking/main.cpp +++ b/examples/example_spider_walking/main.cpp @@ -106,7 +106,7 @@ int main(int argc, char** argv) { objectives.add( LinkObjectives(base_link->id(), k) .pose(Pose3(Rot3(), Point3(0, 0.0, 0.5)), Isotropic::Sigma(6, 5e-5)) - .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-5))); + .twist(gtsam::Vector6::Zero(), Isotropic::Sigma(6, 5e-5))); } // Add link and joint boundary conditions to FG. diff --git a/examples/scripts/alejandro_yetong_01_id_four_bar.cpp b/examples/scripts/alejandro_yetong_01_id_four_bar.cpp index 226536375..c3ef8c864 100644 --- a/examples/scripts/alejandro_yetong_01_id_four_bar.cpp +++ b/examples/scripts/alejandro_yetong_01_id_four_bar.cpp @@ -52,7 +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::Vector6::Zero(), gtsam::noiseModel::Constrained::All(6)); } graph.add(prior_factors); diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.cpp b/gtdynamics/dynamics/ChainDynamicsGraph.cpp index 3b8b3468a..12b33909c 100644 --- a/gtdynamics/dynamics/ChainDynamicsGraph.cpp +++ b/gtdynamics/dynamics/ChainDynamicsGraph.cpp @@ -125,7 +125,7 @@ NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactors( std::vector wrench_keys; constexpr int root = 0; // TODO(Frank): Hard-coded for A1. - const Vector6 wrench_zero = gtsam::Z_6x1; + const Vector6 wrench_zero = gtsam::Vector6::Zero(); for (int leg = 0; leg < 4; ++leg) { bool foot_in_contact = false; diff --git a/gtdynamics/dynamics/DynamicsGraph.cpp b/gtdynamics/dynamics/DynamicsGraph.cpp index 19fb5d33f..d18c1a61e 100644 --- a/gtdynamics/dynamics/DynamicsGraph.cpp +++ b/gtdynamics/dynamics/DynamicsGraph.cpp @@ -42,7 +42,7 @@ using gtsam::PriorFactor; using gtsam::Values; using gtsam::Vector; using gtsam::Vector6; -using gtsam::Z_6x1; +using gtsam::Vector6::Zero(); namespace gtdynamics { diff --git a/gtdynamics/dynamics/DynamicsSlice.cpp b/gtdynamics/dynamics/DynamicsSlice.cpp index 3fb49ee98..31d8b5358 100644 --- a/gtdynamics/dynamics/DynamicsSlice.cpp +++ b/gtdynamics/dynamics/DynamicsSlice.cpp @@ -30,7 +30,7 @@ NonlinearFactorGraph Dynamics::aFactors( for (auto&& link : robot.links()) { if (link->isFixed()) { graph.addPrior(TwistAccelKey(link->id(), slice.k), - gtsam::Z_6x1, p_.ba_cost_model); + gtsam::Vector6::Zero(), p_.ba_cost_model); } } for (auto&& joint : robot.joints()) { diff --git a/gtdynamics/dynamics/tests/testChain.cpp b/gtdynamics/dynamics/tests/testChain.cpp index ddd2712f1..b96750ba0 100644 --- a/gtdynamics/dynamics/tests/testChain.cpp +++ b/gtdynamics/dynamics/tests/testChain.cpp @@ -986,7 +986,7 @@ gtsam::Values OldGraphOneLeg() { OptimizerSetting opt(1e-4, 1e-4, 1e-4, 1e-4); DynamicsGraph graph_builder(opt, gravity); - gtsam::Vector6 wrench_zero = gtsam::Z_6x1; + gtsam::Vector6 wrench_zero = gtsam::Vector6::Zero(); gtsam::NonlinearFactorGraph graph; @@ -1094,7 +1094,7 @@ gtsam::Values NewGraphOneLeg() { if (joint->id() > 2) continue; InsertJointAngle(&init_values, joint->id(), 0, 0.0); } - gtsam::Vector6 wrench_zero = gtsam::Z_6x1; + gtsam::Vector6 wrench_zero = gtsam::Vector6::Zero(); init_values.insert(gtdynamics::WrenchKey(0, 0, 0), wrench_zero); /// Solve the constraint problem with LM optimizer. @@ -1199,7 +1199,7 @@ gtsam::Values OldGraphFourLegs() { OptimizerSetting opt(5e-5, 5e-5, 5e-5, 5e-5); DynamicsGraph graph_builder(opt, gravity); - gtsam::Vector6 wrench_zero = gtsam::Z_6x1; + gtsam::Vector6 wrench_zero = gtsam::Vector6::Zero(); gtsam::NonlinearFactorGraph graph; @@ -1276,7 +1276,7 @@ gtsam::Values NewGraphFourLegs() { //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; + gtsam::Vector6 wrench_zero = gtsam::Vector6::Zero(); auto constrained_model = gtsam::noiseModel::Constrained::All(6); auto bp_cost_model(gtsam::noiseModel::Isotropic::Sigma(6, 1e-5)); diff --git a/gtdynamics/dynamics/tests/testDynamicsGraph.cpp b/gtdynamics/dynamics/tests/testDynamicsGraph.cpp index fa90c39ad..cd9f63e3b 100644 --- a/gtdynamics/dynamics/tests/testDynamicsGraph.cpp +++ b/gtdynamics/dynamics/tests/testDynamicsGraph.cpp @@ -70,7 +70,7 @@ TEST(linearDynamicsFactorGraph, simple_urdf_eq_mass_values) { int t = 777; auto j = robot.joint("j1")->id(); InsertPose(&values, l1->id(), t, l1->bMcom()); - InsertTwist(&values, l1->id(), t, gtsam::Z_6x1); + InsertTwist(&values, l1->id(), t, gtsam::Vector6::Zero()); // Do forward kinematics. Values fk_results = robot.forwardKinematics(values, t, prior_link_name); @@ -127,7 +127,7 @@ TEST(dynamicsFactorGraph_FD, simple_urdf_eq_mass) { int i = link->id(); graph.addPrior(PoseKey(i, t), link->bMcom(), graph_builder.opt().bp_cost_model); - graph.addPrior(TwistKey(i, t), gtsam::Z_6x1, + graph.addPrior(TwistKey(i, t), gtsam::Vector6::Zero(), graph_builder.opt().bv_cost_model); } Initializer initializer; @@ -164,7 +164,7 @@ TEST(dynamicsFactorGraph_FD, four_bar_linkage_pure) { int i = link->id(); prior_factors.addPrior(PoseKey(i, 0), link->bMcom(), graph_builder.opt().bp_cost_model); - prior_factors.addPrior(TwistKey(i, 0), gtsam::Z_6x1, + prior_factors.addPrior(TwistKey(i, 0), gtsam::Vector6::Zero(), graph_builder.opt().bv_cost_model); } @@ -465,7 +465,7 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rr) { poseWithContactHeight(l0->bMcom(), contact_in_com, kGroundHeight), gtsam::noiseModel::Constrained::All(6)); - prior_factors.addPrior(TwistKey(l0->id(), 0), gtsam::Z_6x1, + prior_factors.addPrior(TwistKey(l0->id(), 0), gtsam::Vector6::Zero(), gtsam::noiseModel::Constrained::All(6)); graph.add(prior_factors); @@ -523,9 +523,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::Vector6::Zero(), graph_builder.opt().bv_cost_model); - prior_factors.addPrior(TwistAccelKey(body->id(), 0), gtsam::Z_6x1, + prior_factors.addPrior(TwistAccelKey(body->id(), 0), gtsam::Vector6::Zero(), graph_builder.opt().ba_cost_model); graph.add(prior_factors); @@ -602,7 +602,7 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rrr) { poseWithContactHeight(l0->bMcom(), contact_in_com, kGroundHeight), gtsam::noiseModel::Constrained::All(6)); - prior_factors.addPrior(TwistKey(l0->id(), 0), gtsam::Z_6x1, + prior_factors.addPrior(TwistKey(l0->id(), 0), gtsam::Vector6::Zero(), gtsam::noiseModel::Constrained::All(6)); graph.add(prior_factors); diff --git a/gtdynamics/kinematics/tests/testKinematicsSlice.cpp b/gtdynamics/kinematics/tests/testKinematicsSlice.cpp index 4fe3642ef..071fbed39 100644 --- a/gtdynamics/kinematics/tests/testKinematicsSlice.cpp +++ b/gtdynamics/kinematics/tests/testKinematicsSlice.cpp @@ -44,7 +44,7 @@ TEST(Slice, InverseKinematics) { // Set twists to zero for FK. TODO(frank): separate kinematics from velocity? for (auto&& link : robot.links()) { - InsertTwist(&values, link->id(), k, gtsam::Z_6x1); + InsertTwist(&values, link->id(), k, gtsam::Vector6::Zero()); } // Do forward kinematics diff --git a/gtdynamics/kinematics/tests/testPoseFactor.cpp b/gtdynamics/kinematics/tests/testPoseFactor.cpp index d9528a090..6868391d8 100644 --- a/gtdynamics/kinematics/tests/testPoseFactor.cpp +++ b/gtdynamics/kinematics/tests/testPoseFactor.cpp @@ -34,7 +34,7 @@ using gtsam::Pose3; using gtsam::Rot3; using gtsam::Values; using gtsam::Vector6; -using gtsam::Z_6x1; +using gtsam::Vector6::Zero(); using gtsam::noiseModel::Gaussian; namespace example { diff --git a/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp b/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp index 5e1105126..58d96c975 100644 --- a/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp +++ b/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp @@ -35,7 +35,7 @@ gtsam::noiseModel::Gaussian::shared_ptr cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::I_3x3); const DynamicsSymbol wrench_key = WrenchKey(2, 1, 777); gtsam::Pose3 kMj; // doesn't matter -auto [joint, links] = make_joint(kMj, gtsam::Z_6x1); +auto [joint, links] = make_joint(kMj, gtsam::Vector6::Zero()); } // namespace example // Test wrench planar factor for x-axis diff --git a/gtdynamics/statics/Statics.cpp b/gtdynamics/statics/Statics.cpp index fd90114f4..8c5cd1da1 100644 --- a/gtdynamics/statics/Statics.cpp +++ b/gtdynamics/statics/Statics.cpp @@ -43,7 +43,7 @@ Vector6 GravityWrench(const gtsam::Vector3 &gravity, double mass, Vector6 ResultantWrench(const std::vector &wrenches, gtsam::OptionalMatrixVecType H) { - Vector6 sum = gtsam::Z_6x1; + Vector6 sum = gtsam::Vector6::Zero(); const size_t n = wrenches.size(); for (size_t i = 0; i < n; i++) { sum += wrenches[i]; diff --git a/gtdynamics/universal_robot/Link.cpp b/gtdynamics/universal_robot/Link.cpp index 3ceb6a714..662f8f555 100644 --- a/gtdynamics/universal_robot/Link.cpp +++ b/gtdynamics/universal_robot/Link.cpp @@ -64,7 +64,7 @@ gtsam::Vector6_ Link::wrenchConstraint( } // Calculate resultant wrench. - gtsam::Vector6_ error(gtsam::Z_6x1); + gtsam::Vector6_ error(gtsam::Vector6::Zero()); for (const auto& wrench : wrenches) { error += wrench; } diff --git a/gtdynamics/utils/Initializer.cpp b/gtdynamics/utils/Initializer.cpp index 6b014816f..cf458bfe6 100644 --- a/gtdynamics/utils/Initializer.cpp +++ b/gtdynamics/utils/Initializer.cpp @@ -96,7 +96,7 @@ Values Initializer::InitializePosesAndJoints( // Compute forward dynamics to obtain remaining link poses. Pose3 wTl_i_processed = AddGaussianNoiseToPose(wTl_i, sampler); InsertPose(&fk_input, robot.link(link_name)->id(), wTl_i_processed); - InsertTwist(&fk_input, robot.link(link_name)->id(), gtsam::Z_6x1); + InsertTwist(&fk_input, robot.link(link_name)->id(), gtsam::Vector6::Zero()); auto fk_results = robot.forwardKinematics(fk_input, 0, link_name); for (auto&& link : robot.links()) { @@ -143,7 +143,7 @@ Values Initializer::InitializeSolutionInterpolation( link_name + " is fixed."); } InsertPose(&init_vals, link->id(), t, wTl_t); - InsertTwist(&init_vals, link->id(), t, gtsam::Z_6x1); + InsertTwist(&init_vals, link->id(), t, gtsam::Vector6::Zero()); init_vals = robot.forwardKinematics(init_vals, t, link_name); const Values zero_values = diff --git a/gtdynamics/utils/Trajectory.cpp b/gtdynamics/utils/Trajectory.cpp index 4c86894d4..6e9795bed 100644 --- a/gtdynamics/utils/Trajectory.cpp +++ b/gtdynamics/utils/Trajectory.cpp @@ -27,7 +27,7 @@ using gtsam::NonlinearFactorGraph; using gtsam::Point3; using gtsam::SharedNoiseModel; using gtsam::Values; -using gtsam::Z_6x1; +using gtsam::Vector6::Zero(); using std::map; using std::string; using std::to_string; diff --git a/tests/testObjectiveFactors.cpp b/tests/testObjectiveFactors.cpp index 6f8fd05bc..4ec728ccf 100644 --- a/tests/testObjectiveFactors.cpp +++ b/tests/testObjectiveFactors.cpp @@ -42,7 +42,7 @@ TEST(ObjectiveFactors, PoseAndTwist) { EXPECT_LONGS_EQUAL(1, graph.size()); graph.add(LinkObjectives(id, k) .pose(Pose3(), kModel6) - .twist(gtsam::Z_6x1, kModel6)); + .twist(gtsam::Vector6::Zero(), kModel6)); EXPECT_LONGS_EQUAL(3, graph.size()); } @@ -50,8 +50,8 @@ TEST(ObjectiveFactors, TwistWithDerivatives) { NonlinearFactorGraph graph; constexpr int id = 5, k = 777; graph.add(LinkObjectives(id, k) - .twist(gtsam::Z_6x1, kModel6) - .twistAccel(gtsam::Z_6x1, kModel6)); + .twist(gtsam::Vector6::Zero(), kModel6) + .twistAccel(gtsam::Vector6::Zero(), kModel6)); EXPECT_LONGS_EQUAL(2, graph.size()); } diff --git a/tests/testSpiderWalking.cpp b/tests/testSpiderWalking.cpp index 240322a8e..903cc7006 100644 --- a/tests/testSpiderWalking.cpp +++ b/tests/testSpiderWalking.cpp @@ -116,7 +116,7 @@ TEST(testSpiderWalking, WholeEnchilada) { objectives.add( LinkObjectives(base_link->id(), k) .pose(Pose3(Rot3(), Point3(0, 0.0, 0.5)), Isotropic::Sigma(6, 5e-5)) - .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-5))); + .twist(gtsam::Vector6::Zero(), Isotropic::Sigma(6, 5e-5))); } // Add link and joint boundary conditions to FG. From 15ff002830c296fa91ada6f7751a0b7697406e2c Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 1 Jul 2026 14:01:41 -0400 Subject: [PATCH 16/18] z-fix v2 --- gtdynamics/dynamics/DynamicsGraph.cpp | 3 +-- gtdynamics/dynamics/tests/testWrenchFactor.cpp | 6 +++--- gtdynamics/factors/ObjectiveFactors.h | 4 ++-- gtdynamics/kinematics/tests/testPoseFactor.cpp | 7 +++---- gtdynamics/statics/tests/testStaticWrenchFactor.cpp | 2 +- gtdynamics/utils/Trajectory.cpp | 7 +++---- 6 files changed, 13 insertions(+), 16 deletions(-) diff --git a/gtdynamics/dynamics/DynamicsGraph.cpp b/gtdynamics/dynamics/DynamicsGraph.cpp index d18c1a61e..edfe0d205 100644 --- a/gtdynamics/dynamics/DynamicsGraph.cpp +++ b/gtdynamics/dynamics/DynamicsGraph.cpp @@ -42,7 +42,6 @@ using gtsam::PriorFactor; using gtsam::Values; using gtsam::Vector; using gtsam::Vector6; -using gtsam::Vector6::Zero(); namespace gtdynamics { @@ -64,7 +63,7 @@ GaussianFactorGraph DynamicsGraph::linearDynamicsGraph( if (link->isFixed()) { // prior on twist acceleration for fixed link // A_i = 0 - graph.add(TwistAccelKey(i, k), I_6x6, Z_6x1, all_constrained); + graph.add(TwistAccelKey(i, k), I_6x6, gtsam::Vector6::Zero(), all_constrained); } else { // wrench factor // G_i * A_i - F_i_j1 - .. - F_i_jn = ad(V_i)^T * G_i * V*i + m_i * R_i^T diff --git a/gtdynamics/dynamics/tests/testWrenchFactor.cpp b/gtdynamics/dynamics/tests/testWrenchFactor.cpp index f9847cf46..e119c321f 100644 --- a/gtdynamics/dynamics/tests/testWrenchFactor.cpp +++ b/gtdynamics/dynamics/tests/testWrenchFactor.cpp @@ -62,7 +62,7 @@ TEST(WrenchFactor, Case1) { InsertPose(&x, id, Pose3(Rot3(), Point3(1, 0, 0))); Vector6 actual_errors = factor->unwhitenedError(x); - Vector6 expected_errors = Z_6x1; + Vector6 expected_errors = gtsam::Vector6::Zero(); EXPECT(assert_equal(expected_errors, actual_errors, 1e-6)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(*factor, x, diffDelta, tol); @@ -86,7 +86,7 @@ TEST(WrenchFactor, Case2) { InsertPose(&x, id, Pose3(Rot3(), Point3(1, 0, 0))); Vector6 actual_errors = factor->unwhitenedError(x); - Vector6 expected_errors = Z_6x1; + Vector6 expected_errors = gtsam::Vector6::Zero(); EXPECT(assert_equal(expected_errors, actual_errors, 1e-6)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(*factor, x, diffDelta, tol); @@ -109,7 +109,7 @@ TEST(WrenchFactor, NonzeroTwistCase) { InsertPose(&x, id, Pose3(Rot3(), Point3(1, 0, 0))); Vector6 actual_errors = factor->unwhitenedError(x); - Vector6 expected_errors = Z_6x1; + Vector6 expected_errors = gtsam::Vector6::Zero(); EXPECT(assert_equal(expected_errors, actual_errors, 1e-6)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(*factor, x, diffDelta, tol); diff --git a/gtdynamics/factors/ObjectiveFactors.h b/gtdynamics/factors/ObjectiveFactors.h index 9917ec182..2f8bd795f 100644 --- a/gtdynamics/factors/ObjectiveFactors.h +++ b/gtdynamics/factors/ObjectiveFactors.h @@ -31,8 +31,8 @@ namespace gtdynamics { * * Example Usage: * LinkObjectives(graph, id, k).pose(Pose3(), noise) - * .twist(Z_6x1, noise) - * .twistAccel(Z_6x1, noise); + * .twist(gtsam::Vector6::Zero(), noise) + * .twistAccel(gtsam::Vector6::Zero(), noise); */ class LinkObjectives : public gtsam::NonlinearFactorGraph { private: diff --git a/gtdynamics/kinematics/tests/testPoseFactor.cpp b/gtdynamics/kinematics/tests/testPoseFactor.cpp index 6868391d8..f36f71baf 100644 --- a/gtdynamics/kinematics/tests/testPoseFactor.cpp +++ b/gtdynamics/kinematics/tests/testPoseFactor.cpp @@ -34,7 +34,6 @@ using gtsam::Pose3; using gtsam::Rot3; using gtsam::Values; using gtsam::Vector6; -using gtsam::Vector6::Zero(); using gtsam::noiseModel::Gaussian; namespace example { @@ -87,7 +86,7 @@ TEST(PoseFactor, breaking) { InsertPose(&values, 1, Pose3(Rot3(), Point3(1, 0, 0))); InsertPose(&values, 2, Pose3(Rot3(), Point3(3, 0, 0))); InsertJointAngle(&values, 1, 0.0); - EXPECT(assert_equal(Z_6x1, factor->unwhitenedError(values), 1e-6)); + EXPECT(assert_equal(gtsam::Vector6::Zero(), factor->unwhitenedError(values), 1e-6)); } // check prediction at half PI @@ -96,7 +95,7 @@ TEST(PoseFactor, breaking) { InsertPose(&values, 1, Pose3(Rot3(), Point3(1, 0, 0))); InsertPose(&values, 2, Pose3(Rot3::Rz(M_PI / 2), Point3(2, 1, 0))); InsertJointAngle(&values, 1, M_PI / 2); - EXPECT(assert_equal(Z_6x1, factor->unwhitenedError(values), 1e-6)); + EXPECT(assert_equal(gtsam::Vector6::Zero(), factor->unwhitenedError(values), 1e-6)); } } @@ -120,7 +119,7 @@ TEST(PoseFactor, breaking_rr) { InsertPose(&values, 1, Pose3()); InsertPose(&values, 2, j1->relativePoseOf(l2, M_PI / 4)); InsertJointAngle(&values, 1, M_PI / 4); - EXPECT(assert_equal(Z_6x1, factor->unwhitenedError(values), 1e-6)); + EXPECT(assert_equal(gtsam::Vector6::Zero(), factor->unwhitenedError(values), 1e-6)); } // Test non-zero cMp rotation case diff --git a/gtdynamics/statics/tests/testStaticWrenchFactor.cpp b/gtdynamics/statics/tests/testStaticWrenchFactor.cpp index 1d8aaf814..0855020e8 100644 --- a/gtdynamics/statics/tests/testStaticWrenchFactor.cpp +++ b/gtdynamics/statics/tests/testStaticWrenchFactor.cpp @@ -60,7 +60,7 @@ TEST(StaticWrenchFactor, GravityCompensation) { InsertPose(&x, id, Pose3(Rot3(), Point3(1, 0, 0))); Vector6 actual_errors = factor.unwhitenedError(x); - Vector6 expected_errors = Z_6x1; + Vector6 expected_errors = gtsam::Vector6::Zero(); EXPECT(assert_equal(expected_errors, actual_errors, 1e-6)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(factor, x, diffDelta, tol); diff --git a/gtdynamics/utils/Trajectory.cpp b/gtdynamics/utils/Trajectory.cpp index 6e9795bed..10b9c7779 100644 --- a/gtdynamics/utils/Trajectory.cpp +++ b/gtdynamics/utils/Trajectory.cpp @@ -27,7 +27,6 @@ using gtsam::NonlinearFactorGraph; using gtsam::Point3; using gtsam::SharedNoiseModel; using gtsam::Values; -using gtsam::Vector6::Zero(); using std::map; using std::string; using std::to_string; @@ -119,12 +118,12 @@ void Trajectory::addBoundaryConditions( // Initial link pose, twists. graph->add(LinkObjectives(link->id(), 0) .pose(link->bMcom(), pose_model) - .twist(Z_6x1, twist_model)); + .twist(gtsam::Vector6::Zero(), twist_model)); // Final link twists, accelerations. graph->add(LinkObjectives(link->id(), K) - .twist(Z_6x1, twist_model) - .twistAccel(Z_6x1, twist_acceleration_model)); + .twist(gtsam::Vector6::Zero(), twist_model) + .twistAccel(gtsam::Vector6::Zero(), twist_acceleration_model)); } // Add joint boundary conditions to FG. From ca9015b89c71995279dadbbe45bd70132095932a Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 1 Jul 2026 15:16:07 -0400 Subject: [PATCH 17/18] fixed lack of transitive import --- gtdynamics/optimizer/ConvexIQPSolver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtdynamics/optimizer/ConvexIQPSolver.cpp b/gtdynamics/optimizer/ConvexIQPSolver.cpp index 5ff435800..d70475b56 100644 --- a/gtdynamics/optimizer/ConvexIQPSolver.cpp +++ b/gtdynamics/optimizer/ConvexIQPSolver.cpp @@ -12,6 +12,7 @@ */ #include +#include namespace gtdynamics { From 835ef843aa74310e99ff8671f6bb48a3e694d78f Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Wed, 1 Jul 2026 15:32:11 -0400 Subject: [PATCH 18/18] fixed I initialization --- .../QuadrupedUtils.cpp | 2 +- .../main_arm_kinematic_planning.cpp | 6 +-- .../main_connected_poses.cpp | 4 +- .../main_range_constraint.cpp | 4 +- .../main.cpp | 2 +- .../main_rotate.cpp | 2 +- .../main_walk_forward.cpp | 2 +- .../example_spider_walking/main_rotate.cpp | 2 +- .../alejandro_yetong_01_id_four_bar.cpp | 2 +- .../factors/CableAccelerationFactor.cpp | 4 +- .../factors/CableAccelerationFactor.h | 2 +- .../cablerobot/factors/CableLengthFactor.h | 2 +- .../cablerobot/factors/CableTensionFactor.cpp | 6 +-- .../cablerobot/factors/CableTensionFactor.h | 2 +- .../factors/CableVelocityFactor.cpp | 2 +- .../cablerobot/factors/CableVelocityFactor.h | 2 +- .../constrained_optimizer/IPOptOptimizer.cpp | 4 +- gtdynamics/dynamics/Chain.cpp | 6 +-- gtdynamics/dynamics/DynamicsGraph.cpp | 14 +++---- .../testContactDynamicsFrictionConeFactor.cpp | 4 +- .../tests/testContactDynamicsMomentFactor.cpp | 2 +- .../testContactKinematicsAccelFactor.cpp | 2 +- .../dynamics/tests/testTwistAccelFactor.cpp | 2 +- .../dynamics/tests/testWrenchFactor.cpp | 2 +- gtdynamics/factors/CollocationFactors.h | 36 ++++++++--------- gtdynamics/factors/MinTorqueFactor.h | 2 +- .../factors/PreintegratedContactFactors.h | 10 ++--- .../factors/JRCollocationFactors.h | 4 +- .../factors/PneumaticActuatorFactors.h | 4 +- .../jumpingrobot/factors/PneumaticFactors.h | 2 +- gtdynamics/kinematics/JointLimitFactor.h | 6 +-- .../tests/testContactHeightFactor.cpp | 4 +- .../testContactKinematicsTwistFactor.cpp | 2 +- .../kinematics/tests/testPoseFactor.cpp | 2 +- .../kinematics/tests/testTwistFactor.cpp | 2 +- .../mechanics/tests/testTorqueFactor.cpp | 2 +- .../tests/testWrenchEquivalenceFactor.cpp | 2 +- .../tests/testWrenchPlanarFactor.cpp | 2 +- .../scenarios/IEQuadrupedUtilsCosts.cpp | 4 +- gtdynamics/statics/Statics.cpp | 4 +- .../statics/tests/testStaticWrenchFactor.cpp | 2 +- gtdynamics/universal_robot/Joint.cpp | 8 ++-- gtdynamics/universal_robot/Link.cpp | 2 +- tests/testCollocationFactor.cpp | 2 +- tests/testConvexIQPSolver.cpp | 8 ++-- tests/testMinTorqueFactor.cpp | 2 +- tests/testMultiJacobian.cpp | 32 +++++++-------- tests/testPreintegratedContactFactors.cpp | 40 +++++++++---------- tests/testTspaceBasis.cpp | 8 ++-- tests/testUtils.cpp | 2 +- 50 files changed, 137 insertions(+), 139 deletions(-) diff --git a/examples/example_constraint_manifold/QuadrupedUtils.cpp b/examples/example_constraint_manifold/QuadrupedUtils.cpp index 8294876f7..0d11bc009 100644 --- a/examples/example_constraint_manifold/QuadrupedUtils.cpp +++ b/examples/example_constraint_manifold/QuadrupedUtils.cpp @@ -40,7 +40,7 @@ Vector3 get_contact_force(const Pose3 &pose, const Vector6 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; + J_fl_wrench << gtsam::Matrix3::Zero(), gtsam::Matrix3::Identity(); Matrix36 J_rot_pose; Rot3 rot = pose.rotation(J_rot_pose); diff --git a/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp b/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp index cb027a463..50d058e97 100644 --- a/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp +++ b/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp @@ -124,13 +124,13 @@ class JointLimitFunctor { double operator()(const double& q, OptionalJacobian<1, 1> H_q = nullptr) const { if (q < low_) { - if (H_q) *H_q = -I_1x1; + if (H_q) *H_q = -gtsam::Matrix1::Identity(); return low_ - q; } else if (q <= high_) { - if (H_q) *H_q = Z_1x1; + if (H_q) *H_q = gtsam::Matrix1::Zero(); return 0.0; } else { - if (H_q) *H_q = I_1x1; + if (H_q) *H_q = gtsam::Matrix1::Identity(); return q - high_; } } diff --git a/examples/example_constraint_manifold/main_connected_poses.cpp b/examples/example_constraint_manifold/main_connected_poses.cpp index d59a9741a..12a56f731 100644 --- a/examples/example_constraint_manifold/main_connected_poses.cpp +++ b/examples/example_constraint_manifold/main_connected_poses.cpp @@ -107,14 +107,14 @@ double EvaluatePoseError(const Values& gt, const Values& result) { 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; + Matrix3 diff = rel_pose.matrix() - gtsam::Matrix3::Identity(); 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; + Matrix3 diff = rel_pose.matrix() - gtsam::Matrix3::Identity(); error2 += pow(diff.norm(), 2); } } diff --git a/examples/example_constraint_manifold/main_range_constraint.cpp b/examples/example_constraint_manifold/main_range_constraint.cpp index fba5f2c1d..697e08758 100644 --- a/examples/example_constraint_manifold/main_range_constraint.cpp +++ b/examples/example_constraint_manifold/main_range_constraint.cpp @@ -159,7 +159,7 @@ double EvaluatePoseError(const Values& gt, const Values& result) { 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; + Matrix3 diff = rel_pose.matrix() - gtsam::Matrix3::Identity(); // std::cout << diff << "\n"; // std::cout << diff.norm() << "\n"; error1 += pow(diff.norm(), 2); @@ -168,7 +168,7 @@ double EvaluatePoseError(const Values& gt, const Values& result) { 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; + Matrix3 diff = rel_pose.matrix() - gtsam::Matrix3::Identity(); error2 += pow(diff.norm(), 2); } } diff --git a/examples/example_full_kinodynamic_balancing/main.cpp b/examples/example_full_kinodynamic_balancing/main.cpp index f7b287b18..ad74e63b0 100644 --- a/examples/example_full_kinodynamic_balancing/main.cpp +++ b/examples/example_full_kinodynamic_balancing/main.cpp @@ -178,7 +178,7 @@ int main(int argc, char** argv) { for (auto&& joint : vision60.joints()) objective_factors.add(MinTorqueFactor( TorqueKey(joint->id(), t), - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1))); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix1::Identity()))); } graph.add(objective_factors); diff --git a/examples/example_full_kinodynamic_walking/main_rotate.cpp b/examples/example_full_kinodynamic_walking/main_rotate.cpp index eb4869905..0fe05f498 100644 --- a/examples/example_full_kinodynamic_walking/main_rotate.cpp +++ b/examples/example_full_kinodynamic_walking/main_rotate.cpp @@ -298,7 +298,7 @@ int main(int argc, char** argv) { for (auto&& joint : robot.joints()) objective_factors.add(gtdynamics::MinTorqueFactor( TorqueKey(joint->id(), t), - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1))); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix1::Identity()))); } graph.add(objective_factors); diff --git a/examples/example_full_kinodynamic_walking/main_walk_forward.cpp b/examples/example_full_kinodynamic_walking/main_walk_forward.cpp index dfc58be6b..fc6018206 100644 --- a/examples/example_full_kinodynamic_walking/main_walk_forward.cpp +++ b/examples/example_full_kinodynamic_walking/main_walk_forward.cpp @@ -283,7 +283,7 @@ int main(int argc, char** argv) { for (auto&& joint : robot.joints()) { objective_factors.add(gtdynamics::MinTorqueFactor( TorqueKey(joint->id(), t), - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1))); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix1::Identity()))); } } graph.add(objective_factors); diff --git a/examples/example_spider_walking/main_rotate.cpp b/examples/example_spider_walking/main_rotate.cpp index d4cf271c1..9531b9f3d 100644 --- a/examples/example_spider_walking/main_rotate.cpp +++ b/examples/example_spider_walking/main_rotate.cpp @@ -340,7 +340,7 @@ int main(int argc, char** argv) { for (auto&& joint : robot.joints()) objective_factors.add(gtdynamics::MinTorqueFactor( TorqueKey(joint->id(), t), - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1))); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix1::Identity()))); } graph.add(objective_factors); diff --git a/examples/scripts/alejandro_yetong_01_id_four_bar.cpp b/examples/scripts/alejandro_yetong_01_id_four_bar.cpp index c3ef8c864..e0cbd73d5 100644 --- a/examples/scripts/alejandro_yetong_01_id_four_bar.cpp +++ b/examples/scripts/alejandro_yetong_01_id_four_bar.cpp @@ -60,7 +60,7 @@ int main(int argc, char** argv) { for (auto joint : robot.joints()) graph.add( MinTorqueFactor(TorqueKey(joint->id(), 0), - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1))); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix1::Identity()))); // Initialize solution. gtsam::Values init_values = initializer.ZeroValues(robot, 0); diff --git a/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp b/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp index d8c93126a..1294de778 100644 --- a/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp +++ b/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp @@ -38,8 +38,8 @@ double CableAccelerationFactor::computeLddot( Vector3 xAb = VAx.bottomRows<3>(); // acceleration of point b in x's frame Vector3 wAb = wTx.rotation().rotate(xAb, H_wTx ? &wAb_H_wRx : nullptr, H_VAx ? &wAb_H_xAb : nullptr); - if (H_wTx) wAb_H_wTx << wAb_H_wRx, Z_3x3; - if (H_VAx) wAb_H_VAx << Z_3x3, wAb_H_xAb; + if (H_wTx) wAb_H_wTx << wAb_H_wRx, gtsam::Matrix3::Zero(); + if (H_VAx) wAb_H_VAx << gtsam::Matrix3::Zero(), wAb_H_xAb; // cable direction Point3 wPb = wTx.transformFrom(xPb_, H_wTx ? &wPb_H_wTx : nullptr); Vector3 dir = normalize(wPb - wPa_, H_wTx ? &dir_H_wPb : nullptr); diff --git a/gtdynamics/cablerobot/factors/CableAccelerationFactor.h b/gtdynamics/cablerobot/factors/CableAccelerationFactor.h index ea74a51d9..35329ebd3 100644 --- a/gtdynamics/cablerobot/factors/CableAccelerationFactor.h +++ b/gtdynamics/cablerobot/factors/CableAccelerationFactor.h @@ -86,7 +86,7 @@ class CableAccelerationFactor gtsam::OptionalMatrixType H_Vx = nullptr, gtsam::OptionalMatrixType H_VAx = nullptr) const override { double expected_lddot = computeLddot(wTx, Vx, VAx, H_wTx, H_Vx, H_VAx); - if (H_lddot) *H_lddot = gtsam::I_1x1; + if (H_lddot) *H_lddot = gtsam::Matrix1::Identity(); if (H_wTx) *H_wTx = -(*H_wTx); if (H_Vx) *H_Vx = -(*H_Vx); if (H_VAx) *H_VAx = -(*H_VAx); diff --git a/gtdynamics/cablerobot/factors/CableLengthFactor.h b/gtdynamics/cablerobot/factors/CableLengthFactor.h index b6fc0a375..0db86b65a 100644 --- a/gtdynamics/cablerobot/factors/CableLengthFactor.h +++ b/gtdynamics/cablerobot/factors/CableLengthFactor.h @@ -60,7 +60,7 @@ class CableLengthFactor gtsam::Matrix13 H_wPb; auto wPb = wTx.transformFrom(xPb_, H_wTx ? &wPb_H_wTx : 0); double expected_l = gtsam::distance3(wPb, wPa_, H_wTx ? &H_wPb : 0); - if (H_l) *H_l = gtsam::I_1x1; + if (H_l) *H_l = gtsam::Matrix1::Identity(); if (H_wTx) *H_wTx = -H_wPb * wPb_H_wTx; return gtsam::Vector1(l - expected_l); } diff --git a/gtdynamics/cablerobot/factors/CableTensionFactor.cpp b/gtdynamics/cablerobot/factors/CableTensionFactor.cpp index 569de1a7b..b7d38900c 100644 --- a/gtdynamics/cablerobot/factors/CableTensionFactor.cpp +++ b/gtdynamics/cablerobot/factors/CableTensionFactor.cpp @@ -41,7 +41,7 @@ Vector6 CableTensionFactor::computeWrench( // force->wrench Vector3 wf = -t * dir; if (H_t) wf_H_t = -dir; - if (H_wTx) wf_H_dir = -t * I_3x3; + if (H_wTx) wf_H_dir = -t * gtsam::Matrix3::Identity(); 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); @@ -50,7 +50,7 @@ Vector6 CableTensionFactor::computeWrench( (H_t || H_wTx) ? &xm_H_xf : 0); Vector6 F = (Vector6() << xm, xf).finished(); - if (H_t || H_wTx) H_xf << xm_H_xf, I_3x3; + if (H_t || H_wTx) H_xf << xm_H_xf, gtsam::Matrix3::Identity(); if (H_t) *H_t = H_xf * xf_H_wf * wf_H_t; if (H_wTx) { *(H_wTx) = H_xf * xf_H_wf * wf_H_dir * dir_H_wPb * wPb_H_wTx; @@ -77,7 +77,7 @@ Vector6 CableTensionFactor::computeWrenchUsingAdjoint( Pose3 bTx = Pose3(wTx.rotation().inverse(), xPb_).inverse(); Vector6 wF = (Vector6() << 0, 0, 0, -t * dir).finished(); if (H_t) wF_H_t = (Matrix61() << 0, 0, 0, -dir).finished(); - if (H_wTx) wF_H_dir = (Matrix63() << Z_3x3, -t * I_3x3).finished(); + if (H_wTx) wF_H_dir = (Matrix63() << gtsam::Matrix3::Zero(), -t * gtsam::Matrix3::Identity()).finished(); Vector6 F = bTx.AdjointMap().transpose() * wF; // TODO(gerry): find jacobian of adjoint map return F; diff --git a/gtdynamics/cablerobot/factors/CableTensionFactor.h b/gtdynamics/cablerobot/factors/CableTensionFactor.h index 102d68eaa..08d877bdc 100644 --- a/gtdynamics/cablerobot/factors/CableTensionFactor.h +++ b/gtdynamics/cablerobot/factors/CableTensionFactor.h @@ -88,7 +88,7 @@ class CableTensionFactor (Vector6() << Fx - computeWrench(t, wTx, H_t, H_wTx)).finished(); if (H_t) *H_t = -(*H_t); if (H_wTx) *H_wTx = -(*H_wTx); - if (H_Fx) *H_Fx = gtsam::I_6x6; + if (H_Fx) *H_Fx = gtsam::Matrix6::Identity(); return error; } diff --git a/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp b/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp index 80f1a331f..f711ac331 100644 --- a/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp +++ b/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp @@ -41,7 +41,7 @@ double CableVelocityFactor::computeLdot(const Pose3 &wTx, const Vector6 &Vx, // TODO(gerry): use Adjoint Vector3 eePDOTem = Vx.tail<3>() + cross(Vx.head<3>(), xPb_, H_Vx ? &cross_H_omega : 0); - if (H_Vx) xPDOTb_H_Vx << cross_H_omega, I_3x3; + if (H_Vx) xPDOTb_H_Vx << cross_H_omega, gtsam::Matrix3::Identity(); Vector3 wPDOTem = wTx.rotation().rotate(eePDOTem, // H_wTx ? &wPDOTb_H_wRx : 0, H_Vx ? &wPDOTb_H_xPDOTb : 0); diff --git a/gtdynamics/cablerobot/factors/CableVelocityFactor.h b/gtdynamics/cablerobot/factors/CableVelocityFactor.h index 871c730ea..fec0aef7e 100644 --- a/gtdynamics/cablerobot/factors/CableVelocityFactor.h +++ b/gtdynamics/cablerobot/factors/CableVelocityFactor.h @@ -74,7 +74,7 @@ class CableVelocityFactor 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_ldot) *H_ldot = gtsam::Matrix1::Identity(); if (H_wTx) *H_wTx = -(*H_wTx); if (H_Vx) *H_Vx = -(*H_Vx); return gtsam::Vector1(ldot - expected_ldot); diff --git a/gtdynamics/constrained_optimizer/IPOptOptimizer.cpp b/gtdynamics/constrained_optimizer/IPOptOptimizer.cpp index 61e1dab0d..4ffc3c4fe 100644 --- a/gtdynamics/constrained_optimizer/IPOptOptimizer.cpp +++ b/gtdynamics/constrained_optimizer/IPOptOptimizer.cpp @@ -16,8 +16,8 @@ Pose3 IFOptTranslator::VecToPose(const Vector6 &vec, OptionalJacobian<6, 6> 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; + H_angles_vec << gtsam::Matrix3::Identity(), gtsam::Matrix3::Zero(); + H_trans_vec << gtsam::Matrix3::Zero(), gtsam::Matrix3::Identity(); Matrix33 H_rot_angles; Rot3 rot = Rot3::RzRyRx(euler_angles, H_rot_angles); Matrix63 H_pose_rot, H_pose_trans; diff --git a/gtdynamics/dynamics/Chain.cpp b/gtdynamics/dynamics/Chain.cpp index 77d6243a0..25f420064 100644 --- a/gtdynamics/dynamics/Chain.cpp +++ b/gtdynamics/dynamics/Chain.cpp @@ -108,7 +108,7 @@ gtsam::Matrix6 AdjointMapJacobianQ(double q, const gtsam::Pose3 &jMi, 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::Matrix6 H = gtsam::Matrix6::Zero(); gtsam::insertSub(H, H_R, 0, 0); gtsam::insertSub(H, H_TR, 3, 0); gtsam::insertSub(H, H_R, 3, 3); @@ -138,7 +138,7 @@ gtsam::Vector3 Chain::DynamicalEquality3( // 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; + Matrix A = gtsam::Matrix3::Zero(); // Calculate the Adjoint and take its derivative in relation to angles auto ad_J_angles1 = AdjointMapJacobianQ(angles(1), Pose3(), axes_.col(1)); @@ -159,7 +159,7 @@ gtsam::Vector3 Chain::DynamicalEquality3( } if (H_torques) { // derivative of difference with respect to torques - *H_torques = -gtsam::I_3x3; + *H_torques = -gtsam::Matrix3::Identity(); } return (J.transpose() * wrench - torques); diff --git a/gtdynamics/dynamics/DynamicsGraph.cpp b/gtdynamics/dynamics/DynamicsGraph.cpp index edfe0d205..ff4ad8782 100644 --- a/gtdynamics/dynamics/DynamicsGraph.cpp +++ b/gtdynamics/dynamics/DynamicsGraph.cpp @@ -33,8 +33,6 @@ using gtsam::Double_; using gtsam::ExpressionFactor; using gtsam::GaussianFactorGraph; -using gtsam::I_1x1; -using gtsam::I_6x6; using gtsam::Key; using gtsam::NonlinearFactorGraph; using gtsam::Pose3; @@ -63,7 +61,7 @@ GaussianFactorGraph DynamicsGraph::linearDynamicsGraph( if (link->isFixed()) { // prior on twist acceleration for fixed link // A_i = 0 - graph.add(TwistAccelKey(i, k), I_6x6, gtsam::Vector6::Zero(), all_constrained); + graph.add(TwistAccelKey(i, k), gtsam::Matrix6::Identity(), gtsam::Vector6::Zero(), all_constrained); } else { // wrench factor // G_i * A_i - F_i_j1 - .. - F_i_jn = ad(V_i)^T * G_i * V*i + m_i * R_i^T @@ -86,10 +84,10 @@ GaussianFactorGraph DynamicsGraph::linearDynamicsGraph( graph.add(accel_key, G_i, rhs, all_constrained); } else if (connected_joints.size() == 1) { graph.add(accel_key, G_i, WrenchKey(i, connected_joints[0]->id(), k), - -I_6x6, rhs, all_constrained); + -gtsam::Matrix6::Identity(), rhs, all_constrained); } else if (connected_joints.size() == 2) { graph.add(accel_key, G_i, WrenchKey(i, connected_joints[0]->id(), k), - -I_6x6, WrenchKey(i, connected_joints[1]->id(), k), -I_6x6, + -gtsam::Matrix6::Identity(), WrenchKey(i, connected_joints[1]->id(), k), -gtsam::Matrix6::Identity(), rhs, all_constrained); } } @@ -120,7 +118,7 @@ GaussianFactorGraph DynamicsGraph::linearIDPriors( int j = joint->id(); double accel = JointAccel(joint_accels, j, k); gtsam::Vector1 rhs(accel); - graph.add(JointAccelKey(j, k), I_1x1, rhs, all_constrained); + graph.add(JointAccelKey(j, k), gtsam::Matrix1::Identity(), rhs, all_constrained); } return graph; } @@ -333,8 +331,8 @@ void DynamicsGraph::addCollocationFactorDouble( double multDouble(const double &d1, const double &d2, gtsam::OptionalJacobian<1, 1> H1, gtsam::OptionalJacobian<1, 1> H2) { - if (H1) *H1 = gtsam::I_1x1 * d2; - if (H2) *H2 = gtsam::I_1x1 * d1; + if (H1) *H1 = gtsam::Matrix1::Identity() * d2; + if (H2) *H2 = gtsam::Matrix1::Identity() * d1; return d1 * d2; } diff --git a/gtdynamics/dynamics/tests/testContactDynamicsFrictionConeFactor.cpp b/gtdynamics/dynamics/tests/testContactDynamicsFrictionConeFactor.cpp index ab9a4f6a3..62a99bd68 100644 --- a/gtdynamics/dynamics/tests/testContactDynamicsFrictionConeFactor.cpp +++ b/gtdynamics/dynamics/tests/testContactDynamicsFrictionConeFactor.cpp @@ -39,7 +39,7 @@ using gtsam::assert_equal; * Test the evaluateError method with various link contact wrenches and angles. **/ TEST(ContactDynamicsFrictionConeFactor, error) { - auto cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1); + auto cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix1::Identity()); LabeledSymbol pose_key = LabeledSymbol('p', 0, 0); LabeledSymbol contact_wrench_key = LabeledSymbol('C', 0, 0); @@ -116,7 +116,7 @@ TEST(ContactDynamicsFrictionConeFactor, error) { * lies within the friction cone. **/ TEST(ContactDynamicsFrictionConeFactor, optimization) { - auto cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1); + auto cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix1::Identity()); LabeledSymbol pose_key = LabeledSymbol('p', 0, 0); LabeledSymbol contact_wrench_key = LabeledSymbol('C', 0, 0); diff --git a/gtdynamics/dynamics/tests/testContactDynamicsMomentFactor.cpp b/gtdynamics/dynamics/tests/testContactDynamicsMomentFactor.cpp index a3587fc3d..47d8bc9ca 100644 --- a/gtdynamics/dynamics/tests/testContactDynamicsMomentFactor.cpp +++ b/gtdynamics/dynamics/tests/testContactDynamicsMomentFactor.cpp @@ -36,7 +36,7 @@ TEST(ContactDynamicsMomentFactor, error) { auto robot = simple_urdf::getRobot(); gtsam::noiseModel::Gaussian::shared_ptr cost_model = - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_3x3); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix3::Identity()); gtsam::LabeledSymbol contact_wrench_key = gtsam::LabeledSymbol('C', 0, 0); diff --git a/gtdynamics/dynamics/tests/testContactKinematicsAccelFactor.cpp b/gtdynamics/dynamics/tests/testContactKinematicsAccelFactor.cpp index b787d4928..9c4a55a12 100644 --- a/gtdynamics/dynamics/tests/testContactKinematicsAccelFactor.cpp +++ b/gtdynamics/dynamics/tests/testContactKinematicsAccelFactor.cpp @@ -36,7 +36,7 @@ TEST(ContactKinematicsAccelFactor, error) { auto robot = simple_urdf::getRobot(); gtsam::noiseModel::Gaussian::shared_ptr cost_model = - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_3x3); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix3::Identity()); gtsam::LabeledSymbol twist_accel_key = gtsam::LabeledSymbol('A', 0, 0); diff --git a/gtdynamics/dynamics/tests/testTwistAccelFactor.cpp b/gtdynamics/dynamics/tests/testTwistAccelFactor.cpp index c914f5f47..aa46fc994 100644 --- a/gtdynamics/dynamics/tests/testTwistAccelFactor.cpp +++ b/gtdynamics/dynamics/tests/testTwistAccelFactor.cpp @@ -35,7 +35,7 @@ namespace example { // noise model gtsam::noiseModel::Gaussian::shared_ptr cost_model = - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_6x6); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix6::Identity()); gtsam::Key qKey = JointAngleKey(1), qVelKey = JointVelKey(1), qAccelKey = JointAccelKey(1), twistKey = TwistKey(2), diff --git a/gtdynamics/dynamics/tests/testWrenchFactor.cpp b/gtdynamics/dynamics/tests/testWrenchFactor.cpp index e119c321f..718ebd052 100644 --- a/gtdynamics/dynamics/tests/testWrenchFactor.cpp +++ b/gtdynamics/dynamics/tests/testWrenchFactor.cpp @@ -38,7 +38,7 @@ const Matrix6 inertia = link->inertiaMatrix(); const Vector3 gravity(0, -9.8, 0); noiseModel::Gaussian::shared_ptr cost_model = - noiseModel::Gaussian::Covariance(I_6x6); + noiseModel::Gaussian::Covariance(gtsam::Matrix6::Identity()); } // namespace example diff --git a/gtdynamics/factors/CollocationFactors.h b/gtdynamics/factors/CollocationFactors.h index be9d4ead7..41cf73d42 100644 --- a/gtdynamics/factors/CollocationFactors.h +++ b/gtdynamics/factors/CollocationFactors.h @@ -85,7 +85,7 @@ class EulerPoseCollocationFactor auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); gtsam::Vector error = pose_t1.logmap(pose_t1_hat); if (H_pose_t1) { - *H_pose_t1 = -gtsam::I_6x6; + *H_pose_t1 = -gtsam::Matrix6::Identity(); } if (H_twist) { *H_twist = H_twistdt * dt; @@ -166,7 +166,7 @@ class TrapezoidalPoseCollocationFactor auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); gtsam::Vector error = pose_t1.logmap(pose_t1_hat); if (H_pose_t1) { - *H_pose_t1 = -gtsam::I_6x6; + *H_pose_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { *H_twist_t0 = 0.5 * dt * H_twistdt; @@ -248,7 +248,7 @@ class FixTimeEulerPoseCollocationFactor auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); gtsam::Vector error = pose_t1.logmap(pose_t1_hat); if (H_pose_t1) { - *H_pose_t1 = -gtsam::I_6x6; + *H_pose_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { *H_twist_t0 = dt_ * H_twistdt; @@ -327,7 +327,7 @@ class FixTimeTrapezoidalPoseCollocationFactor auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); gtsam::Vector error = pose_t1.logmap(pose_t1_hat); if (H_pose_t1) { - *H_pose_t1 = -gtsam::I_6x6; + *H_pose_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { *H_twist_t0 = 0.5 * dt_ * H_twistdt; @@ -401,13 +401,13 @@ class EulerTwistCollocationFactor gtsam::OptionalMatrixType H_dt = nullptr) const override { gtsam::Vector error = twist_t0 + dt * accel - twist_t1; if (H_twist_t1) { - *H_twist_t1 = -gtsam::I_6x6; + *H_twist_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { - *H_twist_t0 = gtsam::I_6x6; + *H_twist_t0 = gtsam::Matrix6::Identity(); } if (H_accel) { - *H_accel = gtsam::I_6x6 * dt; + *H_accel = gtsam::Matrix6::Identity() * dt; } if (H_dt) { *H_dt = accel; @@ -483,16 +483,16 @@ class TrapezoidalTwistCollocationFactor gtsam::Vector error = twist_t0 + 0.5 * dt * (accel_t0 + accel_t1) - twist_t1; if (H_twist_t1) { - *H_twist_t1 = -gtsam::I_6x6; + *H_twist_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { - *H_twist_t0 = gtsam::I_6x6; + *H_twist_t0 = gtsam::Matrix6::Identity(); } if (H_accel_t0) { - *H_accel_t0 = 0.5 * dt * gtsam::I_6x6; + *H_accel_t0 = 0.5 * dt * gtsam::Matrix6::Identity(); } if (H_accel_t1) { - *H_accel_t1 = 0.5 * dt * gtsam::I_6x6; + *H_accel_t1 = 0.5 * dt * gtsam::Matrix6::Identity(); } if (H_dt) { *H_dt = 0.5 * (accel_t0 + accel_t1); @@ -564,13 +564,13 @@ class FixTimeEulerTwistCollocationFactor gtsam::OptionalMatrixType H_accel_t0 = nullptr) const override { gtsam::Vector error = twist_t0 + dt_ * accel_t0 - twist_t1; if (H_twist_t1) { - *H_twist_t1 = -gtsam::I_6x6; + *H_twist_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { - *H_twist_t0 = gtsam::I_6x6; + *H_twist_t0 = gtsam::Matrix6::Identity(); } if (H_accel_t0) { - *H_accel_t0 = dt_ * gtsam::I_6x6; + *H_accel_t0 = dt_ * gtsam::Matrix6::Identity(); } return error; } @@ -644,16 +644,16 @@ class FixTimeTrapezoidalTwistCollocationFactor gtsam::Vector error = twist_t0 + 0.5 * dt_ * (accel_t0 + accel_t1) - twist_t1; if (H_twist_t1) { - *H_twist_t1 = -gtsam::I_6x6; + *H_twist_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { - *H_twist_t0 = gtsam::I_6x6; + *H_twist_t0 = gtsam::Matrix6::Identity(); } if (H_accel_t0) { - *H_accel_t0 = 0.5 * dt_ * gtsam::I_6x6; + *H_accel_t0 = 0.5 * dt_ * gtsam::Matrix6::Identity(); } if (H_accel_t1) { - *H_accel_t1 = 0.5 * dt_ * gtsam::I_6x6; + *H_accel_t1 = 0.5 * dt_ * gtsam::Matrix6::Identity(); } return error; } diff --git a/gtdynamics/factors/MinTorqueFactor.h b/gtdynamics/factors/MinTorqueFactor.h index 22c4544cf..af2d93246 100644 --- a/gtdynamics/factors/MinTorqueFactor.h +++ b/gtdynamics/factors/MinTorqueFactor.h @@ -52,7 +52,7 @@ class MinTorqueFactor : public gtsam::NoiseModelFactorN { gtsam::Vector error(1); error(0) = torque; - if (H_torque) *H_torque = gtsam::I_1x1; + if (H_torque) *H_torque = gtsam::Matrix1::Identity(); return error; } diff --git a/gtdynamics/factors/PreintegratedContactFactors.h b/gtdynamics/factors/PreintegratedContactFactors.h index 50486e90a..488638d1f 100644 --- a/gtdynamics/factors/PreintegratedContactFactors.h +++ b/gtdynamics/factors/PreintegratedContactFactors.h @@ -142,12 +142,12 @@ class PreintegratedPointContactFactor // https://arxiv.org/src/1712.05873v2/anc/icra-supplementary-material.pdf if (H_wTb_i) { gtsam::Matrix36 H; - H << gtsam::SO3::Hat(error), gtsam::Z_3x3; + H << gtsam::SO3::Hat(error), gtsam::Matrix3::Zero(); *H_wTb_i = H; } if (H_wTc_i) { gtsam::Matrix36 H; - H << gtsam::Z_3x3, -gtsam::I_3x3; + H << gtsam::Matrix3::Zero(), -gtsam::Matrix3::Identity(); *H_wTc_i = H; } if (H_wTb_j) { @@ -163,7 +163,7 @@ class PreintegratedPointContactFactor "Body rotation and contact rotation are not equal."); } gtsam::Matrix36 H; - H << gtsam::Z_3x3, + H << gtsam::Matrix3::Zero(), (wTc_i.rotation().inverse() * wTc_j.rotation()).matrix(); *H_wTc_j = H; } @@ -239,7 +239,7 @@ class PreintegratedRigidContactMeasurements { const gtsam::Matrix3 &linearVelocityCovariance, double dt) { gtsam::Matrix6 C; - C << angularVelocityCovariance, gtsam::Z_3x3, gtsam::Z_3x3, + C << angularVelocityCovariance, gtsam::Matrix3::Zero(), gtsam::Matrix3::Zero(), linearVelocityCovariance; preintMeasCov_ += (C * dt * dt); } @@ -251,7 +251,7 @@ class PreintegratedRigidContactMeasurements { * steps. */ void integrateMeasurement(double deltaT) { - preintMeasCov_ << wCov_, gtsam::Z_3x3, gtsam::Z_3x3, vCov_; + preintMeasCov_ << wCov_, gtsam::Matrix3::Zero(), gtsam::Matrix3::Zero(), vCov_; preintMeasCov_ *= deltaT; } diff --git a/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h b/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h index 726ff4114..2be1b27ac 100644 --- a/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h +++ b/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h @@ -29,8 +29,8 @@ namespace gtdynamics { double multDouble1(const double& d1, const double& d2, gtsam::OptionalJacobian<1, 1> H1, gtsam::OptionalJacobian<1, 1> H2) { - if (H1) *H1 = gtsam::I_1x1 * d2; - if (H2) *H2 = gtsam::I_1x1 * d1; + if (H1) *H1 = gtsam::Matrix1::Identity() * d2; + if (H2) *H2 = gtsam::Matrix1::Identity() * d1; return d1 * d2; } diff --git a/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h b/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h index 8395ad59f..8f330547d 100644 --- a/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h +++ b/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h @@ -457,7 +457,7 @@ class ClippingActuatorFactor H_p->setConstant(1, 1, derivative_y); } if (H_f) { - *H_f = -gtsam::I_1x1; + *H_f = -gtsam::Matrix1::Identity(); } return gtsam::Vector1(f_expected - f); } @@ -527,7 +527,7 @@ class ActuatorVolumeFactor : public gtsam::NoiseModelFactorN { gtsam::OptionalMatrixType H_l = nullptr) const override { double expected_v = computeVolume(l, H_l); if (H_v) { - *H_v = -gtsam::I_1x1; + *H_v = -gtsam::Matrix1::Identity(); } return gtsam::Vector1(expected_v - v); } diff --git a/gtdynamics/jumpingrobot/factors/PneumaticFactors.h b/gtdynamics/jumpingrobot/factors/PneumaticFactors.h index 8cc5ba665..a76cb421e 100644 --- a/gtdynamics/jumpingrobot/factors/PneumaticFactors.h +++ b/gtdynamics/jumpingrobot/factors/PneumaticFactors.h @@ -149,7 +149,7 @@ class MassFlowRateFactor double expected_mdot = computeExpectedMassFlow(pm, ps, mdot, H_pm, H_ps, H_mdot); if (H_mdot) { - *H_mdot = *H_mdot - gtsam::I_1x1; + *H_mdot = *H_mdot - gtsam::Matrix1::Identity(); } return gtsam::Vector1(expected_mdot - mdot); } diff --git a/gtdynamics/kinematics/JointLimitFactor.h b/gtdynamics/kinematics/JointLimitFactor.h index e61252017..3eb7a7dae 100644 --- a/gtdynamics/kinematics/JointLimitFactor.h +++ b/gtdynamics/kinematics/JointLimitFactor.h @@ -69,13 +69,13 @@ class JointLimitFactor : public gtsam::NoiseModelFactorN { gtsam::OptionalMatrixType H_q) const override { gtsam::Vector error(1); if (q < low_) { - if (H_q) *H_q = -gtsam::I_1x1; + if (H_q) *H_q = -gtsam::Matrix1::Identity(); error(0) = low_ - q; } else if (q <= high_) { - if (H_q) *H_q = gtsam::Z_1x1; + if (H_q) *H_q = gtsam::Matrix1::Zero(); error(0) = 0.0; } else { - if (H_q) *H_q = gtsam::I_1x1; + if (H_q) *H_q = gtsam::Matrix1::Identity(); error(0) = q - high_; } return error; diff --git a/gtdynamics/kinematics/tests/testContactHeightFactor.cpp b/gtdynamics/kinematics/tests/testContactHeightFactor.cpp index 857776d20..e8a98de2a 100644 --- a/gtdynamics/kinematics/tests/testContactHeightFactor.cpp +++ b/gtdynamics/kinematics/tests/testContactHeightFactor.cpp @@ -36,7 +36,7 @@ static constexpr double kGroundHeight = 4.2; **/ TEST(ContactHeightFactor, Error) { gtsam::noiseModel::Gaussian::shared_ptr cost_model = - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix1::Identity()); gtsam::LabeledSymbol pose_key = gtsam::LabeledSymbol('p', 0, 0); @@ -87,7 +87,7 @@ TEST(ContactHeightFactor, Error) { **/ TEST(ContactHeightFactor, ErrorWithHeight) { gtsam::noiseModel::Gaussian::shared_ptr cost_model = - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix1::Identity()); gtsam::LabeledSymbol pose_key = gtsam::LabeledSymbol('p', 0, 0); diff --git a/gtdynamics/kinematics/tests/testContactKinematicsTwistFactor.cpp b/gtdynamics/kinematics/tests/testContactKinematicsTwistFactor.cpp index 5745edd80..c0d03881d 100644 --- a/gtdynamics/kinematics/tests/testContactKinematicsTwistFactor.cpp +++ b/gtdynamics/kinematics/tests/testContactKinematicsTwistFactor.cpp @@ -36,7 +36,7 @@ TEST(ContactKinematicsTwistFactor, error) { auto robot = simple_urdf::getRobot(); gtsam::noiseModel::Gaussian::shared_ptr cost_model = - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_3x3); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix3::Identity()); gtsam::LabeledSymbol twist_key = gtsam::LabeledSymbol('V', 0, 0); diff --git a/gtdynamics/kinematics/tests/testPoseFactor.cpp b/gtdynamics/kinematics/tests/testPoseFactor.cpp index f36f71baf..8d360173d 100644 --- a/gtdynamics/kinematics/tests/testPoseFactor.cpp +++ b/gtdynamics/kinematics/tests/testPoseFactor.cpp @@ -38,7 +38,7 @@ using gtsam::noiseModel::Gaussian; namespace example { // nosie model -Gaussian::shared_ptr cost_model = Gaussian::Covariance(gtsam::I_6x6); +Gaussian::shared_ptr cost_model = Gaussian::Covariance(gtsam::Matrix6::Identity()); gtsam::Key wTp_key = PoseKey(1), wTc_key = PoseKey(2), q_key = JointAngleKey(1); } // namespace example diff --git a/gtdynamics/kinematics/tests/testTwistFactor.cpp b/gtdynamics/kinematics/tests/testTwistFactor.cpp index ebb872cee..fbb595c07 100644 --- a/gtdynamics/kinematics/tests/testTwistFactor.cpp +++ b/gtdynamics/kinematics/tests/testTwistFactor.cpp @@ -32,7 +32,7 @@ using gtsam::assert_equal; namespace example { gtsam::noiseModel::Gaussian::shared_ptr cost_model = - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_6x6); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix6::Identity()); gtsam::Key twist_p_key = TwistKey(1), twist_c_key = TwistKey(2), qKey = JointAngleKey(1), qVelKey = JointVelKey(1); } // namespace example diff --git a/gtdynamics/mechanics/tests/testTorqueFactor.cpp b/gtdynamics/mechanics/tests/testTorqueFactor.cpp index 8dffbb1be..3aee47030 100644 --- a/gtdynamics/mechanics/tests/testTorqueFactor.cpp +++ b/gtdynamics/mechanics/tests/testTorqueFactor.cpp @@ -41,7 +41,7 @@ TEST(TorqueFactor, error) { auto [joint, links] = make_joint(kMj, screw_axis); // Create factor. - auto cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1); + auto cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix1::Identity()); auto factor = TorqueFactor(cost_model, joint, 777); // Check keys. diff --git a/gtdynamics/mechanics/tests/testWrenchEquivalenceFactor.cpp b/gtdynamics/mechanics/tests/testWrenchEquivalenceFactor.cpp index ab1dc5f43..284d9ee96 100644 --- a/gtdynamics/mechanics/tests/testWrenchEquivalenceFactor.cpp +++ b/gtdynamics/mechanics/tests/testWrenchEquivalenceFactor.cpp @@ -36,7 +36,7 @@ using gtsam::Vector6, gtsam::Vector3, gtsam::Vector, gtsam::Pose3, gtsam::Rot3, namespace example { // Noise model. gtsam::noiseModel::Gaussian::shared_ptr cost_model = - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_6x6); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix6::Identity()); const DynamicsSymbol wrench_j_key = WrenchKey(1, 1, 777), wrench_k_key = WrenchKey(2, 1, 777), qKey = JointAngleKey(1, 777); diff --git a/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp b/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp index 58d96c975..42c330849 100644 --- a/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp +++ b/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp @@ -32,7 +32,7 @@ using gtsam::assert_equal; namespace example { // noise model gtsam::noiseModel::Gaussian::shared_ptr cost_model = - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_3x3); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix3::Identity()); const DynamicsSymbol wrench_key = WrenchKey(2, 1, 777); gtsam::Pose3 kMj; // doesn't matter auto [joint, links] = make_joint(kMj, gtsam::Vector6::Zero()); diff --git a/gtdynamics/scenarios/IEQuadrupedUtilsCosts.cpp b/gtdynamics/scenarios/IEQuadrupedUtilsCosts.cpp index 231f5115a..c0417a7c7 100644 --- a/gtdynamics/scenarios/IEQuadrupedUtilsCosts.cpp +++ b/gtdynamics/scenarios/IEQuadrupedUtilsCosts.cpp @@ -33,7 +33,7 @@ Vector3 IEVision60Robot::GetContactForce(const Pose3 &pose, 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; + J_fl_wrench << gtsam::Matrix3::Zero(), gtsam::Matrix3::Identity(); Matrix36 J_rot_pose; Rot3 rot = pose.rotation(J_rot_pose); @@ -769,7 +769,7 @@ NoiseModelFactor::shared_ptr IEVision60Robot::statePointVelCostFactor( Pose3 lTc(Rot3::Identity(), point_l); Pose3 cTl = lTc.inverse(); Matrix36 H_vel_c; - H_vel_c << Z_3x3, I_3x3; + H_vel_c << gtsam::Matrix3::Zero(), gtsam::Matrix3::Identity(); H_vel_c = H_vel_c * cTl.AdjointMap(); const std::function f = [H_vel_c](const Vector6 &A) { return H_vel_c * A; }; diff --git a/gtdynamics/statics/Statics.cpp b/gtdynamics/statics/Statics.cpp index 8c5cd1da1..afcb30723 100644 --- a/gtdynamics/statics/Statics.cpp +++ b/gtdynamics/statics/Statics.cpp @@ -49,7 +49,7 @@ Vector6 ResultantWrench(const std::vector &wrenches, sum += wrenches[i]; } if (H) { - std::fill(H->begin(), H->begin() + n, gtsam::I_6x6); + std::fill(H->begin(), H->begin() + n, gtsam::Matrix6::Identity()); } return sum; } @@ -72,7 +72,7 @@ Vector6 ResultantWrench(const std::vector &wrenches, double mass, return external_wrench + gravity_wrench; } else { if (H) { - H->back() = gtsam::Z_6x6; + H->back() = gtsam::Matrix6::Zero(); } return external_wrench; } diff --git a/gtdynamics/statics/tests/testStaticWrenchFactor.cpp b/gtdynamics/statics/tests/testStaticWrenchFactor.cpp index 0855020e8..32cca1796 100644 --- a/gtdynamics/statics/tests/testStaticWrenchFactor.cpp +++ b/gtdynamics/statics/tests/testStaticWrenchFactor.cpp @@ -38,7 +38,7 @@ const double mass = robot.links()[0]->mass(); const Vector3 gravity(0, -9.8, 0); noiseModel::Gaussian::shared_ptr cost_model = - noiseModel::Gaussian::Covariance(I_6x6); + noiseModel::Gaussian::Covariance(gtsam::Matrix6::Identity()); } // namespace example diff --git a/gtdynamics/universal_robot/Joint.cpp b/gtdynamics/universal_robot/Joint.cpp index 1a58ee317..0e7dd9cc3 100644 --- a/gtdynamics/universal_robot/Joint.cpp +++ b/gtdynamics/universal_robot/Joint.cpp @@ -174,7 +174,7 @@ gtsam::GaussianFactorGraph Joint::linearFDPriors( size_t t, const gtsam::Values &known_values) const { gtsam::GaussianFactorGraph priors; gtsam::Vector1 rhs(Torque(known_values, id(), t)); - priors.add(TorqueKey(id(), t), gtsam::I_1x1, rhs, + priors.add(TorqueKey(id(), t), gtsam::Matrix1::Identity(), rhs, gtsam::noiseModel::Constrained::All(1)); return priors; } @@ -196,7 +196,7 @@ gtsam::GaussianFactorGraph Joint::linearAFactors( // twist acceleration factor // A_i2 - Ad(T_21) * A_i1 - S_i2_j * a_j = ad(V_i2) * S_i2_j * v_j Vector6 rhs_tw = Pose3::adjointMap(V_i2) * S_i2_j * v_j; - graph.add(TwistAccelKey(child()->id(), t), gtsam::I_6x6, + graph.add(TwistAccelKey(child()->id(), t), gtsam::Matrix6::Identity(), TwistAccelKey(parent()->id(), t), -T_i2i1.AdjointMap(), JointAccelKey(id(), t), -S_i2_j, rhs_tw, gtsam::noiseModel::Constrained::All(6)); @@ -219,13 +219,13 @@ gtsam::GaussianFactorGraph Joint::linearDynamicsFactors( // S_i_j^T * F_i_j - tau = 0 gtsam::Vector1 rhs_torque = gtsam::Vector1::Zero(); graph.add(WrenchKey(child()->id(), id(), t), S_i2_j.transpose(), - TorqueKey(id(), t), -gtsam::I_1x1, rhs_torque, + TorqueKey(id(), t), -gtsam::Matrix1::Identity(), rhs_torque, gtsam::noiseModel::Constrained::All(1)); // wrench equivalence factor // F_i1_j + Ad(T_i2i1)^T F_i2_j = 0 Vector6 rhs_weq = Vector6::Zero(); - graph.add(WrenchKey(parent()->id(), id(), t), gtsam::I_6x6, + graph.add(WrenchKey(parent()->id(), id(), t), gtsam::Matrix6::Identity(), WrenchKey(child()->id(), id(), t), T_i2i1.AdjointMap().transpose(), rhs_weq, gtsam::noiseModel::Constrained::All(6)); diff --git a/gtdynamics/universal_robot/Link.cpp b/gtdynamics/universal_robot/Link.cpp index 662f8f555..77b9b449e 100644 --- a/gtdynamics/universal_robot/Link.cpp +++ b/gtdynamics/universal_robot/Link.cpp @@ -75,7 +75,7 @@ gtsam::Vector6_ Link::wrenchConstraint( gtsam::Matrix6 Link::inertiaMatrix() const { std::vector gmm; gmm.push_back(inertia_); - gmm.push_back(gtsam::I_3x3 * mass_); + gmm.push_back(gtsam::Matrix3::Identity() * mass_); return gtsam::diag(gmm); } diff --git a/tests/testCollocationFactor.cpp b/tests/testCollocationFactor.cpp index cb2ac80f3..3a3a500ec 100644 --- a/tests/testCollocationFactor.cpp +++ b/tests/testCollocationFactor.cpp @@ -30,7 +30,7 @@ using gtsam::assert_equal, gtsam::Pose3, gtsam::Vector6, gtsam::Rot3; namespace example { // noise model -auto cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::I_6x6); +auto cost_model = gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix6::Identity()); gtsam::Symbol pose_p_key('p', 1), pose_c_key('p', 2), twist_p_key('v', 1), twist_c_key('v', 2), accel_p_key('a', 1), accel_c_key('a', 2), dt_key('t', 0); diff --git a/tests/testConvexIQPSolver.cpp b/tests/testConvexIQPSolver.cpp index b21b7d390..75b2438c9 100644 --- a/tests/testConvexIQPSolver.cpp +++ b/tests/testConvexIQPSolver.cpp @@ -32,12 +32,12 @@ TEST(SolveConvexIQP, example_2D) { GaussianFactorGraph graph; auto model = noiseModel::Unit::Create(1); - graph.add(JacobianFactor(x_key, I_1x1, Vector::Zero(1), model)); - graph.add(JacobianFactor(y_key, I_1x1, Vector1(1), model)); + graph.add(JacobianFactor(x_key, gtsam::Matrix1::Identity(), Vector::Zero(1), model)); + graph.add(JacobianFactor(y_key, gtsam::Matrix1::Identity(), Vector1(1), model)); LinearInequalityConstraints constraints; - auto factor1 = std::make_shared(x_key, I_1x1, y_key, -I_1x1, Vector::Zero(1), model); - auto factor2 = std::make_shared(x_key, I_1x1, y_key, I_1x1, Vector::Zero(1), model); + auto factor1 = std::make_shared(x_key, gtsam::Matrix1::Identity(), y_key, -gtsam::Matrix1::Identity(), Vector::Zero(1), model); + auto factor2 = std::make_shared(x_key, gtsam::Matrix1::Identity(), y_key, gtsam::Matrix1::Identity(), Vector::Zero(1), model); constraints.emplace_shared(factor1); constraints.emplace_shared(factor2); diff --git a/tests/testMinTorqueFactor.cpp b/tests/testMinTorqueFactor.cpp index 1a3e57cd2..550e9ebd1 100644 --- a/tests/testMinTorqueFactor.cpp +++ b/tests/testMinTorqueFactor.cpp @@ -31,7 +31,7 @@ namespace example { // noise model gtsam::noiseModel::Gaussian::shared_ptr cost_model = - gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1); + gtsam::noiseModel::Gaussian::Covariance(gtsam::Matrix1::Identity()); gtsam::Key torque_key = gtsam::Symbol('t', 1); } // namespace example diff --git a/tests/testMultiJacobian.cpp b/tests/testMultiJacobian.cpp index 56a28f264..03c3c8e43 100644 --- a/tests/testMultiJacobian.cpp +++ b/tests/testMultiJacobian.cpp @@ -55,7 +55,7 @@ TEST(MultiJacobian, Add_Mult) { jac12_sum += jac2; EXPECT(expected_sum.equals(jac12_sum)); - Matrix m = I_2x2 * 2; + Matrix m = gtsam::Matrix2::Identity() * 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()}); @@ -80,26 +80,26 @@ TEST(MultiJacobians, Mult) { 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_x1.insert({x1, gtsam::Matrix2::Identity()}); + jac1_x2.insert({x2, gtsam::Matrix2::Identity()}); + jac1_x3.insert({x3, gtsam::Matrix2::Identity()}); 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}); + jac2_x1.insert({x1, gtsam::Matrix2::Identity()}); + jac2_x2.insert({x2, gtsam::Matrix2::Identity()}); + jac2_x3.insert({x1, 2*gtsam::Matrix2::Identity()}); + jac2_x3.insert({x2, -1*gtsam::Matrix2::Identity()}); 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_x1.insert({x1, gtsam::Matrix2::Identity()}); + expected_jac_x2.insert({x2, gtsam::Matrix2::Identity()}); + expected_jac_x3.insert({x1, 2*gtsam::Matrix2::Identity()}); + expected_jac_x3.insert({x2, -1*gtsam::Matrix2::Identity()}); 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()}); @@ -121,11 +121,11 @@ TEST(MultiJacobian, ComputeBayesNetJacobian) { 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)); + graph.add(JacobianFactor(x1, gtsam::Matrix1::Identity(), x2, gtsam::Matrix1::Identity(), x3, -gtsam::Matrix1::Identity(), 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)); + graph.add(JacobianFactor(x3, H_3, x4, H_4, x5, -gtsam::Matrix2::Identity(), Vector2(0, 0), model2)); Ordering ordering; ordering.push_back(x5); @@ -143,8 +143,8 @@ TEST(MultiJacobian, ComputeBayesNetJacobian) { 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_x3.addJacobian(x1, gtsam::Matrix1::Identity()); + jacobian_x3.addJacobian(x2, gtsam::Matrix1::Identity()); jacobian_x5.addJacobian(x1, H_3); jacobian_x5.addJacobian(x2, H_3); jacobian_x5.addJacobian(x4, H_4); diff --git a/tests/testPreintegratedContactFactors.cpp b/tests/testPreintegratedContactFactors.cpp index ad6fb67d1..dd30ae591 100644 --- a/tests/testPreintegratedContactFactors.cpp +++ b/tests/testPreintegratedContactFactors.cpp @@ -36,8 +36,8 @@ using gtsam::assert_equal; TEST(PreintegratedPointContactMeasurements, Constructor) { PreintegratedPointContactMeasurements(); PreintegratedPointContactMeasurements pcm( - Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), 0.01, I_3x3); - EXPECT(assert_equal(I_3x3 * 1e-4, pcm.preintMeasCov())); + Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), 0.01, gtsam::Matrix3::Identity()); + EXPECT(assert_equal(gtsam::Matrix3::Identity() * 1e-4, pcm.preintMeasCov())); } /* ************************************************************************* */ @@ -45,17 +45,17 @@ TEST(PreintegratedPointContactMeasurements, Constructor) { TEST(PreintegratedPointContactMeasurements, IntegrateMeasurement) { double dt = 0.01; PreintegratedPointContactMeasurements pcm( - Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), dt, I_3x3); + Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), dt, gtsam::Matrix3::Identity()); Rot3 deltaRik = Rot3::Ry(M_PI_4); Pose3 contact_k(Rot3(), Vector3(0, 0, 1)); // Regression pcm.integrateMeasurement(deltaRik, contact_k, dt); - EXPECT(assert_equal(I_3x3 * 2e-4, pcm.preintMeasCov())); + EXPECT(assert_equal(gtsam::Matrix3::Identity() * 2e-4, pcm.preintMeasCov())); // Regression pcm.integrateMeasurement(deltaRik, Pose3(Rot3(), Vector3(0, 0, 1.01)), dt); - EXPECT(assert_equal(I_3x3 * 3e-4, pcm.preintMeasCov())); + EXPECT(assert_equal(gtsam::Matrix3::Identity() * 3e-4, pcm.preintMeasCov())); } /* ************************************************************************* */ @@ -63,7 +63,7 @@ TEST(PreintegratedPointContactMeasurements, IntegrateMeasurement) { TEST(PreintegratedPointContactFactor, Constructor) { double dt = 0.01; PreintegratedPointContactMeasurements pcm( - Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), dt, I_3x3); + Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), dt, gtsam::Matrix3::Identity()); size_t base_id = 0, contact_id = 1; PreintegratedPointContactFactor ppcf( PoseKey(base_id, 0), PoseKey(contact_id, 0), PoseKey(base_id, 1), @@ -83,7 +83,7 @@ TEST(PreintegratedPointContactFactor, Error) { wTb_j = Pose3(Rot3(), Point3(0.1, 0, 0)), wTc_j = Pose3(Rot3::Ry(M_PI_4), Point3(0, 0, 1)); - PreintegratedPointContactMeasurements pcm(wTb_i, wTc_i, 0.01, I_3x3); + PreintegratedPointContactMeasurements pcm(wTb_i, wTc_i, 0.01, gtsam::Matrix3::Identity()); PreintegratedPointContactFactor factor( PoseKey(base_id, t0), PoseKey(contact_id, t0), PoseKey(base_id, t1), PoseKey(contact_id, t1), pcm); @@ -104,7 +104,7 @@ TEST(PreintegratedPointContactFactor, Jacobians) { wTb_j = Pose3(Rot3::Ry(M_PI_4), Point3(0.1, 0, 0)), wTc_j = Pose3(Rot3::Ry(M_PI_4), Point3(0, 0, 1)); - PreintegratedPointContactMeasurements pcm(wTb_i, wTc_i, 0.01, I_3x3); + PreintegratedPointContactMeasurements pcm(wTb_i, wTc_i, 0.01, gtsam::Matrix3::Identity()); PreintegratedPointContactFactor factor( PoseKey(base_id, t0), PoseKey(contact_id, t0), PoseKey(base_id, t1), PoseKey(contact_id, t1), pcm); @@ -124,9 +124,9 @@ TEST(PreintegratedPointContactFactor, Jacobians) { TEST(PreintegratedRigidContactMeasurements, Constructor) { PreintegratedRigidContactMeasurements(); - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::Matrix3::Identity(), gtsam::Matrix3::Identity()); // Covariance is 0 initially - EXPECT(assert_equal(Z_6x6, pcm.preintMeasCov())); + EXPECT(assert_equal(gtsam::Matrix6::Zero(), pcm.preintMeasCov())); } /* ************************************************************************* */ @@ -134,11 +134,11 @@ TEST(PreintegratedRigidContactMeasurements, Constructor) { // Measurements object works as expected. TEST(PreintegratedRigidContactMeasurements, IntegrateMeasurementConstant) { double deltaT = 0.01; - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::Matrix3::Identity(), gtsam::Matrix3::Identity()); // Regression pcm.integrateMeasurement(deltaT); - EXPECT(assert_equal(I_6x6 * deltaT, pcm.preintMeasCov())); + EXPECT(assert_equal(gtsam::Matrix6::Identity() * deltaT, pcm.preintMeasCov())); } /* ************************************************************************* */ @@ -146,17 +146,17 @@ TEST(PreintegratedRigidContactMeasurements, IntegrateMeasurementConstant) { // Contact Measurements object works as expected. TEST(PreintegratedRigidContactMeasurements, IntegrateMeasurementVarying) { double dt = 0.01; - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::Matrix3::Identity(), gtsam::Matrix3::Identity()); Matrix6 expected; // Regression - pcm.integrateMeasurement(I_3x3 * 0.05, I_3x3 * 0.01, dt); - expected << I_3x3 * 0.05, Z_3x3, Z_3x3, I_3x3 * 0.01; + pcm.integrateMeasurement(gtsam::Matrix3::Identity() * 0.05, gtsam::Matrix3::Identity() * 0.01, dt); + expected << gtsam::Matrix3::Identity() * 0.05, gtsam::Matrix3::Zero(), gtsam::Matrix3::Zero(), gtsam::Matrix3::Identity() * 0.01; EXPECT(assert_equal(expected * dt * dt, pcm.preintMeasCov())); // Regression 2 - pcm.integrateMeasurement(I_3x3 * 0.06, I_3x3 * 0.02, dt); - expected << I_3x3 * (0.05 + 0.06), Z_3x3, Z_3x3, I_3x3 * (0.01 + 0.02); + pcm.integrateMeasurement(gtsam::Matrix3::Identity() * 0.06, gtsam::Matrix3::Identity() * 0.02, dt); + expected << gtsam::Matrix3::Identity() * (0.05 + 0.06), gtsam::Matrix3::Zero(), gtsam::Matrix3::Zero(), gtsam::Matrix3::Identity() * (0.01 + 0.02); EXPECT(assert_equal(expected * dt * dt, pcm.preintMeasCov())); } @@ -164,7 +164,7 @@ TEST(PreintegratedRigidContactMeasurements, IntegrateMeasurementVarying) { // Test constructor for Preintegrated Rigid Contact Factor. TEST(PreintegratedRigidContactFactor, Constructor) { size_t contact_id = 0; - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::Matrix3::Identity(), gtsam::Matrix3::Identity()); PreintegratedRigidContactFactor ppcf(PoseKey(contact_id, 0), PoseKey(contact_id, 1), pcm); } @@ -179,7 +179,7 @@ TEST(PreintegratedRigidContactFactor, Error) { Pose3 wTc_i = Pose3(Rot3(), Point3(0, 0, 1)), wTc_j = Pose3(Rot3::Ry(M_PI_4), Point3(0, 0, 1)); - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::Matrix3::Identity(), gtsam::Matrix3::Identity()); // Integrate with constant noise pcm.integrateMeasurement(0.1); @@ -198,7 +198,7 @@ TEST(PreintegratedRigidContactFactor, Jacobians) { size_t contact_id = 0; size_t t0 = 0, t1 = 1; - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::Matrix3::Identity(), gtsam::Matrix3::Identity()); // Integrate with constant noise pcm.integrateMeasurement(1); diff --git a/tests/testTspaceBasis.cpp b/tests/testTspaceBasis.cpp index 170d99cdd..926abdf82 100644 --- a/tests/testTspaceBasis.cpp +++ b/tests/testTspaceBasis.cpp @@ -174,10 +174,10 @@ TEST(TspaceBasis, linear_system) { 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; + Matrix expected_H_x1 = gtsam::Matrix1::Identity(); + Matrix expected_H_x2 = gtsam::Matrix1::Identity()*0; + Matrix expected_H_x3 = gtsam::Matrix1::Identity() * -1; + Matrix expected_H_x4 = gtsam::Matrix1::Identity(); 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))); diff --git a/tests/testUtils.cpp b/tests/testUtils.cpp index e2cf0d26d..4f98dbf4e 100644 --- a/tests/testUtils.cpp +++ b/tests/testUtils.cpp @@ -48,7 +48,7 @@ TEST(utils, calcPhi) { // Test calculate covariance matrix for GP TEST(utils, calcQ) { double t = 0.1; - gtsam::Matrix Qc = gtsam::I_6x6; + gtsam::Matrix Qc = gtsam::Matrix6::Identity(); gtsam::Matrix expected_Q = (gtsam::Matrix(3 * Qc.rows(), 3 * Qc.rows()) << 1.0 / 20 * pow(t, 5.0) * Qc,