Finishing the refactor...

gtdynamics.i

@@ -361,34 +361,92 @@ class Kinematics {
 };
 
 /********************** dynamics graph **********************/
-#include <gtdynamics/dynamics/OptimizerSetting.h>
-class OptimizerSetting : gtdynamics::KinematicsParameters {
-  OptimizerSetting();
-  OptimizerSetting(double sigma_dynamics, double sigma_linear = 0.001,
-                   double sigma_contact = 0.001, double sigma_joint = 0.001,
-                   double sigma_collocation = 0.001, double sigma_time = 0.001);
+#include <gtdynamics/mechanics/MechanicsParameters.h>
+class MechanicsParameters {
+  std::optional<gtsam::Vector3> gravity;
+  std::optional<gtsam::Vector3> planar_axis;
+  gtsam::SharedNoiseModel f_cost_model;
+  gtsam::SharedNoiseModel t_cost_model;
+  gtsam::SharedNoiseModel planar_cost_model;
+
+  MechanicsParameters(double sigma_dynamics = 1e-5);
+  MechanicsParameters(double sigma_dynamics,
+                      const std::optional<gtsam::Vector3> &gravity,
+                      const std::optional<gtsam::Vector3> &planar_axis);
+};
+
+#include <gtdynamics/dynamics/DynamicsParameters.h>
+class DynamicsParameters : gtdynamics::MechanicsParameters {
   gtsam::noiseModel::SharedNoiseModel ba_cost_model;             // acceleration of fixed link
   gtsam::noiseModel::SharedNoiseModel a_cost_model;              // acceleration factor
-  gtsam::noiseModel::SharedNoiseModel linear_a_cost_model;       // linear acceleration factor
-  gtsam::noiseModel::SharedNoiseModel f_cost_model;              // wrench equivalence factor
-  gtsam::noiseModel::SharedNoiseModel linear_f_cost_model;       // linear wrench equivalence factor
   gtsam::noiseModel::SharedNoiseModel fa_cost_model;             // wrench factor
-  gtsam::noiseModel::SharedNoiseModel t_cost_model;              // torque factor
-  gtsam::noiseModel::SharedNoiseModel linear_t_cost_model;       // linear torque factor
   gtsam::noiseModel::SharedNoiseModel cfriction_cost_model;      // contact friction cone
   gtsam::noiseModel::SharedNoiseModel ca_cost_model;             // contact acceleration
   gtsam::noiseModel::SharedNoiseModel cm_cost_model;             // contact moment
-  gtsam::noiseModel::SharedNoiseModel planar_cost_model;         // planar factor
-  gtsam::noiseModel::SharedNoiseModel linear_planar_cost_model;  // linear planar factor
-  gtsam::noiseModel::SharedNoiseModel prior_qv_cost_model;       // joint velocity prior factor
-  gtsam::noiseModel::SharedNoiseModel prior_qa_cost_model;       // joint acceleration prior factor
-  gtsam::noiseModel::SharedNoiseModel prior_t_cost_model;        // joint torque prior factor
-  gtsam::noiseModel::SharedNoiseModel q_col_cost_model;          // joint collocation factor
-  gtsam::noiseModel::SharedNoiseModel v_col_cost_model;          // joint vel collocation factor
-  gtsam::noiseModel::SharedNoiseModel pose_col_cost_model;       // pose collocation factor
-  gtsam::noiseModel::SharedNoiseModel twist_col_cost_model;      // twist collocation factor
-  gtsam::noiseModel::SharedNoiseModel time_cost_model;           // time prior
-  gtsam::noiseModel::SharedNoiseModel jl_cost_model;             // joint limit factor
+
+  DynamicsParameters();
+  DynamicsParameters(double sigma_dynamics, double sigma_linear = 0.001,
+                     double sigma_contact = 0.001, double sigma_joint = 0.001,
+                     double sigma_collocation = 0.001,
+                     double sigma_time = 0.001);
+};
+
+#include <gtdynamics/dynamics/OptimizerSetting.h>
+class OptimizerSetting : gtdynamics::KinematicsParameters {
+  std::optional<gtsam::Vector3> gravity;
+  std::optional<gtsam::Vector3> planar_axis;
+  gtsam::SharedNoiseModel f_cost_model;
+  gtsam::SharedNoiseModel t_cost_model;
+  gtsam::SharedNoiseModel planar_cost_model;
+  gtsam::noiseModel::SharedNoiseModel ba_cost_model;
+  gtsam::noiseModel::SharedNoiseModel a_cost_model;
+  gtsam::noiseModel::SharedNoiseModel linear_a_cost_model;
+  gtsam::noiseModel::SharedNoiseModel linear_f_cost_model;
+  gtsam::noiseModel::SharedNoiseModel fa_cost_model;
+  gtsam::noiseModel::SharedNoiseModel linear_t_cost_model;
+  gtsam::noiseModel::SharedNoiseModel cfriction_cost_model;
+  gtsam::noiseModel::SharedNoiseModel ca_cost_model;
+  gtsam::noiseModel::SharedNoiseModel cm_cost_model;
+  gtsam::noiseModel::SharedNoiseModel linear_planar_cost_model;
+  gtsam::noiseModel::SharedNoiseModel prior_qv_cost_model;
+  gtsam::noiseModel::SharedNoiseModel prior_qa_cost_model;
+  gtsam::noiseModel::SharedNoiseModel prior_t_cost_model;
+  gtsam::noiseModel::SharedNoiseModel q_col_cost_model;
+  gtsam::noiseModel::SharedNoiseModel v_col_cost_model;
+  gtsam::noiseModel::SharedNoiseModel pose_col_cost_model;
+  gtsam::noiseModel::SharedNoiseModel twist_col_cost_model;
+  gtsam::noiseModel::SharedNoiseModel time_cost_model;
+  gtsam::noiseModel::SharedNoiseModel jl_cost_model;
+  gtsam::SharedNoiseModel p_cost_model;
+  gtsam::SharedNoiseModel g_cost_model;
+  gtsam::SharedNoiseModel prior_q_cost_model;
+  gtsam::SharedNoiseModel bp_cost_model;
+  gtsam::SharedNoiseModel cp_cost_model;
+  gtsam::SharedNoiseModel bv_cost_model;
+  gtsam::SharedNoiseModel v_cost_model;
+  gtsam::SharedNoiseModel cv_cost_model;
+  gtsam::LevenbergMarquardtParams lm_parameters;
+  double rel_thresh;
+  int max_iter;
+  double epsilon;
+  double obsSigma;
+  OptimizerSetting();
+  OptimizerSetting(double sigma_dynamics, double sigma_linear = 0.001,
+                   double sigma_contact = 0.001, double sigma_joint = 0.001,
+                   double sigma_collocation = 0.001, double sigma_time = 0.001);
+};
+
+#include <gtdynamics/dynamics/Dynamics.h>
+class Dynamics {
+  Dynamics(gtdynamics::DynamicsParameters parameters =
+               gtdynamics::DynamicsParameters());
+  gtsam::NonlinearFactorGraph aFactors(
+      const gtdynamics::Slice &slice, const gtdynamics::Robot &robot,
+      const std::optional<gtdynamics::PointOnLinks> &contact_points) const;
+  gtsam::NonlinearFactorGraph graph(
+      const gtdynamics::Slice &slice, const gtdynamics::Robot &robot,
+      const std::optional<gtdynamics::PointOnLinks> &contact_points,
+      const std::optional<double> &mu) const;
 };
 
 

gtdynamics/CMakeLists.txt

@@ -1,5 +1,5 @@
 # All subdirectories that contain source code relevant to this library.
-set(SOURCE_SUBDIRS universal_robot utils factors constraints optimizer constrained_optimizer kinematics statics dynamics cmopt cmcopt scenarios)
+set(SOURCE_SUBDIRS universal_robot utils factors constraints optimizer constrained_optimizer kinematics mechanics statics dynamics cmopt cmcopt scenarios)
 
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 

gtdynamics/dynamics/Dynamics.h

@@ -13,13 +13,24 @@
 
 #pragma once
 
+#include <gtdynamics/dynamics/DynamicsParameters.h>
+#include <gtdynamics/universal_robot/Robot.h>
+#include <gtdynamics/utils/Interval.h>
+#include <gtdynamics/utils/PointOnLink.h>
+#include <gtdynamics/utils/Slice.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/Vector.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
 namespace gtdynamics {
 
-/// calculate Coriolis term and jacobian w.r.t. joint coordinate twist
+/**
+ * Calculate Coriolis wrench term and Jacobian with respect to twist.
+ * @param inertia Spatial inertia matrix.
+ * @param twist Spatial twist.
+ * @param H_twist Optional Jacobian with respect to twist.
+ */
 gtsam::Vector6 Coriolis(const gtsam::Matrix6 &inertia,
                         const gtsam::Vector6 &twist,
                         gtsam::OptionalJacobian<6, 6> H_twist = {});
@@ -36,4 +47,47 @@ inline Eigen::Matrix<double, M, 1> MatVecMult(
   return constant_matrix * vector;
 }
 
+/**
+ * Dynamics factor builder for moving configurations.
+ *
+ * For the templated API below, `CONTEXT` is typically `Slice`, `Interval`,
+ * or `Phase`.
+ */
+class Dynamics {
+ protected:
+  const DynamicsParameters p_;
+
+ public:
+  /**
+   * Constructor.
+   * @param parameters Dynamics parameter bundle with mechanics + dynamic-only
+   * noise models and settings.
+   */
+  Dynamics(const DynamicsParameters& parameters = DynamicsParameters())
+      : p_(parameters) {}
+
+  /**
+   * Return acceleration-level factor graph.
+   * @param contact_points Optional contact points with zero-acceleration
+   * constraints.
+   */
+  template <class CONTEXT>
+  gtsam::NonlinearFactorGraph aFactors(
+      const CONTEXT& context, const Robot& robot,
+      const std::optional<PointOnLinks>& contact_points = {}) const;
+
+  /**
+   * Return dynamic-only wrench factors for a context.
+   * This excludes factor groups provided via the Statics slice interface.
+   * @param contact_points Optional contact points that add friction/moment
+   * factors and contact wrench keys.
+   * @param mu Optional friction coefficient (defaults to 1.0).
+   */
+  template <class CONTEXT>
+  gtsam::NonlinearFactorGraph graph(
+      const CONTEXT& context, const Robot& robot,
+      const std::optional<PointOnLinks>& contact_points = {},
+      const std::optional<double>& mu = {}) const;
+};
+
 }  // namespace gtdynamics

gtdynamics/dynamics/DynamicsGraph.cpp

@@ -12,10 +12,6 @@
  */
 
 #include <gtdynamics/dynamics/DynamicsGraph.h>
-#include <gtdynamics/factors/ContactDynamicsFrictionConeFactor.h>
-#include <gtdynamics/factors/ContactDynamicsMomentFactor.h>
-#include <gtdynamics/factors/ContactKinematicsAccelFactor.h>
-#include <gtdynamics/factors/ContactKinematicsTwistFactor.h>
 #include <gtdynamics/kinematics/Kinematics.h>
 #include <gtdynamics/universal_robot/Joint.h>
 #include <gtdynamics/utils/Slice.h>
@@ -50,6 +46,15 @@ using gtsam::Z_6x1;
 
 namespace gtdynamics {
 
+OptimizerSetting DynamicsGraph::configuredSettings(
+    const OptimizerSetting& opt, const gtsam::Vector3& gravity,
+    const std::optional<gtsam::Vector3>& planar_axis) {
+  OptimizerSetting configured = opt;
+  configured.gravity = gravity;
+  configured.planar_axis = planar_axis;
+  return configured;
+}
+
 GaussianFactorGraph DynamicsGraph::linearDynamicsGraph(
     const Robot &robot, const int k, const gtsam::Values &known_values) const {
   GaussianFactorGraph graph;
@@ -91,22 +96,20 @@ GaussianFactorGraph DynamicsGraph::linearDynamicsGraph(
     }
   }
 
-  OptimizerSetting opt_;
   for (auto &&joint : robot.joints()) {
-    graph.push_back(joint->linearAFactors(k, known_values, opt_, planar_axis_));
+    graph.push_back(joint->linearAFactors(k, known_values, planar_axis_));
     graph.push_back(
-        joint->linearDynamicsFactors(k, known_values, opt_, planar_axis_));
+        joint->linearDynamicsFactors(k, known_values, planar_axis_));
   }
 
   return graph;
 }
 
 GaussianFactorGraph DynamicsGraph::linearFDPriors(
     const Robot &robot, const int k, const gtsam::Values &torques) {
-  OptimizerSetting opt_ = OptimizerSetting();
   GaussianFactorGraph graph;
   for (auto &&joint : robot.joints())
-    graph.push_back(joint->linearFDPriors(k, torques, opt_));
+    graph.push_back(joint->linearFDPriors(k, torques));
   return graph;
 }
 
