borglab · dellaert · Jul 2, 2026 · Jun 11, 2026 · Jun 11, 2026 · Jun 11, 2026
diff --git a/examples/example_a1_walking/main.cpp b/examples/example_a1_walking/main.cpp
@@ -11,6 +11,8 @@
  * @author: Dan Barladeanu
  */
 
+#include <gtsam/base/VectorConstants.h>
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/dynamics/ChainDynamicsGraph.h>
 #include <gtdynamics/factors/ObjectiveFactors.h>
 #include <gtdynamics/universal_robot/sdf.h>
@@ -218,7 +220,7 @@ int oldGraph() {
   for (auto&& link : robot.links()) {
     if (link->name().find("trunk") == std::string::npos) {
       link->setMass(0.0);
-      link->setInertia(gtsam::Matrix3::Zero());
+      link->setInertia(gtsam::Z_3x3);
     }
   }
 
@@ -372,7 +374,7 @@ int newGraph() {
   for (auto&& link : robot.links()) {
     if (link->name().find("trunk") == std::string::npos) {
       link->setMass(0.0);
-      link->setInertia(gtsam::Matrix3::Zero());
+      link->setInertia(gtsam::Z_3x3);
     }
   }
 

diff --git a/examples/example_a1_walking/main_combined.cpp b/examples/example_a1_walking/main_combined.cpp
@@ -11,6 +11,8 @@
  * @author: Dan Barladeanu
  */
 
+#include <gtsam/base/VectorConstants.h>
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/dynamics/ChainDynamicsGraph.h>
 #include <gtdynamics/factors/ObjectiveFactors.h>
 #include <gtdynamics/universal_robot/sdf.h>
@@ -77,7 +79,7 @@ int CombinedRun(bool add_mass_to_body) {
   for (auto&& link : robot_massless.links()) {
     if (link->name().find("trunk") == std::string::npos) {
       link->setMass(0.0);
-      link->setInertia(gtsam::Matrix3::Zero());
+      link->setInertia(gtsam::Z_3x3);
     }
   }
 

diff --git a/examples/example_constraint_manifold/QuadrupedUtils.cpp b/examples/example_constraint_manifold/QuadrupedUtils.cpp
@@ -11,6 +11,7 @@
  * @author: Yetong Zhang
  */
 
+#include <gtsam/base/MatrixConstants.h>
 #include "QuadrupedUtils.h"
 
 #include <gtdynamics/dynamics/DynamicsGraph.h>
@@ -40,7 +41,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::Z_3x3, gtsam::I_3x3;
 
     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
@@ -18,6 +18,7 @@
 #include <gtdynamics/kinematics/Kinematics.h>
 #include <gtdynamics/universal_robot/RobotModels.h>
 #include <gtdynamics/utils/Slice.h>
+#include <gtsam/base/MatrixConstants.h>
 #include <gtsam/constrained/NonlinearEqualityConstraint.h>
 #include <gtsam/slam/BetweenFactor.h>
 

diff --git a/examples/example_constraint_manifold/main_connected_poses.cpp b/examples/example_constraint_manifold/main_connected_poses.cpp
@@ -12,6 +12,7 @@
  * @author Yetong Zhang
  */
 
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/cmopt/NonlinearMOptimizer.h>
 #include <gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h>
 #include <gtsam/constrained/NonlinearEqualityConstraint.h>
@@ -107,14 +108,14 @@ double EvaluatePoseError(const Values& gt, const Values& result) {
       Pose2 gt_pose = gt.at<Pose2>(A(k));
       Pose2 est_pose = result.at<Pose2>(A(k));
       Pose2 rel_pose = est_pose.inverse().compose(gt_pose);
-      Matrix3 diff = rel_pose.matrix() - I_3x3;
+      Matrix3 diff = rel_pose.matrix() - gtsam::I_3x3;
       error1 += pow(diff.norm(), 2);
     }
     {
       Pose2 gt_pose = gt.at<Pose2>(B(k));
       Pose2 est_pose = result.at<Pose2>(B(k));
       Pose2 rel_pose = est_pose.inverse().compose(gt_pose);
-      Matrix3 diff = rel_pose.matrix() - I_3x3;
+      Matrix3 diff = rel_pose.matrix() - gtsam::I_3x3;
       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
@@ -11,6 +11,7 @@
  * @author Yetong Zhang
  */
 
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/config.h>
 #include <gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h>
 #include <gtsam/constrained/NonlinearEqualityConstraint.h>
@@ -159,7 +160,7 @@ double EvaluatePoseError(const Values& gt, const Values& result) {
       Pose2 gt_pose = gt.at<Pose2>(A(k));
       Pose2 est_pose = result.at<Pose2>(A(k));
       Pose2 rel_pose = est_pose.inverse().compose(gt_pose);
-      Matrix3 diff = rel_pose.matrix() - I_3x3;
+      Matrix3 diff = rel_pose.matrix() - gtsam::I_3x3;
       // std::cout << diff << "\n";
       // std::cout << diff.norm() << "\n";
       error1 += pow(diff.norm(), 2);
@@ -168,7 +169,7 @@ double EvaluatePoseError(const Values& gt, const Values& result) {
       Pose2 gt_pose = gt.at<Pose2>(B(k));
       Pose2 est_pose = result.at<Pose2>(B(k));
       Pose2 rel_pose = est_pose.inverse().compose(gt_pose);
-      Matrix3 diff = rel_pose.matrix() - I_3x3;
+      Matrix3 diff = rel_pose.matrix() - gtsam::I_3x3;
       error2 += pow(diff.norm(), 2);
     }
   }

diff --git a/examples/example_full_kinodynamic_balancing/main.cpp b/examples/example_full_kinodynamic_balancing/main.cpp
@@ -11,6 +11,8 @@
  * @author Alejandro Escontrela
  */
 
+#include <gtsam/base/VectorConstants.h>
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/dynamics/DynamicsGraph.h>
 #include <gtdynamics/dynamics/OptimizerSetting.h>
 #include <gtdynamics/factors/MinTorqueFactor.h>

diff --git a/examples/example_full_kinodynamic_walking/main_rotate.cpp b/examples/example_full_kinodynamic_walking/main_rotate.cpp
@@ -11,6 +11,7 @@
  * @Author: Alejandro Escontrela
  */
 
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/dynamics/DynamicsGraph.h>
 #include <gtdynamics/dynamics/OptimizerSetting.h>
 #include <gtdynamics/factors/MinTorqueFactor.h>

diff --git a/examples/example_full_kinodynamic_walking/main_walk_forward.cpp b/examples/example_full_kinodynamic_walking/main_walk_forward.cpp
@@ -11,6 +11,7 @@
  * @Author: Alejandro Escontrela
  */
 
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/dynamics/DynamicsGraph.h>
 #include <gtdynamics/dynamics/OptimizerSetting.h>
 #include <gtdynamics/factors/MinTorqueFactor.h>

diff --git a/examples/example_spider_walking/main.cpp b/examples/example_spider_walking/main.cpp
@@ -12,6 +12,7 @@
  * Gandhi, Varun Agrawal
  */
 
+#include <gtsam/base/VectorConstants.h>
 #include <gtdynamics/factors/ObjectiveFactors.h>
 #include <gtdynamics/universal_robot/sdf.h>
 #include <gtdynamics/utils/Trajectory.h>

diff --git a/examples/example_spider_walking/main_rotate.cpp b/examples/example_spider_walking/main_rotate.cpp
@@ -14,6 +14,7 @@
  * @Author: Tarushree Gandhi
  */
 
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/dynamics/DynamicsGraph.h>
 #include <gtdynamics/dynamics/OptimizerSetting.h>
 #include <gtdynamics/factors/MinTorqueFactor.h>

diff --git a/examples/scripts/alejandro_yetong_01_id_four_bar.cpp b/examples/scripts/alejandro_yetong_01_id_four_bar.cpp
@@ -11,6 +11,8 @@
  * @author Alejandro Escontrela and Yetong Zhang
  */
 
+#include <gtsam/base/VectorConstants.h>
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/dynamics/DynamicsGraph.h>
 #include <gtdynamics/factors/MinTorqueFactor.h>
 #include <gtdynamics/universal_robot/RobotModels.h>

diff --git a/gtdynamics.i b/gtdynamics.i
@@ -374,6 +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(gtsam::Key joint_key, double sigma);
+  void setJointLimit(gtsam::Key joint_key, double lower, double upper);
 };
 
 class Kinematics {

diff --git a/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp b/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp
@@ -6,6 +6,7 @@
  * @author Gerry Chen
  */
 
+#include <gtsam/base/MatrixConstants.h>
 #include "CableAccelerationFactor.h"
 
 #include <gtsam/base/Matrix.h>
@@ -38,8 +39,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::Z_3x3;
+  if (H_VAx) wAb_H_VAx << gtsam::Z_3x3, 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
@@ -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
@@ -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
@@ -6,6 +6,7 @@
  * @author Gerry Chen
  */
 
+#include <gtsam/base/MatrixConstants.h>
 #include "CableTensionFactor.h"
 
 #include <gtdynamics/utils/DynamicsSymbol.h>
@@ -41,7 +42,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::I_3x3;
   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 +51,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::I_3x3;
   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 +78,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::Z_3x3, -t * gtsam::I_3x3).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
@@ -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
@@ -6,6 +6,7 @@
  * @author Gerry Chen
  */
 
+#include <gtsam/base/MatrixConstants.h>
 #include "CableVelocityFactor.h"
 
 #include <gtsam/base/Matrix.h>
@@ -41,7 +42,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::I_3x3;
   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
@@ -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
@@ -1,3 +1,4 @@
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/constrained_optimizer/IPOptOptimizer.h>
 #include <ifopt/ipopt_solver.h>
 #include <ifopt/problem.h>
@@ -16,8 +17,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::I_3x3, gtsam::Z_3x3;
+    H_trans_vec << gtsam::Z_3x3, gtsam::I_3x3;
     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
@@ -11,6 +11,7 @@
  * @author Dan Barladeanu, Frank Dellaert.
  */
 
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/dynamics/Chain.h>
 
 namespace gtdynamics {

diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.cpp b/gtdynamics/dynamics/ChainDynamicsGraph.cpp
@@ -11,6 +11,7 @@
  * @author Dan Barladeanu
  */
 
+#include <gtsam/base/VectorConstants.h>
 #include "gtdynamics/dynamics/ChainDynamicsGraph.h"
 
 #include <gtdynamics/kinematics/Kinematics.h>
@@ -192,7 +193,7 @@ gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors(
         chain_joints_[i], base_key, end_effector_key, t);
 
     gtsam::ExpressionEqualityConstraint<gtsam::Vector6> chain_constraint(
-        chain_expression, gtsam::Vector6::Zero(), tolerance);
+        chain_expression, gtsam::Z_6x1, tolerance);
 
     graph.add(chain_constraint.penaltyFactor(1.0));
   }

diff --git a/gtdynamics/dynamics/DynamicsGraph.cpp b/gtdynamics/dynamics/DynamicsGraph.cpp
@@ -11,6 +11,8 @@
  * @author Yetong Zhang, Alejandro Escontrela
  */
 
+#include <gtsam/base/VectorConstants.h>
+#include <gtsam/base/MatrixConstants.h>
 #include <gtdynamics/dynamics/DynamicsGraph.h>
 #include <gtdynamics/kinematics/Kinematics.h>
 #include <gtdynamics/universal_robot/Joint.h>
@@ -33,16 +35,13 @@
 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;
 using gtsam::PriorFactor;
 using gtsam::Values;
 using gtsam::Vector;
 using gtsam::Vector6;
-using gtsam::Z_6x1;
 
 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), gtsam::I_6x6, gtsam::Z_6x1, 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
@@ -87,10 +86,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::I_6x6, 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::I_6x6, WrenchKey(i, connected_joints[1]->id(), k), -gtsam::I_6x6,
                   rhs, all_constrained);
       }
     }
@@ -121,7 +120,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::I_1x1, rhs, all_constrained);
   }
   return graph;
 }

diff --git a/gtdynamics/dynamics/DynamicsSlice.cpp b/gtdynamics/dynamics/DynamicsSlice.cpp
@@ -10,6 +10,7 @@
  * @brief Dynamics factors for a single time slice.
  */
 
+#include <gtsam/base/VectorConstants.h>
 #include <gtdynamics/dynamics/Dynamics.h>
 #include <gtdynamics/dynamics/ContactDynamicsFrictionConeFactor.h>
 #include <gtdynamics/dynamics/ContactDynamicsMomentFactor.h>