Gpmp2 self collision

gtdynamics.i

@@ -1143,6 +1143,58 @@ class ObstacleSDFFactorGP : gtsam::NoiseModelFactor {
                                        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/factors/SelfCollisionSphereFactor.cpp

@@ -0,0 +1,116 @@
+/* ----------------------------------------------------------------------------
+ * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  SelfCollisionSphereFactor.cpp
+ * @brief Self collision cost factor over a set of query point pairs.
+ * @author Karthik Shaji - Adapted from gpmp2 by Mustafa Mukadam.
+ */
+
+#include <gtdynamics/factors/SelfCollisionSphereFactor.h>
+
+#include <map>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace gtdynamics {
+
+/* ************************************************************************* */
+void validateSelfCollisionPairs(const RobotQueryPoints &robot,
+                                const SelfCollisionPairs &pairs,
+                                const gtsam::Vector &radii,
+                                const std::string &factorName) {
+  if (static_cast<size_t>(radii.size()) != robot.nrPoints()) {
+    throw std::invalid_argument(
+        factorName + ": radii must have one entry per point.");
+  }
+  if ((radii.array() < 0.0).any()) {
+    throw std::invalid_argument(factorName + ": radii must be >= 0.");
+  }
+  for (const auto &pair : pairs) {
+    if (pair.epsilon < 0.0) {
+      throw std::invalid_argument(factorName +
+                                  ": a pair epsilon must be >= 0.");
+    }
+    if (pair.a >= robot.nrPoints() || pair.b >= robot.nrPoints()) {
+      throw std::invalid_argument(factorName +
+                                  ": pair point index out of range.");
+    }
+    if (pair.a == pair.b) {
+      throw std::invalid_argument(factorName +
+                                  ": a pair must use two distinct points.");
+    }
+    // Points on one rigid link keep a constant separation, so the hinge has
+    // no gradient; reject such a pair rather than fire it permanently.
+    if (robot.points()[pair.a].link->id() ==
+        robot.points()[pair.b].link->id()) {
+      throw std::invalid_argument(
+          factorName + ": a pair must use points on different links.");
+    }
+  }
+}
+
+/* ************************************************************************* */
+std::shared_ptr<const RobotQueryPoints> restrictToReferencedPoints(
+    const std::shared_ptr<const RobotQueryPoints> &robot,
+    SelfCollisionPairs *pairs, gtsam::Vector *radii) {
+  // Unique original point indices pairs references, in first-occurrence
+  // order, so evaluation never queries a point no pair uses.
+  std::vector<size_t> uniqueIndices;
+  std::map<size_t, size_t> compactOf;
+  for (const auto &pair : *pairs) {
+    for (size_t idx : {pair.a, pair.b}) {
+      if (compactOf.emplace(idx, uniqueIndices.size()).second) {
+        uniqueIndices.push_back(idx);
+      }
+    }
+  }
+
+  gtsam::Vector compactRadii(uniqueIndices.size());
+  for (size_t i = 0; i < uniqueIndices.size(); ++i) {
+    compactRadii(i) = (*radii)(uniqueIndices[i]);
+  }
+  *radii = compactRadii;
+
+  for (auto &pair : *pairs) {
+    pair.a = compactOf.at(pair.a);
+    pair.b = compactOf.at(pair.b);
+  }
+
+  return std::make_shared<const RobotQueryPoints>(robot, uniqueIndices);
+}
+
+/* ************************************************************************* */
+gtsam::Vector SelfCollisionSphereFactor::evaluateError(
+    const gtsam::Vector &q, gtsam::OptionalMatrixType H1) const {
+  const size_t nrPairs = pairs_.size();
+  gtsam::Vector err(nrPairs);
+
+  std::vector<gtsam::Point3> wPts;
+  std::vector<gtsam::Matrix> ptJacobians;
+  robot_->queryPoints(q, &wPts, H1 ? &ptJacobians : nullptr);
+  if (H1) *H1 = gtsam::Matrix::Zero(nrPairs, robot_->dof());
+
+  for (size_t r = 0; r < nrPairs; ++r) {
+    const SelfCollisionPair &pair = pairs_[r];
+    // The standoff folds in both spheres' radii.
+    const double eps = pair.epsilon + radii_(pair.a) + radii_(pair.b);
+    if (H1) {
+      gtsam::Matrix13 HptA, HptB;
+      err(r) = hingeLossSelfCollisionCost(wPts[pair.a], wPts[pair.b], eps,
+                                          HptA, HptB);
+      H1->row(r) = HptA * ptJacobians[pair.a] + HptB * ptJacobians[pair.b];
+    } else {
+      err(r) = hingeLossSelfCollisionCost(wPts[pair.a], wPts[pair.b], eps);
+    }
+  }
+  return err;
+}
+
+}  // namespace gtdynamics

gtdynamics/factors/SelfCollisionSphereFactor.h