@@ -209,87 +212,20 @@ gtsam::NonlinearFactorGraph DynamicsGraph::vFactors(
 gtsam::NonlinearFactorGraph DynamicsGraph::aFactors(
     const Robot &robot, const int k,
     const std::optional<PointOnLinks> &contact_points) const {
-  NonlinearFactorGraph graph;
-  for (auto &&link : robot.links())
-    if (link->isFixed())
-      graph.addPrior<gtsam::Vector6>(TwistAccelKey(link->id(), k), gtsam::Z_6x1,
-                                     opt_.ba_cost_model);
-  for (auto &&joint : robot.joints())
-    graph.add(TwistAccelFactor(opt_.a_cost_model, joint, k));
-
-  // Add contact factors.
-  if (contact_points) {
-    for (auto &&cp : *contact_points) {
-      ContactKinematicsAccelFactor contact_accel_factor(
-          TwistAccelKey(cp.link->id(), k), opt_.ca_cost_model,
-          gtsam::Pose3(gtsam::Rot3(), -cp.point));
-      graph.add(contact_accel_factor);
-    }
-  }
-
-  return graph;
+  return dynamics_.aFactors(Slice(k), robot, contact_points);
 }
 
-// TODO(frank): migrate to Dynamics::graph<Slice>
 gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors(
     const Robot &robot, const int k,
     const std::optional<PointOnLinks> &contact_points,
     const std::optional<double> &mu) const {
+  const Slice slice(k);
   NonlinearFactorGraph graph;
-
-  double mu_;  // Static friction coefficient.
-  if (mu)
-    mu_ = *mu;
-  else
-    mu_ = 1.0;
-
-  for (auto &&link : robot.links()) {
-    int i = link->id();
-    if (!link->isFixed()) {
-      const auto &connected_joints = link->joints();
-      std::vector<gtsam::Key> wrench_keys;
-
-      // Add wrench keys for joints.
-      for (auto &&joint : connected_joints)
-        wrench_keys.push_back(WrenchKey(i, joint->id(), k));
-
-      // Add wrench keys for contact points.
-      if (contact_points) {
-        for (auto &&cp : *contact_points) {
-          if (cp.link->id() != i) continue;
-          // TODO(frank): allow multiple contact points on one link, id = 0,1,..
-          auto wrench_key = ContactWrenchKey(i, 0, k);
-          wrench_keys.push_back(wrench_key);
-
-          // Add contact dynamics constraints.
-          graph.emplace_shared<ContactDynamicsFrictionConeFactor>(
-              PoseKey(i, k), wrench_key, opt_.cfriction_cost_model, mu_,
-              gravity_);
-
-          graph.emplace_shared<ContactDynamicsMomentFactor>(
-              wrench_key, opt_.cm_cost_model,
-              gtsam::Pose3(gtsam::Rot3(), -cp.point));
-        }
-      }
-
-      // add wrench factor for link
-      graph.add(
-          WrenchFactor(opt_.fa_cost_model, link, wrench_keys, k, gravity_));
-    }
-  }
-
-  // TODO(frank): use Statics<Slice> calls
-  // TODO(frank): sort out const shared ptr mess
-  for (auto &&joint : robot.joints()) {
-    auto j = joint->id(), child_id = joint->child()->id();
-    auto const_joint = joint;
-    graph.add(WrenchEquivalenceFactor(opt_.f_cost_model, const_joint, k));
-    graph.add(TorqueFactor(opt_.t_cost_model, const_joint, k));
-    if (planar_axis_) {
-      graph.add(WrenchPlanarFactor(opt_.planar_cost_model, *planar_axis_,
-                                   const_joint, k));
-    }
-  }
+  // Compose shared statics factors with dynamic-only delta factors.
+  graph.add(mechanics_.wrenchEquivalenceFactors(slice, robot));
+  graph.add(mechanics_.torqueFactors(slice, robot));
+  graph.add(mechanics_.wrenchPlanarFactors(slice, robot));
+  graph.add(dynamics_.graph(slice, robot, contact_points, mu));
   return graph;
 }
 
@@ -558,7 +494,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::jointLimitFactors(
     const Robot &robot, const int k) const {
   NonlinearFactorGraph graph;
   for (auto &&joint : robot.joints())
-    graph.add(joint->jointLimitFactors(k, opt_));
+    graph.add(joint->jointLimitFactors(k, opt_.jl_cost_model));
   return graph;
 }