From 3b0cd7adb4487c3c7bccd911d6fe8ce4a53ed38b Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Sat, 25 Jul 2026 18:04:24 -0400 Subject: [PATCH 1/4] Add self collision: SelfCollisionSphereFactor and its GP variant Builds on the obstacle avoidance layer to add self collision: a hinge loss cost between pairs of query points (own file, split out of ObstacleCost since it needs no field), a unary factor over a set of query point pairs with per-point sphere radii, and a GP-interpolated variant checking collisions between support states without adding trajectory variables for them. Validates against same-link and duplicate-point pairs and falls back to a finite direction at coincident points instead of a NaN Jacobian. Co-Authored-By: Claude Sonnet 5 --- gtdynamics.i | 52 +++ .../factors/SelfCollisionSphereFactor.cpp | 84 +++++ .../factors/SelfCollisionSphereFactor.h | 158 +++++++++ .../factors/SelfCollisionSphereFactorGP.cpp | 58 ++++ .../factors/SelfCollisionSphereFactorGP.h | 166 +++++++++ gtdynamics/gpmp2/SelfCollisionCost.cpp | 44 +++ gtdynamics/gpmp2/SelfCollisionCost.h | 44 +++ .../tests/testSelfCollisionSphereFactor.cpp | 326 ++++++++++++++++++ .../tests/testSelfCollisionSphereFactorGP.cpp | 244 +++++++++++++ python/gtdynamics/preamble/gtdynamics.h | 1 + .../gtdynamics/specializations/gtdynamics.h | 1 + python/tests/test_self_collision.py | 119 +++++++ 12 files changed, 1297 insertions(+) create mode 100644 gtdynamics/factors/SelfCollisionSphereFactor.cpp create mode 100644 gtdynamics/factors/SelfCollisionSphereFactor.h create mode 100644 gtdynamics/factors/SelfCollisionSphereFactorGP.cpp create mode 100644 gtdynamics/factors/SelfCollisionSphereFactorGP.h create mode 100644 gtdynamics/gpmp2/SelfCollisionCost.cpp create mode 100644 gtdynamics/gpmp2/SelfCollisionCost.h create mode 100644 gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp create mode 100644 gtdynamics/gpmp2/tests/testSelfCollisionSphereFactorGP.cpp create mode 100644 python/tests/test_self_collision.py diff --git a/gtdynamics.i b/gtdynamics.i index 8168a52d5..9571c710a 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -1143,6 +1143,58 @@ class ObstacleSDFFactorGP : gtsam::NoiseModelFactor { gtdynamics::GTDKeyFormatter); }; +#include +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 +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 string GtdFormat(const gtsam::Values &t, const string &s = ""); diff --git a/gtdynamics/factors/SelfCollisionSphereFactor.cpp b/gtdynamics/factors/SelfCollisionSphereFactor.cpp new file mode 100644 index 000000000..b335e37f7 --- /dev/null +++ b/gtdynamics/factors/SelfCollisionSphereFactor.cpp @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + * 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 + +#include +#include +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +void validateSelfCollisionPairs(const RobotQueryPoints &robot, + const SelfCollisionPairs &pairs, + const gtsam::Vector &radii, + const std::string &factorName) { + if (static_cast(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."); + } + } +} + +/* ************************************************************************* */ +gtsam::Vector SelfCollisionSphereFactor::evaluateError( + const gtsam::Vector &q, gtsam::OptionalMatrixType H1) const { + const size_t nrPairs = pairs_.size(); + gtsam::Vector err(nrPairs); + + std::vector wPts; + std::vector 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 diff --git a/gtdynamics/factors/SelfCollisionSphereFactor.h b/gtdynamics/factors/SelfCollisionSphereFactor.h new file mode 100644 index 000000000..eb04ef988 --- /dev/null +++ b/gtdynamics/factors/SelfCollisionSphereFactor.h @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------------- + * 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 +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +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; + +/** + * Reject inconsistent indices, radii or standoffs, shared by every self + * collision factor. factorName prefixes the error messages. + */ +void validateSelfCollisionPairs(const RobotQueryPoints &robot, + const SelfCollisionPairs &pairs, + const gtsam::Vector &radii, + const std::string &factorName); + +/** + * 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 SelfCollisionSphereFactor + : public gtsam::NoiseModelFactorN { + private: + using This = SelfCollisionSphereFactor; + using Base = gtsam::NoiseModelFactorN; + + std::shared_ptr robot_; + gtsam::Vector radii_; ///< one radius per query point, zero if unspecified + SelfCollisionPairs pairs_; + + /// Reject a null model and inconsistent indices, radii or standoffs. + void validate() const { + if (!robot_) { + throw std::invalid_argument( + "SelfCollisionSphereFactor: robot must not be null."); + } + validateSelfCollisionPairs(*robot_, pairs_, radii_, + "SelfCollisionSphereFactor"); + } + + 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 &robot, + const SelfCollisionPairs &pairs, + const gtsam::Vector &radii, double costSigma) + : Base(gtsam::noiseModel::Isotropic::Sigma(pairs.size(), costSigma), + qKey), + robot_(robot), + radii_(radii), + pairs_(pairs) { + validate(); + } + + /** + * 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 &robot, + const SelfCollisionPairs &pairs, + const gtsam::Vector &radii, const gtsam::Vector &sigmas) + : Base(gtsam::noiseModel::Diagonal::Sigmas(sigmas), qKey), + robot_(robot), + radii_(radii), + pairs_(pairs) { + if (static_cast(sigmas.size()) != pairs.size()) { + throw std::invalid_argument( + "SelfCollisionSphereFactor: sigmas must have one entry per pair."); + } + validate(); + } + + ~SelfCollisionSphereFactor() override {} + + /// Return a deep copy of this factor. + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + 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 diff --git a/gtdynamics/factors/SelfCollisionSphereFactorGP.cpp b/gtdynamics/factors/SelfCollisionSphereFactorGP.cpp new file mode 100644 index 000000000..7a186d4b4 --- /dev/null +++ b/gtdynamics/factors/SelfCollisionSphereFactorGP.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SelfCollisionSphereFactorGP.cpp + * @brief Self collision cost factor at a Gaussian process interpolated state. + * @author Karthik Shaji + */ + +#include + +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +gtsam::Vector SelfCollisionSphereFactorGP::evaluateError( + const gtsam::Vector &q1, const gtsam::Vector &v1, const gtsam::Vector &q2, + const gtsam::Vector &v2, gtsam::OptionalMatrixType H1, + gtsam::OptionalMatrixType H2, gtsam::OptionalMatrixType H3, + gtsam::OptionalMatrixType H4) const { + const bool computeJacobians = (H1 || H2 || H3 || H4); + const size_t nrPairs = pairs_.size(); + + const gtsam::Vector q = interpolator_.interpolatePose(q1, v1, q2, v2); + + std::vector wPts; + std::vector ptJacobians; + robot_->queryPoints(q, &wPts, computeJacobians ? &ptJacobians : nullptr); + + gtsam::Vector err(nrPairs); + gtsam::Matrix errJacobian = 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 (computeJacobians) { + gtsam::Matrix13 HptA, HptB; + err(r) = hingeLossSelfCollisionCost(wPts[pair.a], wPts[pair.b], eps, + HptA, HptB); + errJacobian.row(r) = + HptA * ptJacobians[pair.a] + HptB * ptJacobians[pair.b]; + } else { + err(r) = hingeLossSelfCollisionCost(wPts[pair.a], wPts[pair.b], eps); + } + } + + if (computeJacobians) { + interpolator_.updatePoseJacobians(errJacobian, H1, H2, H3, H4); + } + return err; +} + +} // namespace gtdynamics diff --git a/gtdynamics/factors/SelfCollisionSphereFactorGP.h b/gtdynamics/factors/SelfCollisionSphereFactorGP.h new file mode 100644 index 000000000..3d288557a --- /dev/null +++ b/gtdynamics/factors/SelfCollisionSphereFactorGP.h @@ -0,0 +1,166 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SelfCollisionSphereFactorGP.h + * @brief Self collision cost factor at a Gaussian process interpolated state. + * @author Karthik Shaji + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace gtdynamics { + +/** + * Self collision cost evaluated at a state interpolated between two support + * states, so that self collisions can be checked between the states of a + * trajectory without adding variables for them. The pairs follow the same + * semantics and restrictions as SelfCollisionSphereFactor. The interpolation is + * only the posterior mean of the Gaussian process prior if a GPLinearPrior with + * the same QcModel and deltaT connects the same two support states in the + * graph. + */ +class SelfCollisionSphereFactorGP + : public gtsam::NoiseModelFactorN { + private: + using This = SelfCollisionSphereFactorGP; + using Base = gtsam::NoiseModelFactorN; + + std::shared_ptr robot_; + gtsam::Vector radii_; ///< one radius per query point, zero if unspecified + SelfCollisionPairs pairs_; + GPLinearInterpolator interpolator_; + + /// Reject a null model and inconsistent indices, radii or standoffs. + void validate() const { + if (!robot_) { + throw std::invalid_argument( + "SelfCollisionSphereFactorGP: robot must not be null."); + } + validateSelfCollisionPairs(*robot_, pairs_, radii_, + "SelfCollisionSphereFactorGP"); + } + + public: + /** + * Constructor with a single sigma across every pair. + * @param qKey1 key of the joint angles of the first support state + * @param vKey1 key of the joint velocities of the first support state + * @param qKey2 key of the joint angles of the second support state + * @param vKey2 key of the joint velocities of the second support state + * @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 + * @param QcModel Gaussian noise model whose covariance is Qc + * @param deltaT time between the two support states + * @param tau time from the first support state to the interpolated state + */ + SelfCollisionSphereFactorGP(gtsam::Key qKey1, gtsam::Key vKey1, + gtsam::Key qKey2, gtsam::Key vKey2, + const std::shared_ptr &robot, + const SelfCollisionPairs &pairs, + const gtsam::Vector &radii, double costSigma, + const gtsam::SharedNoiseModel &QcModel, + double deltaT, double tau) + : Base(gtsam::noiseModel::Isotropic::Sigma(pairs.size(), costSigma), + qKey1, vKey1, qKey2, vKey2), + robot_(robot), + radii_(radii), + pairs_(pairs), + interpolator_(QcModel, deltaT, tau) { + // deltaT and tau are checked by the interpolator constructor. + validate(); + } + + /** + * Constructor with a sigma per pair. + * @param qKey1 key of the joint angles of the first support state + * @param vKey1 key of the joint velocities of the first support state + * @param qKey2 key of the joint angles of the second support state + * @param vKey2 key of the joint velocities of the second support state + * @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 + * @param QcModel Gaussian noise model whose covariance is Qc + * @param deltaT time between the two support states + * @param tau time from the first support state to the interpolated state + */ + SelfCollisionSphereFactorGP(gtsam::Key qKey1, gtsam::Key vKey1, + gtsam::Key qKey2, gtsam::Key vKey2, + const std::shared_ptr &robot, + const SelfCollisionPairs &pairs, + const gtsam::Vector &radii, + const gtsam::Vector &sigmas, + const gtsam::SharedNoiseModel &QcModel, + double deltaT, double tau) + : Base(gtsam::noiseModel::Diagonal::Sigmas(sigmas), qKey1, vKey1, + qKey2, vKey2), + robot_(robot), + radii_(radii), + pairs_(pairs), + interpolator_(QcModel, deltaT, tau) { + if (static_cast(sigmas.size()) != pairs.size()) { + throw std::invalid_argument( + "SelfCollisionSphereFactorGP: sigmas must have one entry per pair."); + } + validate(); + } + + ~SelfCollisionSphereFactorGP() override {} + + /// Return a deep copy of this factor. + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + 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 at the interpolated state, and its + /// Jacobians with respect to the support states. + gtsam::Vector evaluateError( + const gtsam::Vector &q1, const gtsam::Vector &v1, const gtsam::Vector &q2, + const gtsam::Vector &v2, gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr, + gtsam::OptionalMatrixType H3 = nullptr, + gtsam::OptionalMatrixType H4 = 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 << "SelfCollisionSphereFactorGP with " << pairs_.size() + << " pairs" << std::endl; + Base::print("", keyFormatter); + } +}; // \class SelfCollisionSphereFactorGP + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/SelfCollisionCost.cpp b/gtdynamics/gpmp2/SelfCollisionCost.cpp new file mode 100644 index 000000000..65830fed7 --- /dev/null +++ b/gtdynamics/gpmp2/SelfCollisionCost.cpp @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SelfCollisionCost.cpp + * @brief Hinge loss self collision cost between two world frame points. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +double hingeLossSelfCollisionCost(const gtsam::Point3 &pA, + const gtsam::Point3 &pB, double epsilon, + gtsam::OptionalJacobian<1, 3> HptA, + gtsam::OptionalJacobian<1, 3> HptB) { + // Below this separation the direction between the points is numerically + // meaningless (and NaN at exact overlap), so fall back to a fixed one. + constexpr double kMinDist = 1e-9; + + const gtsam::Vector3 diff = pA - pB; + const double dist = diff.norm(); + + if (dist > epsilon) { + if (HptA) *HptA = gtsam::Matrix13::Zero(); + if (HptB) *HptB = gtsam::Matrix13::Zero(); + return 0.0; + } + // Closer than epsilon: cost falls as the points separate. + const gtsam::Matrix13 direction = + dist < kMinDist ? gtsam::Matrix13(gtsam::Vector3::UnitX().transpose()) + : gtsam::Matrix13(diff.transpose() / dist); + if (HptA) *HptA = -direction; + if (HptB) *HptB = direction; + return epsilon - dist; +} + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/SelfCollisionCost.h b/gtdynamics/gpmp2/SelfCollisionCost.h new file mode 100644 index 000000000..465d8a3ce --- /dev/null +++ b/gtdynamics/gpmp2/SelfCollisionCost.h @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SelfCollisionCost.h + * @brief Hinge loss self collision cost between two world frame points. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtdynamics { + +/** + * Hinge loss self collision cost between two world frame points. The cost is + * epsilon - dist whenever the two points are within epsilon of each other, and + * zero beyond it, so epsilon is the standoff kept between them. Since there is + * no field, epsilon must fold in both points' radii plus any margin. + * + * Near-coincident points fall back to a fixed separation direction, so the + * Jacobian stays finite and still pushes the pair apart. + * + * @param pA first query point, in the world frame + * @param pB second query point, in the world frame + * @param epsilon standoff distance at which the cost becomes non-zero + * @param HptA optional Jacobian of the cost with respect to pA + * @param HptB optional Jacobian of the cost with respect to pB + * @return the hinge loss cost + */ +double hingeLossSelfCollisionCost(const gtsam::Point3 &pA, + const gtsam::Point3 &pB, double epsilon, + gtsam::OptionalJacobian<1, 3> HptA = {}, + gtsam::OptionalJacobian<1, 3> HptB = {}); + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp b/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp new file mode 100644 index 000000000..ff8104b3c --- /dev/null +++ b/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp @@ -0,0 +1,326 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testSelfCollisionSphereFactor.cpp + * @brief test the self collision hinge primitive and factor on bar_lab. + * @author Karthik Shaji + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace gtdynamics; +using gtsam::assert_equal; +using gtsam::Matrix; +using gtsam::Matrix13; +using gtsam::Point3; +using gtsam::Values; +using gtsam::Vector; +using gtsam::Vector3; +using gtsam::noiseModel::Isotropic; +using gtsam::symbol_shorthand::X; + +static const Robot kRobot = + CreateRobotFromFile(kUrdfPath + std::string("bar_lab.urdf")); + +/* ************************ point-to-point primitive ******************** */ + +// Beyond epsilon the cost is zero; within it, epsilon - dist with Jacobians +// pointing along the separation. Probed at a fixed distance away from the knee. +TEST(SelfCollisionCost, hingeAndJacobian) { + const Point3 pA(1.0, 0.5, 0.2), pB(1.3, 0.5, 0.2); // 0.3 apart + const double eps = 0.5; + + EXPECT_DOUBLES_EQUAL(eps - 0.3, hingeLossSelfCollisionCost(pA, pB, eps), + 1e-9); + // Beyond epsilon: no cost, zero Jacobians. + Matrix13 Hfar; + EXPECT_DOUBLES_EQUAL( + 0.0, hingeLossSelfCollisionCost(pA, Point3(3.0, 0.5, 0.2), eps, Hfar), + 1e-9); + EXPECT(assert_equal(Matrix(Matrix13::Zero()), Matrix(Hfar), 1e-9)); + + Matrix13 HptA, HptB; + hingeLossSelfCollisionCost(pA, pB, eps, HptA, HptB); + std::function fa = [&](const Point3 &p) { + return hingeLossSelfCollisionCost(p, pB, eps); + }; + std::function fb = [&](const Point3 &p) { + return hingeLossSelfCollisionCost(pA, p, eps); + }; + EXPECT(assert_equal( + Matrix(gtsam::numericalDerivative11(fa, pA)), + Matrix(HptA), 1e-5)); + EXPECT(assert_equal( + Matrix(gtsam::numericalDerivative11(fb, pB)), + Matrix(HptB), 1e-5)); + + // Coincident points: full standoff cost, finite fixed-direction Jacobian. + Matrix13 HcA, HcB; + EXPECT_DOUBLES_EQUAL(eps, hingeLossSelfCollisionCost(pA, pA, eps, HcA, HcB), + 1e-9); + EXPECT(HcA.allFinite()); + EXPECT(HcB.allFinite()); + EXPECT(assert_equal(Matrix(-HcB), Matrix(HcA), 1e-9)); +} + +/* ************************ bar_lab 18-DOF model ************************ */ + +// All eighteen joints, robot1's nine then robot2's nine. +static std::vector bothArmJoints() { + const std::vector names = { + "bridge1_joint_EA_X", "robot1_joint_EA_Y", "robot1_joint_EA_Z", + "robot1_joint_1", "robot1_joint_2", "robot1_joint_3", + "robot1_joint_4", "robot1_joint_5", "robot1_joint_6", + "bridge2_joint_EA_X", "robot2_joint_EA_Y", "robot2_joint_EA_Z", + "robot2_joint_1", "robot2_joint_2", "robot2_joint_3", + "robot2_joint_4", "robot2_joint_5", "robot2_joint_6"}; + std::vector joints; + for (auto &&name : names) joints.push_back(kRobot.joint(name)); + return joints; +} + +// Query points: two wrists (cross-arm pairs), robot1's forearm (same-arm +// pairs), and the two arm bases (reliably close for the push-apart test). +static std::vector queryPoints() { + return {PointOnLink(kRobot.link("robot1_link_6"), Point3(0, 0, 0)), + PointOnLink(kRobot.link("robot2_link_6"), Point3(0, 0, 0)), + PointOnLink(kRobot.link("robot1_link_4"), Point3(0, 0, 0)), + PointOnLink(kRobot.link("robot1_base"), Point3(0, 0, 0)), + PointOnLink(kRobot.link("robot2_base"), Point3(0, 0, 0))}; +} + +static const Vector kArm = + (Vector(6) << 0.2, -0.5, -1.0, 0.3, 0.5, 0.2).finished(); + +// Bridges far apart on the rail. +static Vector configApart() { + Vector q(18); + q << 2.0, 3.0, 1.0, kArm, 9.0, 3.0, 1.0, kArm; + return q; +} +// Bridges 0.2 m apart, so the two arm bases nearly coincide. +static Vector configClose() { + Vector q(18); + q << 5.0, 3.0, 1.0, kArm, 5.2, 3.0, 1.0, kArm; + return q; +} + +/* ************************ factor Jacobians *************************** */ + +// Point-to-point across the two arms. Epsilon is set a metre past the measured +// wrist distance so the hinge is active and the numerical check exercises it. +TEST(SelfCollisionSphereFactor, pointPointJacobians) { + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), queryPoints()); + const Vector q = configApart(); + std::vector wPts; + model->queryPoints(q, &wPts); + const double eps = (wPts[0] - wPts[1]).norm() + 1.0; + + const std::vector pairs = { + SelfCollisionPair(0, 1, eps)}; // wrist1 vs wrist2 + SelfCollisionSphereFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); + + EXPECT(factor.evaluateError(q)(0) > 0.0); // active branch + + Values values; + values.insert(X(0), q); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +// Several pairs in one factor, one cross-arm and one within robot1's arm, so a +// single row couples both arms' DOFs and another only robot1's. +TEST(SelfCollisionSphereFactor, multiplePairs) { + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), queryPoints()); + const Vector q = configApart(); + std::vector wPts; + model->queryPoints(q, &wPts); + + const std::vector pairs = { + SelfCollisionPair(0, 1, (wPts[0] - wPts[1]).norm() + 1.0), // cross-arm + SelfCollisionPair(2, 0, (wPts[2] - wPts[0]).norm() + 1.0)}; // same arm + SelfCollisionSphereFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); + + const Vector err = factor.evaluateError(q); + EXPECT_LONGS_EQUAL(2, err.size()); + EXPECT(err(0) > 0.0); // both branches active + EXPECT(err(1) > 0.0); + + Values values; + values.insert(X(0), q); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************ behavioral push-apart ********************** */ + +// The two arm bases start within epsilon; the factor drives them apart. Epsilon +// is 0.3 m past the measured start distance so the pair starts in collision. +TEST(SelfCollisionSphereFactor, pushesPointsApart) { + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), queryPoints()); + + const Vector q0 = configClose(); + std::vector startPts; + model->queryPoints(q0, &startPts); + const double startDistance = (startPts[3] - startPts[4]).norm(); + const double eps = startDistance + 0.3; + + const std::vector pairs = { + SelfCollisionPair(3, 4, eps)}; // base1 vs base2 + SelfCollisionSphereFactor factor(X(0), model, pairs, Vector::Zero(5), 0.01); + + gtsam::NonlinearFactorGraph graph; + graph.add(factor); + // A weak prior keeps the rest of the configuration near its start. + graph.addPrior(X(0), q0, Isotropic::Sigma(18, 0.5)); + + Values init; + init.insert(X(0), q0); + const Values result = + gtsam::LevenbergMarquardtOptimizer(graph, init).optimize(); + + std::vector pts; + model->queryPoints(result.at(X(0)), &pts); + // The weak prior pulls back slightly, so equilibrium sits just under eps. + EXPECT((pts[3] - pts[4]).norm() > eps - 0.05); + EXPECT((pts[3] - pts[4]).norm() > startDistance + 0.15); // clearly separated +} + +/* ************************ per-point radii *************************** */ + +// eps is set 0.05 below the measured distance so the points are clear, then two +// 0.1 radii push eps + rA + rB past dist, turning the pair into a collision. +TEST(SelfCollisionSphereFactor, radiiPointPoint) { + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), queryPoints()); + const Vector q = configApart(); + std::vector wPts; + model->queryPoints(q, &wPts); + const double dist = (wPts[0] - wPts[1]).norm(); + + const double eps = dist - 0.05; + const std::vector pairs = { + SelfCollisionPair(0, 1, eps)}; + + SelfCollisionSphereFactor clear(X(0), model, pairs, Vector::Zero(5), 0.1); + EXPECT_DOUBLES_EQUAL(0.0, clear.evaluateError(q)(0), 1e-9); + + Vector radii = Vector::Zero(5); + radii(0) = 0.1; + radii(1) = 0.1; + SelfCollisionSphereFactor inflated(X(0), model, pairs, radii, 0.1); + EXPECT_DOUBLES_EQUAL(eps + 0.2 - dist, inflated.evaluateError(q)(0), 1e-9); + + // The same total radius on either point gives the same standoff. + Vector radiiOnA = Vector::Zero(5), radiiOnB = Vector::Zero(5); + radiiOnA(0) = 0.2; + radiiOnB(1) = 0.2; + SelfCollisionSphereFactor factorA(X(0), model, pairs, radiiOnA, 0.1); + SelfCollisionSphereFactor factorB(X(0), model, pairs, radiiOnB, 0.1); + EXPECT_DOUBLES_EQUAL(factorA.evaluateError(q)(0), factorB.evaluateError(q)(0), + 1e-9); + + // A radius on an uninvolved point is ignored. + Vector other = Vector::Zero(5); + other(2) = 0.2; + SelfCollisionSphereFactor factorOther(X(0), model, pairs, other, 0.1); + EXPECT_DOUBLES_EQUAL(0.0, factorOther.evaluateError(q)(0), 1e-9); +} + +// In the active branch a radius adds directly to the cost, once per sphere. +TEST(SelfCollisionSphereFactor, radiiAddToTheStandoff) { + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), queryPoints()); + const Vector q = configApart(); + std::vector wPts; + model->queryPoints(q, &wPts); + + // Active by construction: eps sits a metre past the measured distance. + const double eps = (wPts[0] - wPts[1]).norm() + 1.0; + const std::vector pairs = {SelfCollisionPair(0, 1, eps)}; + + SelfCollisionSphereFactor base(X(0), model, pairs, Vector::Zero(5), 0.1); + const double err0 = base.evaluateError(q)(0); + EXPECT(err0 > 0.0); + + // Each sphere's radius adds to the standoff, so the cost rises by their sum. + Vector radii = Vector::Zero(5); + radii(0) = 0.1; + radii(1) = 0.2; + SelfCollisionSphereFactor inflated(X(0), model, pairs, radii, 0.1); + EXPECT_DOUBLES_EQUAL(err0 + 0.3, inflated.evaluateError(q)(0), 1e-9); + + // A radius on an uninvolved point is ignored. + Vector other = Vector::Zero(5); + other(2) = 0.2; + SelfCollisionSphereFactor factorOther(X(0), model, pairs, other, 0.1); + EXPECT_DOUBLES_EQUAL(err0, factorOther.evaluateError(q)(0), 1e-9); +} + +/* ************************ input validation *************************** */ + +TEST(SelfCollisionSphereFactor, rejectsBadInput) { + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), queryPoints()); + const std::vector pairs = { + SelfCollisionPair(0, 1, 1.0)}; + + // the model must not be null. + CHECK_EXCEPTION( + SelfCollisionSphereFactor(X(0), nullptr, pairs, Vector::Zero(5), 0.1), + std::invalid_argument); + // radii length must equal the number of query points. + CHECK_EXCEPTION( + SelfCollisionSphereFactor(X(0), model, pairs, Vector::Zero(3), 0.1), + std::invalid_argument); + // a point index out of range. + const std::vector bad = { + SelfCollisionPair(0, 99, 1.0)}; + CHECK_EXCEPTION( + SelfCollisionSphereFactor(X(0), model, bad, Vector::Zero(5), 0.1), + std::invalid_argument); +} + +// A pair whose two spheres sit on the same rigid link has a constant separation +// and no gradient, so it is rejected. +TEST(SelfCollisionSphereFactor, rejectsSameLinkPair) { + const LinkSharedPtr link = kRobot.link("robot1_link_6"); + const std::vector points = { + PointOnLink(link, Point3(0.0, 0.0, 0.0)), + PointOnLink(link, Point3(0.1, 0.0, 0.0))}; // both on link_6 + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), points); + const std::vector pairs = {SelfCollisionPair(0, 1, 0.1)}; + + CHECK_EXCEPTION( + SelfCollisionSphereFactor(X(0), model, pairs, Vector::Zero(2), 0.1), + std::invalid_argument); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactorGP.cpp b/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactorGP.cpp new file mode 100644 index 000000000..9b655c234 --- /dev/null +++ b/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactorGP.cpp @@ -0,0 +1,244 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testSelfCollisionSphereFactorGP.cpp + * @brief test the self collision factor at a GP interpolated state on bar_lab. + * @author Karthik Shaji + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace gtdynamics; +using gtsam::assert_equal; +using gtsam::Point3; +using gtsam::Values; +using gtsam::Vector; +using gtsam::noiseModel::Isotropic; +using gtsam::symbol_shorthand::V; +using gtsam::symbol_shorthand::X; + +static const Robot kRobot = + CreateRobotFromFile(kUrdfPath + std::string("bar_lab.urdf")); + +// All eighteen joints, robot1's nine then robot2's nine. +static std::vector bothArmJoints() { + const std::vector names = { + "bridge1_joint_EA_X", "robot1_joint_EA_Y", "robot1_joint_EA_Z", + "robot1_joint_1", "robot1_joint_2", "robot1_joint_3", + "robot1_joint_4", "robot1_joint_5", "robot1_joint_6", + "bridge2_joint_EA_X", "robot2_joint_EA_Y", "robot2_joint_EA_Z", + "robot2_joint_1", "robot2_joint_2", "robot2_joint_3", + "robot2_joint_4", "robot2_joint_5", "robot2_joint_6"}; + std::vector joints; + for (auto &&name : names) joints.push_back(kRobot.joint(name)); + return joints; +} + +// Query points: the two wrists, for a cross-arm pair. +static std::vector wristPoints() { + return {PointOnLink(kRobot.link("robot1_link_6"), Point3(0, 0, 0)), + PointOnLink(kRobot.link("robot2_link_6"), Point3(0, 0, 0))}; +} + +static const Vector kArm = + (Vector(6) << 0.2, -0.5, -1.0, 0.3, 0.5, 0.2).finished(); + +// Bridges far apart on the rail. +static Vector supportConfig1() { + Vector q(18); + q << 2.0, 3.0, 1.0, kArm, 9.0, 3.0, 1.0, kArm; + return q; +} +// The bridges have moved toward each other. +static Vector supportConfig2() { + Vector q(18); + q << 3.0, 3.0, 1.0, kArm, 8.0, 3.0, 1.0, kArm; + return q; +} +// Constant velocity consistent with the two support states over deltaT = 0.5. +static Vector supportVelocity() { + Vector v = Vector::Zero(18); + v(0) = 2.0; + v(9) = -2.0; + return v; +} + +/* ************************ endpoint agreement ************************** */ + +// At tau = 0 the interpolation reproduces the first support state exactly, so +// the interpolated factor must agree with the unary factor at q1; likewise at +// tau = deltaT with q2. +TEST(SelfCollisionSphereFactorGP, agreesWithUnaryFactorAtEndpoints) { + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), wristPoints()); + const size_t dof = model->dof(); + + const Vector q1 = supportConfig1(), q2 = supportConfig2(); + const Vector v1 = supportVelocity(), v2 = supportVelocity(); + const double deltaT = 0.5, costSigma = 0.1; + + std::vector wPts; + model->queryPoints(q1, &wPts); + // Active by construction: eps sits a metre past the measured distance. + const double eps = (wPts[0] - wPts[1]).norm() + 1.0; + + const std::vector pairs = {SelfCollisionPair(0, 1, eps)}; + const Vector radii = Vector::Zero(2); + SelfCollisionSphereFactor unary(X(0), model, pairs, radii, costSigma); + + SelfCollisionSphereFactorGP atStart(X(0), V(0), X(1), V(1), model, pairs, + radii, costSigma, + Isotropic::Sigma(dof, 1.0), deltaT, + 0.0); + EXPECT(assert_equal(unary.evaluateError(q1), + atStart.evaluateError(q1, v1, q2, v2), 1e-9)); + + SelfCollisionSphereFactorGP atEnd(X(0), V(0), X(1), V(1), model, pairs, + radii, costSigma, + Isotropic::Sigma(dof, 1.0), deltaT, + deltaT); + EXPECT(assert_equal(unary.evaluateError(q2), + atEnd.evaluateError(q1, v1, q2, v2), 1e-9)); +} + +/* ************************ interpolated Jacobians ********************** */ + +// At a tau strictly between the support states, with the hinge active, the +// analytic Jacobians w.r.t. all four support state variables must match the +// numerical ones. +TEST(SelfCollisionSphereFactorGP, interpolatedJacobians) { + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), wristPoints()); + const size_t dof = model->dof(); + + const Vector q1 = supportConfig1(), q2 = supportConfig2(); + const Vector v1 = supportVelocity(), v2 = supportVelocity(); + const double deltaT = 0.5, tau = 0.3 * deltaT; + + // Standoff a metre past the wrist distance at the interpolated state, so the + // active branch is the one the numerical check exercises. + GPLinearInterpolator interp(Isotropic::Sigma(dof, 1.0), deltaT, tau); + std::vector wPts; + model->queryPoints(interp.interpolatePose(q1, v1, q2, v2), &wPts); + const double eps = (wPts[0] - wPts[1]).norm() + 1.0; + + const std::vector pairs = {SelfCollisionPair(0, 1, eps)}; + SelfCollisionSphereFactorGP factor(X(0), V(0), X(1), V(1), model, pairs, + Vector::Zero(2), 0.1, + Isotropic::Sigma(dof, 1.0), deltaT, tau); + + EXPECT(factor.evaluateError(q1, v1, q2, v2)(0) > 0.0); // active branch + + Values values; + values.insert(X(0), q1); + values.insert(V(0), v1); + values.insert(X(1), q2); + values.insert(V(1), v2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************ per-point radii ***************************** */ + +// The radii fold into the standoff at the interpolated state exactly as they +// do in the unary factor: each sphere's radius adds to the active-branch cost. +TEST(SelfCollisionSphereFactorGP, radiiAddToTheStandoff) { + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), wristPoints()); + const size_t dof = model->dof(); + + const Vector q1 = supportConfig1(), q2 = supportConfig2(); + const Vector v1 = supportVelocity(), v2 = supportVelocity(); + const double deltaT = 0.5, tau = 0.3 * deltaT; + + GPLinearInterpolator interp(Isotropic::Sigma(dof, 1.0), deltaT, tau); + std::vector wPts; + model->queryPoints(interp.interpolatePose(q1, v1, q2, v2), &wPts); + // Active by construction: eps sits a metre past the measured distance. + const double eps = (wPts[0] - wPts[1]).norm() + 1.0; + + const std::vector pairs = {SelfCollisionPair(0, 1, eps)}; + SelfCollisionSphereFactorGP base(X(0), V(0), X(1), V(1), model, pairs, + Vector::Zero(2), 0.1, + Isotropic::Sigma(dof, 1.0), deltaT, tau); + const double err0 = base.evaluateError(q1, v1, q2, v2)(0); + EXPECT(err0 > 0.0); + + Vector radii(2); + radii << 0.1, 0.2; + SelfCollisionSphereFactorGP inflated(X(0), V(0), X(1), V(1), model, pairs, + radii, 0.1, Isotropic::Sigma(dof, 1.0), + deltaT, tau); + EXPECT_DOUBLES_EQUAL(err0 + 0.3, inflated.evaluateError(q1, v1, q2, v2)(0), + 1e-9); +} + +/* ************************ input validation **************************** */ + +TEST(SelfCollisionSphereFactorGP, rejectsBadInput) { + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), wristPoints()); + const size_t dof = model->dof(); + const auto QcModel = Isotropic::Sigma(dof, 1.0); + const double deltaT = 0.5, tau = 0.1; + + // radii length must equal the number of query points. + const std::vector pairs = {SelfCollisionPair(0, 1, 1.0)}; + CHECK_EXCEPTION( + SelfCollisionSphereFactorGP(X(0), V(0), X(1), V(1), model, pairs, + Vector::Zero(3), 0.1, QcModel, deltaT, tau), + std::invalid_argument); + // a point index out of range. + const std::vector bad = {SelfCollisionPair(0, 99, 1.0)}; + CHECK_EXCEPTION( + SelfCollisionSphereFactorGP(X(0), V(0), X(1), V(1), model, bad, + Vector::Zero(2), 0.1, QcModel, deltaT, tau), + std::invalid_argument); + // sigmas must have one entry per pair. + CHECK_EXCEPTION( + SelfCollisionSphereFactorGP(X(0), V(0), X(1), V(1), model, pairs, + Vector::Zero(2), Vector::Ones(3), QcModel, + deltaT, tau), + std::invalid_argument); +} + +// A pair whose two spheres sit on the same rigid link has a constant separation +// and no gradient, so it is rejected here just as in the unary factor. +TEST(SelfCollisionSphereFactorGP, rejectsSameLinkPair) { + const LinkSharedPtr link = kRobot.link("robot1_link_6"); + const std::vector points = { + PointOnLink(link, Point3(0.0, 0.0, 0.0)), + PointOnLink(link, Point3(0.1, 0.0, 0.0))}; // both on link_6 + const auto model = std::make_shared( + kRobot, "columns", bothArmJoints(), points); + const std::vector pairs = {SelfCollisionPair(0, 1, 0.1)}; + + CHECK_EXCEPTION( + SelfCollisionSphereFactorGP(X(0), V(0), X(1), V(1), model, pairs, + Vector::Zero(2), 0.1, + Isotropic::Sigma(model->dof(), 1.0), 0.5, 0.1), + std::invalid_argument); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/python/gtdynamics/preamble/gtdynamics.h b/python/gtdynamics/preamble/gtdynamics.h index 79caefd0a..18e43126f 100644 --- a/python/gtdynamics/preamble/gtdynamics.h +++ b/python/gtdynamics/preamble/gtdynamics.h @@ -1,4 +1,5 @@ // Ensure these STL aliases are treated as opaque container types so the // bind_vector/bind_map specializations behave predictably in Python. PYBIND11_MAKE_OPAQUE(gtdynamics::PointOnLinks); +PYBIND11_MAKE_OPAQUE(gtdynamics::SelfCollisionPairs); PYBIND11_MAKE_OPAQUE(gtdynamics::ContactPointGoals); diff --git a/python/gtdynamics/specializations/gtdynamics.h b/python/gtdynamics/specializations/gtdynamics.h index 539dcd785..f92371245 100644 --- a/python/gtdynamics/specializations/gtdynamics.h +++ b/python/gtdynamics/specializations/gtdynamics.h @@ -1,4 +1,5 @@ // Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html // These are required to save one copy operation on Python calls py::bind_vector(m_, "PointOnLinks"); +py::bind_vector(m_, "SelfCollisionPairs"); py::bind_map(m_, "ContactPointGoals"); diff --git a/python/tests/test_self_collision.py b/python/tests/test_self_collision.py new file mode 100644 index 000000000..f835a0a8a --- /dev/null +++ b/python/tests/test_self_collision.py @@ -0,0 +1,119 @@ +""" +GTDynamics Copyright 2020, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +See LICENSE for the license information + +Self collision on the bar_lab platform: one factor over the full 18-DOF state +that keeps the two arms clear of each other. Mirrors +gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp. +Author: Karthik Shaji +""" + +import unittest +# pylint: disable=no-name-in-module, import-error, no-member +from pathlib import Path + +import gtsam +import numpy as np +from gtsam.symbol_shorthand import X +from gtsam.utils.test_case import GtsamTestCase + +import gtdynamics as gtd + +# The eighteen joints of both arms, robot1's nine then robot2's nine. +BOTH_ARM_JOINTS = [ + "bridge1_joint_EA_X", "robot1_joint_EA_Y", "robot1_joint_EA_Z", + "robot1_joint_1", "robot1_joint_2", "robot1_joint_3", "robot1_joint_4", + "robot1_joint_5", "robot1_joint_6", + "bridge2_joint_EA_X", "robot2_joint_EA_Y", "robot2_joint_EA_Z", + "robot2_joint_1", "robot2_joint_2", "robot2_joint_3", "robot2_joint_4", + "robot2_joint_5", "robot2_joint_6" +] + +ARM = [0.2, -0.5, -1.0, 0.3, 0.5, 0.2] +# Bridges 0.2 m apart on the rail, so the two arm bases nearly coincide. +Q_CLOSE = np.array([5.0, 3.0, 1.0] + ARM + [5.2, 3.0, 1.0] + ARM) +EPSILON = 0.4 + + +class TestSelfCollision(GtsamTestCase): + """One 18-DOF factor keeping the two arm bases apart.""" + + def setUp(self): + self.robot = gtd.CreateRobotFromFile( + str(Path(gtd.URDF_PATH) / "bar_lab.urdf")) + joints = [self.robot.joint(name) for name in BOTH_ARM_JOINTS] + + # A query point on each arm's base; these move with the gantry only, so + # the pair is reliably close when the bridges are. + points = gtd.PointOnLinks() + points.append( + gtd.PointOnLink(self.robot.link("robot1_base"), np.zeros(3))) + points.append( + gtd.PointOnLink(self.robot.link("robot2_base"), np.zeros(3))) + + self.model = gtd.RobotQueryPoints(self.robot, "columns", joints, points) + + def make_factor(self, sigma=0.01): + pairs = gtd.SelfCollisionPairs() + pairs.append(gtd.SelfCollisionPair(0, 1, EPSILON)) + return gtd.SelfCollisionSphereFactor(X(0), self.model, pairs, + np.zeros(2), sigma) + + def separation(self, q): + pts = self.model.worldPoints(q) + return np.linalg.norm(pts[:, 0] - pts[:, 1]) + + def test_construct(self): + """The factor reports one pair over the 18-DOF state.""" + factor = self.make_factor() + self.assertEqual(factor.nrPairs(), 1) + self.assertEqual(self.model.dof(), 18) + + def test_pushes_arms_apart(self): + """The bases start within epsilon; the factor drives them apart.""" + self.assertLess(self.separation(Q_CLOSE), EPSILON) # in collision + + graph = gtsam.NonlinearFactorGraph() + graph.add(self.make_factor()) + # A weak prior keeps the rest of the configuration near its start. + graph.add( + gtsam.PriorFactorVector(X(0), Q_CLOSE, + gtsam.noiseModel.Isotropic.Sigma(18, 0.5))) + + init = gtsam.Values() + init.insert(X(0), Q_CLOSE) + result = gtsam.LevenbergMarquardtOptimizer(graph, init).optimize() + + self.assertGreater(self.separation(result.atVector(X(0))), + EPSILON - 0.02) + + def test_radii_inflate_the_points(self): + """A clear pair becomes a collision once the points carry a radius.""" + d = self.separation(Q_CLOSE) + eps = d - 0.05 # bases clear by 0.05 without any radius + pairs = gtd.SelfCollisionPairs() + pairs.append(gtd.SelfCollisionPair(0, 1, eps)) + + values = gtsam.Values() + values.insert(X(0), Q_CLOSE) + clear = gtd.SelfCollisionSphereFactor(X(0), self.model, pairs, + np.zeros(2), 0.1) + inflated = gtd.SelfCollisionSphereFactor(X(0), self.model, pairs, + np.array([0.1, 0.1]), 0.1) + + self.assertAlmostEqual(clear.error(values), 0.0, places=9) + self.assertGreater(inflated.error(values), 0.0) + + def test_rejects_bad_radii(self): + """A radii vector of the wrong length is rejected.""" + pairs = gtd.SelfCollisionPairs() + pairs.append(gtd.SelfCollisionPair(0, 1, EPSILON)) + with self.assertRaises(ValueError): + gtd.SelfCollisionSphereFactor(X(0), self.model, pairs, np.zeros(3), + 0.01) + + +if __name__ == "__main__": + unittest.main() From 8f3ff536f4b0a3b13cb8022d1734e2c2459b31d5 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Sun, 26 Jul 2026 22:40:03 -0400 Subject: [PATCH 2/4] added gtsam_export --- gtdynamics/factors/SelfCollisionSphereFactor.h | 10 +++++----- gtdynamics/factors/SelfCollisionSphereFactorGP.h | 2 +- gtdynamics/gpmp2/SelfCollisionCost.h | 8 ++++---- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtdynamics/factors/SelfCollisionSphereFactor.h b/gtdynamics/factors/SelfCollisionSphereFactor.h index eb04ef988..09a4a2eab 100644 --- a/gtdynamics/factors/SelfCollisionSphereFactor.h +++ b/gtdynamics/factors/SelfCollisionSphereFactor.h @@ -47,10 +47,10 @@ using SelfCollisionPairs = std::vector; * Reject inconsistent indices, radii or standoffs, shared by every self * collision factor. factorName prefixes the error messages. */ -void validateSelfCollisionPairs(const RobotQueryPoints &robot, - const SelfCollisionPairs &pairs, - const gtsam::Vector &radii, - const std::string &factorName); +GTSAM_EXPORT void validateSelfCollisionPairs(const RobotQueryPoints &robot, + const SelfCollisionPairs &pairs, + const gtsam::Vector &radii, + const std::string &factorName); /** * Unary factor keeping the robot clear of itself over a set of query point @@ -62,7 +62,7 @@ void validateSelfCollisionPairs(const RobotQueryPoints &robot, * (constant separation). Coincident points fall back to a fixed separation * direction rather than a non-finite gradient. */ -class SelfCollisionSphereFactor +class GTSAM_EXPORT SelfCollisionSphereFactor : public gtsam::NoiseModelFactorN { private: using This = SelfCollisionSphereFactor; diff --git a/gtdynamics/factors/SelfCollisionSphereFactorGP.h b/gtdynamics/factors/SelfCollisionSphereFactorGP.h index 3d288557a..d4c8706a3 100644 --- a/gtdynamics/factors/SelfCollisionSphereFactorGP.h +++ b/gtdynamics/factors/SelfCollisionSphereFactorGP.h @@ -40,7 +40,7 @@ namespace gtdynamics { * the same QcModel and deltaT connects the same two support states in the * graph. */ -class SelfCollisionSphereFactorGP +class GTSAM_EXPORT SelfCollisionSphereFactorGP : public gtsam::NoiseModelFactorN { private: diff --git a/gtdynamics/gpmp2/SelfCollisionCost.h b/gtdynamics/gpmp2/SelfCollisionCost.h index 465d8a3ce..e242c79ac 100644 --- a/gtdynamics/gpmp2/SelfCollisionCost.h +++ b/gtdynamics/gpmp2/SelfCollisionCost.h @@ -36,9 +36,9 @@ namespace gtdynamics { * @param HptB optional Jacobian of the cost with respect to pB * @return the hinge loss cost */ -double hingeLossSelfCollisionCost(const gtsam::Point3 &pA, - const gtsam::Point3 &pB, double epsilon, - gtsam::OptionalJacobian<1, 3> HptA = {}, - gtsam::OptionalJacobian<1, 3> HptB = {}); +GTSAM_EXPORT double hingeLossSelfCollisionCost( + const gtsam::Point3 &pA, const gtsam::Point3 &pB, double epsilon, + gtsam::OptionalJacobian<1, 3> HptA = {}, + gtsam::OptionalJacobian<1, 3> HptB = {}); } // namespace gtdynamics From e80277d675378e1633c53edbe3c9f0e369497d5d Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Mon, 27 Jul 2026 08:14:34 -0400 Subject: [PATCH 3/4] new querypoints api --- gtdynamics/gpmp2/RobotQueryPoints.cpp | 76 ++++++++++++++++++++++++--- gtdynamics/gpmp2/RobotQueryPoints.h | 19 ++++++- 2 files changed, 86 insertions(+), 9 deletions(-) diff --git a/gtdynamics/gpmp2/RobotQueryPoints.cpp b/gtdynamics/gpmp2/RobotQueryPoints.cpp index 1d7c85ae9..97b3f3942 100644 --- a/gtdynamics/gpmp2/RobotQueryPoints.cpp +++ b/gtdynamics/gpmp2/RobotQueryPoints.cpp @@ -13,6 +13,7 @@ #include +#include #include #include #include @@ -26,17 +27,17 @@ RobotQueryPoints::RobotQueryPoints(const Robot &robot, const std::vector &joints, const PointOnLinks &points, const gtsam::Pose3 &wTbase) - : wTbase_(wTbase), joints_(joints), points_(points) { + : wTbase_(wTbase), points_(points), dof_(joints.size()) { std::map jointColumn; - for (size_t i = 0; i < joints_.size(); ++i) { - if (!joints_[i]) { + for (size_t i = 0; i < joints.size(); ++i) { + if (!joints[i]) { throw std::invalid_argument( "RobotQueryPoints: joints must not be null."); } // A repeated joint would leave its earlier column of q unused. - if (!jointColumn.emplace(joints_[i]->id(), i).second) { + if (!jointColumn.emplace(joints[i]->id(), i).second) { throw std::invalid_argument( - "RobotQueryPoints: joint " + joints_[i]->name() + + "RobotQueryPoints: joint " + joints[i]->name() + " appears twice in the joint list."); } } @@ -63,8 +64,8 @@ RobotQueryPoints::RobotQueryPoints(const Robot &robot, } // Every listed joint must be traversed, or its column of q would be unused. - if (steps_.size() != joints_.size()) { - for (const auto &joint : joints_) { + if (steps_.size() != joints.size()) { + for (const auto &joint : joints) { bool used = false; for (const auto &step : steps_) used = used || step.joint == joint; if (!used) { @@ -94,6 +95,67 @@ RobotQueryPoints::RobotQueryPoints(const Robot &robot, nrLinks_ = linkSlot.size(); } +/* ************************************************************************* */ +RobotQueryPoints::RobotQueryPoints( + const std::shared_ptr &model, + const std::vector &pointIndices) { + if (!model) { + throw std::invalid_argument( + "RobotQueryPoints: parent model must not be null."); + } + wTbase_ = model->wTbase_; + dof_ = model->dof_; + + points_.reserve(pointIndices.size()); + std::vector globalSlots; + globalSlots.reserve(pointIndices.size()); + for (size_t idx : pointIndices) { + if (idx >= model->points_.size()) { + throw std::invalid_argument( + "RobotQueryPoints: point index out of range of the parent model."); + } + points_.push_back(model->points_[idx]); + globalSlots.push_back(model->pointSlots_[idx]); + } + + // A step is needed iff its child slot is needed; walking the parent's + // topologically ordered steps backward propagates that to every ancestor + // in a single pass. + std::vector needed(model->nrLinks_, false); + for (size_t slot : globalSlots) needed[slot] = true; + std::vector keepStep(model->steps_.size(), false); + for (size_t i = model->steps_.size(); i-- > 0;) { + const auto &step = model->steps_[i]; + if (needed[step.childSlot]) { + keepStep[i] = true; + needed[step.parentSlot] = true; + } + } + + // Compact the retained slots into 0..k-1, with the true base always slot 0. + // Forward order guarantees a step's parent is already assigned by the time + // its turn comes, since the parent slot is either 0 or an earlier step's + // child slot. + std::vector localOfGlobal(model->nrLinks_, + std::numeric_limits::max()); + localOfGlobal[0] = 0; + size_t nextLocal = 1; + for (size_t i = 0; i < model->steps_.size(); ++i) { + if (!keepStep[i]) continue; + const auto &step = model->steps_[i]; + if (localOfGlobal[step.childSlot] == std::numeric_limits::max()) { + localOfGlobal[step.childSlot] = nextLocal++; + } + steps_.push_back({step.joint, step.childLink, + localOfGlobal[step.parentSlot], + localOfGlobal[step.childSlot], step.qCol}); + } + nrLinks_ = nextLocal; + + pointSlots_.reserve(globalSlots.size()); + for (size_t slot : globalSlots) pointSlots_.push_back(localOfGlobal[slot]); +} + /* ************************************************************************* */ void RobotQueryPoints::computeForwardKinematics( const gtsam::Vector &q, std::vector *poses, diff --git a/gtdynamics/gpmp2/RobotQueryPoints.h b/gtdynamics/gpmp2/RobotQueryPoints.h index 32217739e..f877fd02a 100644 --- a/gtdynamics/gpmp2/RobotQueryPoints.h +++ b/gtdynamics/gpmp2/RobotQueryPoints.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -35,8 +36,8 @@ namespace gtdynamics { class GTSAM_EXPORT RobotQueryPoints { private: gtsam::Pose3 wTbase_; - std::vector joints_; PointOnLinks points_; + size_t dof_; ///< number of joints spanned by q /// Place the link at childSlot by applying q(qCol) to the parentSlot link. struct TraversalStep { @@ -69,8 +70,22 @@ class GTSAM_EXPORT RobotQueryPoints { const PointOnLinks &points, const gtsam::Pose3 &wTbase = gtsam::Pose3()); + /** + * Construct a model restricted to a subset of another model's query points, + * for factors that only reference a few of a larger model's points (e.g. + * sparse self collision pairs). Replays only the traversal steps needed to + * reach the requested points, instead of the whole tree. + * @param model the model to draw a subset of points from + * @param pointIndices indices into model->points(), in the order this + * model's own query points (and queryPoints/worldPoints output) will + * be returned + * @throw std::invalid_argument if model is null or an index is out of range + */ + RobotQueryPoints(const std::shared_ptr &model, + const std::vector &pointIndices); + /// Return the number of joints spanned by q. - size_t dof() const { return joints_.size(); } + size_t dof() const { return dof_; } /// Return the number of query points. size_t nrPoints() const { return points_.size(); } From 7f1577603d864cfe754408f82b28ccfa66352b76 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Mon, 27 Jul 2026 08:25:48 -0400 Subject: [PATCH 4/4] overloded querypoints api, with current intent for selfCollisoin --- .../factors/SelfCollisionSphereFactor.cpp | 32 +++++++++++++++++++ .../factors/SelfCollisionSphereFactor.h | 28 +++++++++++----- .../factors/SelfCollisionSphereFactorGP.h | 16 +++++----- 3 files changed, 60 insertions(+), 16 deletions(-) diff --git a/gtdynamics/factors/SelfCollisionSphereFactor.cpp b/gtdynamics/factors/SelfCollisionSphereFactor.cpp index b335e37f7..8a59ea9e8 100644 --- a/gtdynamics/factors/SelfCollisionSphereFactor.cpp +++ b/gtdynamics/factors/SelfCollisionSphereFactor.cpp @@ -13,6 +13,8 @@ #include +#include +#include #include #include #include @@ -54,6 +56,36 @@ void validateSelfCollisionPairs(const RobotQueryPoints &robot, } } +/* ************************************************************************* */ +std::shared_ptr restrictToReferencedPoints( + const std::shared_ptr &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 uniqueIndices; + std::map 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(robot, uniqueIndices); +} + /* ************************************************************************* */ gtsam::Vector SelfCollisionSphereFactor::evaluateError( const gtsam::Vector &q, gtsam::OptionalMatrixType H1) const { diff --git a/gtdynamics/factors/SelfCollisionSphereFactor.h b/gtdynamics/factors/SelfCollisionSphereFactor.h index 09a4a2eab..a83ca23a4 100644 --- a/gtdynamics/factors/SelfCollisionSphereFactor.h +++ b/gtdynamics/factors/SelfCollisionSphereFactor.h @@ -52,6 +52,17 @@ GTSAM_EXPORT void validateSelfCollisionPairs(const RobotQueryPoints &robot, 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 restrictToReferencedPoints( + const std::shared_ptr &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 @@ -72,14 +83,17 @@ class GTSAM_EXPORT SelfCollisionSphereFactor gtsam::Vector radii_; ///< one radius per query point, zero if unspecified SelfCollisionPairs pairs_; - /// Reject a null model and inconsistent indices, radii or standoffs. - void validate() const { - if (!robot_) { + /// 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 &robot) { + if (!robot) { throw std::invalid_argument( "SelfCollisionSphereFactor: robot must not be null."); } - validateSelfCollisionPairs(*robot_, pairs_, radii_, + validateSelfCollisionPairs(*robot, pairs_, radii_, "SelfCollisionSphereFactor"); + robot_ = restrictToReferencedPoints(robot, &pairs_, &radii_); } public: @@ -97,10 +111,9 @@ class GTSAM_EXPORT SelfCollisionSphereFactor const gtsam::Vector &radii, double costSigma) : Base(gtsam::noiseModel::Isotropic::Sigma(pairs.size(), costSigma), qKey), - robot_(robot), radii_(radii), pairs_(pairs) { - validate(); + validateAndRestrict(robot); } /** @@ -116,14 +129,13 @@ class GTSAM_EXPORT SelfCollisionSphereFactor const SelfCollisionPairs &pairs, const gtsam::Vector &radii, const gtsam::Vector &sigmas) : Base(gtsam::noiseModel::Diagonal::Sigmas(sigmas), qKey), - robot_(robot), radii_(radii), pairs_(pairs) { if (static_cast(sigmas.size()) != pairs.size()) { throw std::invalid_argument( "SelfCollisionSphereFactor: sigmas must have one entry per pair."); } - validate(); + validateAndRestrict(robot); } ~SelfCollisionSphereFactor() override {} diff --git a/gtdynamics/factors/SelfCollisionSphereFactorGP.h b/gtdynamics/factors/SelfCollisionSphereFactorGP.h index d4c8706a3..272422988 100644 --- a/gtdynamics/factors/SelfCollisionSphereFactorGP.h +++ b/gtdynamics/factors/SelfCollisionSphereFactorGP.h @@ -53,14 +53,16 @@ class GTSAM_EXPORT SelfCollisionSphereFactorGP SelfCollisionPairs pairs_; GPLinearInterpolator interpolator_; - /// Reject a null model and inconsistent indices, radii or standoffs. - void validate() const { - if (!robot_) { + /// Reject a null model or inconsistent indices/radii/standoffs, then + /// restrict robot_ to the points pairs_ references and remap pairs_/radii_. + void validateAndRestrict(const std::shared_ptr &robot) { + if (!robot) { throw std::invalid_argument( "SelfCollisionSphereFactorGP: robot must not be null."); } - validateSelfCollisionPairs(*robot_, pairs_, radii_, + validateSelfCollisionPairs(*robot, pairs_, radii_, "SelfCollisionSphereFactorGP"); + robot_ = restrictToReferencedPoints(robot, &pairs_, &radii_); } public: @@ -87,12 +89,11 @@ class GTSAM_EXPORT SelfCollisionSphereFactorGP double deltaT, double tau) : Base(gtsam::noiseModel::Isotropic::Sigma(pairs.size(), costSigma), qKey1, vKey1, qKey2, vKey2), - robot_(robot), radii_(radii), pairs_(pairs), interpolator_(QcModel, deltaT, tau) { // deltaT and tau are checked by the interpolator constructor. - validate(); + validateAndRestrict(robot); } /** @@ -119,7 +120,6 @@ class GTSAM_EXPORT SelfCollisionSphereFactorGP double deltaT, double tau) : Base(gtsam::noiseModel::Diagonal::Sigmas(sigmas), qKey1, vKey1, qKey2, vKey2), - robot_(robot), radii_(radii), pairs_(pairs), interpolator_(QcModel, deltaT, tau) { @@ -127,7 +127,7 @@ class GTSAM_EXPORT SelfCollisionSphereFactorGP throw std::invalid_argument( "SelfCollisionSphereFactorGP: sigmas must have one entry per pair."); } - validate(); + validateAndRestrict(robot); } ~SelfCollisionSphereFactorGP() override {}