Make factors and others type-agnostic (precursor to SphericalJoints)

gtdynamics.i

@@ -36,9 +36,8 @@ typedef gtdynamics::JointMeasurementFactor<gtdynamics::RevoluteJoint> RevoluteJo
 
 #include <gtdynamics/factors/PoseFactor.h>
 class PoseFactor : gtsam::NonlinearFactor {
-  PoseFactor(gtsam::Key wTp_key, gtsam::Key wTc_key, gtsam::Key q_key,
-             const gtsam::noiseModel::Base* cost_model,
-             const gtdynamics::Joint* joint);
+  PoseFactor(const gtsam::noiseModel::Base *cost_model,
+             const gtdynamics::Joint *joint, int k = 0);
 
   void print(const string &s="",
              const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
@@ -93,21 +92,17 @@ class ContactPointFactor : gtsam::NoiseModelFactor {
 
 #include <gtdynamics/factors/TwistFactor.h>
 class TwistFactor : gtsam::NonlinearFactor {
-  TwistFactor(gtsam::Key twistP_key, gtsam::Key twistC_key, gtsam::Key q_key,
-              gtsam::Key qVel_key,
-              const gtsam::noiseModel::Base* cost_model,
-              const gtdynamics::Joint* joint);
+  TwistFactor(const gtsam::noiseModel::Base *cost_model,
+              const gtdynamics::Joint *joint, int k = 0);
 
   void print(const string &s="",
              const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
 };
 
 #include <gtdynamics/factors/TwistAccelFactor.h>
 class TwistAccelFactor : gtsam::NonlinearFactor {
-  TwistAccelFactor(gtsam::Key twist_key_c, gtsam::Key twistAccel_key_p, gtsam::Key twistAccel_key_c,
-              gtsam::Key q_key, gtsam::Key qVel_key, gtsam::Key qAccel_key,
-              const gtsam::noiseModel::Base* cost_model,
-              const gtdynamics::JointTyped* joint);
+  TwistAccelFactor(const gtsam::noiseModel::Base *cost_model,
+                   const gtdynamics::Joint *joint, int k = 0);
 
   void print(const string &s="",
              const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
@@ -116,7 +111,7 @@ class TwistAccelFactor : gtsam::NonlinearFactor {
 #include <gtdynamics/factors/TorqueFactor.h>
 class TorqueFactor : gtsam::NonlinearFactor {
   TorqueFactor(const gtsam::noiseModel::Base *cost_model,
-               const gtdynamics::JointTyped *joint, size_t k=0);
+               const gtdynamics::Joint *joint, size_t k = 0);
 
   void print(const string &s="",
              const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
@@ -145,15 +140,16 @@ class WrenchFactor : gtsam::NonlinearFactor {
 #include <gtdynamics/factors/WrenchEquivalenceFactor.h>
 class WrenchEquivalenceFactor : gtsam::NonlinearFactor{
   WrenchEquivalenceFactor(const gtsam::noiseModel::Base *cost_model,
-                          gtdynamics::JointTyped *joint, size_t k = 0);
+                          gtdynamics::Joint *joint, size_t k = 0);
   void print(const string &s="",
              const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
 };
 
 #include <gtdynamics/factors/WrenchPlanarFactor.h>
 class WrenchPlanarFactor : gtsam::NonlinearFactor {
   WrenchPlanarFactor(const gtsam::noiseModel::Base *cost_model,
-                     Vector planar_axis, gtdynamics::JointTyped *joint, size_t k=0);
+                     Vector planar_axis, gtdynamics::Joint *joint,
+                     size_t k = 0);
   void print(const string &s="",
              const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
 };

gtdynamics/dynamics/DynamicsGraph.cpp

@@ -204,10 +204,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::qFactors(
 
   // TODO(frank): call Kinematics::graph<Slice> instead
   for (auto &&joint : robot.joints()) {
-    graph.emplace_shared<PoseFactor>(
-        internal::PoseKey(joint->parent()->id(), k),
-        internal::PoseKey(joint->child()->id(), k),
-        internal::JointAngleKey(joint->id(), k), opt_.p_cost_model, joint);
+    graph.emplace_shared<PoseFactor>(opt_.p_cost_model, joint, k);
   }
 
   // TODO(frank): whoever write this should clean up this mess.
@@ -240,11 +237,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::vFactors(
                                      gtsam::Z_6x1, opt_.bv_cost_model);
 
   for (auto &&joint : robot.joints())
-    graph.emplace_shared<TwistFactor>(internal::TwistKey(joint->parent()->id(), t),
-                                    internal::TwistKey(joint->child()->id(), t),
-                                    internal::JointAngleKey(joint->id(), t),
-                                    internal::JointVelKey(joint->id(), t),
-                                    opt_.v_cost_model, joint);
+    graph.emplace_shared<TwistFactor>(opt_.v_cost_model, joint, t);
 
   // Add contact factors.
   if (contact_points) {
@@ -268,13 +261,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::aFactors(
       graph.addPrior<gtsam::Vector6>(internal::TwistAccelKey(link->id(), t),
                                      gtsam::Z_6x1, opt_.ba_cost_model);
   for (auto &&joint : robot.joints())
-    graph.emplace_shared<TwistAccelFactor>(
-      internal::TwistKey(joint->child()->id(), t),
-      internal::TwistAccelKey(joint->parent()->id(), t),
-      internal::TwistAccelKey(joint->child()->id(), t),
-      internal::JointAngleKey(joint->id(), t), internal::JointVelKey(joint->id(), t),
-      internal::JointAccelKey(joint->id(), t), opt_.a_cost_model,
-      boost::static_pointer_cast<const JointTyped>(joint));
+    graph.emplace_shared<TwistAccelFactor>(opt_.a_cost_model, joint, t);
 
   // Add contact factors.
   if (contact_points) {
@@ -351,13 +338,11 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors(
   // 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 = boost::static_pointer_cast<const JointTyped>(joint);
-    graph.emplace_shared<WrenchEquivalenceFactor>(opt_.f_cost_model,
-                                                  const_joint, k);
-    graph.emplace_shared<TorqueFactor>(opt_.t_cost_model, const_joint, k);
+    graph.emplace_shared<WrenchEquivalenceFactor>(opt_.f_cost_model, joint, k);
+    graph.emplace_shared<TorqueFactor>(opt_.t_cost_model, joint, k);
     if (planar_axis_)
       graph.emplace_shared<WrenchPlanarFactor>(opt_.planar_cost_model,
-                                               *planar_axis_, const_joint, k);
+                                               *planar_axis_, joint, k);
   }
   return graph;
 }

gtdynamics/factors/JointLimitFactor.h → gtdynamics/factors/JointLimitDoubleFactor.h

@@ -6,7 +6,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file  JointLimitFactor.h
+ * @file  JointLimitDoubleFactor.h
  * @brief apply joint limit
  * @author: Frank Dellaert, Mandy Xie, Stephanie McCormick, Varun Agrawal
  */
@@ -24,21 +24,19 @@
 #include <string>
 #include <vector>
 
-#include "gtdynamics/universal_robot/JointTyped.h"
+#include "gtdynamics/universal_robot/Joint.h"
 
 namespace gtdynamics {
 
 /**
- * JointLimitFactor is a class which enforces joint angle, velocity,
- * acceleration and torque value to be within limi
+ * JointLimitDoubleFactor is a class which enforces joint angle, velocity,
+ * acceleration and torque value to be within limits
  */
-class JointLimitFactor
-    : public gtsam::NoiseModelFactor1<JointTyped::JointCoordinateType> {
+class JointLimitDoubleFactor : public gtsam::NoiseModelFactor1<double> {
  private:
-  using JointCoordinateType = JointTyped::JointCoordinateType;
-  using This = JointLimitFactor;
-  using Base = gtsam::NoiseModelFactor1<JointCoordinateType>;
-  JointCoordinateType low_, high_;
+  using This = JointLimitDoubleFactor;
+  using Base = gtsam::NoiseModelFactor1<double>;
+  double low_, high_;
 
  public:
   /**
@@ -49,16 +47,16 @@ class JointLimitFactor
    * @param upper_limit joint upper limit
    * @param limit_threshold joint limit threshold
    */
-  JointLimitFactor(gtsam::Key q_key,
-                   const gtsam::noiseModel::Base::shared_ptr &cost_model,
-                   JointCoordinateType lower_limit,
-                   JointCoordinateType upper_limit,
-                   JointCoordinateType limit_threshold)
+  JointLimitDoubleFactor(gtsam::Key q_key,
+                         const gtsam::noiseModel::Base::shared_ptr &cost_model,
+                         double lower_limit,
+                         double upper_limit,
+                         double limit_threshold)
       : Base(cost_model, q_key),
         low_(lower_limit + limit_threshold),
         high_(upper_limit - limit_threshold) {}
 
-  virtual ~JointLimitFactor() {}
+  virtual ~JointLimitDoubleFactor() {}
 
  public:
   /**
@@ -72,7 +70,7 @@ class JointLimitFactor
    * @param q joint value
    */
   gtsam::Vector evaluateError(
-      const JointCoordinateType &q,
+      const double &q,
       boost::optional<gtsam::Matrix &> H_q = boost::none) const override {
     if (q < low_) {
       if (H_q) *H_q = -gtsam::I_1x1;
@@ -96,7 +94,7 @@ class JointLimitFactor
   void print(const std::string &s = "",
              const gtsam::KeyFormatter &keyFormatter =
                  gtsam::DefaultKeyFormatter) const override {
-    std::cout << s << "JointLimitFactor" << std::endl;
+    std::cout << s << "JointLimitDoubleFactor" << std::endl;
     Base::print("", keyFormatter);
   }
 

gtdynamics/factors/PoseFactor.h

@@ -28,8 +28,6 @@
 
 namespace gtdynamics {
 
-using boost::assign::cref_list_of;
-
 /**
  * PoseFactor is a three-way nonlinear factor between a joint's parent link
  * pose, child link pose, and the joint angle relating the two poses.
@@ -56,33 +54,13 @@ class PoseFactor : public gtsam::NoiseModelFactor {
   PoseFactor(const gtsam::SharedNoiseModel &cost_model,
              const JointConstSharedPtr &joint, int time)
       : Base(cost_model,
-             cref_list_of<3>(
+             boost::assign::cref_list_of<3>(
                  internal::PoseKey(joint->parent()->id(), time).key())(
                  internal::PoseKey(joint->child()->id(), time).key())(
                  internal::JointAngleKey(joint->id(), time).key())),
         t_(time),
         joint_(joint) {}
 
-  /**
-   * Create single factor relating this link's pose (COM) with previous one.
-   *
-   * Please use the joint based constructor above if possible.
-   *
-   * @param wTp_key Key for parent link's CoM pose in world frame.
-   * @param wTc_key Key for child link's CoM pose in world frame.
-   * @param q_key Key for joint value.
-   * @param cost_model The noise model for this factor.
-   * @param joint The joint connecting the two poses
-   */
-  PoseFactor(DynamicsSymbol wTp_key, DynamicsSymbol wTc_key,
-             DynamicsSymbol q_key,
-             const gtsam::noiseModel::Base::shared_ptr &cost_model,
-             JointConstSharedPtr joint)
-      : Base(cost_model,
-             cref_list_of<3>(wTp_key.key())(wTc_key.key())(q_key.key())),
-        t_(wTp_key.time()),
-        joint_(joint) {}
-
   virtual ~PoseFactor() {}
 
   /**
@@ -101,18 +79,18 @@ class PoseFactor : public gtsam::NoiseModelFactor {
                        wTc = x.at<gtsam::Pose3>(keys_[1]);
     // TODO(frank): logmap derivative is close to identity when error is small
     gtsam::Matrix6 wTc_hat_H_wTp, H_wTc_hat, H_wTc;
-    // TODO(gerry): figure out how to make this work better for dynamic matrices
     gtsam::Matrix wTc_hat_H_q;
-    boost::optional<gtsam::Matrix &> wTc_hat_H_q_ref;
-    if (H) wTc_hat_H_q_ref = wTc_hat_H_q;
 
-    auto wTc_hat = joint_->poseOf(joint_->child(), wTp, x, t_,
-                                  H ? &wTc_hat_H_wTp : 0, wTc_hat_H_q_ref);
+    // Due to quirks of OptionalJacobian, I think this is more readable
+    gtsam::Pose3 wTc_hat = H ? joint_->poseOf(joint_->child(), wTp, x, t_,
+                                              wTc_hat_H_wTp, wTc_hat_H_q)
+                             : joint_->poseOf(joint_->child(), wTp, x, t_);
+
     gtsam::Vector6 error =
         wTc.logmap(wTc_hat, H ? &H_wTc : 0, H ? &H_wTc_hat : 0);
     if (H) {
       (*H)[0] = H_wTc_hat * wTc_hat_H_wTp;
-      (*H)[1] = H_wTc;
+      (*H)[1] = H_wTc; // this is trading off readability for extra copy op
       (*H)[2] = H_wTc_hat * wTc_hat_H_q;
     }
     return error;

gtdynamics/factors/PreintegratedContactFactors.h

@@ -24,7 +24,7 @@
 #include <memory>
 #include <string>
 
-#include "gtdynamics/universal_robot/JointTyped.h"
+#include "gtdynamics/universal_robot/Joint.h"
 #include "gtdynamics/universal_robot/Link.h"
 
 namespace gtdynamics {

gtdynamics/factors/TorqueFactor.h

@@ -17,11 +17,12 @@
 #include <gtsam/base/Vector.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
+#include <boost/assign/list_of.hpp>
 #include <boost/optional.hpp>
 #include <memory>
 #include <string>
 
-#include "gtdynamics/universal_robot/JointTyped.h"
+#include "gtdynamics/universal_robot/Joint.h"
 #include "gtdynamics/universal_robot/Link.h"
 #include "gtdynamics/utils/values.h"
 
@@ -31,21 +32,20 @@ namespace gtdynamics {
  * TorqueFactor is a two-way nonlinear factor which enforces relation between
  * wrench and torque on each link
  */
-class TorqueFactor
-    : public gtsam::NoiseModelFactor2<gtsam::Vector6,
-                                      typename JointTyped::JointTorque> {
+class TorqueFactor : public gtsam::NoiseModelFactor {
  private:
-  using JointTorque = typename JointTyped::JointTorque;
   using This = TorqueFactor;
-  using Base = gtsam::NoiseModelFactor2<gtsam::Vector6, JointTorque>;
-  using MyJointConstSharedPtr = boost::shared_ptr<const JointTyped>;
+  using Base = gtsam::NoiseModelFactor;
+  using MyJointConstSharedPtr = boost::shared_ptr<const Joint>;
   MyJointConstSharedPtr joint_;
 
   /// Private constructor with arbitrary keys
   TorqueFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model,
                gtsam::Key wrench_key, gtsam::Key torque_key,
                const MyJointConstSharedPtr &joint)
-      : Base(cost_model, wrench_key, torque_key), joint_(joint) {}
+      : Base(cost_model,
+             boost::assign::cref_list_of<2>(wrench_key)(torque_key)),
+        joint_(joint) {}
 
  public:
   /**
@@ -68,20 +68,27 @@ class TorqueFactor
  public:
   /**
    * Evaluate wrench balance errors
-   * @param wrench wrench on this link
-   * @param torque torque on this link joint
    */
-  gtsam::Vector evaluateError(
-      const gtsam::Vector6 &wrench, const JointTorque &torque,
-      boost::optional<gtsam::Matrix &> H_wrench = boost::none,
-      boost::optional<gtsam::Matrix &> H_torque = boost::none) const override {
-    if (H_torque) {
-      *H_torque = -JointTyped::MatrixN::Identity();
+  gtsam::Vector unwhitenedError(const gtsam::Values &x,
+                                boost::optional<std::vector<gtsam::Matrix> &>
+                                    H = boost::none) const override {
+    const gtsam::Vector6 &wrench = x.at<gtsam::Vector6>(keys_[0]);
+    gtsam::Vector torque;
+    try {
+      torque = x.at<gtsam::Vector>(keys_[1]);
+    } catch (const gtsam::ValuesIncorrectType &e) {
+      torque = gtsam::Vector1(x.at<double>(keys_[1]));
+    }
+
+    if (H) {
+      (*H)[1] = -gtsam::Matrix::Identity(torque.size(), torque.size());
+      return joint_->transformWrenchToTorqueVector(joint_->child(), wrench,
+                                                   (*H)[0]) -
+             torque;
+    } else {
+      return joint_->transformWrenchToTorqueVector(joint_->child(), wrench) -
+             torque;
     }
-    // TODO(G+S): next PR will generalize this from Vector1
-    return gtsam::Vector1(
-        joint_->transformWrenchToTorque(joint_->child(), wrench, H_wrench) -
-        torque);
   }
 
   /// Returns the joint