GPMP2 Addition

gtdynamics.i

@@ -1033,6 +1033,168 @@ class Trajectory {
   void writeToFile(const gtdynamics::Robot &robot, const string &name, const gtsam::Values &results) const;
 };
 
+/********************** gpmp2  **********************/
+#include <gtdynamics/gpmp2/SignedDistanceField.h>
+class SignedDistanceField {
+  SignedDistanceField();
+  SignedDistanceField(const gtsam::Matrix &positions,
+                      const gtsam::Vector &distances);
+  SignedDistanceField(const gtsam::Matrix &positions,
+                      const gtsam::Vector &distances, double tol);
+  SignedDistanceField(const gtsam::Point3 &origin, double cellSize,
+                      size_t fieldRows, size_t fieldCols, size_t fieldZ,
+                      const gtsam::Vector &values);
+
+  double getSignedDistance(const gtsam::Point3 &point) const;
+
+  gtsam::Point3 origin() const;
+  size_t xCount() const;
+  size_t yCount() const;
+  size_t zCount() const;
+  double cellSize() const;
+
+  bool equals(const gtdynamics::SignedDistanceField &expected,
+              double tol) const;
+  void print(const string &s = "") const;
+};
+
+#include <gtdynamics/gpmp2/RobotQueryPoints.h>
+class RobotQueryPoints {
+  RobotQueryPoints(const gtdynamics::Robot &robot,
+                   const string &baseLinkName,
+                   const std::vector<gtdynamics::Joint *> &joints,
+                   const gtdynamics::PointOnLinks &points);
+  RobotQueryPoints(const gtdynamics::Robot &robot,
+                   const string &baseLinkName,
+                   const std::vector<gtdynamics::Joint *> &joints,
+                   const gtdynamics::PointOnLinks &points,
+                   const gtsam::Pose3 &wTbase);
+
+  size_t dof() const;
+  size_t nrPoints() const;
+  gtsam::Matrix worldPoints(const gtsam::Vector &q) const;
+};
+
+#include <gtdynamics/factors/GPLinearPriorFactor.h>
+class GPLinearPrior : gtsam::NoiseModelFactor {
+  GPLinearPrior(gtsam::Key poseKey1, gtsam::Key velKey1, gtsam::Key poseKey2,
+                gtsam::Key velKey2, double deltaT,
+                const gtsam::noiseModel::Base *QcModel);
+
+  size_t dof() const;
+  double deltaT() const;
+  void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter =
+                                       gtdynamics::GTDKeyFormatter);
+};
+
+#include <gtdynamics/factors/ObstacleSDFFactor.h>
+class ObstacleSDFFactor : gtsam::NoiseModelFactor {
+  ObstacleSDFFactor(gtsam::Key qKey, const gtdynamics::RobotQueryPoints *robot,
+                    const gtdynamics::SignedDistanceField *sdf,
+                    double costSigma, double epsilon);
+  ObstacleSDFFactor(gtsam::Key qKey, const gtdynamics::RobotQueryPoints *robot,
+                    const gtdynamics::SignedDistanceField *sdf,
+                    double costSigma, double epsilon,
+                    const gtsam::Vector &radii);
+
+  double epsilon() const;
+  gtsam::Vector radii() const;
+  void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter =
+                                       gtdynamics::GTDKeyFormatter);
+};
+
+#include <gtdynamics/factors/JointLimitFactorVector.h>
+class JointLimitFactorVector : gtsam::NoiseModelFactor {
+  JointLimitFactorVector(gtsam::Key qKey,
+                         const gtsam::noiseModel::Base *costModel,
+                         const gtsam::Vector &downLimit,
+                         const gtsam::Vector &upLimit,
+                         const gtsam::Vector &limitThreshold);
+
+  size_t dof() const;
+  void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter =
+                                       gtdynamics::GTDKeyFormatter);
+};
+
+#include <gtdynamics/factors/VelocityLimitFactorVector.h>
+class VelocityLimitFactorVector : gtsam::NoiseModelFactor {
+  VelocityLimitFactorVector(gtsam::Key vKey,
+                            const gtsam::noiseModel::Base *costModel,
+                            const gtsam::Vector &velLimit,
+                            const gtsam::Vector &limitThreshold);
+
+  size_t dof() const;
+  void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter =
+                                       gtdynamics::GTDKeyFormatter);
+};
+
+#include <gtdynamics/factors/ObstacleSDFFactorGP.h>
+class ObstacleSDFFactorGP : gtsam::NoiseModelFactor {
+  ObstacleSDFFactorGP(gtsam::Key qKey1, gtsam::Key vKey1, gtsam::Key qKey2,
+                      gtsam::Key vKey2,
+                      const gtdynamics::RobotQueryPoints *robot,
+                      const gtdynamics::SignedDistanceField *sdf,
+                      double costSigma, double epsilon,
+                      const gtsam::noiseModel::Base *QcModel, double deltaT,
+                      double tau);
+
+  double epsilon() const;
+  void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter =
+                                       gtdynamics::GTDKeyFormatter);
+};
+
+#include <gtdynamics/factors/SelfCollisionSphereFactor.h>
+class SelfCollisionPair {
+  SelfCollisionPair();
+  SelfCollisionPair(size_t a, size_t b, double epsilon);
+
+  size_t a;
+  size_t b;
+  double epsilon;
+};
+// SelfCollisionPairs defined in specializations.h
+
+class SelfCollisionSphereFactor : gtsam::NoiseModelFactor {
+  SelfCollisionSphereFactor(gtsam::Key qKey,
+                            const gtdynamics::RobotQueryPoints *robot,
+                            const gtdynamics::SelfCollisionPairs &pairs,
+                            const gtsam::Vector &radii, double costSigma);
+  SelfCollisionSphereFactor(gtsam::Key qKey,
+                            const gtdynamics::RobotQueryPoints *robot,
+                            const gtdynamics::SelfCollisionPairs &pairs,
+                            const gtsam::Vector &radii,
+                            const gtsam::Vector &sigmas);
+
+  size_t nrPairs() const;
+  gtsam::Vector radii() const;
+  void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter =
+                                       gtdynamics::GTDKeyFormatter);
+};
+
+#include <gtdynamics/factors/SelfCollisionSphereFactorGP.h>
+class SelfCollisionSphereFactorGP : gtsam::NoiseModelFactor {
+  SelfCollisionSphereFactorGP(gtsam::Key qKey1, gtsam::Key vKey1,
+                              gtsam::Key qKey2, gtsam::Key vKey2,
+                              const gtdynamics::RobotQueryPoints *robot,
+                              const gtdynamics::SelfCollisionPairs &pairs,
+                              const gtsam::Vector &radii, double costSigma,
+                              const gtsam::noiseModel::Base *QcModel,
+                              double deltaT, double tau);
+  SelfCollisionSphereFactorGP(gtsam::Key qKey1, gtsam::Key vKey1,
+                              gtsam::Key qKey2, gtsam::Key vKey2,
+                              const gtdynamics::RobotQueryPoints *robot,
+                              const gtdynamics::SelfCollisionPairs &pairs,
+                              const gtsam::Vector &radii,
+                              const gtsam::Vector &sigmas,
+                              const gtsam::noiseModel::Base *QcModel,
+                              double deltaT, double tau);
+
+  size_t nrPairs() const;
+  gtsam::Vector radii() const;
+  void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter =
+                                       gtdynamics::GTDKeyFormatter);
+};
+
 /********************** Utilities  **********************/
 #include <gtdynamics/utils/format.h>
 string GtdFormat(const gtsam::Values &t, const string &s = "");

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 mechanics statics dynamics cmopt cmcopt scenarios)
+set(SOURCE_SUBDIRS universal_robot utils factors constraints optimizer constrained_optimizer kinematics mechanics statics dynamics cmopt cmcopt scenarios gpmp2)
 
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 