@@ -0,0 +1,170 @@
+/* ----------------------------------------------------------------------------
+ * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  SelfCollisionSphereFactor.h
+ * @brief Self collision cost factor over a set of query point pairs.
+ * @author Karthik Shaji - Adapted from gpmp2 by Mustafa Mukadam.
+ */
+
+#pragma once
+
+#include <gtdynamics/gpmp2/RobotQueryPoints.h>
+#include <gtdynamics/gpmp2/SelfCollisionCost.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/nonlinear/NoiseModelFactorN.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <iostream>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace gtdynamics {
+
+/// One self collision check between two query points, each inflated to a sphere
+/// by its radius and kept epsilon apart on top of that.
+struct SelfCollisionPair {
+  size_t a;        ///< first query point index
+  size_t b;        ///< second query point index
+  double epsilon;  ///< standoff between the two, added to their radii
+
+  SelfCollisionPair() : a(0), b(0), epsilon(0.0) {}
+  SelfCollisionPair(size_t a, size_t b, double epsilon)
+      : a(a), b(b), epsilon(epsilon) {}
+};
+
+using SelfCollisionPairs = std::vector<SelfCollisionPair>;
+
+/**
+ * Reject inconsistent indices, radii or standoffs, shared by every self
+ * collision factor. factorName prefixes the error messages.
+ */
+GTSAM_EXPORT void validateSelfCollisionPairs(const RobotQueryPoints &robot,
+                                             const SelfCollisionPairs &pairs,
+                                             const gtsam::Vector &radii,
+                                             const std::string &factorName);
+
+/**
+ * Restrict robot to just the points pairs references, and remap pairs/radii
+ * from robot's point indices into the restricted model's own compact ones.
+ * Shared by every self collision factor so evaluation only ever queries the
+ * points a factor actually checks, not every point of a larger shared model.
+ * Mutates *pairs and *radii in place; robot itself is left untouched.
+ */
+GTSAM_EXPORT std::shared_ptr<const RobotQueryPoints> restrictToReferencedPoints(
+    const std::shared_ptr<const RobotQueryPoints> &robot,
+    SelfCollisionPairs *pairs, gtsam::Vector *radii);
+
+/**
+ * Unary factor keeping the robot clear of itself over a set of query point
+ * pairs, one hinge loss row per pair. Both points of every pair move with q, so
+ * build the RobotQueryPoints with the union of every joint involved and a
+ * common base, so a cross-arm pair couples all their DOFs in one row.
+ *
+ * Same-link pairs are rejected; the caller must also avoid adjacent-link pairs
+ * (constant separation). Coincident points fall back to a fixed separation
+ * direction rather than a non-finite gradient.
+ */
+class GTSAM_EXPORT SelfCollisionSphereFactor
+    : public gtsam::NoiseModelFactorN<gtsam::Vector> {
+ private:
+  using This = SelfCollisionSphereFactor;
+  using Base = gtsam::NoiseModelFactorN<gtsam::Vector>;
+
+  std::shared_ptr<const RobotQueryPoints> robot_;
+  gtsam::Vector radii_;  ///< one radius per query point, zero if unspecified
+  SelfCollisionPairs pairs_;
+
+  /// Reject a null model or inconsistent indices/radii/standoffs (checked
+  /// against robot's own point indices), then restrict robot_ to just the
+  /// points pairs_ references and remap pairs_/radii_ to match.
+  void validateAndRestrict(const std::shared_ptr<const RobotQueryPoints> &robot) {
+    if (!robot) {
+      throw std::invalid_argument(
+          "SelfCollisionSphereFactor: robot must not be null.");
+    }
+    validateSelfCollisionPairs(*robot, pairs_, radii_,
+                               "SelfCollisionSphereFactor");
+    robot_ = restrictToReferencedPoints(robot, &pairs_, &radii_);
+  }
+
+ public:
+  /**
+   * Constructor with a single sigma across every pair.
+   * @param qKey key of the stacked joint angle vector
+   * @param robot query point model, spanning every joint involved
+   * @param pairs query point pairs to check
+   * @param radii radius of each query point, one per point of the model
+   * @param costSigma cost function sigma, shared by every pair
+   */
+  SelfCollisionSphereFactor(gtsam::Key qKey,
+                      const std::shared_ptr<const RobotQueryPoints> &robot,
+                      const SelfCollisionPairs &pairs,
+                      const gtsam::Vector &radii, double costSigma)
+      : Base(gtsam::noiseModel::Isotropic::Sigma(pairs.size(), costSigma),
+             qKey),
+        radii_(radii),
+        pairs_(pairs) {
+    validateAndRestrict(robot);
+  }
+
+  /**
+   * Constructor with a sigma per pair.
+   * @param qKey key of the stacked joint angle vector
+   * @param robot query point model, spanning every joint involved
+   * @param pairs query point pairs to check
+   * @param radii radius of each query point, one per point of the model
+   * @param sigmas cost function sigma of each pair, one per pair
+   */
+  SelfCollisionSphereFactor(gtsam::Key qKey,
+                      const std::shared_ptr<const RobotQueryPoints> &robot,
+                      const SelfCollisionPairs &pairs,
+                      const gtsam::Vector &radii, const gtsam::Vector &sigmas)
+      : Base(gtsam::noiseModel::Diagonal::Sigmas(sigmas), qKey),
+        radii_(radii),
+        pairs_(pairs) {
+    if (static_cast<size_t>(sigmas.size()) != pairs.size()) {
+      throw std::invalid_argument(
+          "SelfCollisionSphereFactor: sigmas must have one entry per pair.");
+    }
+    validateAndRestrict(robot);
+  }
+
+  ~SelfCollisionSphereFactor() 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)));
+  }
+
+  /// Return the number of collision pairs.
+  size_t nrPairs() const { return pairs_.size(); }
+
+  /// Evaluate the hinge loss of every pair, and its Jacobian.
+  gtsam::Vector evaluateError(
+      const gtsam::Vector &q,
+      gtsam::OptionalMatrixType H1 = nullptr) const override;
+
+  /// Return the per query point radii.
+  const gtsam::Vector &radii() const { return radii_; }
+
+  /// Print contents.
+  void print(const std::string &s = "",
+             const gtsam::KeyFormatter &keyFormatter =
+                 gtsam::DefaultKeyFormatter) const override {
+    std::cout << s << "SelfCollisionSphereFactor with " << pairs_.size()
+              << " pairs" << std::endl;
+    Base::print("", keyFormatter);
+  }
+};  // \class SelfCollisionSphereFactor
+
+}  // namespace gtdynamics