gtdynamics/factors/CMakeLists.txt

@@ -0,0 +1,3 @@
+if(GTSAM_BUILD_TESTS AND EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/tests/CMakeLists.txt)
+  add_subdirectory(tests)
+endif()

gtdynamics/factors/GPLiePriorFactor.h

@@ -0,0 +1,172 @@
+/* ----------------------------------------------------------------------------
+ * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  GPLiePriorFactor.h
+ * @brief Gaussian process prior factor on any Lie group, see Barfoot14rss.
+ * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong.
+ */
+
+#pragma once
+
+#include <gtdynamics/gpmp2/GPUtils.h>
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/base/concepts.h>
+#include <gtsam/nonlinear/NoiseModelFactorN.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <cmath>
+#include <iostream>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/nvp.hpp>
+#endif
+
+namespace gtdynamics {
+
+/**
+ * A 4-way GaussianProcess prior factor on any Lie group T.
+ * Implemented similarly to that used in the GPMP2 paper. Each state consists of
+ * a pose in T and a velocity in the tangent space, separated by deltaT. With
+ * r = Logmap(Inverse(pose1) * pose2), the error is
+ * [r - deltaT * vel1; vel2 - vel1].
+ */
+template <typename T>
+class GPLiePrior
+    : public gtsam::NoiseModelFactorN<T, gtsam::Vector, T, gtsam::Vector> {
+ private:
+  GTSAM_CONCEPT_ASSERT(gtsam::IsLieGroup<T>);
+
+  using This = GPLiePrior<T>;
+  using Base = gtsam::NoiseModelFactorN<T, gtsam::Vector, T, gtsam::Vector>;
+
+  size_t dof_;
+  double deltaT_;
+
+ public:
+  /// Default constructor, only for serialization.
+  GPLiePrior() {}
+
+  /**
+   * Constructor from the keys of the two states.
+   * @param poseKey1 key for the pose of the first state
+   * @param velKey1 key for the velocity of the first state
+   * @param poseKey2 key for the pose of the second state
+   * @param velKey2 key for the velocity of the second state
+   * @param deltaT time between the two states
+   * @param QcModel Gaussian noise model whose covariance is Qc
+   */
+  GPLiePrior(gtsam::Key poseKey1, gtsam::Key velKey1, gtsam::Key poseKey2,
+             gtsam::Key velKey2, double deltaT,
+             const gtsam::SharedNoiseModel &QcModel)
+      : Base(gtsam::noiseModel::Gaussian::Covariance(
+                 calcQAccel(getQc(QcModel), deltaT)),
+             poseKey1, velKey1, poseKey2, velKey2),
+        dof_(QcModel->dim()),
+        deltaT_(deltaT) {
+    checkGPDeltaT(deltaT_);
+    // A mismatched Qc dimension silently mis-sizes the error and Jacobians, so
+    // reject it up front. Only checkable when T has a fixed dimension.
+    if (gtsam::traits<T>::dimension != Eigen::Dynamic &&
+        dof_ != static_cast<size_t>(gtsam::traits<T>::dimension)) {
+      throw std::invalid_argument(
+          "GPLiePrior: QcModel dimension must equal the tangent dimension of "
+          "the Lie group.");
+    }
+  }
+
+  ~GPLiePrior() override {}
+
+  /// Return a deep copy of this factor.
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /// Evaluate the Gaussian process prior error and its Jacobians.
+  gtsam::Vector evaluateError(
+      const T &pose1, const gtsam::Vector &vel1, const T &pose2,
+      const gtsam::Vector &vel2, gtsam::OptionalMatrixType H1 = nullptr,
+      gtsam::OptionalMatrixType H2 = nullptr,
+      gtsam::OptionalMatrixType H3 = nullptr,
+      gtsam::OptionalMatrixType H4 = nullptr) const override {
+    // Relative increment between the two poses, in the tangent space.
+    gtsam::Matrix Hinv, Hcomp1, Hcomp2, Hlogmap;
+    gtsam::Vector r;
+    if (H1 || H2 || H3 || H4) {
+      r = gtsam::traits<T>::Logmap(
+          gtsam::traits<T>::Compose(gtsam::traits<T>::Inverse(pose1, Hinv),
+                                    pose2, Hcomp1, Hcomp2),
+          Hlogmap);
+    } else {
+      r = gtsam::traits<T>::Logmap(
+          gtsam::traits<T>::Compose(gtsam::traits<T>::Inverse(pose1), pose2));
+    }
+
+    const gtsam::Matrix identity = gtsam::Matrix::Identity(dof_, dof_);
+    const gtsam::Matrix zero = gtsam::Matrix::Zero(dof_, dof_);
+
+    if (H1)
+      *H1 = (gtsam::Matrix(2 * dof_, dof_) << Hlogmap * Hcomp1 * Hinv, zero)
+                .finished();
+    if (H2)
+      *H2 = (gtsam::Matrix(2 * dof_, dof_) << -deltaT_ * identity, -identity)
+                .finished();
+    if (H3)
+      *H3 =
+          (gtsam::Matrix(2 * dof_, dof_) << Hlogmap * Hcomp2, zero).finished();
+    if (H4) *H4 = (gtsam::Matrix(2 * dof_, dof_) << zero, identity).finished();
+
+    return (gtsam::Vector(2 * dof_) << (r - vel1 * deltaT_), (vel2 - vel1))
+        .finished();
+  }
+
+  /// Return the degrees of freedom of a single state.
+  size_t dof() const { return dof_; }
+
+  /// Return the time between the two states.
+  double deltaT() const { return deltaT_; }
+
+  /// Equality up to a tolerance.
+  bool equals(const gtsam::NonlinearFactor &expected,
+              double tol = 1e-9) const override {
+    const This *e = dynamic_cast<const This *>(&expected);
+    return e != nullptr && Base::equals(*e, tol) &&
+           std::fabs(this->deltaT_ - e->deltaT_) < tol;
+  }
+
+  /// Print contents.
+  void print(const std::string &s = "",
+             const gtsam::KeyFormatter &keyFormatter =
+                 gtsam::DefaultKeyFormatter) const override {
+    std::cout << s << "4-way Gaussian process prior factor on Lie<" << dof_
+              << ">" << std::endl;
+    Base::print("", keyFormatter);
+  }
+
+ private:
+#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION
+  /// Serialization function
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {  // NOLINT
+    ar &boost::serialization::make_nvp(
+        "NoiseModelFactorN", boost::serialization::base_object<Base>(*this));
+    ar &BOOST_SERIALIZATION_NVP(dof_);
+    ar &BOOST_SERIALIZATION_NVP(deltaT_);
+  }
+#endif
+};  // \class GPLiePrior
+
+}  // namespace gtdynamics