From 5e96627a27be904406798460c875c94bc4608019 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Thu, 9 Jul 2026 11:03:05 -0400 Subject: [PATCH 01/27] starting work on gpmp2 --- gtdynamics/CMakeLists.txt | 2 +- gtdynamics/factors/GPLinearPriorFactor.h | 154 +++++++++++++++++++++++ gtdynamics/gpmp2/CMakeLists.txt | 3 + gtdynamics/gpmp2/GPutils.h | 71 +++++++++++ 4 files changed, 229 insertions(+), 1 deletion(-) create mode 100644 gtdynamics/factors/GPLinearPriorFactor.h create mode 100644 gtdynamics/gpmp2/CMakeLists.txt create mode 100644 gtdynamics/gpmp2/GPutils.h diff --git a/gtdynamics/CMakeLists.txt b/gtdynamics/CMakeLists.txt index c43cb15cf..ce29c3681 100644 --- a/gtdynamics/CMakeLists.txt +++ b/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) diff --git a/gtdynamics/factors/GPLinearPriorFactor.h b/gtdynamics/factors/GPLinearPriorFactor.h new file mode 100644 index 000000000..4bb01b0ac --- /dev/null +++ b/gtdynamics/factors/GPLinearPriorFactor.h @@ -0,0 +1,154 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file GPLinearPriorFactor.h + * @brief Linear Gaussian process prior factor, see Barfoot14rss. + * @author Karthik Shaji - Adapted from gpmp2 by Xinyan Yan and Jing Dong. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#include +#endif + +namespace gtdynamics { + +/** + * A 4-way GaussianProcess prior factor, linear version. + * Implemented similarly to that used in the GPMP2 paper. Each state consists of + * a pose and a velocity, separated by delta_t. The error is + * calcPhi(dof, delta_t) * x1 - x2, where x1 and x2 stack the pose and velocity + * of the first and second state respectively. + */ +class GaussianProcessPriorLinear + : public gtsam::NoiseModelFactorN { + private: + using This = GaussianProcessPriorLinear; + using Base = gtsam::NoiseModelFactorN; + + size_t dof_; + double delta_t_; + + public: + /// Default constructor, only for serialization. + GaussianProcessPriorLinear() {} + + /** + * Constructor from the keys of the two states. + * @param pose_key1 key for the pose of the first state + * @param vel_key1 key for the velocity of the first state + * @param pose_key2 key for the pose of the second state + * @param vel_key2 key for the velocity of the second state + * @param delta_t time between the two states + * @param Qc_model Gaussian noise model whose covariance is Qc + */ + GaussianProcessPriorLinear(gtsam::Key pose_key1, gtsam::Key vel_key1, + gtsam::Key pose_key2, gtsam::Key vel_key2, + double delta_t, + const gtsam::SharedNoiseModel &Qc_model) + : Base(gtsam::noiseModel::Gaussian::Covariance( + calcQ(getQc(Qc_model), delta_t)), + pose_key1, vel_key1, pose_key2, vel_key2), + dof_(Qc_model->dim()), + delta_t_(delta_t) {} + + ~GaussianProcessPriorLinear() 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))); + } + + /// Evaluate the Gaussian process prior error and its Jacobians. + gtsam::Vector evaluateError( + const gtsam::Vector &pose1, const gtsam::Vector &vel1, + const gtsam::Vector &pose2, const gtsam::Vector &vel2, + gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr, + gtsam::OptionalMatrixType H3 = nullptr, + gtsam::OptionalMatrixType H4 = nullptr) const override { + const gtsam::Matrix identity = gtsam::Matrix::Identity(dof_, dof_); + const gtsam::Matrix zero = gtsam::Matrix::Zero(dof_, dof_); + + // Stack pose and velocity into the two states. + gtsam::Vector x1(2 * dof_), x2(2 * dof_); + x1 << pose1, vel1; + x2 << pose2, vel2; + + if (H1) *H1 = (gtsam::Matrix(2 * dof_, dof_) << identity, zero).finished(); + if (H2) + *H2 = (gtsam::Matrix(2 * dof_, dof_) << delta_t_ * identity, identity) + .finished(); + if (H3) *H3 = (gtsam::Matrix(2 * dof_, dof_) << -identity, zero).finished(); + if (H4) *H4 = (gtsam::Matrix(2 * dof_, dof_) << zero, -identity).finished(); + + return calcPhi(dof_, delta_t_) * x1 - x2; + } + + /// 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 delta_t_; } + + /// Equality up to a tolerance. + bool equals(const gtsam::NonlinearFactor &expected, + double tol = 1e-9) const override { + const This *e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + std::fabs(this->delta_t_ - e->delta_t_) < 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, linear(" << dof_ + << ")" << std::endl; + Base::print("", keyFormatter); + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { // NOLINT + ar &boost::serialization::make_nvp( + "NoiseModelFactorN", boost::serialization::base_object(*this)); + ar &BOOST_SERIALIZATION_NVP(dof_); + ar &BOOST_SERIALIZATION_NVP(delta_t_); + } +#endif +}; // \class GaussianProcessPriorLinear + +} // namespace gtdynamics + +/// traits +namespace gtsam { +template <> +struct traits + : public Testable {}; +} // namespace gtsam diff --git a/gtdynamics/gpmp2/CMakeLists.txt b/gtdynamics/gpmp2/CMakeLists.txt new file mode 100644 index 000000000..177a09f0b --- /dev/null +++ b/gtdynamics/gpmp2/CMakeLists.txt @@ -0,0 +1,3 @@ +if(GTSAM_BUILD_TESTS AND EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/tests/CMakeLists.txt) + add_subdirectory(tests) +endif() diff --git a/gtdynamics/gpmp2/GPutils.h b/gtdynamics/gpmp2/GPutils.h new file mode 100644 index 000000000..7d1f4c08c --- /dev/null +++ b/gtdynamics/gpmp2/GPutils.h @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file GPutils.h + * @brief Gaussian process utilities: Qc, Q and Phi matrices. + * @author Karthik Shaji - Adapted from gpmp2 by Xinyan Yan and Jing Dong. + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + +namespace gtdynamics { + +/** + * @fn Extract the Qc covariance matrix from a Gaussian noise model. + * @param Qc_model Gaussian noise model whose covariance is Qc. + * @returns the n x n Qc covariance matrix. + */ +inline gtsam::Matrix getQc(const gtsam::SharedNoiseModel &Qc_model) { + auto gaussian = + std::dynamic_pointer_cast(Qc_model); + if (!gaussian) { + throw std::invalid_argument("getQc: Qc_model must be a Gaussian model."); + } + return gaussian->covariance(); +} + +/** + * @fn Compute the process covariance Q over a time interval. + * @param Qc n x n power spectral density matrix. + * @param tau time interval between the two states. + * @returns the 2n x 2n process covariance matrix. + */ +inline gtsam::Matrix calcQ(const gtsam::Matrix &Qc, double tau) { + assert(Qc.rows() == Qc.cols()); + const auto n = Qc.rows(); + return (gtsam::Matrix(2 * n, 2 * n) << // + std::pow(tau, 3.0) / 3.0 * Qc, std::pow(tau, 2.0) / 2.0 * Qc, // + std::pow(tau, 2.0) / 2.0 * Qc, tau * Qc) + .finished(); +} + +/** + * @fn Compute the state transition matrix Phi over a time interval. + * @param dof degrees of freedom of a single state. + * @param tau time interval between the two states. + * @returns the 2n x 2n state transition matrix. + */ +inline gtsam::Matrix calcPhi(size_t dof, double tau) { + const gtsam::Matrix identity = gtsam::Matrix::Identity(dof, dof); + const gtsam::Matrix zero = gtsam::Matrix::Zero(dof, dof); + return (gtsam::Matrix(2 * dof, 2 * dof) << // + identity, tau * identity, // + zero, identity) + .finished(); +} + +} // namespace gtdynamics From 8344e2ac66def84616d2c8aaa83b6567d3d3d781 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Thu, 9 Jul 2026 11:16:51 -0400 Subject: [PATCH 02/27] added more gp factors --- gtdynamics/factors/GPLiePriorFactor.h | 160 +++++++++++++++++++++++ gtdynamics/factors/GPLinearPriorFactor.h | 16 +-- gtdynamics/factors/GPPose3PriorFactor.h | 24 ++++ 3 files changed, 192 insertions(+), 8 deletions(-) create mode 100644 gtdynamics/factors/GPLiePriorFactor.h create mode 100644 gtdynamics/factors/GPPose3PriorFactor.h diff --git a/gtdynamics/factors/GPLiePriorFactor.h b/gtdynamics/factors/GPLiePriorFactor.h new file mode 100644 index 000000000..4a7175dd1 --- /dev/null +++ b/gtdynamics/factors/GPLiePriorFactor.h @@ -0,0 +1,160 @@ +/* ---------------------------------------------------------------------------- + * 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 +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#include +#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 delta_t. With + * r = Logmap(Inverse(pose1) * pose2), the error is + * [r - delta_t * vel1; vel2 - vel1]. + */ +template +class GPLiePrior + : public gtsam::NoiseModelFactorN { + private: + GTSAM_CONCEPT_ASSERT(gtsam::IsLieGroup); + + using This = GPLiePrior; + using Base = gtsam::NoiseModelFactorN; + + size_t dof_; + double delta_t_; + + public: + /// Default constructor, only for serialization. + GPLiePrior() {} + + /** + * Constructor from the keys of the two states. + * @param pose_key1 key for the pose of the first state + * @param vel_key1 key for the velocity of the first state + * @param pose_key2 key for the pose of the second state + * @param vel_key2 key for the velocity of the second state + * @param delta_t time between the two states + * @param Qc_model Gaussian noise model whose covariance is Qc + */ + GPLiePrior(gtsam::Key pose_key1, gtsam::Key vel_key1, gtsam::Key pose_key2, + gtsam::Key vel_key2, double delta_t, + const gtsam::SharedNoiseModel &Qc_model) + : Base(gtsam::noiseModel::Gaussian::Covariance( + calcQ(getQc(Qc_model), delta_t)), + pose_key1, vel_key1, pose_key2, vel_key2), + dof_(Qc_model->dim()), + delta_t_(delta_t) {} + + ~GPLiePrior() 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))); + } + + /// 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::Logmap( + gtsam::traits::Compose(gtsam::traits::Inverse(pose1, Hinv), + pose2, Hcomp1, Hcomp2), + Hlogmap); + } else { + r = gtsam::traits::Logmap( + gtsam::traits::Compose(gtsam::traits::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_) << -delta_t_ * 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 * delta_t_), (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 delta_t_; } + + /// Equality up to a tolerance. + bool equals(const gtsam::NonlinearFactor &expected, + double tol = 1e-9) const override { + const This *e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + std::fabs(this->delta_t_ - e->delta_t_) < 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 + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { // NOLINT + ar &boost::serialization::make_nvp( + "NoiseModelFactorN", boost::serialization::base_object(*this)); + ar &BOOST_SERIALIZATION_NVP(dof_); + ar &BOOST_SERIALIZATION_NVP(delta_t_); + } +#endif +}; // \class GPLiePrior + +} // namespace gtdynamics diff --git a/gtdynamics/factors/GPLinearPriorFactor.h b/gtdynamics/factors/GPLinearPriorFactor.h index 4bb01b0ac..60c14b70c 100644 --- a/gtdynamics/factors/GPLinearPriorFactor.h +++ b/gtdynamics/factors/GPLinearPriorFactor.h @@ -39,11 +39,11 @@ namespace gtdynamics { * calcPhi(dof, delta_t) * x1 - x2, where x1 and x2 stack the pose and velocity * of the first and second state respectively. */ -class GaussianProcessPriorLinear +class GPLinearPrior : public gtsam::NoiseModelFactorN { private: - using This = GaussianProcessPriorLinear; + using This = GPLinearPrior; using Base = gtsam::NoiseModelFactorN; @@ -52,7 +52,7 @@ class GaussianProcessPriorLinear public: /// Default constructor, only for serialization. - GaussianProcessPriorLinear() {} + GPLinearPrior() {} /** * Constructor from the keys of the two states. @@ -63,7 +63,7 @@ class GaussianProcessPriorLinear * @param delta_t time between the two states * @param Qc_model Gaussian noise model whose covariance is Qc */ - GaussianProcessPriorLinear(gtsam::Key pose_key1, gtsam::Key vel_key1, + GPLinearPrior(gtsam::Key pose_key1, gtsam::Key vel_key1, gtsam::Key pose_key2, gtsam::Key vel_key2, double delta_t, const gtsam::SharedNoiseModel &Qc_model) @@ -73,7 +73,7 @@ class GaussianProcessPriorLinear dof_(Qc_model->dim()), delta_t_(delta_t) {} - ~GaussianProcessPriorLinear() override {} + ~GPLinearPrior() override {} /// Return a deep copy of this factor. gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -142,13 +142,13 @@ class GaussianProcessPriorLinear ar &BOOST_SERIALIZATION_NVP(delta_t_); } #endif -}; // \class GaussianProcessPriorLinear +}; // \class GPLinearPrior } // namespace gtdynamics /// traits namespace gtsam { template <> -struct traits - : public Testable {}; +struct traits + : public Testable {}; } // namespace gtsam diff --git a/gtdynamics/factors/GPPose3PriorFactor.h b/gtdynamics/factors/GPPose3PriorFactor.h new file mode 100644 index 000000000..b08e2899b --- /dev/null +++ b/gtdynamics/factors/GPPose3PriorFactor.h @@ -0,0 +1,24 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file GPPose3PriorFactor.h + * @brief Gaussian process prior factor on Pose3. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#pragma once + +#include +#include + +namespace gtdynamics { + +/// Gaussian process prior factor on Pose3, with velocity in the tangent space. +using GPPose3Prior = GPLiePrior; + +} // namespace gtdynamics From 06c354706bf1c441444ce69421b9291b06f59602 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Thu, 9 Jul 2026 11:53:35 -0400 Subject: [PATCH 03/27] added interpolators --- gtdynamics/factors/GPLiePriorFactor.h | 12 +- gtdynamics/gpmp2/GPLieInterpolator.h | 217 ++++++++++++++++++++++++ gtdynamics/gpmp2/GPLinearInterpolator.h | 170 +++++++++++++++++++ gtdynamics/gpmp2/GPPose3Interpolator.h | 25 +++ gtdynamics/gpmp2/GPutils.h | 45 +++++ 5 files changed, 468 insertions(+), 1 deletion(-) create mode 100644 gtdynamics/gpmp2/GPLieInterpolator.h create mode 100644 gtdynamics/gpmp2/GPLinearInterpolator.h create mode 100644 gtdynamics/gpmp2/GPPose3Interpolator.h diff --git a/gtdynamics/factors/GPLiePriorFactor.h b/gtdynamics/factors/GPLiePriorFactor.h index 4a7175dd1..14f0fa5be 100644 --- a/gtdynamics/factors/GPLiePriorFactor.h +++ b/gtdynamics/factors/GPLiePriorFactor.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION @@ -72,7 +73,16 @@ class GPLiePrior calcQ(getQc(Qc_model), delta_t)), pose_key1, vel_key1, pose_key2, vel_key2), dof_(Qc_model->dim()), - delta_t_(delta_t) {} + delta_t_(delta_t) { + // 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::dimension != Eigen::Dynamic && + dof_ != static_cast(gtsam::traits::dimension)) { + throw std::invalid_argument( + "GPLiePrior: Qc_model dimension must equal the tangent dimension of " + "the Lie group."); + } + } ~GPLiePrior() override {} diff --git a/gtdynamics/gpmp2/GPLieInterpolator.h b/gtdynamics/gpmp2/GPLieInterpolator.h new file mode 100644 index 000000000..8144dba6f --- /dev/null +++ b/gtdynamics/gpmp2/GPLieInterpolator.h @@ -0,0 +1,217 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file GPLieInterpolator.h + * @brief Gaussian process interpolator on any Lie group. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#include +#endif + +namespace gtdynamics { + +/** + * Gaussian process interpolator on any Lie group T. Given the two support + * states (pose1, vel1) and (pose2, vel2) separated by delta_t, interpolates the + * pose and velocity at time tau after the first support state. + */ +template +class GPLieInterpolator { + private: + GTSAM_CONCEPT_ASSERT(gtsam::IsLieGroup); + + using This = GPLieInterpolator; + + size_t dof_; + double delta_t_; ///< time between the two support states + double tau_; ///< time from the first support state + + gtsam::Matrix Qc_; + gtsam::Matrix Lambda_; + gtsam::Matrix Psi_; + + public: + /// Default constructor, only for serialization. + GPLieInterpolator() {} + + /** + * Constructor. + * @param Qc_model Gaussian noise model whose covariance is Qc. Its dimension + * must equal the tangent dimension of T, e.g. 6 for Pose3. + * @param delta_t time between the two support states + * @param tau time from the first support state to the interpolated state + */ + GPLieInterpolator(const gtsam::SharedNoiseModel &Qc_model, double delta_t, + double tau) + : dof_(Qc_model->dim()), delta_t_(delta_t), tau_(tau) { + // A mismatched Qc dimension silently mis-slices Lambda and Psi, so reject + // it up front. Only checkable when T has a fixed dimension. + if (gtsam::traits::dimension != Eigen::Dynamic && + dof_ != static_cast(gtsam::traits::dimension)) { + throw std::invalid_argument( + "GPLieInterpolator: Qc_model dimension must equal the tangent " + "dimension of the Lie group."); + } + Qc_ = getQc(Qc_model); + Lambda_ = calcLambda(Qc_, delta_t_, tau_); + Psi_ = calcPsi(Qc_, delta_t_, tau_); + } + + ~GPLieInterpolator() {} + + /// Interpolate the pose at tau, with Jacobians w.r.t. the support states. + T interpolatePose(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 { + const bool use_H = (H1 || H2 || H3 || H4); + + gtsam::Vector r1(2 * dof_); + r1 << gtsam::Vector::Zero(dof_), vel1; + + // Relative increment between the two support poses, in the tangent space. + gtsam::Matrix Hinv, Hcomp11, Hcomp12, Hlogmap; + gtsam::Vector r; + if (use_H) { + r = gtsam::traits::Logmap( + gtsam::traits::Compose(gtsam::traits::Inverse(pose1, Hinv), + pose2, Hcomp11, Hcomp12), + Hlogmap); + } else { + r = gtsam::traits::Logmap( + gtsam::traits::Compose(gtsam::traits::Inverse(pose1), pose2)); + } + gtsam::Vector r2(2 * dof_); + r2 << r, vel2; + + const gtsam::Vector xi = Lambda_.block(0, 0, dof_, 2 * dof_) * r1 + + Psi_.block(0, 0, dof_, 2 * dof_) * r2; + + if (!use_H) { + return gtsam::traits::Compose(pose1, gtsam::traits::Expmap(xi)); + } + + gtsam::Matrix Hcomp21, Hcomp22, Hexp; + const T pose = gtsam::traits::Compose( + pose1, gtsam::traits::Expmap(xi, Hexp), Hcomp21, Hcomp22); + const gtsam::Matrix Hexpr1 = Hcomp22 * Hexp; + + if (H1) + *H1 = Hcomp21 + + Hexpr1 * Psi_.block(0, 0, dof_, dof_) * Hlogmap * Hcomp11 * Hinv; + if (H2) *H2 = Hexpr1 * Lambda_.block(0, dof_, dof_, dof_); + if (H3) *H3 = Hexpr1 * Psi_.block(0, 0, dof_, dof_) * Hlogmap * Hcomp12; + if (H4) *H4 = Hexpr1 * Psi_.block(0, dof_, dof_, dof_); + + return pose; + } + + /// Interpolate the velocity at tau, with Jacobians w.r.t. the support states. + gtsam::Vector interpolateVelocity( + 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 { + const bool use_H = (H1 || H2 || H3 || H4); + + gtsam::Vector r1(2 * dof_); + r1 << gtsam::Vector::Zero(dof_), vel1; + + gtsam::Matrix Hinv, Hcomp11, Hcomp12, Hlogmap; + gtsam::Vector r; + if (use_H) { + r = gtsam::traits::Logmap( + gtsam::traits::Compose(gtsam::traits::Inverse(pose1, Hinv), + pose2, Hcomp11, Hcomp12), + Hlogmap); + } else { + r = gtsam::traits::Logmap( + gtsam::traits::Compose(gtsam::traits::Inverse(pose1), pose2)); + } + gtsam::Vector r2(2 * dof_); + r2 << r, vel2; + + if (H1) *H1 = Psi_.block(dof_, 0, dof_, dof_) * Hlogmap * Hcomp11 * Hinv; + if (H2) *H2 = Lambda_.block(dof_, dof_, dof_, dof_); + if (H3) *H3 = Psi_.block(dof_, 0, dof_, dof_) * Hlogmap * Hcomp12; + if (H4) *H4 = Psi_.block(dof_, dof_, dof_, dof_); + + return Lambda_.block(dof_, 0, dof_, 2 * dof_) * r1 + + Psi_.block(dof_, 0, dof_, 2 * dof_) * r2; + } + + /// Chain the Jacobian of a cost at the interpolated pose back to the states. + static void updatePoseJacobians( + const gtsam::Matrix &Hpose, const gtsam::Matrix &Hint1, + const gtsam::Matrix &Hint2, const gtsam::Matrix &Hint3, + const gtsam::Matrix &Hint4, gtsam::OptionalMatrixType H1, + gtsam::OptionalMatrixType H2, gtsam::OptionalMatrixType H3, + gtsam::OptionalMatrixType H4) { + if (H1) *H1 = Hpose * Hint1; + if (H2) *H2 = Hpose * Hint2; + if (H3) *H3 = Hpose * Hint3; + if (H4) *H4 = Hpose * Hint4; + } + + /// Return the degrees of freedom of a single state. + size_t dim() const { return dof_; } + + /// Equality up to a tolerance. + bool equals(const This &expected, double tol = 1e-9) const { + return std::fabs(this->delta_t_ - expected.delta_t_) < tol && + std::fabs(this->tau_ - expected.tau_) < tol && + gtsam::equal_with_abs_tol(this->Qc_, expected.Qc_, tol) && + gtsam::equal_with_abs_tol(this->Lambda_, expected.Lambda_, tol) && + gtsam::equal_with_abs_tol(this->Psi_, expected.Psi_, tol); + } + + /// Print contents. + void print(const std::string &s = "") const { + std::cout << s << "GPLieInterpolator<" << dof_ << ">" << std::endl; + std::cout << "delta_t = " << delta_t_ << ", tau = " << tau_ << std::endl; + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { // NOLINT + using boost::serialization::make_array; + using boost::serialization::make_nvp; + ar &BOOST_SERIALIZATION_NVP(dof_); + ar &BOOST_SERIALIZATION_NVP(delta_t_); + ar &BOOST_SERIALIZATION_NVP(tau_); + ar &make_nvp("Qc", make_array(Qc_.data(), Qc_.size())); + ar &make_nvp("Lambda", make_array(Lambda_.data(), Lambda_.size())); + ar &make_nvp("Psi", make_array(Psi_.data(), Psi_.size())); + } +#endif +}; // \class GPLieInterpolator + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/GPLinearInterpolator.h b/gtdynamics/gpmp2/GPLinearInterpolator.h new file mode 100644 index 000000000..f1a72dcf4 --- /dev/null +++ b/gtdynamics/gpmp2/GPLinearInterpolator.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 GPLinearInterpolator.h + * @brief Gaussian process interpolator, linear version. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong and Xinyan Yan. + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#include +#endif + +namespace gtdynamics { + +/** + * Gaussian process interpolator, linear version. Given the two support states + * (pose1, vel1) and (pose2, vel2) separated by delta_t, interpolates the pose + * and velocity at time tau after the first support state. + */ +class GPLinearInterpolator { + private: + using This = GPLinearInterpolator; + + size_t dof_; + double delta_t_; ///< time between the two support states + double tau_; ///< time from the first support state + + gtsam::Matrix Qc_; + gtsam::Matrix Lambda_; + gtsam::Matrix Psi_; + + public: + /// Default constructor, only for serialization. + GPLinearInterpolator() {} + + /** + * Constructor. + * @param Qc_model Gaussian noise model whose covariance is Qc + * @param delta_t time between the two support states + * @param tau time from the first support state to the interpolated state + */ + GPLinearInterpolator(const gtsam::SharedNoiseModel &Qc_model, double delta_t, + double tau) + : dof_(Qc_model->dim()), delta_t_(delta_t), tau_(tau) { + Qc_ = getQc(Qc_model); + Lambda_ = calcLambda(Qc_, delta_t_, tau_); + Psi_ = calcPsi(Qc_, delta_t_, tau_); + } + + ~GPLinearInterpolator() {} + + /// Interpolate the pose at tau, with Jacobians w.r.t. the support states. + gtsam::Vector interpolatePose( + const gtsam::Vector &pose1, const gtsam::Vector &vel1, + const gtsam::Vector &pose2, const gtsam::Vector &vel2, + gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr, + gtsam::OptionalMatrixType H3 = nullptr, + gtsam::OptionalMatrixType H4 = nullptr) const { + gtsam::Vector x1(2 * dof_), x2(2 * dof_); + x1 << pose1, vel1; + x2 << pose2, vel2; + + if (H1) *H1 = Lambda_.block(0, 0, dof_, dof_); + if (H2) *H2 = Lambda_.block(0, dof_, dof_, dof_); + if (H3) *H3 = Psi_.block(0, 0, dof_, dof_); + if (H4) *H4 = Psi_.block(0, dof_, dof_, dof_); + + // Only the upper block of the interpolated state is needed. + return Lambda_.block(0, 0, dof_, 2 * dof_) * x1 + + Psi_.block(0, 0, dof_, 2 * dof_) * x2; + } + + /// Interpolate the velocity at tau, with Jacobians w.r.t. the support states. + gtsam::Vector interpolateVelocity( + const gtsam::Vector &pose1, const gtsam::Vector &vel1, + const gtsam::Vector &pose2, const gtsam::Vector &vel2, + gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr, + gtsam::OptionalMatrixType H3 = nullptr, + gtsam::OptionalMatrixType H4 = nullptr) const { + gtsam::Vector x1(2 * dof_), x2(2 * dof_); + x1 << pose1, vel1; + x2 << pose2, vel2; + + if (H1) *H1 = Lambda_.block(dof_, 0, dof_, dof_); + if (H2) *H2 = Lambda_.block(dof_, dof_, dof_, dof_); + if (H3) *H3 = Psi_.block(dof_, 0, dof_, dof_); + if (H4) *H4 = Psi_.block(dof_, dof_, dof_, dof_); + + // Only the lower block of the interpolated state is needed. + return Lambda_.block(dof_, 0, dof_, 2 * dof_) * x1 + + Psi_.block(dof_, 0, dof_, 2 * dof_) * x2; + } + + /// Chain the Jacobian of a cost at the interpolated pose back to the states. + static void updatePoseJacobians( + const gtsam::Matrix &Hpose, const gtsam::Matrix &Hint1, + const gtsam::Matrix &Hint2, const gtsam::Matrix &Hint3, + const gtsam::Matrix &Hint4, gtsam::OptionalMatrixType H1, + gtsam::OptionalMatrixType H2, gtsam::OptionalMatrixType H3, + gtsam::OptionalMatrixType H4) { + if (H1) *H1 = Hpose * Hint1; + if (H2) *H2 = Hpose * Hint2; + if (H3) *H3 = Hpose * Hint3; + if (H4) *H4 = Hpose * Hint4; + } + + /// Return the degrees of freedom of a single state. + size_t dim() const { return dof_; } + + /// Equality up to a tolerance. + bool equals(const This &expected, double tol = 1e-9) const { + return std::fabs(this->delta_t_ - expected.delta_t_) < tol && + std::fabs(this->tau_ - expected.tau_) < tol && + gtsam::equal_with_abs_tol(this->Qc_, expected.Qc_, tol) && + gtsam::equal_with_abs_tol(this->Lambda_, expected.Lambda_, tol) && + gtsam::equal_with_abs_tol(this->Psi_, expected.Psi_, tol); + } + + /// Print contents. + void print(const std::string &s = "") const { + std::cout << s << "GPLinearInterpolator(" << dof_ << ")" << std::endl; + std::cout << "delta_t = " << delta_t_ << ", tau = " << tau_ << std::endl; + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { // NOLINT + using boost::serialization::make_array; + using boost::serialization::make_nvp; + ar &BOOST_SERIALIZATION_NVP(dof_); + ar &BOOST_SERIALIZATION_NVP(delta_t_); + ar &BOOST_SERIALIZATION_NVP(tau_); + ar &make_nvp("Qc", make_array(Qc_.data(), Qc_.size())); + ar &make_nvp("Lambda", make_array(Lambda_.data(), Lambda_.size())); + ar &make_nvp("Psi", make_array(Psi_.data(), Psi_.size())); + } +#endif +}; // \class GPLinearInterpolator + +} // namespace gtdynamics + +/// traits +namespace gtsam { +template <> +struct traits + : public Testable {}; +} // namespace gtsam diff --git a/gtdynamics/gpmp2/GPPose3Interpolator.h b/gtdynamics/gpmp2/GPPose3Interpolator.h new file mode 100644 index 000000000..e1c7d62f0 --- /dev/null +++ b/gtdynamics/gpmp2/GPPose3Interpolator.h @@ -0,0 +1,25 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file GPPose3Interpolator.h + * @brief Gaussian process interpolator on Pose3. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#pragma once + +#include +#include + +namespace gtdynamics { + +/// Gaussian process interpolator on Pose3. Requires a 6-dimensional Qc_model, +/// which the GPLieInterpolator constructor enforces. +using GPPose3Interpolator = GPLieInterpolator; + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/GPutils.h b/gtdynamics/gpmp2/GPutils.h index 7d1f4c08c..9d69834fd 100644 --- a/gtdynamics/gpmp2/GPutils.h +++ b/gtdynamics/gpmp2/GPutils.h @@ -68,4 +68,49 @@ inline gtsam::Matrix calcPhi(size_t dof, double tau) { .finished(); } +/** + * @fn Compute the inverse of the process covariance Q over a time interval. + * @param Qc n x n power spectral density matrix. + * @param tau time interval between the two states. + * @returns the 2n x 2n inverse process covariance matrix. + */ +inline gtsam::Matrix calcQ_inv(const gtsam::Matrix &Qc, double tau) { + assert(Qc.rows() == Qc.cols()); + const auto n = Qc.rows(); + const gtsam::Matrix Qc_inv = Qc.inverse(); + return (gtsam::Matrix(2 * n, 2 * n) << // + 12.0 * std::pow(tau, -3.0) * Qc_inv, -6.0 * std::pow(tau, -2.0) * Qc_inv, // + -6.0 * std::pow(tau, -2.0) * Qc_inv, 4.0 * std::pow(tau, -1.0) * Qc_inv) + .finished(); +} + +/** + * @fn Compute the Lambda matrix used to interpolate at time tau. + * @param Qc n x n power spectral density matrix. + * @param delta_t time between the two support states. + * @param tau time from the first support state to the interpolated state. + * @returns the 2n x 2n Lambda matrix. + */ +inline gtsam::Matrix calcLambda(const gtsam::Matrix &Qc, double delta_t, + double tau) { + assert(Qc.rows() == Qc.cols()); + return calcPhi(Qc.rows(), tau) - + calcQ(Qc, tau) * calcPhi(Qc.rows(), delta_t - tau).transpose() * + calcQ_inv(Qc, delta_t) * calcPhi(Qc.rows(), delta_t); +} + +/** + * @fn Compute the Psi matrix used to interpolate at time tau. + * @param Qc n x n power spectral density matrix. + * @param delta_t time between the two support states. + * @param tau time from the first support state to the interpolated state. + * @returns the 2n x 2n Psi matrix. + */ +inline gtsam::Matrix calcPsi(const gtsam::Matrix &Qc, double delta_t, + double tau) { + assert(Qc.rows() == Qc.cols()); + return calcQ(Qc, tau) * calcPhi(Qc.rows(), delta_t - tau).transpose() * + calcQ_inv(Qc, delta_t); +} + } // namespace gtdynamics From 1d4dca314ee351b05973fddb4dc7c9c1beffa93e Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Thu, 9 Jul 2026 13:11:20 -0400 Subject: [PATCH 04/27] added more code for factor handling --- gtdynamics/factors/ObstacleSDFFactor.h | 108 ++++++++++ gtdynamics/factors/ObstacleSDFFactorGP.h | 141 +++++++++++++ gtdynamics/gpmp2/ObstacleCost.h | 94 +++++++++ gtdynamics/gpmp2/RobotQueryPoints.h | 165 +++++++++++++++ gtdynamics/gpmp2/SDFexception.h | 26 +++ gtdynamics/gpmp2/SignedDistanceField.h | 254 +++++++++++++++++++++++ 6 files changed, 788 insertions(+) create mode 100644 gtdynamics/factors/ObstacleSDFFactor.h create mode 100644 gtdynamics/factors/ObstacleSDFFactorGP.h create mode 100644 gtdynamics/gpmp2/ObstacleCost.h create mode 100644 gtdynamics/gpmp2/RobotQueryPoints.h create mode 100644 gtdynamics/gpmp2/SDFexception.h create mode 100644 gtdynamics/gpmp2/SignedDistanceField.h diff --git a/gtdynamics/factors/ObstacleSDFFactor.h b/gtdynamics/factors/ObstacleSDFFactor.h new file mode 100644 index 000000000..35923db34 --- /dev/null +++ b/gtdynamics/factors/ObstacleSDFFactor.h @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ObstacleSDFFactor.h + * @brief Obstacle avoidance cost factor on a signed distance field. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtdynamics { + +/** + * Unary factor keeping every query point on the robot at least epsilon away + * from the obstacles encoded in a world frame signed distance field. The error + * has one row per query point, each the hinge loss epsilon - d(wP). + */ +class ObstacleSDFFactor : public gtsam::NoiseModelFactorN { + private: + using This = ObstacleSDFFactor; + using Base = gtsam::NoiseModelFactorN; + + double epsilon_; + RobotQueryPoints robot_; + std::shared_ptr sdf_; + + public: + /** + * Constructor. + * @param q_key key of the stacked joint angle vector + * @param robot query point model of the robot + * @param sdf signed distance field of the obstacles, in the world frame + * @param cost_sigma cost function sigma, one per query point + * @param epsilon standoff distance kept from every obstacle + */ + ObstacleSDFFactor(gtsam::Key q_key, const RobotQueryPoints &robot, + const std::shared_ptr &sdf, + double cost_sigma, double epsilon) + : Base(gtsam::noiseModel::Isotropic::Sigma(robot.nrPoints(), cost_sigma), + q_key), + epsilon_(epsilon), + robot_(robot), + sdf_(sdf) {} + + ~ObstacleSDFFactor() 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))); + } + + /// Evaluate the hinge loss at every query point, and its Jacobian. + gtsam::Vector evaluateError( + const gtsam::Vector &q, + gtsam::OptionalMatrixType H1 = nullptr) const override { + const size_t m = robot_.nrPoints(); + gtsam::Vector err(m); + + std::vector wPs; + std::vector Jps; + robot_.queryPoints(q, &wPs, H1 ? &Jps : nullptr); + if (H1) *H1 = gtsam::Matrix::Zero(m, robot_.dof()); + + for (size_t i = 0; i < m; ++i) { + if (H1) { + gtsam::Matrix13 Herr_point; + err(i) = hingeLossObstacleCost(wPs[i], *sdf_, epsilon_, Herr_point); + H1->row(i) = Herr_point * Jps[i]; + } else { + err(i) = hingeLossObstacleCost(wPs[i], *sdf_, epsilon_); + } + } + return err; + } + + /// Return the standoff distance. + double epsilon() const { return epsilon_; } + + /// Print contents. + void print(const std::string &s = "", + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "ObstacleSDFFactor with " << robot_.nrPoints() + << " query points" << std::endl; + Base::print("", keyFormatter); + } +}; // \class ObstacleSDFFactor + +} // namespace gtdynamics diff --git a/gtdynamics/factors/ObstacleSDFFactorGP.h b/gtdynamics/factors/ObstacleSDFFactorGP.h new file mode 100644 index 000000000..b22a7c2c4 --- /dev/null +++ b/gtdynamics/factors/ObstacleSDFFactorGP.h @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ObstacleSDFFactorGP.h + * @brief Obstacle avoidance cost factor at a Gaussian process interpolated + * state, using a signed distance field. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtdynamics { + +/** + * Obstacle avoidance cost evaluated at a state interpolated between two support + * states, so that collisions can be checked between the states of a trajectory + * without adding variables for them. The interpolation is only the posterior + * mean of the Gaussian process prior if a GPLinearPrior with the same Qc_model + * and delta_t connects the same two support states in the graph. + */ +class ObstacleSDFFactorGP + : public gtsam::NoiseModelFactorN { + private: + using This = ObstacleSDFFactorGP; + using Base = gtsam::NoiseModelFactorN; + + double epsilon_; + RobotQueryPoints robot_; + std::shared_ptr sdf_; + GPLinearInterpolator GPbase_; + + public: + /** + * Constructor. + * @param q_key1 key of the joint angles of the first support state + * @param v_key1 key of the joint velocities of the first support state + * @param q_key2 key of the joint angles of the second support state + * @param v_key2 key of the joint velocities of the second support state + * @param robot query point model of the robot + * @param sdf signed distance field of the obstacles, in the world frame + * @param cost_sigma cost function sigma, one per query point + * @param epsilon standoff distance kept from every obstacle + * @param Qc_model Gaussian noise model whose covariance is Qc + * @param delta_t time between the two support states + * @param tau time from the first support state to the interpolated state + */ + ObstacleSDFFactorGP(gtsam::Key q_key1, gtsam::Key v_key1, gtsam::Key q_key2, + gtsam::Key v_key2, const RobotQueryPoints &robot, + const std::shared_ptr &sdf, + double cost_sigma, double epsilon, + const gtsam::SharedNoiseModel &Qc_model, double delta_t, + double tau) + : Base(gtsam::noiseModel::Isotropic::Sigma(robot.nrPoints(), cost_sigma), + q_key1, v_key1, q_key2, v_key2), + epsilon_(epsilon), + robot_(robot), + sdf_(sdf), + GPbase_(Qc_model, delta_t, tau) {} + + ~ObstacleSDFFactorGP() 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))); + } + + /// Evaluate the hinge loss at the interpolated state, and its Jacobians. + 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 { + const bool use_H = (H1 || H2 || H3 || H4); + const size_t m = robot_.nrPoints(); + + gtsam::Matrix Jq_q1, Jq_v1, Jq_q2, Jq_v2; + const gtsam::Vector q = + use_H ? GPbase_.interpolatePose(q1, v1, q2, v2, &Jq_q1, &Jq_v1, &Jq_q2, + &Jq_v2) + : GPbase_.interpolatePose(q1, v1, q2, v2); + + std::vector wPs; + std::vector Jps; + robot_.queryPoints(q, &wPs, use_H ? &Jps : nullptr); + + gtsam::Vector err(m); + gtsam::Matrix Jerr_q = gtsam::Matrix::Zero(m, robot_.dof()); + for (size_t i = 0; i < m; ++i) { + if (use_H) { + gtsam::Matrix13 Herr_point; + err(i) = hingeLossObstacleCost(wPs[i], *sdf_, epsilon_, Herr_point); + Jerr_q.row(i) = Herr_point * Jps[i]; + } else { + err(i) = hingeLossObstacleCost(wPs[i], *sdf_, epsilon_); + } + } + + if (use_H) { + GPLinearInterpolator::updatePoseJacobians(Jerr_q, Jq_q1, Jq_v1, Jq_q2, + Jq_v2, H1, H2, H3, H4); + } + return err; + } + + /// Return the standoff distance. + double epsilon() const { return epsilon_; } + + /// Print contents. + void print(const std::string &s = "", + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "ObstacleSDFFactorGP with " << robot_.nrPoints() + << " query points" << std::endl; + Base::print("", keyFormatter); + } +}; // \class ObstacleSDFFactorGP + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/ObstacleCost.h b/gtdynamics/gpmp2/ObstacleCost.h new file mode 100644 index 000000000..588aee8bc --- /dev/null +++ b/gtdynamics/gpmp2/ObstacleCost.h @@ -0,0 +1,94 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ObstacleCost.h + * @brief Hinge loss obstacle cost functions on a signed distance field. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +/** + * Hinge loss obstacle cost for a query point expressed in the same frame as the + * field. The cost is epsilon - d(point) whenever the signed distance d falls + * within epsilon of the surface, and zero beyond it, so epsilon is the standoff + * distance kept from every obstacle in the field. + * + * A point outside the grid is treated as free space, which means an undersized + * field fails open: pad every field by at least epsilon past its geometry. + * + * @param point query point, in the frame of the field + * @param sdf signed distance field + * @param epsilon standoff distance at which the cost becomes non-zero + * @param H_point optional Jacobian of the cost with respect to the point + * @return the hinge loss cost + */ +inline double hingeLossObstacleCost( + const gtsam::Point3 &point, const SignedDistanceField &sdf, double epsilon, + gtsam::OptionalJacobian<1, 3> H_point = {}) { + gtsam::Vector3 field_gradient; + double dist_signed; + try { + dist_signed = sdf.getSignedDistance(point, field_gradient); + } catch (const SDFQueryOutOfRange &) { + if (H_point) *H_point = gtsam::Matrix13::Zero(); + return 0.0; + } + + if (dist_signed > epsilon) { + if (H_point) *H_point = gtsam::Matrix13::Zero(); + return 0.0; + } + // Inside the obstacle, or outside it but closer than epsilon. + if (H_point) *H_point = -field_gradient.transpose(); + return epsilon - dist_signed; +} + +/** + * Hinge loss obstacle cost for a field rigidly attached to a moving frame s. + * The query point is given in the world frame and transformed into s before the + * field is read, so the same field serves as a world obstacle field (with sTw + * the identity), as the collision geometry of a moving obstacle, or as the + * collision geometry of a robot link for self collision. + * + * @param wTs pose of the field's frame s in the world frame + * @param point query point, in the world frame + * @param sdf signed distance field, expressed in frame s + * @param epsilon standoff distance at which the cost becomes non-zero + * @param H_pose optional Jacobian of the cost with respect to wTs + * @param H_point optional Jacobian of the cost with respect to the point + * @return the hinge loss cost + */ +inline double hingeLossObstacleCost( + const gtsam::Pose3 &wTs, const gtsam::Point3 &point, + const SignedDistanceField &sdf, double epsilon, + gtsam::OptionalJacobian<1, 6> H_pose = {}, + gtsam::OptionalJacobian<1, 3> H_point = {}) { + gtsam::Matrix36 Hlocal_pose; + gtsam::Matrix3 Hlocal_point; + const gtsam::Point3 sP = wTs.transformTo(point, Hlocal_pose, Hlocal_point); + + gtsam::Matrix13 Herr_local; + const double cost = hingeLossObstacleCost(sP, sdf, epsilon, Herr_local); + + if (H_pose) *H_pose = Herr_local * Hlocal_pose; + if (H_point) *H_point = Herr_local * Hlocal_point; + return cost; +} + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/RobotQueryPoints.h b/gtdynamics/gpmp2/RobotQueryPoints.h new file mode 100644 index 000000000..72eefd657 --- /dev/null +++ b/gtdynamics/gpmp2/RobotQueryPoints.h @@ -0,0 +1,165 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file RobotQueryPoints.h + * @brief Forward kinematics of query points on a robot, with Jacobians. + * @author Karthik Shaji + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +/** + * Maps a stacked joint angle vector q to the world positions of a fixed set of + * query points on the robot, together with their Jacobians with respect to q. + * This is the sphere free counterpart of gpmp2's robot model: a query point + * carries no radius, since the standoff distance lives in the obstacle cost. + * + * The joints given to the constructor define the ordering of q. Any joint of + * the robot left out of that list is not traversed, so the subtree beyond it is + * absent from the model; a query point on such a link raises an exception. + * Loops are not supported, only trees. + */ +class RobotQueryPoints { + private: + Robot robot_; + std::string base_link_name_; + gtsam::Pose3 wTbase_; + std::vector joints_; + std::vector points_; + std::map joint_column_; + + public: + /** + * Constructor. + * @param robot the robot model + * @param base_link_name the link the kinematic tree is rooted at + * @param joints the joints spanned by q, in the order q indexes them + * @param points the query points, each in its link's CoM frame + * @param wTbase pose of the base link in the world frame + */ + RobotQueryPoints(const Robot &robot, const std::string &base_link_name, + const std::vector &joints, + const std::vector &points, + const gtsam::Pose3 &wTbase = gtsam::Pose3()) + : robot_(robot), + base_link_name_(base_link_name), + wTbase_(wTbase), + joints_(joints), + points_(points) { + for (size_t i = 0; i < joints_.size(); ++i) { + joint_column_[joints_[i]->id()] = i; + } + } + + /// Return the number of joints spanned by q. + size_t dof() const { return joints_.size(); } + + /// Return the number of query points. + size_t nrPoints() const { return points_.size(); } + + /// Return the query points, each in its link's CoM frame. + const std::vector &points() const { return points_; } + + /** + * Forward kinematics over the tree rooted at the base link. + * @param q stacked joint angles, ordered as the constructor's joints + * @param wTl filled with the world pose of every reachable link, by link id + * @param Jl if non-null, filled with d(wTl)/dq, a 6 x dof matrix per link + */ + void forwardKinematics(const gtsam::Vector &q, + std::map *wTl, + std::map *Jl = nullptr) const { + if (static_cast(q.size()) != dof()) { + throw std::invalid_argument( + "RobotQueryPoints: q size must equal the number of joints."); + } + const LinkSharedPtr base = robot_.link(base_link_name_); + (*wTl)[base->id()] = wTbase_; + if (Jl) (*Jl)[base->id()] = gtsam::Matrix::Zero(6, dof()); + + std::set visited{base->id()}; + std::queue frontier; + frontier.push(base); + + while (!frontier.empty()) { + const LinkSharedPtr link = frontier.front(); + frontier.pop(); + for (auto &&joint : link->joints()) { + auto it = joint_column_.find(joint->id()); + if (it == joint_column_.end()) continue; // subtree excluded from q + const LinkSharedPtr other = joint->otherLink(link); + if (visited.count(other->id())) continue; + visited.insert(other->id()); + + const size_t col = it->second; + const gtsam::Pose3 &wTp = wTl->at(link->id()); + if (Jl) { + gtsam::Matrix6 H_wTp; + gtsam::Vector6 H_qj; + (*wTl)[other->id()] = joint->poseOf(other, wTp, q(col), H_wTp, H_qj); + gtsam::Matrix J = H_wTp * Jl->at(link->id()); + J.col(col) += H_qj; + (*Jl)[other->id()] = J; + } else { + (*wTl)[other->id()] = joint->poseOf(other, wTp, q(col)); + } + frontier.push(other); + } + } + } + + /** + * World positions of the query points. + * @param q stacked joint angles + * @param wPs filled with the world position of each query point + * @param J if non-null, filled with d(wP)/dq, a 3 x dof matrix per point + */ + void queryPoints(const gtsam::Vector &q, std::vector *wPs, + std::vector *J = nullptr) const { + std::map poses; + std::map link_jacobians; + forwardKinematics(q, &poses, J ? &link_jacobians : nullptr); + + wPs->resize(nrPoints()); + if (J) J->resize(nrPoints()); + for (size_t i = 0; i < nrPoints(); ++i) { + const uint8_t id = points_[i].link->id(); + auto it = poses.find(id); + if (it == poses.end()) { + throw std::runtime_error( + "RobotQueryPoints: query point on a link not reachable from the " + "base through the given joints."); + } + if (J) { + gtsam::Matrix36 H_pose; + (*wPs)[i] = it->second.transformFrom(points_[i].point, H_pose); + (*J)[i] = H_pose * link_jacobians.at(id); + } else { + (*wPs)[i] = it->second.transformFrom(points_[i].point); + } + } + } +}; // \class RobotQueryPoints + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/SDFexception.h b/gtdynamics/gpmp2/SDFexception.h new file mode 100644 index 000000000..53c32b14d --- /dev/null +++ b/gtdynamics/gpmp2/SDFexception.h @@ -0,0 +1,26 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SDFexception.h + * @brief Custom exceptions for signed distance fields. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#pragma once + +#include + +namespace gtdynamics { + +/// Thrown when a query point falls outside the signed distance field grid. +class SDFQueryOutOfRange : public std::runtime_error { + public: + SDFQueryOutOfRange() : std::runtime_error("Querying SDF out of range") {} +}; + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/SignedDistanceField.h b/gtdynamics/gpmp2/SignedDistanceField.h new file mode 100644 index 000000000..c4735c81b --- /dev/null +++ b/gtdynamics/gpmp2/SignedDistanceField.h @@ -0,0 +1,254 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SignedDistanceField.h + * @brief Signed distance field with trilinear interpolation. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong and Mustafa Mukadam. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +/** + * Signed distance field sampled on a uniform grid, queried by trilinear + * interpolation. The field is expressed in some frame s: to use it as a world + * obstacle field let s be the world frame, and to use it as the collision + * geometry of a rigid link let s be that link's CoM frame. + * + * The data is a vector of matrices, one per z layer, where each matrix is + * indexed as data[z](row, col) with row spanning y and col spanning x. A numpy + * array laid out as sdf[nx, ny, nz] therefore transposes into layer z as + * sdf[:, :, z].T -- passing sdf[:, :, z] directly swaps the x and y axes, which + * leaves the distances plausible but silently transposes the gradient. + */ +class SignedDistanceField { + public: + /// Fractional grid index of a point, in (row, col, z) = (y, x, z) order. + struct FloatIndex { + double row, col, z; + FloatIndex(double row, double col, double z) : row(row), col(col), z(z) {} + }; + + using shared_ptr = std::shared_ptr; + + private: + using This = SignedDistanceField; + + gtsam::Point3 origin_; + size_t field_rows_, field_cols_, field_z_; + double cell_size_; + std::vector data_; + + public: + /// Default constructor, only for serialization. + SignedDistanceField() : field_rows_(0), field_cols_(0), field_z_(0), + cell_size_(0.0) {} + + /** + * Constructor with all data. + * @param origin the (x, y, z) position of cell (0, 0, 0), in frame s + * @param cell_size the side length of a grid cell + * @param data one matrix per z layer, each indexed as (row = y, col = x) + */ + SignedDistanceField(const gtsam::Point3 &origin, double cell_size, + const std::vector &data) + : origin_(origin), + field_rows_(data.at(0).rows()), + field_cols_(data.at(0).cols()), + field_z_(data.size()), + cell_size_(cell_size), + data_(data) {} + + /// Constructor with no data, to be filled in later by initFieldData. + SignedDistanceField(const gtsam::Point3 &origin, double cell_size, + size_t field_rows, size_t field_cols, size_t field_z) + : origin_(origin), + field_rows_(field_rows), + field_cols_(field_cols), + field_z_(field_z), + cell_size_(cell_size), + data_(std::vector(field_z)) {} + + ~SignedDistanceField() {} + + /// Insert one z layer of the field, indexed as (row = y, col = x). + void initFieldData(size_t z_idx, const gtsam::Matrix &field_layer) { + if (z_idx >= field_z_) { + throw std::runtime_error( + "[SignedDistanceField] matrix layer out of index"); + } + data_[z_idx] = field_layer; + } + + /// Return the signed distance at a point expressed in frame s. + double getSignedDistance(const gtsam::Point3 &point) const { + return signedDistance(convertPoint3toCell(point)); + } + + /// Return the signed distance at a point, and its gradient in metric units. + double getSignedDistance(const gtsam::Point3 &point, + gtsam::Vector3 &g) const { + const FloatIndex pidx = convertPoint3toCell(point); + const gtsam::Vector3 g_idx = gradient(pidx); + // The gradient comes back in (row, col, z) order, so swap to (x, y, z). + g = gtsam::Vector3(g_idx(1), g_idx(0), g_idx(2)) / cell_size_; + return signedDistance(pidx); + } + + /// Convert a point in frame s to a fractional grid index. + FloatIndex convertPoint3toCell(const gtsam::Point3 &point) const { + if (point.x() < origin_.x() || + point.x() > (origin_.x() + (field_cols_ - 1.0) * cell_size_) || + point.y() < origin_.y() || + point.y() > (origin_.y() + (field_rows_ - 1.0) * cell_size_) || + point.z() < origin_.z() || + point.z() > (origin_.z() + (field_z_ - 1.0) * cell_size_)) { + throw SDFQueryOutOfRange(); + } + return FloatIndex((point.y() - origin_.y()) / cell_size_, + (point.x() - origin_.x()) / cell_size_, + (point.z() - origin_.z()) / cell_size_); + } + + /// Convert a fractional grid index to a point in frame s. + gtsam::Point3 convertCelltoPoint3(const FloatIndex &cell) const { + return origin_ + gtsam::Point3(cell.col * cell_size_, cell.row * cell_size_, + cell.z * cell_size_); + } + + /// Trilinear interpolation of the signed distance at a fractional index. + double signedDistance(const FloatIndex &idx) const { + const double lr = std::floor(idx.row), lc = std::floor(idx.col), + lz = std::floor(idx.z); + const double hr = lr + 1.0, hc = lc + 1.0, hz = lz + 1.0; + const size_t lri = static_cast(lr), lci = static_cast(lc), + lzi = static_cast(lz); + // Clamp so a query exactly on the far face does not read past the grid. + const size_t hri = std::min(lri + 1, field_rows_ - 1), + hci = std::min(lci + 1, field_cols_ - 1), + hzi = std::min(lzi + 1, field_z_ - 1); + return (hr - idx.row) * (hc - idx.col) * (hz - idx.z) * + signedDistance(lri, lci, lzi) + + (idx.row - lr) * (hc - idx.col) * (hz - idx.z) * + signedDistance(hri, lci, lzi) + + (hr - idx.row) * (idx.col - lc) * (hz - idx.z) * + signedDistance(lri, hci, lzi) + + (idx.row - lr) * (idx.col - lc) * (hz - idx.z) * + signedDistance(hri, hci, lzi) + + (hr - idx.row) * (hc - idx.col) * (idx.z - lz) * + signedDistance(lri, lci, hzi) + + (idx.row - lr) * (hc - idx.col) * (idx.z - lz) * + signedDistance(hri, lci, hzi) + + (hr - idx.row) * (idx.col - lc) * (idx.z - lz) * + signedDistance(lri, hci, hzi) + + (idx.row - lr) * (idx.col - lc) * (idx.z - lz) * + signedDistance(hri, hci, hzi); + } + + /// Gradient of the trilinear interpolation, with respect to the index. + /// Not differentiable exactly at a grid point. + gtsam::Vector3 gradient(const FloatIndex &idx) const { + const double lr = std::floor(idx.row), lc = std::floor(idx.col), + lz = std::floor(idx.z); + const double hr = lr + 1.0, hc = lc + 1.0, hz = lz + 1.0; + const size_t lri = static_cast(lr), lci = static_cast(lc), + lzi = static_cast(lz); + // Clamp so a query exactly on the far face does not read past the grid. + const size_t hri = std::min(lri + 1, field_rows_ - 1), + hci = std::min(lci + 1, field_cols_ - 1), + hzi = std::min(lzi + 1, field_z_ - 1); + return gtsam::Vector3( + (hc - idx.col) * (hz - idx.z) * + (signedDistance(hri, lci, lzi) - signedDistance(lri, lci, lzi)) + + (idx.col - lc) * (hz - idx.z) * + (signedDistance(hri, hci, lzi) - signedDistance(lri, hci, lzi)) + + (hc - idx.col) * (idx.z - lz) * + (signedDistance(hri, lci, hzi) - signedDistance(lri, lci, hzi)) + + (idx.col - lc) * (idx.z - lz) * + (signedDistance(hri, hci, hzi) - signedDistance(lri, hci, hzi)), + + (hr - idx.row) * (hz - idx.z) * + (signedDistance(lri, hci, lzi) - signedDistance(lri, lci, lzi)) + + (idx.row - lr) * (hz - idx.z) * + (signedDistance(hri, hci, lzi) - signedDistance(hri, lci, lzi)) + + (hr - idx.row) * (idx.z - lz) * + (signedDistance(lri, hci, hzi) - signedDistance(lri, lci, hzi)) + + (idx.row - lr) * (idx.z - lz) * + (signedDistance(hri, hci, hzi) - signedDistance(hri, lci, hzi)), + + (hr - idx.row) * (hc - idx.col) * + (signedDistance(lri, lci, hzi) - signedDistance(lri, lci, lzi)) + + (idx.row - lr) * (hc - idx.col) * + (signedDistance(hri, lci, hzi) - signedDistance(hri, lci, lzi)) + + (hr - idx.row) * (idx.col - lc) * + (signedDistance(lri, hci, hzi) - signedDistance(lri, hci, lzi)) + + (idx.row - lr) * (idx.col - lc) * + (signedDistance(hri, hci, hzi) - signedDistance(hri, hci, lzi))); + } + + /// Raw access to one grid cell. + double signedDistance(size_t r, size_t c, size_t z) const { + return data_[z](r, c); + } + + const gtsam::Point3 &origin() const { return origin_; } + size_t xCount() const { return field_cols_; } + size_t yCount() const { return field_rows_; } + size_t zCount() const { return field_z_; } + double cellSize() const { return cell_size_; } + const std::vector &rawData() const { return data_; } + + /// Equality up to a tolerance. + bool equals(const This &expected, double tol = 1e-9) const { + if (field_rows_ != expected.field_rows_ || + field_cols_ != expected.field_cols_ || field_z_ != expected.field_z_ || + std::fabs(cell_size_ - expected.cell_size_) > tol || + !gtsam::traits::Equals(origin_, expected.origin_, tol)) { + return false; + } + for (size_t z = 0; z < field_z_; ++z) { + if (!gtsam::equal_with_abs_tol(data_[z], expected.data_[z], tol)) { + return false; + } + } + return true; + } + + /// Print contents. + void print(const std::string &s = "") const { + std::cout << s; + std::cout << "field origin: "; + origin_.print(); + std::cout << "field resolution: " << cell_size_ << std::endl; + std::cout << "field size: " << field_cols_ << " x " << field_rows_ + << " x " << field_z_ << std::endl; + } +}; // \class SignedDistanceField + +} // namespace gtdynamics + +/// traits +namespace gtsam { +template <> +struct traits + : public Testable {}; +} // namespace gtsam From f393d2b9f3b4aa858a14a69f2770cc347bdeab88 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Thu, 9 Jul 2026 13:42:23 -0400 Subject: [PATCH 05/27] initial unit tests --- .../factors/tests/testGPPriorFactors.cpp | 170 ++++++++ gtdynamics/gpmp2/tests/CMakeLists.txt | 3 + .../gpmp2/tests/testGPInterpolators.cpp | 221 ++++++++++ .../gpmp2/tests/testObstacleFactors.cpp | 391 ++++++++++++++++++ 4 files changed, 785 insertions(+) create mode 100644 gtdynamics/factors/tests/testGPPriorFactors.cpp create mode 100644 gtdynamics/gpmp2/tests/CMakeLists.txt create mode 100644 gtdynamics/gpmp2/tests/testGPInterpolators.cpp create mode 100644 gtdynamics/gpmp2/tests/testObstacleFactors.cpp diff --git a/gtdynamics/factors/tests/testGPPriorFactors.cpp b/gtdynamics/factors/tests/testGPPriorFactors.cpp new file mode 100644 index 000000000..e76993088 --- /dev/null +++ b/gtdynamics/factors/tests/testGPPriorFactors.cpp @@ -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 testGPPriorFactors.cpp + * @brief test Gaussian process prior factors, linear and Lie group versions. + * @author Karthik Shaji + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace gtdynamics; +using gtsam::assert_equal; +using gtsam::Matrix; +using gtsam::Point3; +using gtsam::Pose3; +using gtsam::Rot3; +using gtsam::Values; +using gtsam::Vector; +using gtsam::Vector3; +using gtsam::Vector6; +using gtsam::noiseModel::Isotropic; + +static const double kDeltaT = 0.4; +static const gtsam::Key kP1 = 0, kV1 = 1, kP2 = 2, kV2 = 3; + +// The linear prior error is Phi(dt) * x1 - x2, so a constant velocity state +// pair, where q2 = q1 + dt * v1 and v2 == v1, lies exactly on the prior. +TEST(GPLinearPrior, errorOnConstantVelocity) { + const size_t dof = 3; + auto Qc_model = Isotropic::Sigma(dof, 1.0); + GPLinearPrior factor(kP1, kV1, kP2, kV2, kDeltaT, Qc_model); + + const Vector q1 = (Vector(3) << 1.0, -2.0, 0.5).finished(); + const Vector v1 = (Vector(3) << 0.3, 0.7, -0.2).finished(); + const Vector q2 = q1 + kDeltaT * v1; + const Vector v2 = v1; + + EXPECT(assert_equal(Vector::Zero(2 * dof), + factor.evaluateError(q1, v1, q2, v2), 1e-9)); +} + +// Away from a constant velocity pair the error is the shortfall in each block. +TEST(GPLinearPrior, errorAndJacobians) { + const size_t dof = 2; + auto Qc_model = Isotropic::Sigma(dof, 0.5); + GPLinearPrior factor(kP1, kV1, kP2, kV2, kDeltaT, Qc_model); + + const Vector q1 = (Vector(2) << 1.0, 2.0).finished(); + const Vector v1 = (Vector(2) << 0.1, -0.4).finished(); + const Vector q2 = (Vector(2) << 1.5, 1.5).finished(); + const Vector v2 = (Vector(2) << 0.2, -0.3).finished(); + + const Vector expected = + (Vector(4) << q1 + kDeltaT * v1 - q2, v1 - v2).finished(); + EXPECT(assert_equal(expected, factor.evaluateError(q1, v1, q2, v2), 1e-9)); + + Values values; + values.insert(kP1, q1); + values.insert(kV1, v1); + values.insert(kP2, q2); + values.insert(kV2, v2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +// The Lie prior error is [Logmap(Inv(p1) * p2) - dt * v1; v2 - v1], so a state +// pair swept out by a constant body twist lies exactly on the prior. Note the +// sign convention differs from GPLinearPrior, whose error is Phi * x1 - x2. +TEST(GPLiePrior, rot3ErrorOnConstantTwist) { + const size_t dof = 3; + auto Qc_model = Isotropic::Sigma(dof, 1.0); + GPLiePrior factor(kP1, kV1, kP2, kV2, kDeltaT, Qc_model); + + const Rot3 R1 = Rot3::RzRyRx(0.1, -0.2, 0.3); + const Vector v1 = (Vector(3) << 0.4, -0.1, 0.6).finished(); + const Rot3 R2 = R1 * Rot3::Expmap(kDeltaT * v1); + const Vector v2 = v1; + + EXPECT(assert_equal(Vector::Zero(2 * dof), + factor.evaluateError(R1, v1, R2, v2), 1e-9)); +} + +// Rot3 exercises the template on a group whose Expmap is not the identity map. +TEST(GPLiePrior, rot3ErrorAndJacobians) { + const size_t dof = 3; + auto Qc_model = Isotropic::Sigma(dof, 1.0); + GPLiePrior factor(kP1, kV1, kP2, kV2, kDeltaT, Qc_model); + + const Rot3 R1 = Rot3::RzRyRx(0.1, -0.2, 0.3); + const Rot3 R2 = Rot3::RzRyRx(-0.3, 0.15, 0.05); + const Vector v1 = (Vector(3) << 0.4, -0.1, 0.6).finished(); + const Vector v2 = (Vector(3) << 0.2, 0.3, -0.5).finished(); + + const Vector r = Rot3::Logmap(R1.inverse() * R2); + const Vector expected = (Vector(6) << r - kDeltaT * v1, v2 - v1).finished(); + EXPECT(assert_equal(expected, factor.evaluateError(R1, v1, R2, v2), 1e-9)); + + Values values; + values.insert(kP1, R1); + values.insert(kV1, v1); + values.insert(kP2, R2); + values.insert(kV2, v2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +// GPPose3Prior is the Pose3 instantiation of GPLiePrior, the 3D case we use. +TEST(GPPose3Prior, errorOnConstantTwist) { + auto Qc_model = Isotropic::Sigma(6, 1.0); + GPPose3Prior factor(kP1, kV1, kP2, kV2, kDeltaT, Qc_model); + + const Pose3 p1(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1.0, 2.0, 3.0)); + const Vector v1 = (Vector(6) << 0.1, -0.2, 0.3, 0.4, 0.5, -0.6).finished(); + const Pose3 p2 = p1 * Pose3::Expmap(kDeltaT * v1); + const Vector v2 = v1; + + EXPECT( + assert_equal(Vector::Zero(12), factor.evaluateError(p1, v1, p2, v2), 1e-9)); +} + +TEST(GPPose3Prior, errorAndJacobians) { + auto Qc_model = Isotropic::Sigma(6, 1.0); + GPPose3Prior factor(kP1, kV1, kP2, kV2, kDeltaT, Qc_model); + + const Pose3 p1(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1.0, 2.0, 3.0)); + const Pose3 p2(Rot3::RzRyRx(-0.2, 0.1, 0.4), Point3(1.4, 1.7, 3.2)); + const Vector v1 = (Vector(6) << 0.1, -0.2, 0.3, 0.4, 0.5, -0.6).finished(); + const Vector v2 = (Vector(6) << 0.0, 0.1, -0.1, 0.2, -0.3, 0.4).finished(); + + const Vector r = Pose3::Logmap(p1.inverse() * p2); + const Vector expected = (Vector(12) << r - kDeltaT * v1, v2 - v1).finished(); + EXPECT(assert_equal(expected, factor.evaluateError(p1, v1, p2, v2), 1e-9)); + + Values values; + values.insert(kP1, p1); + values.insert(kV1, v1); + values.insert(kP2, p2); + values.insert(kV2, v2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +// A Qc whose dimension disagrees with the group would silently mis-size the +// error, so the constructor rejects it. Pose3 demands a 6 dimensional Qc. +TEST(GPPose3Prior, rejectsWrongQcDimension) { + auto Qc_3d = Isotropic::Sigma(3, 1.0); + CHECK_EXCEPTION(GPPose3Prior(kP1, kV1, kP2, kV2, kDeltaT, Qc_3d), + std::invalid_argument); + + auto Qc_6d = Isotropic::Sigma(6, 1.0); + CHECK_EXCEPTION(GPLiePrior(kP1, kV1, kP2, kV2, kDeltaT, Qc_6d), + std::invalid_argument); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtdynamics/gpmp2/tests/CMakeLists.txt b/gtdynamics/gpmp2/tests/CMakeLists.txt new file mode 100644 index 000000000..0cfdbe7ee --- /dev/null +++ b/gtdynamics/gpmp2/tests/CMakeLists.txt @@ -0,0 +1,3 @@ +include_directories(${CMAKE_SOURCE_DIR}/tests) + +gtsamAddTestsGlob(gpmp2 "test*.cpp" "" "gtdynamics") diff --git a/gtdynamics/gpmp2/tests/testGPInterpolators.cpp b/gtdynamics/gpmp2/tests/testGPInterpolators.cpp new file mode 100644 index 000000000..14fa47d1c --- /dev/null +++ b/gtdynamics/gpmp2/tests/testGPInterpolators.cpp @@ -0,0 +1,221 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testGPInterpolators.cpp + * @brief test Gaussian process interpolators, linear and Lie group versions. + * @author Karthik Shaji + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace gtdynamics; +using gtsam::assert_equal; +using gtsam::Matrix; +using gtsam::Point3; +using gtsam::Pose3; +using gtsam::Rot3; +using gtsam::Vector; +using gtsam::noiseModel::Isotropic; + +static const double kDeltaT = 0.4; +static const double kTau = 0.15; + +/* ************************* linear interpolator ************************* */ + +// Lambda and Psi must reproduce the support states exactly at the endpoints: +// Psi(0) = 0 and Lambda(0) = I, while Psi(dt) = I and Lambda(dt) = 0. +TEST(GPLinearInterpolator, reproducesEndpoints) { + auto Qc_model = Isotropic::Sigma(2, 1.0); + const Vector q1 = (Vector(2) << 1.0, -2.0).finished(); + const Vector v1 = (Vector(2) << 0.3, 0.7).finished(); + const Vector q2 = (Vector(2) << 2.5, 0.5).finished(); + const Vector v2 = (Vector(2) << -0.1, 0.2).finished(); + + GPLinearInterpolator at_start(Qc_model, kDeltaT, 0.0); + EXPECT(assert_equal(q1, at_start.interpolatePose(q1, v1, q2, v2), 1e-9)); + EXPECT(assert_equal(v1, at_start.interpolateVelocity(q1, v1, q2, v2), 1e-9)); + + GPLinearInterpolator at_end(Qc_model, kDeltaT, kDeltaT); + EXPECT(assert_equal(q2, at_end.interpolatePose(q1, v1, q2, v2), 1e-9)); + EXPECT(assert_equal(v2, at_end.interpolateVelocity(q1, v1, q2, v2), 1e-9)); +} + +// A minimum acceleration trajectory through a constant velocity pair is the +// straight line itself, so the interpolated state is exactly q1 + tau * v1. +TEST(GPLinearInterpolator, constantVelocityIsALine) { + auto Qc_model = Isotropic::Sigma(2, 1.0); + GPLinearInterpolator interp(Qc_model, kDeltaT, kTau); + + const Vector q1 = (Vector(2) << 1.0, -2.0).finished(); + const Vector v1 = (Vector(2) << 0.3, 0.7).finished(); + const Vector q2 = q1 + kDeltaT * v1; + + EXPECT(assert_equal(Vector(q1 + kTau * v1), + interp.interpolatePose(q1, v1, q2, v1), 1e-9)); + EXPECT(assert_equal(v1, interp.interpolateVelocity(q1, v1, q2, v1), 1e-9)); +} + +TEST(GPLinearInterpolator, jacobians) { + auto Qc_model = Isotropic::Sigma(2, 1.0); + GPLinearInterpolator interp(Qc_model, kDeltaT, kTau); + + const Vector q1 = (Vector(2) << 1.0, -2.0).finished(); + const Vector v1 = (Vector(2) << 0.3, 0.7).finished(); + const Vector q2 = (Vector(2) << 2.5, 0.5).finished(); + const Vector v2 = (Vector(2) << -0.1, 0.2).finished(); + + Matrix H1, H2, H3, H4; + interp.interpolatePose(q1, v1, q2, v2, &H1, &H2, &H3, &H4); + + std::function + pose = [&](const Vector &a, const Vector &b, const Vector &c, + const Vector &d) { return interp.interpolatePose(a, b, c, d); }; + EXPECT(assert_equal(gtsam::numericalDerivative41(pose, q1, v1, q2, v2), H1, + 1e-6)); + EXPECT(assert_equal(gtsam::numericalDerivative42(pose, q1, v1, q2, v2), H2, + 1e-6)); + EXPECT(assert_equal(gtsam::numericalDerivative43(pose, q1, v1, q2, v2), H3, + 1e-6)); + EXPECT(assert_equal(gtsam::numericalDerivative44(pose, q1, v1, q2, v2), H4, + 1e-6)); + + Matrix G1, G2, G3, G4; + interp.interpolateVelocity(q1, v1, q2, v2, &G1, &G2, &G3, &G4); + std::function + vel = [&](const Vector &a, const Vector &b, const Vector &c, + const Vector &d) { + return interp.interpolateVelocity(a, b, c, d); + }; + EXPECT( + assert_equal(gtsam::numericalDerivative41(vel, q1, v1, q2, v2), G1, 1e-6)); + EXPECT( + assert_equal(gtsam::numericalDerivative42(vel, q1, v1, q2, v2), G2, 1e-6)); + EXPECT( + assert_equal(gtsam::numericalDerivative43(vel, q1, v1, q2, v2), G3, 1e-6)); + EXPECT( + assert_equal(gtsam::numericalDerivative44(vel, q1, v1, q2, v2), G4, 1e-6)); +} + +/* *************************** Lie interpolator *************************** */ + +TEST(GPLieInterpolator, rot3ReproducesEndpoints) { + auto Qc_model = Isotropic::Sigma(3, 1.0); + const Rot3 R1 = Rot3::RzRyRx(0.1, -0.2, 0.3); + const Rot3 R2 = Rot3::RzRyRx(-0.3, 0.15, 0.05); + const Vector v1 = (Vector(3) << 0.4, -0.1, 0.6).finished(); + const Vector v2 = (Vector(3) << 0.2, 0.3, -0.5).finished(); + + GPLieInterpolator at_start(Qc_model, kDeltaT, 0.0); + EXPECT(assert_equal(R1, at_start.interpolatePose(R1, v1, R2, v2), 1e-9)); + EXPECT(assert_equal(v1, at_start.interpolateVelocity(R1, v1, R2, v2), 1e-9)); + + GPLieInterpolator at_end(Qc_model, kDeltaT, kDeltaT); + EXPECT(assert_equal(R2, at_end.interpolatePose(R1, v1, R2, v2), 1e-9)); + EXPECT(assert_equal(v2, at_end.interpolateVelocity(R1, v1, R2, v2), 1e-9)); +} + +// Under a constant body twist the interpolated pose must be the exact geodesic +// point p1 * Expmap(tau * v1). This pins the lift, interpolate, retract path. +TEST(GPPose3Interpolator, constantTwistIsAGeodesic) { + auto Qc_model = Isotropic::Sigma(6, 1.0); + GPPose3Interpolator interp(Qc_model, kDeltaT, kTau); + + const Pose3 p1(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1.0, 2.0, 3.0)); + const Vector v1 = (Vector(6) << 0.1, -0.2, 0.3, 0.4, 0.5, -0.6).finished(); + const Pose3 p2 = p1 * Pose3::Expmap(kDeltaT * v1); + + EXPECT(assert_equal(Pose3(p1 * Pose3::Expmap(kTau * v1)), + interp.interpolatePose(p1, v1, p2, v1), 1e-9)); + EXPECT(assert_equal(v1, interp.interpolateVelocity(p1, v1, p2, v1), 1e-9)); +} + +TEST(GPPose3Interpolator, reproducesEndpoints) { + auto Qc_model = Isotropic::Sigma(6, 1.0); + const Pose3 p1(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1.0, 2.0, 3.0)); + const Pose3 p2(Rot3::RzRyRx(-0.2, 0.1, 0.4), Point3(1.4, 1.7, 3.2)); + const Vector v1 = (Vector(6) << 0.1, -0.2, 0.3, 0.4, 0.5, -0.6).finished(); + const Vector v2 = (Vector(6) << 0.0, 0.1, -0.1, 0.2, -0.3, 0.4).finished(); + + GPPose3Interpolator at_start(Qc_model, kDeltaT, 0.0); + EXPECT(assert_equal(p1, at_start.interpolatePose(p1, v1, p2, v2), 1e-9)); + EXPECT(assert_equal(v1, at_start.interpolateVelocity(p1, v1, p2, v2), 1e-9)); + + GPPose3Interpolator at_end(Qc_model, kDeltaT, kDeltaT); + EXPECT(assert_equal(p2, at_end.interpolatePose(p1, v1, p2, v2), 1e-9)); + EXPECT(assert_equal(v2, at_end.interpolateVelocity(p1, v1, p2, v2), 1e-9)); +} + +TEST(GPPose3Interpolator, jacobians) { + auto Qc_model = Isotropic::Sigma(6, 1.0); + GPPose3Interpolator interp(Qc_model, kDeltaT, kTau); + + const Pose3 p1(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1.0, 2.0, 3.0)); + const Pose3 p2(Rot3::RzRyRx(-0.2, 0.1, 0.4), Point3(1.4, 1.7, 3.2)); + const Vector v1 = (Vector(6) << 0.1, -0.2, 0.3, 0.4, 0.5, -0.6).finished(); + const Vector v2 = (Vector(6) << 0.0, 0.1, -0.1, 0.2, -0.3, 0.4).finished(); + + Matrix H1, H2, H3, H4; + interp.interpolatePose(p1, v1, p2, v2, &H1, &H2, &H3, &H4); + + std::function + pose = [&](const Pose3 &a, const Vector &b, const Pose3 &c, + const Vector &d) { return interp.interpolatePose(a, b, c, d); }; + EXPECT(assert_equal(gtsam::numericalDerivative41(pose, p1, v1, p2, v2), H1, + 1e-6)); + EXPECT(assert_equal(gtsam::numericalDerivative42(pose, p1, v1, p2, v2), H2, + 1e-6)); + EXPECT(assert_equal(gtsam::numericalDerivative43(pose, p1, v1, p2, v2), H3, + 1e-6)); + EXPECT(assert_equal(gtsam::numericalDerivative44(pose, p1, v1, p2, v2), H4, + 1e-6)); + + Matrix G1, G2, G3, G4; + interp.interpolateVelocity(p1, v1, p2, v2, &G1, &G2, &G3, &G4); + std::function + vel = [&](const Pose3 &a, const Vector &b, const Pose3 &c, + const Vector &d) { + return interp.interpolateVelocity(a, b, c, d); + }; + EXPECT( + assert_equal(gtsam::numericalDerivative41(vel, p1, v1, p2, v2), G1, 1e-6)); + EXPECT( + assert_equal(gtsam::numericalDerivative42(vel, p1, v1, p2, v2), G2, 1e-6)); + EXPECT( + assert_equal(gtsam::numericalDerivative43(vel, p1, v1, p2, v2), G3, 1e-6)); + EXPECT( + assert_equal(gtsam::numericalDerivative44(vel, p1, v1, p2, v2), G4, 1e-6)); +} + +// A Qc of the wrong dimension would mis-slice Lambda and Psi without crashing, +// so the constructor rejects it. Pose3 demands a 6 dimensional Qc. +TEST(GPPose3Interpolator, rejectsWrongQcDimension) { + CHECK_EXCEPTION(GPPose3Interpolator(Isotropic::Sigma(3, 1.0), kDeltaT, kTau), + std::invalid_argument); + CHECK_EXCEPTION( + GPLieInterpolator(Isotropic::Sigma(6, 1.0), kDeltaT, kTau), + std::invalid_argument); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtdynamics/gpmp2/tests/testObstacleFactors.cpp b/gtdynamics/gpmp2/tests/testObstacleFactors.cpp new file mode 100644 index 000000000..91be65447 --- /dev/null +++ b/gtdynamics/gpmp2/tests/testObstacleFactors.cpp @@ -0,0 +1,391 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testObstacleFactors.cpp + * @brief test the signed distance field, the obstacle cost, and trajectory + * planning around a spherical obstacle in the bar_lab workspace. + * @author Karthik Shaji + */ + +#include +#include +#include +#include +#include +#include +#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::Point3; +using gtsam::Pose3; +using gtsam::Rot3; +using gtsam::Values; +using gtsam::Vector; +using gtsam::Vector3; +using gtsam::noiseModel::Isotropic; +using gtsam::symbol_shorthand::V; +using gtsam::symbol_shorthand::X; + +static const double kCell = 0.05; +static const double kRadius = 0.15; +static const double kEpsilon = 0.10; + +// Sample the exact signed distance to a sphere onto a grid. The layer for z +// index k is indexed as (row = y, col = x), matching SignedDistanceField. +static SignedDistanceField makeSphereSDF(const Point3 ¢er, double radius, + const Point3 &origin, double cell, + size_t nx, size_t ny, size_t nz) { + std::vector data(nz); + for (size_t k = 0; k < nz; ++k) { + Matrix layer(ny, nx); + for (size_t i = 0; i < ny; ++i) { + for (size_t j = 0; j < nx; ++j) { + const Point3 p = + origin + Point3(j * cell, i * cell, k * cell); + layer(i, j) = (p - center).norm() - radius; + } + } + data[k] = layer; + } + return SignedDistanceField(origin, cell, data); +} + +/* ********************** signed distance field ************************** */ + +// Trilinear interpolation of an exact sphere field must recover the distance, +// and its gradient must point radially outward. Probing along each axis in turn +// is what catches a transposed (row = x, col = y) data layout: a swapped grid +// still returns plausible distances but reports the gradient of the wrong axis. +TEST(SignedDistanceField, sphereDistanceAndGradient) { + const Point3 center(1.0, 1.0, 1.0); + const Point3 origin(0.0, 0.0, 0.0); + const size_t n = 41; // spans 0 .. 2.0 m at 5 cm + const SignedDistanceField sdf = + makeSphereSDF(center, kRadius, origin, kCell, n, n, n); + + // Offset by half a cell, since the interpolant is not differentiable exactly + // on a grid point. + const double h = 0.5 * kCell; + struct Probe { + Vector3 offset; + Vector3 expected_gradient; + }; + const std::vector probes = { + {Vector3(0.5 + h, 0.0, 0.0), Vector3(1.0, 0.0, 0.0)}, + {Vector3(0.0, 0.5 + h, 0.0), Vector3(0.0, 1.0, 0.0)}, + {Vector3(0.0, 0.0, 0.5 + h), Vector3(0.0, 0.0, 1.0)}, + {Vector3(-0.4 - h, 0.0, 0.0), Vector3(-1.0, 0.0, 0.0)}, + {Vector3(0.0, -0.4 - h, 0.0), Vector3(0.0, -1.0, 0.0)}, + }; + + for (auto &&probe : probes) { + const Point3 p = center + Point3(probe.offset); + Vector3 gradient; + const double d = sdf.getSignedDistance(p, gradient); + EXPECT_DOUBLES_EQUAL(probe.offset.norm() - kRadius, d, 1e-2); + EXPECT(assert_equal(probe.expected_gradient, gradient, 1e-2)); + } +} + +// A query on the far face of the grid must not read past the data. +TEST(SignedDistanceField, queryOnFarFace) { + const Point3 origin(0.0, 0.0, 0.0); + const SignedDistanceField sdf = + makeSphereSDF(Point3(0.5, 0.5, 0.5), kRadius, origin, kCell, 11, 11, 11); + const double far = 10 * kCell; + EXPECT_DOUBLES_EQUAL((Point3(far, far, far) - Point3(0.5, 0.5, 0.5)).norm() - + kRadius, + sdf.getSignedDistance(Point3(far, far, far)), 1e-9); + CHECK_EXCEPTION(sdf.getSignedDistance(Point3(far + kCell, 0.0, 0.0)), + SDFQueryOutOfRange); +} + +/* ***************************** obstacle cost *************************** */ + +TEST(ObstacleCost, hingeLossAndJacobian) { + const Point3 center(1.0, 1.0, 1.0); + const SignedDistanceField sdf = + makeSphereSDF(center, kRadius, Point3(0, 0, 0), kCell, 41, 41, 41); + + // Beyond epsilon of the surface there is no cost and no gradient. + const Point3 far = center + Point3(0.8, 0.0, 0.0); + Matrix13 H_far; + EXPECT_DOUBLES_EQUAL(0.0, hingeLossObstacleCost(far, sdf, kEpsilon, H_far), + 1e-9); + EXPECT(assert_equal(Matrix13::Zero(), H_far, 1e-9)); + + // Outside the sphere but within epsilon, the cost is epsilon - d. + const double offset = kRadius + 0.5 * kEpsilon; + const Point3 near = center + Point3(offset, 0.0, 0.0); + Matrix13 H_near; + const double cost = hingeLossObstacleCost(near, sdf, kEpsilon, H_near); + EXPECT_DOUBLES_EQUAL(kEpsilon - 0.5 * kEpsilon, cost, 1e-2); + + std::function f = [&](const Point3 &p) { + return hingeLossObstacleCost(p, sdf, kEpsilon); + }; + EXPECT(assert_equal(gtsam::numericalDerivative11(f, near), + Matrix(H_near), 1e-5)); + + // A point outside the grid is treated as free space, so the field fails open. + Matrix13 H_out; + EXPECT_DOUBLES_EQUAL( + 0.0, hingeLossObstacleCost(Point3(-1.0, 0.0, 0.0), sdf, kEpsilon, H_out), + 1e-9); + EXPECT(assert_equal(Matrix13::Zero(), H_out, 1e-9)); +} + +// The frame attached overload reads the same field through a moving frame, so +// it must agree with the world frame overload on the transformed point. +TEST(ObstacleCost, frameAttachedOverload) { + const Point3 center(1.0, 1.0, 1.0); + const SignedDistanceField sdf = + makeSphereSDF(center, kRadius, Point3(0, 0, 0), kCell, 41, 41, 41); + + const Pose3 wTs(Rot3::RzRyRx(0.2, -0.1, 0.35), Point3(0.4, -0.3, 0.25)); + const Point3 wP(1.3, 1.05, 0.9); + + const double expected = + hingeLossObstacleCost(wTs.transformTo(wP), sdf, kEpsilon); + Matrix16 H_pose; + Matrix13 H_point; + const double actual = + hingeLossObstacleCost(wTs, wP, sdf, kEpsilon, H_pose, H_point); + EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9); + + // With the identity pose it degenerates to the world frame overload. + EXPECT_DOUBLES_EQUAL(hingeLossObstacleCost(wP, sdf, kEpsilon), + hingeLossObstacleCost(Pose3(), wP, sdf, kEpsilon), 1e-9); + + std::function f = + [&](const Pose3 &T, const Point3 &p) { + return hingeLossObstacleCost(T, p, sdf, kEpsilon); + }; + EXPECT(assert_equal( + gtsam::numericalDerivative21(f, wTs, wP), + Matrix(H_pose), 1e-5)); + EXPECT(assert_equal( + gtsam::numericalDerivative22(f, wTs, wP), + Matrix(H_point), 1e-5)); +} + +/* ******************** bar_lab robot query points *********************** */ + +static const Robot kRobot = + CreateRobotFromFile(kUrdfPath + std::string("bar_lab.urdf")); + +// The nine movable joints of robot1, in the order q indexes them: the three +// gantry prismatic joints, then the six arm revolute joints. +static std::vector robot1Joints() { + 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"}; + std::vector joints; + for (auto &&name : names) joints.push_back(kRobot.joint(name)); + return joints; +} + +// Two query points on the wrist: the link CoM and a point out along the tool. +static std::vector wristPoints() { + const LinkSharedPtr link = kRobot.link("robot1_link_6"); + return {PointOnLink(link, Point3(0.0, 0.0, 0.0)), + PointOnLink(link, Point3(0.0, 0.0, 0.1))}; +} + +static Vector startConfig() { + return (Vector(9) << 2.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0).finished(); +} +static Vector goalConfig() { + return (Vector(9) << 3.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0).finished(); +} + +// Only robot1's joints are given to the model, so the bridge2 subtree is never +// traversed and robot2 is absent from the query point model entirely. +TEST(RobotQueryPoints, jacobiansAgainstNumerical) { + RobotQueryPoints model(kRobot, "columns", robot1Joints(), wristPoints()); + EXPECT_LONGS_EQUAL(9, model.dof()); + EXPECT_LONGS_EQUAL(2, model.nrPoints()); + + const Vector q = startConfig(); + std::vector wPs; + std::vector Js; + model.queryPoints(q, &wPs, &Js); + + for (size_t i = 0; i < model.nrPoints(); ++i) { + std::function f = [&](const Vector &qq) { + std::vector ps; + model.queryPoints(qq, &ps); + return ps[i]; + }; + EXPECT(assert_equal(gtsam::numericalDerivative11(f, q), + Js[i], 1e-5)); + } +} + +/* ******************** trajectory around a sphere *********************** */ + +// Plan a trajectory for robot1 that slides one metre along the gantry rail, +// with a spherical obstacle planted exactly on the straight line path of the +// wrist. The endpoints are pinned, so the trajectory has to bow around it. +TEST(ObstacleSDFFactor, planAroundSphere) { + RobotQueryPoints model(kRobot, "columns", robot1Joints(), wristPoints()); + const size_t dof = model.dof(), m = model.nrPoints(); + const size_t N = 5; // support states + const double total_time = 2.0, delta_t = total_time / (N - 1); + + const Vector q_start = startConfig(), q_goal = goalConfig(); + const Vector v_avg = (q_goal - q_start) / total_time; + + // Straight line initialisation, and the wrist positions it sweeps through. + std::vector line(N); + std::vector swept; + for (size_t k = 0; k < N; ++k) { + line[k] = q_start + (static_cast(k) / (N - 1)) * (q_goal - q_start); + std::vector ps; + model.queryPoints(line[k], &ps); + swept.insert(swept.end(), ps.begin(), ps.end()); + } + + // Plant the sphere on the wrist CoM at the midpoint of the straight line, so + // the initialisation is deep inside it. + std::vector mid_points; + model.queryPoints(line[N / 2], &mid_points); + const Point3 center = mid_points[0]; + + // Size the grid to contain the whole swept path with room to detour. The + // padding matters: a point that leaves the grid reads as free space. + const double pad = 0.7; + Vector3 lo = swept[0], hi = swept[0]; + for (auto &&p : swept) { + lo = lo.cwiseMin(Vector3(p)); + hi = hi.cwiseMax(Vector3(p)); + } + const Point3 origin(lo.x() - pad, lo.y() - pad, lo.z() - pad); + const Vector3 extent = (hi - lo) + Vector3::Constant(2 * pad); + const size_t nx = static_cast(std::ceil(extent.x() / kCell)) + 1; + const size_t ny = static_cast(std::ceil(extent.y() / kCell)) + 1; + const size_t nz = static_cast(std::ceil(extent.z() / kCell)) + 1; + auto sdf = std::make_shared( + makeSphereSDF(center, kRadius, origin, kCell, nx, ny, nz)); + + const double cost_sigma = 0.01; + ObstacleSDFFactor probe(X(0), model, sdf, cost_sigma, kEpsilon); + + // The problem is only feasible if the pinned endpoints are already clear, and + // only meaningful if the straight line is not. Assert both before planning. + EXPECT(probe.evaluateError(line[N / 2]).maxCoeff() > 0.5 * kEpsilon); + EXPECT_DOUBLES_EQUAL(0.0, probe.evaluateError(q_start).maxCoeff(), 1e-9); + EXPECT_DOUBLES_EQUAL(0.0, probe.evaluateError(q_goal).maxCoeff(), 1e-9); + + gtsam::NonlinearFactorGraph graph; + auto Qc_model = Isotropic::Sigma(dof, 1.0); + auto endpoint_model = Isotropic::Sigma(dof, 1e-4); + + graph.addPrior(X(0), q_start, endpoint_model); + graph.addPrior(V(0), Vector::Zero(dof), endpoint_model); + graph.addPrior(X(N - 1), q_goal, endpoint_model); + graph.addPrior(V(N - 1), Vector::Zero(dof), endpoint_model); + + for (size_t k = 0; k < N; ++k) { + graph.emplace_shared(X(k), model, sdf, cost_sigma, + kEpsilon); + } + // The interpolated obstacle cost is only a Gaussian process posterior mean + // because this prior, with the same Qc_model and delta_t, joins the same + // support states. + for (size_t k = 0; k < N - 1; ++k) { + graph.emplace_shared(X(k), V(k), X(k + 1), V(k + 1), delta_t, + Qc_model); + } + + Values init; + for (size_t k = 0; k < N; ++k) { + init.insert(X(k), line[k]); + init.insert(V(k), Vector(v_avg)); + } + + gtsam::LevenbergMarquardtParams params; + params.setMaxIterations(100); + const Values result = + gtsam::LevenbergMarquardtOptimizer(graph, init, params).optimize(); + + // The endpoints are held, and every query point at every support state has + // been pushed clear of the sphere by at least epsilon. This is checked against + // the analytic sphere rather than the sampled field, so a point that escaped + // the grid cannot pass by reading as free space. + EXPECT(assert_equal(q_start, result.at(X(0)), 1e-3)); + EXPECT(assert_equal(q_goal, result.at(X(N - 1)), 1e-3)); + + for (size_t k = 0; k < N; ++k) { + std::vector ps; + model.queryPoints(result.at(X(k)), &ps); + for (size_t i = 0; i < m; ++i) { + EXPECT((ps[i] - center).norm() > kRadius + kEpsilon - 0.02); + } + } +} + +// The interpolated obstacle factor must agree with the unary factor when tau is +// zero, where the interpolation reproduces the first support state exactly. +TEST(ObstacleSDFFactorGP, agreesWithUnaryFactorAtTauZero) { + RobotQueryPoints model(kRobot, "columns", robot1Joints(), wristPoints()); + const size_t dof = model.dof(); + + const Vector q1 = startConfig(), q2 = goalConfig(); + const Vector v1 = Vector::Zero(dof), v2 = Vector::Zero(dof); + + std::vector mid_points; + model.queryPoints(q1, &mid_points); + const Point3 center = mid_points[0] + Point3(kRadius + 0.5 * kEpsilon, 0, 0); + auto sdf = std::make_shared(makeSphereSDF( + center, kRadius, center - Point3(1.0, 1.0, 1.0), kCell, 41, 41, 41)); + + const double delta_t = 0.5, cost_sigma = 0.01; + ObstacleSDFFactor unary(X(0), model, sdf, cost_sigma, kEpsilon); + ObstacleSDFFactorGP interpolated(X(0), V(0), X(1), V(1), model, sdf, + cost_sigma, kEpsilon, Isotropic::Sigma(dof, 1.0), + delta_t, 0.0); + + EXPECT(assert_equal(unary.evaluateError(q1), + interpolated.evaluateError(q1, v1, q2, v2), 1e-9)); + + Values values; + values.insert(X(0), q1); + values.insert(V(0), v1); + values.insert(X(1), q2); + values.insert(V(1), v2); + ObstacleSDFFactorGP at_tau(X(0), V(0), X(1), V(1), model, sdf, cost_sigma, + kEpsilon, Isotropic::Sigma(dof, 1.0), delta_t, + 0.2 * delta_t); + EXPECT_CORRECT_FACTOR_JACOBIANS(at_tau, values, 1e-7, 1e-5); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} From 7f8ac127d73afbb643e462e9c666551c45457e22 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Thu, 9 Jul 2026 14:45:33 -0400 Subject: [PATCH 06/27] added unit tests --- gtdynamics/factors/CMakeLists.txt | 3 + gtdynamics/factors/GPLiePriorFactor.h | 3 +- gtdynamics/factors/GPLinearPriorFactor.h | 9 +- gtdynamics/factors/ObstacleSDFFactor.h | 1 + gtdynamics/factors/ObstacleSDFFactorGP.h | 1 + gtdynamics/factors/tests/CMakeLists.txt | 3 + .../factors/tests/testGPPriorFactors.cpp | 4 - gtdynamics/gpmp2/GPLieInterpolator.h | 26 +-- gtdynamics/gpmp2/GPLinearInterpolator.h | 26 +-- gtdynamics/gpmp2/GPutils.h | 85 +++++----- gtdynamics/gpmp2/SignedDistanceField.h | 3 +- .../gpmp2/tests/testGPInterpolators.cpp | 150 +++++++++++------- .../gpmp2/tests/testObstacleFactors.cpp | 94 ++++++----- gtdynamics/utils/utils.cpp | 4 + 14 files changed, 236 insertions(+), 176 deletions(-) create mode 100644 gtdynamics/factors/tests/CMakeLists.txt diff --git a/gtdynamics/factors/CMakeLists.txt b/gtdynamics/factors/CMakeLists.txt index e69de29bb..177a09f0b 100644 --- a/gtdynamics/factors/CMakeLists.txt +++ b/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() diff --git a/gtdynamics/factors/GPLiePriorFactor.h b/gtdynamics/factors/GPLiePriorFactor.h index 14f0fa5be..f02ab0f8d 100644 --- a/gtdynamics/factors/GPLiePriorFactor.h +++ b/gtdynamics/factors/GPLiePriorFactor.h @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -70,7 +71,7 @@ class GPLiePrior gtsam::Key vel_key2, double delta_t, const gtsam::SharedNoiseModel &Qc_model) : Base(gtsam::noiseModel::Gaussian::Covariance( - calcQ(getQc(Qc_model), delta_t)), + calcQAccel(getQc(Qc_model), delta_t)), pose_key1, vel_key1, pose_key2, vel_key2), dof_(Qc_model->dim()), delta_t_(delta_t) { diff --git a/gtdynamics/factors/GPLinearPriorFactor.h b/gtdynamics/factors/GPLinearPriorFactor.h index 60c14b70c..9fc707f43 100644 --- a/gtdynamics/factors/GPLinearPriorFactor.h +++ b/gtdynamics/factors/GPLinearPriorFactor.h @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -36,8 +37,8 @@ namespace gtdynamics { * A 4-way GaussianProcess prior factor, linear version. * Implemented similarly to that used in the GPMP2 paper. Each state consists of * a pose and a velocity, separated by delta_t. The error is - * calcPhi(dof, delta_t) * x1 - x2, where x1 and x2 stack the pose and velocity - * of the first and second state respectively. + * calcPhiAccel(dof, delta_t) * x1 - x2, where x1 and x2 stack the pose and + * velocity of the first and second state respectively. */ class GPLinearPrior : public gtsam::NoiseModelFactorNdim()), delta_t_(delta_t) {} @@ -104,7 +105,7 @@ class GPLinearPrior if (H3) *H3 = (gtsam::Matrix(2 * dof_, dof_) << -identity, zero).finished(); if (H4) *H4 = (gtsam::Matrix(2 * dof_, dof_) << zero, -identity).finished(); - return calcPhi(dof_, delta_t_) * x1 - x2; + return calcPhiAccel(dof_, delta_t_) * x1 - x2; } /// Return the degrees of freedom of a single state. diff --git a/gtdynamics/factors/ObstacleSDFFactor.h b/gtdynamics/factors/ObstacleSDFFactor.h index 35923db34..1ac172791 100644 --- a/gtdynamics/factors/ObstacleSDFFactor.h +++ b/gtdynamics/factors/ObstacleSDFFactor.h @@ -19,6 +19,7 @@ #include #include #include +#include #include #include diff --git a/gtdynamics/factors/ObstacleSDFFactorGP.h b/gtdynamics/factors/ObstacleSDFFactorGP.h index b22a7c2c4..766370462 100644 --- a/gtdynamics/factors/ObstacleSDFFactorGP.h +++ b/gtdynamics/factors/ObstacleSDFFactorGP.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include diff --git a/gtdynamics/factors/tests/CMakeLists.txt b/gtdynamics/factors/tests/CMakeLists.txt new file mode 100644 index 000000000..6c1588e81 --- /dev/null +++ b/gtdynamics/factors/tests/CMakeLists.txt @@ -0,0 +1,3 @@ +include_directories(${CMAKE_SOURCE_DIR}/tests) + +gtsamAddTestsGlob(factors "test*.cpp" "" "gtdynamics") diff --git a/gtdynamics/factors/tests/testGPPriorFactors.cpp b/gtdynamics/factors/tests/testGPPriorFactors.cpp index e76993088..71f15a4b3 100644 --- a/gtdynamics/factors/tests/testGPPriorFactors.cpp +++ b/gtdynamics/factors/tests/testGPPriorFactors.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include #include #include @@ -25,14 +24,11 @@ using namespace gtdynamics; using gtsam::assert_equal; -using gtsam::Matrix; using gtsam::Point3; using gtsam::Pose3; using gtsam::Rot3; using gtsam::Values; using gtsam::Vector; -using gtsam::Vector3; -using gtsam::Vector6; using gtsam::noiseModel::Isotropic; static const double kDeltaT = 0.4; diff --git a/gtdynamics/gpmp2/GPLieInterpolator.h b/gtdynamics/gpmp2/GPLieInterpolator.h index 8144dba6f..ce65c60f7 100644 --- a/gtdynamics/gpmp2/GPLieInterpolator.h +++ b/gtdynamics/gpmp2/GPLieInterpolator.h @@ -75,8 +75,8 @@ class GPLieInterpolator { "dimension of the Lie group."); } Qc_ = getQc(Qc_model); - Lambda_ = calcLambda(Qc_, delta_t_, tau_); - Psi_ = calcPsi(Qc_, delta_t_, tau_); + Lambda_ = calcLambdaAccel(Qc_, delta_t_, tau_); + Psi_ = calcPsiAccel(Qc_, delta_t_, tau_); } ~GPLieInterpolator() {} @@ -84,10 +84,10 @@ class GPLieInterpolator { /// Interpolate the pose at tau, with Jacobians w.r.t. the support states. T interpolatePose(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 { + gtsam::Matrix *H1 = nullptr, + gtsam::Matrix *H2 = nullptr, + gtsam::Matrix *H3 = nullptr, + gtsam::Matrix *H4 = nullptr) const { const bool use_H = (H1 || H2 || H3 || H4); gtsam::Vector r1(2 * dof_); @@ -133,10 +133,10 @@ class GPLieInterpolator { /// Interpolate the velocity at tau, with Jacobians w.r.t. the support states. gtsam::Vector interpolateVelocity( 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 { + const gtsam::Vector &vel2, gtsam::Matrix *H1 = nullptr, + gtsam::Matrix *H2 = nullptr, + gtsam::Matrix *H3 = nullptr, + gtsam::Matrix *H4 = nullptr) const { const bool use_H = (H1 || H2 || H3 || H4); gtsam::Vector r1(2 * dof_); @@ -169,9 +169,9 @@ class GPLieInterpolator { static void updatePoseJacobians( const gtsam::Matrix &Hpose, const gtsam::Matrix &Hint1, const gtsam::Matrix &Hint2, const gtsam::Matrix &Hint3, - const gtsam::Matrix &Hint4, gtsam::OptionalMatrixType H1, - gtsam::OptionalMatrixType H2, gtsam::OptionalMatrixType H3, - gtsam::OptionalMatrixType H4) { + const gtsam::Matrix &Hint4, gtsam::Matrix *H1, + gtsam::Matrix *H2, gtsam::Matrix *H3, + gtsam::Matrix *H4) { if (H1) *H1 = Hpose * Hint1; if (H2) *H2 = Hpose * Hint2; if (H3) *H3 = Hpose * Hint3; diff --git a/gtdynamics/gpmp2/GPLinearInterpolator.h b/gtdynamics/gpmp2/GPLinearInterpolator.h index f1a72dcf4..10ff7a307 100644 --- a/gtdynamics/gpmp2/GPLinearInterpolator.h +++ b/gtdynamics/gpmp2/GPLinearInterpolator.h @@ -61,8 +61,8 @@ class GPLinearInterpolator { double tau) : dof_(Qc_model->dim()), delta_t_(delta_t), tau_(tau) { Qc_ = getQc(Qc_model); - Lambda_ = calcLambda(Qc_, delta_t_, tau_); - Psi_ = calcPsi(Qc_, delta_t_, tau_); + Lambda_ = calcLambdaAccel(Qc_, delta_t_, tau_); + Psi_ = calcPsiAccel(Qc_, delta_t_, tau_); } ~GPLinearInterpolator() {} @@ -71,10 +71,10 @@ class GPLinearInterpolator { gtsam::Vector interpolatePose( const gtsam::Vector &pose1, const gtsam::Vector &vel1, const gtsam::Vector &pose2, const gtsam::Vector &vel2, - gtsam::OptionalMatrixType H1 = nullptr, - gtsam::OptionalMatrixType H2 = nullptr, - gtsam::OptionalMatrixType H3 = nullptr, - gtsam::OptionalMatrixType H4 = nullptr) const { + gtsam::Matrix *H1 = nullptr, + gtsam::Matrix *H2 = nullptr, + gtsam::Matrix *H3 = nullptr, + gtsam::Matrix *H4 = nullptr) const { gtsam::Vector x1(2 * dof_), x2(2 * dof_); x1 << pose1, vel1; x2 << pose2, vel2; @@ -93,10 +93,10 @@ class GPLinearInterpolator { gtsam::Vector interpolateVelocity( const gtsam::Vector &pose1, const gtsam::Vector &vel1, const gtsam::Vector &pose2, const gtsam::Vector &vel2, - gtsam::OptionalMatrixType H1 = nullptr, - gtsam::OptionalMatrixType H2 = nullptr, - gtsam::OptionalMatrixType H3 = nullptr, - gtsam::OptionalMatrixType H4 = nullptr) const { + gtsam::Matrix *H1 = nullptr, + gtsam::Matrix *H2 = nullptr, + gtsam::Matrix *H3 = nullptr, + gtsam::Matrix *H4 = nullptr) const { gtsam::Vector x1(2 * dof_), x2(2 * dof_); x1 << pose1, vel1; x2 << pose2, vel2; @@ -115,9 +115,9 @@ class GPLinearInterpolator { static void updatePoseJacobians( const gtsam::Matrix &Hpose, const gtsam::Matrix &Hint1, const gtsam::Matrix &Hint2, const gtsam::Matrix &Hint3, - const gtsam::Matrix &Hint4, gtsam::OptionalMatrixType H1, - gtsam::OptionalMatrixType H2, gtsam::OptionalMatrixType H3, - gtsam::OptionalMatrixType H4) { + const gtsam::Matrix &Hint4, gtsam::Matrix *H1, + gtsam::Matrix *H2, gtsam::Matrix *H3, + gtsam::Matrix *H4) { if (H1) *H1 = Hpose * Hint1; if (H2) *H2 = Hpose * Hint2; if (H3) *H3 = Hpose * Hint3; diff --git a/gtdynamics/gpmp2/GPutils.h b/gtdynamics/gpmp2/GPutils.h index 9d69834fd..948d340a4 100644 --- a/gtdynamics/gpmp2/GPutils.h +++ b/gtdynamics/gpmp2/GPutils.h @@ -7,35 +7,45 @@ /** * @file GPutils.h - * @brief Gaussian process utilities: Qc, Q and Phi matrices. + * @brief Gaussian process utilities for the white noise on acceleration prior. * @author Karthik Shaji - Adapted from gpmp2 by Xinyan Yan and Jing Dong. */ #pragma once +#include #include #include #include #include #include -#include -#include namespace gtdynamics { +/* + * These implement the white noise on acceleration prior of Barfoot14rss, used + * by GPMP2, whose state stacks a pose and a velocity as [p; v]. The white noise + * on jerk counterparts, whose state also carries an acceleration, are calcPhi + * and calcQ in utils.h. The two priors do not share a Qc: here it is the power + * spectral density of the acceleration, there it is that of the jerk. + * + * The Qc covariance itself is extracted by getQc, declared in utils.h. + */ + /** - * @fn Extract the Qc covariance matrix from a Gaussian noise model. - * @param Qc_model Gaussian noise model whose covariance is Qc. - * @returns the n x n Qc covariance matrix. + * @fn Compute the state transition matrix Phi over a time interval. + * @param dof degrees of freedom of a single state. + * @param tau time interval between the two states. + * @returns the 2n x 2n state transition matrix. */ -inline gtsam::Matrix getQc(const gtsam::SharedNoiseModel &Qc_model) { - auto gaussian = - std::dynamic_pointer_cast(Qc_model); - if (!gaussian) { - throw std::invalid_argument("getQc: Qc_model must be a Gaussian model."); - } - return gaussian->covariance(); +inline gtsam::Matrix calcPhiAccel(size_t dof, double tau) { + const gtsam::Matrix identity = gtsam::Matrix::Identity(dof, dof); + const gtsam::Matrix zero = gtsam::Matrix::Zero(dof, dof); + return (gtsam::Matrix(2 * dof, 2 * dof) << // + identity, tau * identity, // + zero, identity) + .finished(); } /** @@ -44,43 +54,30 @@ inline gtsam::Matrix getQc(const gtsam::SharedNoiseModel &Qc_model) { * @param tau time interval between the two states. * @returns the 2n x 2n process covariance matrix. */ -inline gtsam::Matrix calcQ(const gtsam::Matrix &Qc, double tau) { +inline gtsam::Matrix calcQAccel(const gtsam::Matrix &Qc, double tau) { assert(Qc.rows() == Qc.cols()); const auto n = Qc.rows(); - return (gtsam::Matrix(2 * n, 2 * n) << // + return (gtsam::Matrix(2 * n, 2 * n) << // std::pow(tau, 3.0) / 3.0 * Qc, std::pow(tau, 2.0) / 2.0 * Qc, // std::pow(tau, 2.0) / 2.0 * Qc, tau * Qc) .finished(); } -/** - * @fn Compute the state transition matrix Phi over a time interval. - * @param dof degrees of freedom of a single state. - * @param tau time interval between the two states. - * @returns the 2n x 2n state transition matrix. - */ -inline gtsam::Matrix calcPhi(size_t dof, double tau) { - const gtsam::Matrix identity = gtsam::Matrix::Identity(dof, dof); - const gtsam::Matrix zero = gtsam::Matrix::Zero(dof, dof); - return (gtsam::Matrix(2 * dof, 2 * dof) << // - identity, tau * identity, // - zero, identity) - .finished(); -} - /** * @fn Compute the inverse of the process covariance Q over a time interval. * @param Qc n x n power spectral density matrix. * @param tau time interval between the two states. * @returns the 2n x 2n inverse process covariance matrix. */ -inline gtsam::Matrix calcQ_inv(const gtsam::Matrix &Qc, double tau) { +inline gtsam::Matrix calcQinvAccel(const gtsam::Matrix &Qc, double tau) { assert(Qc.rows() == Qc.cols()); const auto n = Qc.rows(); const gtsam::Matrix Qc_inv = Qc.inverse(); - return (gtsam::Matrix(2 * n, 2 * n) << // - 12.0 * std::pow(tau, -3.0) * Qc_inv, -6.0 * std::pow(tau, -2.0) * Qc_inv, // - -6.0 * std::pow(tau, -2.0) * Qc_inv, 4.0 * std::pow(tau, -1.0) * Qc_inv) + return (gtsam::Matrix(2 * n, 2 * n) << // + 12.0 * std::pow(tau, -3.0) * Qc_inv, // + -6.0 * std::pow(tau, -2.0) * Qc_inv, // + -6.0 * std::pow(tau, -2.0) * Qc_inv, // + 4.0 * std::pow(tau, -1.0) * Qc_inv) .finished(); } @@ -91,12 +88,13 @@ inline gtsam::Matrix calcQ_inv(const gtsam::Matrix &Qc, double tau) { * @param tau time from the first support state to the interpolated state. * @returns the 2n x 2n Lambda matrix. */ -inline gtsam::Matrix calcLambda(const gtsam::Matrix &Qc, double delta_t, - double tau) { +inline gtsam::Matrix calcLambdaAccel(const gtsam::Matrix &Qc, double delta_t, + double tau) { assert(Qc.rows() == Qc.cols()); - return calcPhi(Qc.rows(), tau) - - calcQ(Qc, tau) * calcPhi(Qc.rows(), delta_t - tau).transpose() * - calcQ_inv(Qc, delta_t) * calcPhi(Qc.rows(), delta_t); + const size_t dof = static_cast(Qc.rows()); + return calcPhiAccel(dof, tau) - + calcQAccel(Qc, tau) * calcPhiAccel(dof, delta_t - tau).transpose() * + calcQinvAccel(Qc, delta_t) * calcPhiAccel(dof, delta_t); } /** @@ -106,11 +104,12 @@ inline gtsam::Matrix calcLambda(const gtsam::Matrix &Qc, double delta_t, * @param tau time from the first support state to the interpolated state. * @returns the 2n x 2n Psi matrix. */ -inline gtsam::Matrix calcPsi(const gtsam::Matrix &Qc, double delta_t, - double tau) { +inline gtsam::Matrix calcPsiAccel(const gtsam::Matrix &Qc, double delta_t, + double tau) { assert(Qc.rows() == Qc.cols()); - return calcQ(Qc, tau) * calcPhi(Qc.rows(), delta_t - tau).transpose() * - calcQ_inv(Qc, delta_t); + const size_t dof = static_cast(Qc.rows()); + return calcQAccel(Qc, tau) * calcPhiAccel(dof, delta_t - tau).transpose() * + calcQinvAccel(Qc, delta_t); } } // namespace gtdynamics diff --git a/gtdynamics/gpmp2/SignedDistanceField.h b/gtdynamics/gpmp2/SignedDistanceField.h index c4735c81b..9fff44342 100644 --- a/gtdynamics/gpmp2/SignedDistanceField.h +++ b/gtdynamics/gpmp2/SignedDistanceField.h @@ -236,8 +236,7 @@ class SignedDistanceField { /// Print contents. void print(const std::string &s = "") const { std::cout << s; - std::cout << "field origin: "; - origin_.print(); + std::cout << "field origin: " << origin_.transpose() << std::endl; std::cout << "field resolution: " << cell_size_ << std::endl; std::cout << "field size: " << field_cols_ << " x " << field_rows_ << " x " << field_z_ << std::endl; diff --git a/gtdynamics/gpmp2/tests/testGPInterpolators.cpp b/gtdynamics/gpmp2/tests/testGPInterpolators.cpp index 14fa47d1c..ff7ce2ef6 100644 --- a/gtdynamics/gpmp2/tests/testGPInterpolators.cpp +++ b/gtdynamics/gpmp2/tests/testGPInterpolators.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include #include #include @@ -30,6 +29,8 @@ using gtsam::Point3; using gtsam::Pose3; using gtsam::Rot3; using gtsam::Vector; +using gtsam::Vector2; +using gtsam::Vector6; using gtsam::noiseModel::Isotropic; static const double kDeltaT = 0.4; @@ -70,47 +71,65 @@ TEST(GPLinearInterpolator, constantVelocityIsALine) { EXPECT(assert_equal(v1, interp.interpolateVelocity(q1, v1, q2, v1), 1e-9)); } +// numericalDerivative deduces its perturbation size from traits::dimension, +// which is Eigen::Dynamic for gtsam::Vector, so the probed arguments are fixed +// size Vector2 here. They convert to the dynamic Vector the interpolator takes. TEST(GPLinearInterpolator, jacobians) { auto Qc_model = Isotropic::Sigma(2, 1.0); GPLinearInterpolator interp(Qc_model, kDeltaT, kTau); - const Vector q1 = (Vector(2) << 1.0, -2.0).finished(); - const Vector v1 = (Vector(2) << 0.3, 0.7).finished(); - const Vector q2 = (Vector(2) << 2.5, 0.5).finished(); - const Vector v2 = (Vector(2) << -0.1, 0.2).finished(); + const Vector2 q1(1.0, -2.0), v1(0.3, 0.7), q2(2.5, 0.5), v2(-0.1, 0.2); Matrix H1, H2, H3, H4; interp.interpolatePose(q1, v1, q2, v2, &H1, &H2, &H3, &H4); - std::function - pose = [&](const Vector &a, const Vector &b, const Vector &c, - const Vector &d) { return interp.interpolatePose(a, b, c, d); }; - EXPECT(assert_equal(gtsam::numericalDerivative41(pose, q1, v1, q2, v2), H1, - 1e-6)); - EXPECT(assert_equal(gtsam::numericalDerivative42(pose, q1, v1, q2, v2), H2, - 1e-6)); - EXPECT(assert_equal(gtsam::numericalDerivative43(pose, q1, v1, q2, v2), H3, - 1e-6)); - EXPECT(assert_equal(gtsam::numericalDerivative44(pose, q1, v1, q2, v2), H4, - 1e-6)); + std::function + pose = [&](const Vector2 &a, const Vector2 &b, const Vector2 &c, + const Vector2 &d) { + return Vector2(interp.interpolatePose(a, b, c, d)); + }; + EXPECT(assert_equal( + gtsam::numericalDerivative41( + pose, q1, v1, q2, v2), + H1, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative42( + pose, q1, v1, q2, v2), + H2, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative43( + pose, q1, v1, q2, v2), + H3, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative44( + pose, q1, v1, q2, v2), + H4, 1e-6)); Matrix G1, G2, G3, G4; interp.interpolateVelocity(q1, v1, q2, v2, &G1, &G2, &G3, &G4); - std::function - vel = [&](const Vector &a, const Vector &b, const Vector &c, - const Vector &d) { - return interp.interpolateVelocity(a, b, c, d); + std::function + vel = [&](const Vector2 &a, const Vector2 &b, const Vector2 &c, + const Vector2 &d) { + return Vector2(interp.interpolateVelocity(a, b, c, d)); }; - EXPECT( - assert_equal(gtsam::numericalDerivative41(vel, q1, v1, q2, v2), G1, 1e-6)); - EXPECT( - assert_equal(gtsam::numericalDerivative42(vel, q1, v1, q2, v2), G2, 1e-6)); - EXPECT( - assert_equal(gtsam::numericalDerivative43(vel, q1, v1, q2, v2), G3, 1e-6)); - EXPECT( - assert_equal(gtsam::numericalDerivative44(vel, q1, v1, q2, v2), G4, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative41( + vel, q1, v1, q2, v2), + G1, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative42( + vel, q1, v1, q2, v2), + G2, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative43( + vel, q1, v1, q2, v2), + G3, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative44( + vel, q1, v1, q2, v2), + G4, 1e-6)); } /* *************************** Lie interpolator *************************** */ @@ -162,47 +181,64 @@ TEST(GPPose3Interpolator, reproducesEndpoints) { EXPECT(assert_equal(v2, at_end.interpolateVelocity(p1, v1, p2, v2), 1e-9)); } +// As above, the velocity arguments are probed as fixed size Vector6. TEST(GPPose3Interpolator, jacobians) { auto Qc_model = Isotropic::Sigma(6, 1.0); GPPose3Interpolator interp(Qc_model, kDeltaT, kTau); const Pose3 p1(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1.0, 2.0, 3.0)); const Pose3 p2(Rot3::RzRyRx(-0.2, 0.1, 0.4), Point3(1.4, 1.7, 3.2)); - const Vector v1 = (Vector(6) << 0.1, -0.2, 0.3, 0.4, 0.5, -0.6).finished(); - const Vector v2 = (Vector(6) << 0.0, 0.1, -0.1, 0.2, -0.3, 0.4).finished(); + const Vector6 v1 = (Vector6() << 0.1, -0.2, 0.3, 0.4, 0.5, -0.6).finished(); + const Vector6 v2 = (Vector6() << 0.0, 0.1, -0.1, 0.2, -0.3, 0.4).finished(); Matrix H1, H2, H3, H4; interp.interpolatePose(p1, v1, p2, v2, &H1, &H2, &H3, &H4); - std::function - pose = [&](const Pose3 &a, const Vector &b, const Pose3 &c, - const Vector &d) { return interp.interpolatePose(a, b, c, d); }; - EXPECT(assert_equal(gtsam::numericalDerivative41(pose, p1, v1, p2, v2), H1, - 1e-6)); - EXPECT(assert_equal(gtsam::numericalDerivative42(pose, p1, v1, p2, v2), H2, - 1e-6)); - EXPECT(assert_equal(gtsam::numericalDerivative43(pose, p1, v1, p2, v2), H3, - 1e-6)); - EXPECT(assert_equal(gtsam::numericalDerivative44(pose, p1, v1, p2, v2), H4, - 1e-6)); + std::function + pose = [&](const Pose3 &a, const Vector6 &b, const Pose3 &c, + const Vector6 &d) { return interp.interpolatePose(a, b, c, d); }; + EXPECT(assert_equal( + gtsam::numericalDerivative41( + pose, p1, v1, p2, v2), + H1, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative42( + pose, p1, v1, p2, v2), + H2, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative43( + pose, p1, v1, p2, v2), + H3, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative44( + pose, p1, v1, p2, v2), + H4, 1e-6)); Matrix G1, G2, G3, G4; interp.interpolateVelocity(p1, v1, p2, v2, &G1, &G2, &G3, &G4); - std::function - vel = [&](const Pose3 &a, const Vector &b, const Pose3 &c, - const Vector &d) { - return interp.interpolateVelocity(a, b, c, d); + std::function + vel = [&](const Pose3 &a, const Vector6 &b, const Pose3 &c, + const Vector6 &d) { + return Vector6(interp.interpolateVelocity(a, b, c, d)); }; - EXPECT( - assert_equal(gtsam::numericalDerivative41(vel, p1, v1, p2, v2), G1, 1e-6)); - EXPECT( - assert_equal(gtsam::numericalDerivative42(vel, p1, v1, p2, v2), G2, 1e-6)); - EXPECT( - assert_equal(gtsam::numericalDerivative43(vel, p1, v1, p2, v2), G3, 1e-6)); - EXPECT( - assert_equal(gtsam::numericalDerivative44(vel, p1, v1, p2, v2), G4, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative41( + vel, p1, v1, p2, v2), + G1, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative42( + vel, p1, v1, p2, v2), + G2, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative43( + vel, p1, v1, p2, v2), + G3, 1e-6)); + EXPECT(assert_equal( + gtsam::numericalDerivative44( + vel, p1, v1, p2, v2), + G4, 1e-6)); } // A Qc of the wrong dimension would mis-slice Lambda and Psi without crashing, diff --git a/gtdynamics/gpmp2/tests/testObstacleFactors.cpp b/gtdynamics/gpmp2/tests/testObstacleFactors.cpp index 91be65447..2f46d79e8 100644 --- a/gtdynamics/gpmp2/tests/testObstacleFactors.cpp +++ b/gtdynamics/gpmp2/tests/testObstacleFactors.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -30,7 +29,6 @@ #include #include -#include #include #include #include @@ -40,12 +38,15 @@ using namespace gtdynamics; using gtsam::assert_equal; using gtsam::Matrix; +using gtsam::Matrix13; +using gtsam::Matrix16; using gtsam::Point3; using gtsam::Pose3; using gtsam::Rot3; using gtsam::Values; using gtsam::Vector; using gtsam::Vector3; +using gtsam::Vector9; using gtsam::noiseModel::Isotropic; using gtsam::symbol_shorthand::V; using gtsam::symbol_shorthand::X; @@ -54,6 +55,10 @@ static const double kCell = 0.05; static const double kRadius = 0.15; static const double kEpsilon = 0.10; +// Offset a grid by half a cell to put points of interest at cell centres, since +// the trilinear gradient is discontinuous on the nodes. +static const double kHalfCell = 0.5 * kCell; + // Sample the exact signed distance to a sphere onto a grid. The layer for z // index k is indexed as (row = y, col = x), matching SignedDistanceField. static SignedDistanceField makeSphereSDF(const Point3 ¢er, double radius, @@ -82,24 +87,21 @@ static SignedDistanceField makeSphereSDF(const Point3 ¢er, double radius, // still returns plausible distances but reports the gradient of the wrong axis. TEST(SignedDistanceField, sphereDistanceAndGradient) { const Point3 center(1.0, 1.0, 1.0); - const Point3 origin(0.0, 0.0, 0.0); - const size_t n = 41; // spans 0 .. 2.0 m at 5 cm + const Point3 origin(-kHalfCell, -kHalfCell, -kHalfCell); + const size_t n = 41; // spans -0.025 .. 1.975 m at 5 cm const SignedDistanceField sdf = makeSphereSDF(center, kRadius, origin, kCell, n, n, n); - // Offset by half a cell, since the interpolant is not differentiable exactly - // on a grid point. - const double h = 0.5 * kCell; struct Probe { Vector3 offset; Vector3 expected_gradient; }; const std::vector probes = { - {Vector3(0.5 + h, 0.0, 0.0), Vector3(1.0, 0.0, 0.0)}, - {Vector3(0.0, 0.5 + h, 0.0), Vector3(0.0, 1.0, 0.0)}, - {Vector3(0.0, 0.0, 0.5 + h), Vector3(0.0, 0.0, 1.0)}, - {Vector3(-0.4 - h, 0.0, 0.0), Vector3(-1.0, 0.0, 0.0)}, - {Vector3(0.0, -0.4 - h, 0.0), Vector3(0.0, -1.0, 0.0)}, + {Vector3(0.5, 0.0, 0.0), Vector3(1.0, 0.0, 0.0)}, + {Vector3(0.0, 0.5, 0.0), Vector3(0.0, 1.0, 0.0)}, + {Vector3(0.0, 0.0, 0.5), Vector3(0.0, 0.0, 1.0)}, + {Vector3(-0.4, 0.0, 0.0), Vector3(-1.0, 0.0, 0.0)}, + {Vector3(0.0, -0.4, 0.0), Vector3(0.0, -1.0, 0.0)}, }; for (auto &&probe : probes) { @@ -128,15 +130,18 @@ TEST(SignedDistanceField, queryOnFarFace) { TEST(ObstacleCost, hingeLossAndJacobian) { const Point3 center(1.0, 1.0, 1.0); - const SignedDistanceField sdf = - makeSphereSDF(center, kRadius, Point3(0, 0, 0), kCell, 41, 41, 41); + const SignedDistanceField sdf = makeSphereSDF( + center, kRadius, Point3(-kHalfCell, -kHalfCell, -kHalfCell), kCell, 41, + 41, 41); // Beyond epsilon of the surface there is no cost and no gradient. const Point3 far = center + Point3(0.8, 0.0, 0.0); Matrix13 H_far; EXPECT_DOUBLES_EQUAL(0.0, hingeLossObstacleCost(far, sdf, kEpsilon, H_far), 1e-9); - EXPECT(assert_equal(Matrix13::Zero(), H_far, 1e-9)); + // A fixed 1x3 converts to both gtsam::Matrix and gtsam::Vector, so the + // assert_equal overloads tie unless the arguments are made Matrix outright. + EXPECT(assert_equal(Matrix(Matrix13::Zero()), Matrix(H_far), 1e-9)); // Outside the sphere but within epsilon, the cost is epsilon - d. const double offset = kRadius + 0.5 * kEpsilon; @@ -148,48 +153,53 @@ TEST(ObstacleCost, hingeLossAndJacobian) { std::function f = [&](const Point3 &p) { return hingeLossObstacleCost(p, sdf, kEpsilon); }; - EXPECT(assert_equal(gtsam::numericalDerivative11(f, near), - Matrix(H_near), 1e-5)); + EXPECT(assert_equal( + Matrix(gtsam::numericalDerivative11(f, near)), + Matrix(H_near), 1e-5)); // A point outside the grid is treated as free space, so the field fails open. Matrix13 H_out; EXPECT_DOUBLES_EQUAL( 0.0, hingeLossObstacleCost(Point3(-1.0, 0.0, 0.0), sdf, kEpsilon, H_out), 1e-9); - EXPECT(assert_equal(Matrix13::Zero(), H_out, 1e-9)); + EXPECT(assert_equal(Matrix(Matrix13::Zero()), Matrix(H_out), 1e-9)); } // The frame attached overload reads the same field through a moving frame, so // it must agree with the world frame overload on the transformed point. TEST(ObstacleCost, frameAttachedOverload) { const Point3 center(1.0, 1.0, 1.0); - const SignedDistanceField sdf = - makeSphereSDF(center, kRadius, Point3(0, 0, 0), kCell, 41, 41, 41); + const SignedDistanceField sdf = makeSphereSDF( + center, kRadius, Point3(-kHalfCell, -kHalfCell, -kHalfCell), kCell, 41, + 41, 41); const Pose3 wTs(Rot3::RzRyRx(0.2, -0.1, 0.35), Point3(0.4, -0.3, 0.25)); - const Point3 wP(1.3, 1.05, 0.9); + // Place the point in the field's frame and push it out to the world, so it is + // known to land inside the grid rather than reading as free space. + const Point3 sP = center + Point3(kRadius + 0.5 * kEpsilon, 0.03, -0.04); + const Point3 wP = wTs.transformFrom(sP); - const double expected = - hingeLossObstacleCost(wTs.transformTo(wP), sdf, kEpsilon); + const double expected = hingeLossObstacleCost(sP, sdf, kEpsilon); Matrix16 H_pose; Matrix13 H_point; const double actual = hingeLossObstacleCost(wTs, wP, sdf, kEpsilon, H_pose, H_point); EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9); + EXPECT(actual > 0.0); // the point is inside the band, so this is not vacuous // With the identity pose it degenerates to the world frame overload. - EXPECT_DOUBLES_EQUAL(hingeLossObstacleCost(wP, sdf, kEpsilon), - hingeLossObstacleCost(Pose3(), wP, sdf, kEpsilon), 1e-9); + EXPECT_DOUBLES_EQUAL(hingeLossObstacleCost(sP, sdf, kEpsilon), + hingeLossObstacleCost(Pose3(), sP, sdf, kEpsilon), 1e-9); std::function f = [&](const Pose3 &T, const Point3 &p) { return hingeLossObstacleCost(T, p, sdf, kEpsilon); }; EXPECT(assert_equal( - gtsam::numericalDerivative21(f, wTs, wP), + Matrix(gtsam::numericalDerivative21(f, wTs, wP)), Matrix(H_pose), 1e-5)); EXPECT(assert_equal( - gtsam::numericalDerivative22(f, wTs, wP), + Matrix(gtsam::numericalDerivative22(f, wTs, wP)), Matrix(H_point), 1e-5)); } @@ -218,10 +228,12 @@ static std::vector wristPoints() { } static Vector startConfig() { - return (Vector(9) << 2.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0).finished(); + return (Vector(9) << 2.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0) + .finished(); } static Vector goalConfig() { - return (Vector(9) << 3.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0).finished(); + return (Vector(9) << 3.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0) + .finished(); } // Only robot1's joints are given to the model, so the bridge2 subtree is never @@ -236,14 +248,17 @@ TEST(RobotQueryPoints, jacobiansAgainstNumerical) { std::vector Js; model.queryPoints(q, &wPs, &Js); + // traits::dimension is Eigen::Dynamic, which + // numericalDerivative cannot perturb, so q is probed as a fixed size Vector9. for (size_t i = 0; i < model.nrPoints(); ++i) { - std::function f = [&](const Vector &qq) { + std::function f = [&](const Vector9 &qq) { std::vector ps; model.queryPoints(qq, &ps); return ps[i]; }; - EXPECT(assert_equal(gtsam::numericalDerivative11(f, q), - Js[i], 1e-5)); + EXPECT(assert_equal( + gtsam::numericalDerivative11(f, Vector9(q)), Js[i], + 1e-5)); } } @@ -335,9 +350,9 @@ TEST(ObstacleSDFFactor, planAroundSphere) { gtsam::LevenbergMarquardtOptimizer(graph, init, params).optimize(); // The endpoints are held, and every query point at every support state has - // been pushed clear of the sphere by at least epsilon. This is checked against - // the analytic sphere rather than the sampled field, so a point that escaped - // the grid cannot pass by reading as free space. + // been pushed clear of the sphere by at least epsilon. Checking this against + // the analytic sphere rather than the sampled field means a point that + // escaped the grid cannot pass by reading as free space. EXPECT(assert_equal(q_start, result.at(X(0)), 1e-3)); EXPECT(assert_equal(q_goal, result.at(X(N - 1)), 1e-3)); @@ -362,14 +377,15 @@ TEST(ObstacleSDFFactorGP, agreesWithUnaryFactorAtTauZero) { std::vector mid_points; model.queryPoints(q1, &mid_points); const Point3 center = mid_points[0] + Point3(kRadius + 0.5 * kEpsilon, 0, 0); - auto sdf = std::make_shared(makeSphereSDF( - center, kRadius, center - Point3(1.0, 1.0, 1.0), kCell, 41, 41, 41)); + const Point3 origin = center - Point3::Constant(1.0 - kHalfCell); + auto sdf = std::make_shared( + makeSphereSDF(center, kRadius, origin, kCell, 41, 41, 41)); const double delta_t = 0.5, cost_sigma = 0.01; ObstacleSDFFactor unary(X(0), model, sdf, cost_sigma, kEpsilon); ObstacleSDFFactorGP interpolated(X(0), V(0), X(1), V(1), model, sdf, - cost_sigma, kEpsilon, Isotropic::Sigma(dof, 1.0), - delta_t, 0.0); + cost_sigma, kEpsilon, + Isotropic::Sigma(dof, 1.0), delta_t, 0.0); EXPECT(assert_equal(unary.evaluateError(q1), interpolated.evaluateError(q1, v1, q2, v2), 1e-9)); diff --git a/gtdynamics/utils/utils.cpp b/gtdynamics/utils/utils.cpp index 2699d7eb7..9d4025c99 100644 --- a/gtdynamics/utils/utils.cpp +++ b/gtdynamics/utils/utils.cpp @@ -36,6 +36,10 @@ gtsam::Vector radians(const gtsam::Vector °ree) { gtsam::Matrix getQc(const gtsam::SharedNoiseModel Qc_model) { gtsam::noiseModel::Gaussian *Gassian_model = dynamic_cast(Qc_model.get()); + // A non-Gaussian model, such as a robust one, casts to null. + if (!Gassian_model) { + throw std::invalid_argument("getQc: Qc_model must be a Gaussian model."); + } return (Gassian_model->R().transpose() * Gassian_model->R()).inverse(); } From d556c36191325f469902830bdfbd5d4833fa0a49 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Thu, 9 Jul 2026 15:09:32 -0400 Subject: [PATCH 07/27] python tests --- python/tests/test_gpmp2_planning.py | 226 ++++++++++++++++++++++++++++ 1 file changed, 226 insertions(+) create mode 100644 python/tests/test_gpmp2_planning.py diff --git a/python/tests/test_gpmp2_planning.py b/python/tests/test_gpmp2_planning.py new file mode 100644 index 000000000..ed1e11b2e --- /dev/null +++ b/python/tests/test_gpmp2_planning.py @@ -0,0 +1,226 @@ +""" +GTDynamics Copyright 2020, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +See LICENSE for the license information + +Unit tests for planning a trajectory around a spherical obstacle, mirroring +gtdynamics/gpmp2/tests/testObstacleFactors.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 V, X +from gtsam.utils.test_case import GtsamTestCase + +import gtdynamics as gtd + +CELL = 0.05 +RADIUS = 0.15 +EPSILON = 0.10 +COST_SIGMA = 0.01 + +# The nine movable joints of robot1, in the order q indexes them: the three +# gantry prismatic joints, then the six arm revolute joints. +ROBOT1_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" +] + +Q_START = np.array([2.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0]) +Q_GOAL = np.array([3.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0]) + + +def grid_positions(origin, cell, counts): + """Return the 3 x N node positions of a uniform grid.""" + axes = [origin[i] + cell * np.arange(counts[i]) for i in range(3)] + grid = np.meshgrid(*axes, indexing="ij") + return np.vstack([axis.ravel() for axis in grid]) + + +def sphere_sdf(center, radius, origin, cell, counts): + """Sample the exact signed distance to a sphere onto a uniform grid.""" + positions = grid_positions(origin, cell, counts) + distances = np.linalg.norm(positions - center.reshape(3, 1), + axis=0) - radius + # Each distance carries the position it was sampled at, so the order the + # columns arrive in cannot transpose the field. + return gtd.SignedDistanceField(positions, distances) + + +class TestSignedDistanceField(GtsamTestCase): + """Test the signed distance field built from positions and distances.""" + + def setUp(self): + self.center = np.array([0.5, 0.0, 1.25]) + self.origin = np.array([0.3, -0.2, 1.1]) + self.counts = (6, 5, 4) + + def test_interpolates_the_sphere(self): + """Trilinear interpolation must recover the analytic distance.""" + sdf = sphere_sdf(self.center, RADIUS, self.origin, CELL, self.counts) + self.assertEqual(sdf.xCount(), 6) + self.assertEqual(sdf.yCount(), 5) + self.assertEqual(sdf.zCount(), 4) + self.assertAlmostEqual(sdf.cellSize(), CELL, places=9) + np.testing.assert_allclose(sdf.origin(), self.origin, atol=1e-9) + + probe = self.center + np.array([0.5 * CELL, 0.5 * CELL, 0.5 * CELL]) + expected = np.linalg.norm(probe - self.center) - RADIUS + self.assertAlmostEqual(sdf.getSignedDistance(probe), expected, places=2) + + def test_column_order_does_not_matter(self): + """Shuffling the columns must give back an identical field.""" + positions = grid_positions(self.origin, CELL, self.counts) + distances = np.linalg.norm(positions - self.center.reshape(3, 1), + axis=0) - RADIUS + + ordered = gtd.SignedDistanceField(positions, distances) + permutation = np.random.default_rng(42).permutation(positions.shape[1]) + shuffled = gtd.SignedDistanceField(positions[:, permutation], + distances[permutation]) + self.assertTrue(ordered.equals(shuffled, 1e-9)) + + def test_rejects_a_non_grid(self): + """Positions that are not the nodes of a uniform cubic grid throw.""" + positions = grid_positions(self.origin, CELL, self.counts) + distances = np.zeros(positions.shape[1]) + + stretched = positions.copy() + stretched[2, :] *= 2.0 # z spacing no longer matches x and y + with self.assertRaises(ValueError): + gtd.SignedDistanceField(stretched, distances) + + duplicated = positions.copy() + duplicated[:, 1] = duplicated[:, 0] + with self.assertRaises(ValueError): + gtd.SignedDistanceField(duplicated, distances) + + with self.assertRaises(ValueError): + gtd.SignedDistanceField(positions, distances[:-1]) + + +class TestObstaclePlanning(GtsamTestCase): + """Plan a trajectory for robot1 of bar_lab around a spherical obstacle.""" + + def setUp(self): + self.robot = gtd.CreateRobotFromFile( + str(Path(gtd.URDF_PATH) / "bar_lab.urdf")) + + joints = [self.robot.joint(name) for name in ROBOT1_JOINTS] + + # Two query points on the wrist: the link CoM, and a point out along + # the tool. Neither carries a radius; the standoff lives in epsilon. + link6 = self.robot.link("robot1_link_6") + points = gtd.PointOnLinks() + points.append(gtd.PointOnLink(link6, np.array([0.0, 0.0, 0.0]))) + points.append(gtd.PointOnLink(link6, np.array([0.0, 0.0, 0.1]))) + + # Only robot1's joints are given, so the bridge2 subtree is never + # traversed and robot2 is absent from the model entirely. + self.model = gtd.RobotQueryPoints(self.robot, "columns", joints, points) + + def clearance(self, q, center): + """Distance from the closest query point at q to the sphere centre.""" + points = self.model.worldPoints(q) + return np.linalg.norm(points - center.reshape(3, 1), axis=0).min() + + def test_query_points(self): + """The model spans nine joints and carries two query points.""" + self.assertEqual(self.model.dof(), 9) + self.assertEqual(self.model.nrPoints(), 2) + self.assertEqual(self.model.worldPoints(Q_START).shape, (3, 2)) + + def test_plan_around_sphere(self): + """The trajectory bows around a sphere planted on its straight line.""" + dof, n_states = self.model.dof(), 5 + total_time = 2.0 + delta_t = total_time / (n_states - 1) + velocity = (Q_GOAL - Q_START) / total_time + + # Straight line initialisation, and the wrist positions it sweeps. + line = [ + Q_START + (k / (n_states - 1)) * (Q_GOAL - Q_START) + for k in range(n_states) + ] + swept = np.hstack([self.model.worldPoints(q) for q in line]) + + # Plant the sphere on the wrist at the midpoint of the straight line, + # so the initialisation starts deep inside it. + center = self.model.worldPoints(line[n_states // 2])[:, 0] + + # Size the grid to contain the whole swept path with room to detour. A + # point that leaves the grid reads as free space. + pad = 0.7 + origin = swept.min(axis=1) - pad + extent = (swept.max(axis=1) + pad) - origin + counts = np.ceil(extent / CELL).astype(int) + 1 + sdf = sphere_sdf(center, RADIUS, origin, CELL, counts) + + # The field really does describe the sphere we planted. + outside = center + np.array([0.4, 0.0, 0.0]) + self.assertAlmostEqual(sdf.getSignedDistance(outside), 0.4 - RADIUS, + places=2) + + # The problem is only feasible if the pinned endpoints are already + # clear, and only meaningful if the straight line is not. + self.assertLess(self.clearance(line[n_states // 2], center), + RADIUS + EPSILON) + self.assertGreater(self.clearance(Q_START, center), RADIUS + EPSILON) + self.assertGreater(self.clearance(Q_GOAL, center), RADIUS + EPSILON) + + graph = gtsam.NonlinearFactorGraph() + qc_model = gtsam.noiseModel.Isotropic.Sigma(dof, 1.0) + endpoint_model = gtsam.noiseModel.Isotropic.Sigma(dof, 1e-4) + + graph.add(gtsam.PriorFactorVector(X(0), Q_START, endpoint_model)) + graph.add(gtsam.PriorFactorVector(V(0), np.zeros(dof), endpoint_model)) + graph.add( + gtsam.PriorFactorVector(X(n_states - 1), Q_GOAL, endpoint_model)) + graph.add( + gtsam.PriorFactorVector(V(n_states - 1), np.zeros(dof), + endpoint_model)) + + for k in range(n_states): + graph.add( + gtd.ObstacleSDFFactor(X(k), self.model, sdf, COST_SIGMA, + EPSILON)) + # The interpolated obstacle cost would only be a Gaussian process + # posterior mean because this prior, with the same Qc_model and + # delta_t, joins the same support states. + for k in range(n_states - 1): + graph.add( + gtd.GPLinearPrior(X(k), V(k), X(k + 1), V(k + 1), delta_t, + qc_model)) + + initial = gtsam.Values() + for k in range(n_states): + initial.insert(X(k), line[k]) + initial.insert(V(k), velocity) + + params = gtsam.LevenbergMarquardtParams() + params.setMaxIterations(100) + result = gtsam.LevenbergMarquardtOptimizer(graph, initial, + params).optimize() + + # The endpoints are held, and every query point at every support state + # has been pushed clear of the sphere by at least epsilon. Checking + # this against the analytic sphere rather than the sampled field means + # a point that escaped the grid cannot pass by reading as free space. + np.testing.assert_allclose(result.atVector(X(0)), Q_START, atol=1e-3) + np.testing.assert_allclose(result.atVector(X(n_states - 1)), Q_GOAL, + atol=1e-3) + for k in range(n_states): + points = self.model.worldPoints(result.atVector(X(k))) + distances = np.linalg.norm(points - center.reshape(3, 1), axis=0) + self.assertTrue(np.all(distances > RADIUS + EPSILON - 0.02)) + + +if __name__ == "__main__": + unittest.main() From ca2275b35989f4152bedfa7ac67b485a4828660b Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Thu, 9 Jul 2026 15:17:02 -0400 Subject: [PATCH 08/27] edits for python --- gtdynamics.i | 75 ++++++++++++++++++ gtdynamics/gpmp2/RobotQueryPoints.h | 13 ++++ gtdynamics/gpmp2/SignedDistanceField.h | 102 ++++++++++++++++++++++++- 3 files changed, 189 insertions(+), 1 deletion(-) diff --git a/gtdynamics.i b/gtdynamics.i index df428896d..f389b9827 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -1033,6 +1033,81 @@ class Trajectory { void writeToFile(const gtdynamics::Robot &robot, const string &name, const gtsam::Values &results) const; }; +/********************** gpmp2 **********************/ +#include +class SignedDistanceField { + SignedDistanceField(); + SignedDistanceField(const gtsam::Matrix &positions, + const gtsam::Vector &distances); + SignedDistanceField(const gtsam::Matrix &positions, + const gtsam::Vector &distances, double tol); + + 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 +class RobotQueryPoints { + RobotQueryPoints(const gtdynamics::Robot &robot, string base_link_name, + const std::vector &joints, + const gtdynamics::PointOnLinks &points); + RobotQueryPoints(const gtdynamics::Robot &robot, string base_link_name, + const std::vector &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 +class GPLinearPrior : gtsam::NoiseModelFactor { + GPLinearPrior(gtsam::Key pose_key1, gtsam::Key vel_key1, gtsam::Key pose_key2, + gtsam::Key vel_key2, double delta_t, + const gtsam::noiseModel::Base *Qc_model); + + size_t dof() const; + double deltaT() const; + void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter = + gtdynamics::GTDKeyFormatter); +}; + +#include +class ObstacleSDFFactor : gtsam::NoiseModelFactor { + ObstacleSDFFactor(gtsam::Key q_key, const gtdynamics::RobotQueryPoints &robot, + const gtdynamics::SignedDistanceField *sdf, + double cost_sigma, double epsilon); + + double epsilon() const; + void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter = + gtdynamics::GTDKeyFormatter); +}; + +#include +class ObstacleSDFFactorGP : gtsam::NoiseModelFactor { + ObstacleSDFFactorGP(gtsam::Key q_key1, gtsam::Key v_key1, gtsam::Key q_key2, + gtsam::Key v_key2, + const gtdynamics::RobotQueryPoints &robot, + const gtdynamics::SignedDistanceField *sdf, + double cost_sigma, double epsilon, + const gtsam::noiseModel::Base *Qc_model, double delta_t, + double tau); + + double epsilon() 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/gpmp2/RobotQueryPoints.h b/gtdynamics/gpmp2/RobotQueryPoints.h index 72eefd657..197a89040 100644 --- a/gtdynamics/gpmp2/RobotQueryPoints.h +++ b/gtdynamics/gpmp2/RobotQueryPoints.h @@ -160,6 +160,19 @@ class RobotQueryPoints { } } } + + /** + * World positions of the query points, one per column. + * @param q stacked joint angles + * @returns a 3 x nrPoints matrix of world positions + */ + gtsam::Matrix worldPoints(const gtsam::Vector &q) const { + std::vector wPs; + queryPoints(q, &wPs); + gtsam::Matrix points(3, nrPoints()); + for (size_t i = 0; i < nrPoints(); ++i) points.col(i) = wPs[i]; + return points; + } }; // \class RobotQueryPoints } // namespace gtdynamics diff --git a/gtdynamics/gpmp2/SignedDistanceField.h b/gtdynamics/gpmp2/SignedDistanceField.h index 9fff44342..f39c0a333 100644 --- a/gtdynamics/gpmp2/SignedDistanceField.h +++ b/gtdynamics/gpmp2/SignedDistanceField.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -38,7 +39,9 @@ namespace gtdynamics { * indexed as data[z](row, col) with row spanning y and col spanning x. A numpy * array laid out as sdf[nx, ny, nz] therefore transposes into layer z as * sdf[:, :, z].T -- passing sdf[:, :, z] directly swaps the x and y axes, which - * leaves the distances plausible but silently transposes the gradient. + * leaves the distances plausible but silently transposes the gradient. The + * constructor taking positions and distances is immune to this, since every + * value carries the position it was sampled at. */ class SignedDistanceField { public: @@ -88,6 +91,103 @@ class SignedDistanceField { cell_size_(cell_size), data_(std::vector(field_z)) {} + /** + * Constructor from sampled positions and their signed distances. The origin, + * cell size and grid extent are inferred from the positions, which must be + * exactly the nodes of a uniform grid of equal spacing on all three axes. + * Anything else throws rather than yield a warped field. Each distance is + * placed by its own position, so the columns may arrive in any order and the + * axis convention of the caller's array cannot transpose the field. + * + * @param positions 3 x N matrix of node positions, in frame s + * @param distances N signed distances, one per column of positions + * @param tol tolerance for matching a position onto a grid node, in metres + */ + SignedDistanceField(const gtsam::Matrix &positions, + const gtsam::Vector &distances, double tol = 1e-6) { + if (positions.rows() != 3) { + throw std::invalid_argument( + "SignedDistanceField: positions must be a 3 x N matrix."); + } + if (positions.cols() != distances.size()) { + throw std::invalid_argument( + "SignedDistanceField: positions and distances must agree in count."); + } + + // Recover each axis's nodes, then its uniform spacing. + std::vector spacing(3); + std::vector low(3); + std::vector count(3); + for (int axis = 0; axis < 3; ++axis) { + std::vector values; + values.reserve(positions.cols()); + for (Eigen::Index n = 0; n < positions.cols(); ++n) { + values.push_back(positions(axis, n)); + } + std::sort(values.begin(), values.end()); + std::vector nodes; + for (double value : values) { + if (nodes.empty() || value - nodes.back() > tol) nodes.push_back(value); + } + if (nodes.size() < 2) { + throw std::invalid_argument( + "SignedDistanceField: each axis needs at least two distinct " + "positions."); + } + low[axis] = nodes.front(); + count[axis] = nodes.size(); + spacing[axis] = (nodes.back() - nodes.front()) / (nodes.size() - 1); + for (size_t i = 0; i < nodes.size(); ++i) { + if (std::fabs(nodes[i] - (low[axis] + i * spacing[axis])) > tol) { + throw std::invalid_argument( + "SignedDistanceField: positions are not uniformly spaced."); + } + } + } + if (std::fabs(spacing[0] - spacing[1]) > tol || + std::fabs(spacing[0] - spacing[2]) > tol) { + throw std::invalid_argument( + "SignedDistanceField: spacing must be equal on all three axes."); + } + if (static_cast(positions.cols()) != + count[0] * count[1] * count[2]) { + throw std::invalid_argument( + "SignedDistanceField: positions do not fill the grid exactly once."); + } + + origin_ = gtsam::Point3(low[0], low[1], low[2]); + cell_size_ = spacing[0]; + field_cols_ = count[0]; + field_rows_ = count[1]; + field_z_ = count[2]; + data_ = std::vector( + field_z_, gtsam::Matrix::Zero(field_rows_, field_cols_)); + + // Scatter each distance to the node its own position names. + std::vector filled(positions.cols(), false); + for (Eigen::Index n = 0; n < positions.cols(); ++n) { + size_t idx[3]; + for (int axis = 0; axis < 3; ++axis) { + const double fractional = (positions(axis, n) - low[axis]) / cell_size_; + const double rounded = std::round(fractional); + if (std::fabs(fractional - rounded) * cell_size_ > tol || + rounded < 0.0 || rounded >= static_cast(count[axis])) { + throw std::invalid_argument( + "SignedDistanceField: a position does not lie on a grid node."); + } + idx[axis] = static_cast(rounded); + } + const size_t flat = + (idx[2] * field_rows_ + idx[1]) * field_cols_ + idx[0]; + if (filled[flat]) { + throw std::invalid_argument( + "SignedDistanceField: two positions land on the same grid node."); + } + filled[flat] = true; + data_[idx[2]](idx[1], idx[0]) = distances(n); + } + } + ~SignedDistanceField() {} /// Insert one z layer of the field, indexed as (row = y, col = x). From 79ca2e0b958a253fd65e514f911ed90e5aa1ccdd Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Thu, 9 Jul 2026 16:36:43 -0400 Subject: [PATCH 09/27] fixes unit test(?) --- python/tests/test_gpmp2_planning.py | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/python/tests/test_gpmp2_planning.py b/python/tests/test_gpmp2_planning.py index ed1e11b2e..10e531934 100644 --- a/python/tests/test_gpmp2_planning.py +++ b/python/tests/test_gpmp2_planning.py @@ -59,21 +59,29 @@ class TestSignedDistanceField(GtsamTestCase): def setUp(self): self.center = np.array([0.5, 0.0, 1.25]) - self.origin = np.array([0.3, -0.2, 1.1]) - self.counts = (6, 5, 4) + # Counts differ per axis to catch a transposed field, and the grid is + # centred on the sphere so a probe near its surface stays in range. + self.counts = (13, 11, 9) + self.origin = self.center - CELL * (np.array(self.counts) - 1) / 2.0 def test_interpolates_the_sphere(self): """Trilinear interpolation must recover the analytic distance.""" sdf = sphere_sdf(self.center, RADIUS, self.origin, CELL, self.counts) - self.assertEqual(sdf.xCount(), 6) - self.assertEqual(sdf.yCount(), 5) - self.assertEqual(sdf.zCount(), 4) + self.assertEqual(sdf.xCount(), 13) + self.assertEqual(sdf.yCount(), 11) + self.assertEqual(sdf.zCount(), 9) self.assertAlmostEqual(sdf.cellSize(), CELL, places=9) np.testing.assert_allclose(sdf.origin(), self.origin, atol=1e-9) - probe = self.center + np.array([0.5 * CELL, 0.5 * CELL, 0.5 * CELL]) - expected = np.linalg.norm(probe - self.center) - RADIUS - self.assertAlmostEqual(sdf.getSignedDistance(probe), expected, places=2) + probe = self.center + np.array([0.25, 0.0, 0.0]) + self.assertAlmostEqual(sdf.getSignedDistance(probe), 0.25 - RADIUS, + places=2) + + def test_rejects_a_query_outside_the_grid(self): + """A point beyond the grid is out of range, not silently free space.""" + sdf = sphere_sdf(self.center, RADIUS, self.origin, CELL, self.counts) + with self.assertRaises(RuntimeError): + sdf.getSignedDistance(self.center + np.array([10.0, 0.0, 0.0])) def test_column_order_does_not_matter(self): """Shuffling the columns must give back an identical field.""" From b10c0fef4f3a975ec6b13c2d448ad850fc71ba5e Mon Sep 17 00:00:00 2001 From: Karthik Shaji <58968966+kshaji3@users.noreply.github.com> Date: Thu, 9 Jul 2026 21:09:25 -0400 Subject: [PATCH 10/27] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- gtdynamics/utils/utils.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtdynamics/utils/utils.cpp b/gtdynamics/utils/utils.cpp index 9d4025c99..b32b20b85 100644 --- a/gtdynamics/utils/utils.cpp +++ b/gtdynamics/utils/utils.cpp @@ -34,13 +34,13 @@ gtsam::Vector radians(const gtsam::Vector °ree) { } gtsam::Matrix getQc(const gtsam::SharedNoiseModel Qc_model) { - gtsam::noiseModel::Gaussian *Gassian_model = + gtsam::noiseModel::Gaussian *Gaussian_model = dynamic_cast(Qc_model.get()); // A non-Gaussian model, such as a robust one, casts to null. - if (!Gassian_model) { + if (!Gaussian_model) { throw std::invalid_argument("getQc: Qc_model must be a Gaussian model."); } - return (Gassian_model->R().transpose() * Gassian_model->R()).inverse(); + return (Gaussian_model->R().transpose() *Gaussian_model->R()).inverse(); } gtsam::Vector q_trajectory(int i, int total_step, From 9c9bbd4ff6605c8c1bde5a61cc078a4e64e73333 Mon Sep 17 00:00:00 2001 From: Karthik Shaji <58968966+kshaji3@users.noreply.github.com> Date: Thu, 9 Jul 2026 21:10:17 -0400 Subject: [PATCH 11/27] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- gtdynamics/gpmp2/SignedDistanceField.h | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/gtdynamics/gpmp2/SignedDistanceField.h b/gtdynamics/gpmp2/SignedDistanceField.h index f39c0a333..fbea91080 100644 --- a/gtdynamics/gpmp2/SignedDistanceField.h +++ b/gtdynamics/gpmp2/SignedDistanceField.h @@ -75,11 +75,26 @@ class SignedDistanceField { SignedDistanceField(const gtsam::Point3 &origin, double cell_size, const std::vector &data) : origin_(origin), - field_rows_(data.at(0).rows()), - field_cols_(data.at(0).cols()), - field_z_(data.size()), - cell_size_(cell_size), - data_(data) {} + field_rows_(0), + field_cols_(0), + field_z_(0), + cell_size_(cell_size) { + if (data.empty()) { + throw std::invalid_argument( + "SignedDistanceField: data must contain at least one z layer."); + } + field_rows_ = static_cast(data[0].rows()); + field_cols_ = static_cast(data[0].cols()); + field_z_ = data.size(); + for (size_t z = 0; z < field_z_; ++z) { + if (static_cast(data[z].rows()) != field_rows_ || + static_cast(data[z].cols()) != field_cols_) { + throw std::invalid_argument( + "SignedDistanceField: all z layers must have identical dimensions."); + } + } + data_ = data; + } /// Constructor with no data, to be filled in later by initFieldData. SignedDistanceField(const gtsam::Point3 &origin, double cell_size, From f4fdb0efc8a1aa519f2d86a9cadcf98335717c41 Mon Sep 17 00:00:00 2001 From: Karthik Shaji <58968966+kshaji3@users.noreply.github.com> Date: Thu, 9 Jul 2026 21:10:46 -0400 Subject: [PATCH 12/27] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- gtdynamics/gpmp2/SignedDistanceField.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gtdynamics/gpmp2/SignedDistanceField.h b/gtdynamics/gpmp2/SignedDistanceField.h index fbea91080..33677dcca 100644 --- a/gtdynamics/gpmp2/SignedDistanceField.h +++ b/gtdynamics/gpmp2/SignedDistanceField.h @@ -208,8 +208,13 @@ class SignedDistanceField { /// Insert one z layer of the field, indexed as (row = y, col = x). void initFieldData(size_t z_idx, const gtsam::Matrix &field_layer) { if (z_idx >= field_z_) { - throw std::runtime_error( - "[SignedDistanceField] matrix layer out of index"); + throw std::out_of_range( + "SignedDistanceField::initFieldData: z_idx out of range."); + } + if (static_cast(field_layer.rows()) != field_rows_ || + static_cast(field_layer.cols()) != field_cols_) { + throw std::invalid_argument( + "SignedDistanceField::initFieldData: field_layer dimensions must match the field."); } data_[z_idx] = field_layer; } From de4c718b36b27ef299b148a0ebba50592bd5cb79 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Fri, 10 Jul 2026 10:40:37 -0400 Subject: [PATCH 13/27] joint, velocity limits --- gtdynamics/factors/JointLimitFactorVector.h | 156 ++++++++++++++++++ .../factors/VelocityLimitFactorVector.h | 151 +++++++++++++++++ gtdynamics/gpmp2/JointLimitCost.h | 37 +++++ 3 files changed, 344 insertions(+) create mode 100644 gtdynamics/factors/JointLimitFactorVector.h create mode 100644 gtdynamics/factors/VelocityLimitFactorVector.h create mode 100644 gtdynamics/gpmp2/JointLimitCost.h diff --git a/gtdynamics/factors/JointLimitFactorVector.h b/gtdynamics/factors/JointLimitFactorVector.h new file mode 100644 index 000000000..95b50e542 --- /dev/null +++ b/gtdynamics/factors/JointLimitFactorVector.h @@ -0,0 +1,156 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file JointLimitFactorVector.h + * @brief Joint limit factor on a stacked joint angle vector. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#include +#endif + +namespace gtdynamics { + +/// Unary factor keeping every entry of a stacked joint angle vector within its +/// limits, one error row per joint. +class JointLimitFactorVector : public gtsam::NoiseModelFactorN { + private: + using This = JointLimitFactorVector; + using Base = gtsam::NoiseModelFactorN; + + gtsam::Vector down_limit_, up_limit_, limit_thresh_; + + public: + /// Default constructor, only for serialization. + JointLimitFactorVector() {} + + /** + * Constructor. + * @param q_key key of the stacked joint angle vector + * @param cost_model cost function covariance, one row per joint + * @param down_limit lower limit of each joint + * @param up_limit upper limit of each joint + * @param limit_thresh standoff kept from each joint's limits + */ + JointLimitFactorVector(gtsam::Key q_key, + const gtsam::SharedNoiseModel &cost_model, + const gtsam::Vector &down_limit, + const gtsam::Vector &up_limit, + const gtsam::Vector &limit_thresh) + : Base(cost_model, q_key), + down_limit_(down_limit), + up_limit_(up_limit), + limit_thresh_(limit_thresh) { + const size_t dof = cost_model->dim(); + if (static_cast(down_limit.size()) != dof || + static_cast(up_limit.size()) != dof || + static_cast(limit_thresh.size()) != dof) { + throw std::invalid_argument( + "JointLimitFactorVector: limit vectors must match the noise model " + "dimension."); + } + for (size_t i = 0; i < dof; ++i) { + // Both hinges would be active everywhere, leaving the factor no zero. + if (down_limit_(i) + limit_thresh_(i) > up_limit_(i) - limit_thresh_(i)) { + throw std::invalid_argument( + "JointLimitFactorVector: a joint's limits and threshold leave no " + "feasible interval."); + } + } + } + + ~JointLimitFactorVector() 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))); + } + + /// Evaluate the hinge loss at every joint, and its Jacobian. + gtsam::Vector evaluateError( + const gtsam::Vector &q, + gtsam::OptionalMatrixType H1 = nullptr) const override { + const size_t dof = this->dof(); + if (static_cast(q.size()) != dof) { + throw std::invalid_argument( + "JointLimitFactorVector: q size must match the limit vectors."); + } + + if (H1) *H1 = gtsam::Matrix::Zero(dof, dof); + gtsam::Vector err(dof); + for (size_t i = 0; i < dof; ++i) { + if (H1) { + double H_q; + err(i) = hingeLossJointLimitCost(q(i), down_limit_(i), up_limit_(i), + limit_thresh_(i), &H_q); + (*H1)(i, i) = H_q; + } else { + err(i) = hingeLossJointLimitCost(q(i), down_limit_(i), up_limit_(i), + limit_thresh_(i)); + } + } + return err; + } + + /// Return the degrees of freedom. + size_t dof() const { return down_limit_.size(); } + + const gtsam::Vector &downLimit() const { return down_limit_; } + const gtsam::Vector &upLimit() const { return up_limit_; } + const gtsam::Vector &limitThreshold() const { return limit_thresh_; } + + /// Equality up to a tolerance. + bool equals(const gtsam::NonlinearFactor &expected, + double tol = 1e-9) const override { + const This *e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + gtsam::equal_with_abs_tol(down_limit_, e->down_limit_, tol) && + gtsam::equal_with_abs_tol(up_limit_, e->up_limit_, tol) && + gtsam::equal_with_abs_tol(limit_thresh_, e->limit_thresh_, tol); + } + + /// Print contents. + void print(const std::string &s = "", + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "JointLimitFactorVector(" << dof() << ")" << std::endl; + Base::print("", keyFormatter); + std::cout << "limit threshold: " << limit_thresh_.transpose() << std::endl; + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { // NOLINT + ar &boost::serialization::make_nvp( + "NoiseModelFactorN", boost::serialization::base_object(*this)); + } +#endif +}; // \class JointLimitFactorVector + +} // namespace gtdynamics diff --git a/gtdynamics/factors/VelocityLimitFactorVector.h b/gtdynamics/factors/VelocityLimitFactorVector.h new file mode 100644 index 000000000..1f24a66b9 --- /dev/null +++ b/gtdynamics/factors/VelocityLimitFactorVector.h @@ -0,0 +1,151 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file VelocityLimitFactorVector.h + * @brief Velocity limit factor on a stacked joint velocity vector. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION +#include +#include +#include +#endif + +namespace gtdynamics { + +/// Unary factor keeping every entry of a stacked joint velocity vector within +/// its symmetric limit, one error row per joint. +class VelocityLimitFactorVector + : public gtsam::NoiseModelFactorN { + private: + using This = VelocityLimitFactorVector; + using Base = gtsam::NoiseModelFactorN; + + gtsam::Vector vel_limit_, limit_thresh_; + + public: + /// Default constructor, only for serialization. + VelocityLimitFactorVector() {} + + /** + * Constructor. + * @param v_key key of the stacked joint velocity vector + * @param cost_model cost function covariance, one row per joint + * @param vel_limit magnitude of each joint's velocity limit + * @param limit_thresh standoff kept from each joint's limits + */ + VelocityLimitFactorVector(gtsam::Key v_key, + const gtsam::SharedNoiseModel &cost_model, + const gtsam::Vector &vel_limit, + const gtsam::Vector &limit_thresh) + : Base(cost_model, v_key), + vel_limit_(vel_limit), + limit_thresh_(limit_thresh) { + const size_t dof = cost_model->dim(); + if (static_cast(vel_limit.size()) != dof || + static_cast(limit_thresh.size()) != dof) { + throw std::invalid_argument( + "VelocityLimitFactorVector: limit vectors must match the noise model " + "dimension."); + } + for (size_t i = 0; i < dof; ++i) { + // Both hinges would be active everywhere, leaving the factor no zero. + if (limit_thresh_(i) > vel_limit_(i)) { + throw std::invalid_argument( + "VelocityLimitFactorVector: a joint's limit and threshold leave no " + "feasible interval."); + } + } + } + + ~VelocityLimitFactorVector() 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))); + } + + /// Evaluate the hinge loss at every joint, and its Jacobian. + gtsam::Vector evaluateError( + const gtsam::Vector &v, + gtsam::OptionalMatrixType H1 = nullptr) const override { + const size_t dof = this->dof(); + if (static_cast(v.size()) != dof) { + throw std::invalid_argument( + "VelocityLimitFactorVector: v size must match the limit vectors."); + } + + if (H1) *H1 = gtsam::Matrix::Zero(dof, dof); + gtsam::Vector err(dof); + for (size_t i = 0; i < dof; ++i) { + if (H1) { + double H_v; + err(i) = hingeLossJointLimitCost(v(i), -vel_limit_(i), vel_limit_(i), + limit_thresh_(i), &H_v); + (*H1)(i, i) = H_v; + } else { + err(i) = hingeLossJointLimitCost(v(i), -vel_limit_(i), vel_limit_(i), + limit_thresh_(i)); + } + } + return err; + } + + /// Return the degrees of freedom. + size_t dof() const { return vel_limit_.size(); } + + const gtsam::Vector &velLimit() const { return vel_limit_; } + const gtsam::Vector &limitThreshold() const { return limit_thresh_; } + + /// Equality up to a tolerance. + bool equals(const gtsam::NonlinearFactor &expected, + double tol = 1e-9) const override { + const This *e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + gtsam::equal_with_abs_tol(vel_limit_, e->vel_limit_, tol) && + gtsam::equal_with_abs_tol(limit_thresh_, e->limit_thresh_, tol); + } + + /// Print contents. + void print(const std::string &s = "", + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "VelocityLimitFactorVector(" << dof() << ")" << std::endl; + Base::print("", keyFormatter); + std::cout << "limit threshold: " << limit_thresh_.transpose() << std::endl; + } + + private: +#ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { // NOLINT + ar &boost::serialization::make_nvp( + "NoiseModelFactorN", boost::serialization::base_object(*this)); + } +#endif +}; // \class VelocityLimitFactorVector + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/JointLimitCost.h b/gtdynamics/gpmp2/JointLimitCost.h new file mode 100644 index 000000000..0ed17dd43 --- /dev/null +++ b/gtdynamics/gpmp2/JointLimitCost.h @@ -0,0 +1,37 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file JointLimitCost.h + * @brief Hinge loss joint limit cost function. + * @author Karthik Shaji - Adapted from gpmp2 by Jing Dong. + */ + +#pragma once + +namespace gtdynamics { + +/// Hinge loss cost keeping p within its limits, zero until thresh away from +/// either, so thresh is the standoff kept from each limit. +inline double hingeLossJointLimitCost(double p, double down_limit, + double up_limit, double thresh, + double *H_p = nullptr) { + if (p < down_limit + thresh) { + if (H_p) *H_p = -1.0; + return down_limit + thresh - p; + + } else if (p <= up_limit - thresh) { + if (H_p) *H_p = 0.0; + return 0.0; + + } else { + if (H_p) *H_p = 1.0; + return p - up_limit + thresh; + } +} + +} // namespace gtdynamics From db95a61b9fabe25a763cac3f204d5d1919be5ba2 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Fri, 10 Jul 2026 11:17:07 -0400 Subject: [PATCH 14/27] added unit tests for joint limit loss terms --- gtdynamics.i | 25 ++++ gtdynamics/gpmp2/tests/testLimitFactors.cpp | 152 ++++++++++++++++++++ python/tests/test_gpmp2_planning.py | 133 +++++++++++++---- 3 files changed, 285 insertions(+), 25 deletions(-) create mode 100644 gtdynamics/gpmp2/tests/testLimitFactors.cpp diff --git a/gtdynamics.i b/gtdynamics.i index f389b9827..bd198d97c 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -1093,6 +1093,31 @@ class ObstacleSDFFactor : gtsam::NoiseModelFactor { gtdynamics::GTDKeyFormatter); }; +#include +class JointLimitFactorVector : gtsam::NoiseModelFactor { + JointLimitFactorVector(gtsam::Key q_key, + const gtsam::noiseModel::Base *cost_model, + const gtsam::Vector &down_limit, + const gtsam::Vector &up_limit, + const gtsam::Vector &limit_thresh); + + size_t dof() const; + void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter = + gtdynamics::GTDKeyFormatter); +}; + +#include +class VelocityLimitFactorVector : gtsam::NoiseModelFactor { + VelocityLimitFactorVector(gtsam::Key v_key, + const gtsam::noiseModel::Base *cost_model, + const gtsam::Vector &vel_limit, + const gtsam::Vector &limit_thresh); + + size_t dof() const; + void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter = + gtdynamics::GTDKeyFormatter); +}; + #include class ObstacleSDFFactorGP : gtsam::NoiseModelFactor { ObstacleSDFFactorGP(gtsam::Key q_key1, gtsam::Key v_key1, gtsam::Key q_key2, diff --git a/gtdynamics/gpmp2/tests/testLimitFactors.cpp b/gtdynamics/gpmp2/tests/testLimitFactors.cpp new file mode 100644 index 000000000..0a5646d5f --- /dev/null +++ b/gtdynamics/gpmp2/tests/testLimitFactors.cpp @@ -0,0 +1,152 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testLimitFactors.cpp + * @brief test the joint angle and joint velocity limit factors. + * @author Karthik Shaji + */ + +#include +#include +#include +#include +#include +#include + +#include + +using namespace gtdynamics; +using gtsam::assert_equal; +using gtsam::Matrix; +using gtsam::Values; +using gtsam::Vector; +using gtsam::noiseModel::Isotropic; + +static const gtsam::Key kKey = 0; + +/* ********************** joint angle limits *************************** */ + +// Inside the band the cost is zero; outside it rises linearly from thresh away +// from the limit, and the Jacobian is a diagonal of -1, 0 or +1. +TEST(JointLimitFactorVector, errorAndJacobians) { + const Vector down = (Vector(3) << -1.0, 0.0, -2.0).finished(); + const Vector up = (Vector(3) << 1.0, 13.0, 2.0).finished(); + const Vector thresh = (Vector(3) << 0.1, 0.2, 0.05).finished(); + JointLimitFactorVector factor(kKey, Isotropic::Sigma(3, 1.0), down, up, + thresh); + EXPECT_LONGS_EQUAL(3, factor.dof()); + + // Every joint well inside its band. + const Vector inside = (Vector(3) << 0.0, 5.0, 0.0).finished(); + EXPECT(assert_equal(Vector(Vector::Zero(3)), factor.evaluateError(inside), + 1e-9)); + + // Below the first limit, inside the second, above the third. + const Vector outside = (Vector(3) << -1.5, 5.0, 2.5).finished(); + const Vector expected = (Vector(3) << 0.6, 0.0, 0.55).finished(); + EXPECT(assert_equal(expected, factor.evaluateError(outside), 1e-9)); + + Matrix H; + factor.evaluateError(outside, &H); + Matrix expected_H = Matrix::Zero(3, 3); + expected_H(0, 0) = -1.0; + expected_H(2, 2) = 1.0; + EXPECT(assert_equal(expected_H, H, 1e-9)); + + // The hinge is not differentiable at a knee, so probe away from one. + Values values; + values.insert(kKey, outside); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + + Values inside_values; + inside_values.insert(kKey, inside); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, inside_values, 1e-7, 1e-5); +} + +TEST(JointLimitFactorVector, rejectsBadDimensions) { + const Vector down = (Vector(3) << -1.0, 0.0, -2.0).finished(); + const Vector up = (Vector(3) << 1.0, 13.0, 2.0).finished(); + const Vector thresh = (Vector(3) << 0.1, 0.2, 0.05).finished(); + + // A limit vector that disagrees with the noise model dimension. + CHECK_EXCEPTION( + JointLimitFactorVector(kKey, Isotropic::Sigma(2, 1.0), down, up, thresh), + std::invalid_argument); + + // A threshold that leaves the joint no feasible interval. + const Vector wide = (Vector(3) << 0.1, 0.2, 5.0).finished(); + CHECK_EXCEPTION( + JointLimitFactorVector(kKey, Isotropic::Sigma(3, 1.0), down, up, wide), + std::invalid_argument); + + // A configuration whose size disagrees with the limits. + JointLimitFactorVector factor(kKey, Isotropic::Sigma(3, 1.0), down, up, + thresh); + CHECK_EXCEPTION(factor.evaluateError(Vector(Vector::Zero(2))), + std::invalid_argument); +} + +/* ******************** joint velocity limits ************************** */ + +// The velocity limit is the joint limit with symmetric bounds. +TEST(VelocityLimitFactorVector, errorAndJacobians) { + const Vector limit = (Vector(3) << 1.0, 2.0, 3.0).finished(); + const Vector thresh = (Vector(3) << 0.1, 0.2, 0.3).finished(); + VelocityLimitFactorVector factor(kKey, Isotropic::Sigma(3, 1.0), limit, + thresh); + EXPECT_LONGS_EQUAL(3, factor.dof()); + + const Vector inside = (Vector(3) << 0.0, 0.5, -1.0).finished(); + EXPECT(assert_equal(Vector(Vector::Zero(3)), factor.evaluateError(inside), + 1e-9)); + + // Below the first limit, inside the second, above the third. + const Vector outside = (Vector(3) << -1.5, 0.5, 3.0).finished(); + const Vector expected = (Vector(3) << 0.6, 0.0, 0.3).finished(); + EXPECT(assert_equal(expected, factor.evaluateError(outside), 1e-9)); + + Matrix H; + factor.evaluateError(outside, &H); + Matrix expected_H = Matrix::Zero(3, 3); + expected_H(0, 0) = -1.0; + expected_H(2, 2) = 1.0; + EXPECT(assert_equal(expected_H, H, 1e-9)); + + Values values; + values.insert(kKey, outside); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + + Values inside_values; + inside_values.insert(kKey, inside); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, inside_values, 1e-7, 1e-5); +} + +TEST(VelocityLimitFactorVector, rejectsBadDimensions) { + const Vector limit = (Vector(3) << 1.0, 2.0, 3.0).finished(); + const Vector thresh = (Vector(3) << 0.1, 0.2, 0.3).finished(); + + CHECK_EXCEPTION( + VelocityLimitFactorVector(kKey, Isotropic::Sigma(2, 1.0), limit, thresh), + std::invalid_argument); + + // A threshold wider than the limit leaves no feasible interval. + const Vector wide = (Vector(3) << 0.1, 0.2, 4.0).finished(); + CHECK_EXCEPTION( + VelocityLimitFactorVector(kKey, Isotropic::Sigma(3, 1.0), limit, wide), + std::invalid_argument); + + VelocityLimitFactorVector factor(kKey, Isotropic::Sigma(3, 1.0), limit, + thresh); + CHECK_EXCEPTION(factor.evaluateError(Vector(Vector::Zero(2))), + std::invalid_argument); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/python/tests/test_gpmp2_planning.py b/python/tests/test_gpmp2_planning.py index 10e531934..a434b980c 100644 --- a/python/tests/test_gpmp2_planning.py +++ b/python/tests/test_gpmp2_planning.py @@ -36,6 +36,15 @@ Q_START = np.array([2.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0]) Q_GOAL = np.array([3.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0]) +# Joint angle and velocity limits of robot1, in the order q indexes them. +Q_LOWER = np.array( + [0.0, 0.0, 0.0, -2.96706, -1.13446, -3.14159, -5.23599, -2.26893, -5.23599]) +Q_UPPER = np.array( + [13.0, 7.3, 1.75, 2.96706, 2.44346, 1.22173, 5.23599, 2.26893, 5.23599]) +V_LIMIT = np.array( + [2.618, 2.618, 2.618, 2.618, 2.618, 2.618, 6.2832, 6.2832, 7.854]) +LIMIT_THRESH = np.full(9, 1e-3) + def grid_positions(origin, cell, counts): """Return the 3 x N node positions of a uniform grid.""" @@ -130,8 +139,7 @@ def setUp(self): points.append(gtd.PointOnLink(link6, np.array([0.0, 0.0, 0.0]))) points.append(gtd.PointOnLink(link6, np.array([0.0, 0.0, 0.1]))) - # Only robot1's joints are given, so the bridge2 subtree is never - # traversed and robot2 is absent from the model entirely. + # Only robot1's joints are given, so ignore bridge2 subtree self.model = gtd.RobotQueryPoints(self.robot, "columns", joints, points) def clearance(self, q, center): @@ -139,49 +147,53 @@ def clearance(self, q, center): points = self.model.worldPoints(q) return np.linalg.norm(points - center.reshape(3, 1), axis=0).min() - def test_query_points(self): - """The model spans nine joints and carries two query points.""" - self.assertEqual(self.model.dof(), 9) - self.assertEqual(self.model.nrPoints(), 2) - self.assertEqual(self.model.worldPoints(Q_START).shape, (3, 2)) - - def test_plan_around_sphere(self): - """The trajectory bows around a sphere planted on its straight line.""" - dof, n_states = self.model.dof(), 5 - total_time = 2.0 - delta_t = total_time / (n_states - 1) - velocity = (Q_GOAL - Q_START) / total_time - - # Straight line initialisation, and the wrist positions it sweeps. + def sphere_problem(self, n_states): + """Straight line initialisation, and a sphere planted on its midpoint.""" line = [ Q_START + (k / (n_states - 1)) * (Q_GOAL - Q_START) for k in range(n_states) ] swept = np.hstack([self.model.worldPoints(q) for q in line]) - # Plant the sphere on the wrist at the midpoint of the straight line, - # so the initialisation starts deep inside it. + # The sphere sits on the wrist at the midpoint of the straight line, so + # the initialisation starts deep inside it. center = self.model.worldPoints(line[n_states // 2])[:, 0] - # Size the grid to contain the whole swept path with room to detour. A - # point that leaves the grid reads as free space. + # The grid holds the whole swept path with room to detour, since a + # point that leaves it reads as free space. pad = 0.7 origin = swept.min(axis=1) - pad extent = (swept.max(axis=1) + pad) - origin counts = np.ceil(extent / CELL).astype(int) + 1 sdf = sphere_sdf(center, RADIUS, origin, CELL, counts) - # The field really does describe the sphere we planted. - outside = center + np.array([0.4, 0.0, 0.0]) - self.assertAlmostEqual(sdf.getSignedDistance(outside), 0.4 - RADIUS, - places=2) - # The problem is only feasible if the pinned endpoints are already # clear, and only meaningful if the straight line is not. self.assertLess(self.clearance(line[n_states // 2], center), RADIUS + EPSILON) self.assertGreater(self.clearance(Q_START, center), RADIUS + EPSILON) self.assertGreater(self.clearance(Q_GOAL, center), RADIUS + EPSILON) + return line, center, sdf + + def test_query_points(self): + """The model spans nine joints and carries two query points.""" + self.assertEqual(self.model.dof(), 9) + self.assertEqual(self.model.nrPoints(), 2) + self.assertEqual(self.model.worldPoints(Q_START).shape, (3, 2)) + + def test_plan_around_sphere(self): + """The trajectory bows around a sphere planted on its straight line.""" + dof, n_states = self.model.dof(), 5 + total_time = 2.0 + delta_t = total_time / (n_states - 1) + velocity = (Q_GOAL - Q_START) / total_time + + line, center, sdf = self.sphere_problem(n_states) + + # The field really does describe the sphere we planted. + outside = center + np.array([0.4, 0.0, 0.0]) + self.assertAlmostEqual(sdf.getSignedDistance(outside), 0.4 - RADIUS, + places=2) graph = gtsam.NonlinearFactorGraph() qc_model = gtsam.noiseModel.Isotropic.Sigma(dof, 1.0) @@ -229,6 +241,77 @@ def test_plan_around_sphere(self): distances = np.linalg.norm(points - center.reshape(3, 1), axis=0) self.assertTrue(np.all(distances > RADIUS + EPSILON - 0.02)) + def test_plan_under_limits(self): + """The same detour, now respecting joint angle and velocity limits.""" + dof, n_states = self.model.dof(), 5 + total_time = 2.0 + delta_t = total_time / (n_states - 1) + velocity = (Q_GOAL - Q_START) / total_time + + line, center, sdf = self.sphere_problem(n_states) + + # A minimum acceleration move of one metre in two seconds, starting and + # ending at rest, peaks at 0.75 m/s, so the gantry's velocity limit is + # tightened below that to make the velocity factor bind. + v_limit = V_LIMIT.copy() + v_limit[0] = 0.7 + + graph = gtsam.NonlinearFactorGraph() + qc_model = gtsam.noiseModel.Isotropic.Sigma(dof, 1.0) + endpoint_model = gtsam.noiseModel.Isotropic.Sigma(dof, 1e-4) + limit_model = gtsam.noiseModel.Isotropic.Sigma(dof, 1e-3) + + graph.add(gtsam.PriorFactorVector(X(0), Q_START, endpoint_model)) + graph.add(gtsam.PriorFactorVector(V(0), np.zeros(dof), endpoint_model)) + graph.add( + gtsam.PriorFactorVector(X(n_states - 1), Q_GOAL, endpoint_model)) + graph.add( + gtsam.PriorFactorVector(V(n_states - 1), np.zeros(dof), + endpoint_model)) + + for k in range(n_states): + graph.add( + gtd.ObstacleSDFFactor(X(k), self.model, sdf, COST_SIGMA, + EPSILON)) + graph.add( + gtd.JointLimitFactorVector(X(k), limit_model, Q_LOWER, Q_UPPER, + LIMIT_THRESH)) + graph.add( + gtd.VelocityLimitFactorVector(V(k), limit_model, v_limit, + LIMIT_THRESH)) + for k in range(n_states - 1): + graph.add( + gtd.GPLinearPrior(X(k), V(k), X(k + 1), V(k + 1), delta_t, + qc_model)) + + initial = gtsam.Values() + for k in range(n_states): + initial.insert(X(k), line[k]) + initial.insert(V(k), velocity) + + params = gtsam.LevenbergMarquardtParams() + params.setMaxIterations(100) + result = gtsam.LevenbergMarquardtOptimizer(graph, initial, + params).optimize() + + np.testing.assert_allclose(result.atVector(X(0)), Q_START, atol=1e-3) + np.testing.assert_allclose(result.atVector(X(n_states - 1)), Q_GOAL, + atol=1e-3) + + for k in range(n_states): + q, v = result.atVector(X(k)), result.atVector(V(k)) + self.assertTrue(np.all(q > Q_LOWER - 0.02)) + self.assertTrue(np.all(q < Q_UPPER + 0.02)) + self.assertTrue(np.all(np.abs(v) < v_limit + 0.02)) + + points = self.model.worldPoints(q) + distances = np.linalg.norm(points - center.reshape(3, 1), axis=0) + self.assertTrue(np.all(distances > RADIUS + EPSILON - 0.02)) + + # The gantry velocity limit is below the unconstrained peak, so it had + # to bind: without it the midpoint would run at about 0.75 m/s. + self.assertGreater(abs(result.atVector(V(n_states // 2))[0]), 0.3) + if __name__ == "__main__": unittest.main() From 2f359f676af11b7767dd88899dbdf7536a07cd73 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Mon, 13 Jul 2026 17:52:01 -0400 Subject: [PATCH 15/27] added steap impleemntation test --- gtdynamics/gpmp2/tests/testSTEAP.cpp | 294 +++++++++++++++++++++++++++ 1 file changed, 294 insertions(+) create mode 100644 gtdynamics/gpmp2/tests/testSTEAP.cpp diff --git a/gtdynamics/gpmp2/tests/testSTEAP.cpp b/gtdynamics/gpmp2/tests/testSTEAP.cpp new file mode 100644 index 000000000..5ae8dac17 --- /dev/null +++ b/gtdynamics/gpmp2/tests/testSTEAP.cpp @@ -0,0 +1,294 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testSTEAP.cpp + * @brief test simultaneous trajectory estimation and planning: a GPMP2 plan + * solved and then updated with measurement factors through iSAM2. + * @author Karthik Shaji + */ + +#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::Point3; +using gtsam::Values; +using gtsam::Vector; +using gtsam::Vector3; +using gtsam::noiseModel::Isotropic; +using gtsam::symbol_shorthand::V; +using gtsam::symbol_shorthand::X; + +static const double kCell = 0.05; +static const double kRadius = 0.15; +static const double kEpsilon = 0.10; +static const double kCostSigma = 0.01; +static const size_t kNumStates = 5; +static const double kDeltaT = 0.5; + +static const Robot kRobot = + CreateRobotFromFile(kUrdfPath + std::string("bar_lab.urdf")); + +// The nine movable joints of robot1, gantry prismatic then arm revolute. +static std::vector robot1Joints() { + 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"}; + std::vector joints; + for (auto &&name : names) joints.push_back(kRobot.joint(name)); + return joints; +} + +static std::vector wristPoints() { + const LinkSharedPtr link = kRobot.link("robot1_link_6"); + return {PointOnLink(link, Point3(0.0, 0.0, 0.0)), + PointOnLink(link, Point3(0.0, 0.0, 0.1))}; +} + +static Vector startConfig() { + return (Vector(9) << 2.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0) + .finished(); +} +static Vector goalConfig() { + return (Vector(9) << 3.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0) + .finished(); +} + +// Sample the exact signed distance to a sphere onto a grid, layer (row=y,col=x). +static SignedDistanceField makeSphereSDF(const Point3 ¢er, double radius, + const Point3 &origin, double cell, + size_t nx, size_t ny, size_t nz) { + std::vector data(nz); + for (size_t k = 0; k < nz; ++k) { + Matrix layer(ny, nx); + for (size_t i = 0; i < ny; ++i) { + for (size_t j = 0; j < nx; ++j) { + const Point3 p = origin + Point3(j * cell, i * cell, k * cell); + layer(i, j) = (p - center).norm() - radius; + } + } + data[k] = layer; + } + return SignedDistanceField(origin, cell, data); +} + +// Holds the whole planning problem: the query point model, the obstacle field, +// the straight line initialisation, and the sphere centre. +struct SteapProblem { + RobotQueryPoints model; + std::shared_ptr sdf; + std::vector line; + Point3 center; +}; + +static SteapProblem makeProblem() { + RobotQueryPoints model(kRobot, "columns", robot1Joints(), wristPoints()); + const Vector q_start = startConfig(), q_goal = goalConfig(); + + std::vector line(kNumStates); + std::vector swept; + for (size_t k = 0; k < kNumStates; ++k) { + line[k] = q_start + + (static_cast(k) / (kNumStates - 1)) * (q_goal - q_start); + std::vector ps; + model.queryPoints(line[k], &ps); + swept.insert(swept.end(), ps.begin(), ps.end()); + } + + std::vector mid; + model.queryPoints(line[kNumStates / 2], &mid); + const Point3 center = mid[0]; + + const double pad = 0.7; + Vector3 lo = swept[0], hi = swept[0]; + for (auto &&p : swept) { + lo = lo.cwiseMin(Vector3(p)); + hi = hi.cwiseMax(Vector3(p)); + } + const Point3 origin(lo.x() - pad, lo.y() - pad, lo.z() - pad); + const Vector3 extent = (hi - lo) + Vector3::Constant(2 * pad); + const size_t nx = static_cast(std::ceil(extent.x() / kCell)) + 1; + const size_t ny = static_cast(std::ceil(extent.y() / kCell)) + 1; + const size_t nz = static_cast(std::ceil(extent.z() / kCell)) + 1; + auto sdf = std::make_shared( + makeSphereSDF(center, kRadius, origin, kCell, nx, ny, nz)); + + return SteapProblem{model, sdf, line, center}; +} + +// The GPMP2 planning graph: GP priors, obstacle factors, and a start and goal +// fix factor. The position of the goal pose factor is returned so it can later +// be swapped for a measurement, as STEAP does at the end of execution. +static gtsam::NonlinearFactorGraph buildPlanGraph(const SteapProblem &prob, + size_t *goal_factor_pos) { + const size_t dof = prob.model.dof(); + gtsam::NonlinearFactorGraph graph; + auto qc_model = Isotropic::Sigma(dof, 1.0); + auto fix = Isotropic::Sigma(dof, 1e-4); + + graph.addPrior(X(0), startConfig(), fix); + graph.addPrior(V(0), Vector::Zero(dof), fix); + *goal_factor_pos = graph.size(); + graph.addPrior(X(kNumStates - 1), goalConfig(), fix); + graph.addPrior(V(kNumStates - 1), Vector::Zero(dof), fix); + + for (size_t k = 0; k < kNumStates; ++k) { + graph.emplace_shared(X(k), prob.model, prob.sdf, + kCostSigma, kEpsilon); + } + for (size_t k = 0; k < kNumStates - 1; ++k) { + graph.emplace_shared(X(k), V(k), X(k + 1), V(k + 1), kDeltaT, + qc_model); + } + return graph; +} + +static Values straightLineInit(const SteapProblem &prob) { + const size_t dof = prob.model.dof(); + const Vector velocity = + (goalConfig() - startConfig()) / (kNumStates * kDeltaT); + Values init; + for (size_t k = 0; k < kNumStates; ++k) { + init.insert(X(k), prob.line[k]); + init.insert(V(k), velocity); + } + return init; +} + +// iSAM2 is non-linear, so a few updates are needed to reach the batch solution. +static gtsam::ISAM2Params steapParams() { + gtsam::ISAM2Params params; + params.setOptimizationParams(gtsam::ISAM2DoglegParams()); + params.relinearizeThreshold = 0.01; + params.relinearizeSkip = 1; + return params; +} + +static Values iterate(gtsam::ISAM2 &isam, int iterations = 10) { + for (int i = 0; i < iterations; ++i) isam.update(); + return isam.calculateEstimate(); +} + +/* ******************** the initial plan is GPMP2 *********************** */ + +// At the first step, before any measurement, STEAP is exactly the GPMP2 plan. +// Solved through iSAM2, the endpoints are held and every query point is pushed +// clear of the sphere, checked against the analytic sphere so a point that left +// the grid cannot pass by reading as free space. +TEST(STEAP, initialPlanIsCollisionFree) { + const SteapProblem prob = makeProblem(); + size_t goal_pos; + const gtsam::NonlinearFactorGraph graph = buildPlanGraph(prob, &goal_pos); + + gtsam::ISAM2 isam(steapParams()); + isam.update(graph, straightLineInit(prob)); + const Values result = iterate(isam); + + EXPECT(assert_equal(startConfig(), result.at(X(0)), 1e-3)); + EXPECT(assert_equal(goalConfig(), result.at(X(kNumStates - 1)), 1e-3)); + + for (size_t k = 0; k < kNumStates; ++k) { + std::vector ps; + prob.model.queryPoints(result.at(X(k)), &ps); + for (auto &&p : ps) { + EXPECT((p - prob.center).norm() > kRadius + kEpsilon - 0.02); + } + } +} + +/* ****************** a measurement updates the trajectory *************** */ + +// A measurement factor is a unary Gaussian prior on the state at the current +// step, with the sensor reading as its mean. Adding one at an interior state +// pulls that state toward the measurement, while the endpoints stay pinned by +// their fix factors. This is the estimation half of STEAP responding to data. +TEST(STEAP, measurementFactorUpdatesTrajectory) { + const SteapProblem prob = makeProblem(); + const size_t dof = prob.model.dof(); + size_t goal_pos; + const gtsam::NonlinearFactorGraph graph = buildPlanGraph(prob, &goal_pos); + + gtsam::ISAM2 isam(steapParams()); + isam.update(graph, straightLineInit(prob)); + const Values plan = iterate(isam); + + const size_t step = kNumStates / 2; + // A noisy execution has landed the robot a third of a metre further along the + // gantry rail than planned, a collision-free config the sensor reports. + Vector measured = plan.at(X(step)); + measured(0) += 0.35; + + gtsam::NonlinearFactorGraph measurement; + measurement.addPrior(X(step), measured, Isotropic::Sigma(dof, 1e-4)); + isam.update(measurement, Values()); + const Values updated = iterate(isam); + + // The tight measurement dominates, so the state snaps onto it. + EXPECT(assert_equal(measured, updated.at(X(step)), 3e-2)); + // The endpoints are untouched by an interior measurement. + EXPECT(assert_equal(startConfig(), updated.at(X(0)), 1e-3)); + EXPECT( + assert_equal(goalConfig(), updated.at(X(kNumStates - 1)), 1e-3)); +} + +/* *************** the goal factor is swapped at the end **************** */ + +// When the trajectory finishes, STEAP replaces the goal fix factor with a pose +// measurement, so the final posterior is a pure estimation solution. Removing +// the goal factor and adding a measurement at the last state should move it to +// wherever the robot actually ended up. +TEST(STEAP, goalFactorSwappedForMeasurement) { + const SteapProblem prob = makeProblem(); + const size_t dof = prob.model.dof(); + size_t goal_pos; + const gtsam::NonlinearFactorGraph graph = buildPlanGraph(prob, &goal_pos); + + gtsam::ISAM2 isam(steapParams()); + const gtsam::ISAM2Result first = isam.update(graph, straightLineInit(prob)); + iterate(isam); + const gtsam::FactorIndex goal_index = first.newFactorsIndices.at(goal_pos); + + // The robot arrived a little short of the planned goal. + Vector arrived = goalConfig(); + arrived(0) -= 0.2; + + gtsam::NonlinearFactorGraph measurement; + measurement.addPrior(X(kNumStates - 1), arrived, + Isotropic::Sigma(dof, 1e-4)); + gtsam::FactorIndices remove; + remove.push_back(goal_index); + isam.update(measurement, Values(), remove); + const Values updated = iterate(isam); + + // With the goal factor gone, the final state follows the measurement. + EXPECT(assert_equal(arrived, updated.at(X(kNumStates - 1)), 3e-2)); + EXPECT(assert_equal(startConfig(), updated.at(X(0)), 1e-3)); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} From 6923e4172c04674fe6bf8bc6287dafe8b5033b28 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Mon, 13 Jul 2026 18:02:56 -0400 Subject: [PATCH 16/27] steap edits in python --- python/tests/test_steap.py | 209 +++++++++++++++++++++++++++++++++++++ 1 file changed, 209 insertions(+) create mode 100644 python/tests/test_steap.py diff --git a/python/tests/test_steap.py b/python/tests/test_steap.py new file mode 100644 index 000000000..50b689387 --- /dev/null +++ b/python/tests/test_steap.py @@ -0,0 +1,209 @@ +""" +GTDynamics Copyright 2020, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +See LICENSE for the license information + +Simultaneous trajectory estimation and planning: a GPMP2 plan solved and then +updated with measurement factors through iSAM2. Mirrors +gtdynamics/gpmp2/tests/testSTEAP.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 V, X +from gtsam.utils.test_case import GtsamTestCase + +import gtdynamics as gtd + +CELL = 0.05 +RADIUS = 0.15 +EPSILON = 0.10 +COST_SIGMA = 0.01 +N_STATES = 5 +DELTA_T = 0.5 + +# The nine movable joints of robot1, gantry prismatic then arm revolute. +ROBOT1_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" +] + +Q_START = np.array([2.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0]) +Q_GOAL = np.array([3.0, 2.0, 1.0, 0.0, -0.5, -1.0, 0.0, 0.5, 0.0]) + + +def grid_positions(origin, cell, counts): + """Return the 3 x N node positions of a uniform grid.""" + axes = [origin[i] + cell * np.arange(counts[i]) for i in range(3)] + grid = np.meshgrid(*axes, indexing="ij") + return np.vstack([axis.ravel() for axis in grid]) + + +def sphere_sdf(center, radius, origin, cell, counts): + """Sample the exact signed distance to a sphere onto a uniform grid.""" + positions = grid_positions(origin, cell, counts) + distances = np.linalg.norm(positions - center.reshape(3, 1), + axis=0) - radius + return gtd.SignedDistanceField(positions, distances) + + +def steap_params(): + """iSAM2 is non-linear, so a few updates reach the batch solution.""" + params = gtsam.ISAM2Params() + params.setOptimizationParams(gtsam.ISAM2DoglegParams()) + params.setRelinearizeThreshold(0.01) + params.relinearizeSkip = 1 + return params + + +def iterate(isam, iterations=10): + """Drive iSAM2 to convergence and return the current estimate.""" + for _ in range(iterations): + isam.update() + return isam.calculateEstimate() + + +class TestSteap(GtsamTestCase): + """A GPMP2 plan updated with measurement factors through iSAM2.""" + + def setUp(self): + self.robot = gtd.CreateRobotFromFile( + str(Path(gtd.URDF_PATH) / "bar_lab.urdf")) + joints = [self.robot.joint(name) for name in ROBOT1_JOINTS] + + link6 = self.robot.link("robot1_link_6") + points = gtd.PointOnLinks() + points.append(gtd.PointOnLink(link6, np.array([0.0, 0.0, 0.0]))) + points.append(gtd.PointOnLink(link6, np.array([0.0, 0.0, 0.1]))) + + # Only robot1's joints are given, so ignore the bridge2 subtree. + self.model = gtd.RobotQueryPoints(self.robot, "columns", joints, points) + self.dof = self.model.dof() + + # Straight line initialisation, and the wrist positions it sweeps. + self.line = [ + Q_START + (k / (N_STATES - 1)) * (Q_GOAL - Q_START) + for k in range(N_STATES) + ] + swept = np.hstack([self.model.worldPoints(q) for q in self.line]) + + # The sphere sits on the wrist at the midpoint of the straight line. + self.center = self.model.worldPoints(self.line[N_STATES // 2])[:, 0] + + pad = 0.7 + origin = swept.min(axis=1) - pad + extent = (swept.max(axis=1) + pad) - origin + counts = np.ceil(extent / CELL).astype(int) + 1 + self.sdf = sphere_sdf(self.center, RADIUS, origin, CELL, counts) + + def build_plan_graph(self): + """The GPMP2 graph, returning the position of the goal pose factor.""" + graph = gtsam.NonlinearFactorGraph() + qc_model = gtsam.noiseModel.Isotropic.Sigma(self.dof, 1.0) + fix = gtsam.noiseModel.Isotropic.Sigma(self.dof, 1e-4) + + graph.add(gtsam.PriorFactorVector(X(0), Q_START, fix)) + graph.add(gtsam.PriorFactorVector(V(0), np.zeros(self.dof), fix)) + goal_pos = graph.size() + graph.add(gtsam.PriorFactorVector(X(N_STATES - 1), Q_GOAL, fix)) + graph.add( + gtsam.PriorFactorVector(V(N_STATES - 1), np.zeros(self.dof), fix)) + + for k in range(N_STATES): + graph.add( + gtd.ObstacleSDFFactor(X(k), self.model, self.sdf, COST_SIGMA, + EPSILON)) + for k in range(N_STATES - 1): + graph.add( + gtd.GPLinearPrior(X(k), V(k), X(k + 1), V(k + 1), DELTA_T, + qc_model)) + return graph, goal_pos + + def straight_line_init(self): + velocity = (Q_GOAL - Q_START) / (N_STATES * DELTA_T) + init = gtsam.Values() + for k in range(N_STATES): + init.insert(X(k), self.line[k]) + init.insert(V(k), velocity) + return init + + def test_initial_plan_is_collision_free(self): + """Before any measurement, STEAP is exactly the GPMP2 plan.""" + graph, _ = self.build_plan_graph() + isam = gtsam.ISAM2(steap_params()) + isam.update(graph, self.straight_line_init()) + result = iterate(isam) + + np.testing.assert_allclose(result.atVector(X(0)), Q_START, atol=1e-3) + np.testing.assert_allclose(result.atVector(X(N_STATES - 1)), Q_GOAL, + atol=1e-3) + for k in range(N_STATES): + points = self.model.worldPoints(result.atVector(X(k))) + distances = np.linalg.norm(points - self.center.reshape(3, 1), + axis=0) + self.assertTrue(np.all(distances > RADIUS + EPSILON - 0.02)) + + def test_measurement_updates_trajectory(self): + """A measurement factor pulls an interior state, endpoints held.""" + graph, _ = self.build_plan_graph() + isam = gtsam.ISAM2(steap_params()) + isam.update(graph, self.straight_line_init()) + plan = iterate(isam) + + step = N_STATES // 2 + # A noisy execution landed the robot a third of a metre further along + # the rail than planned, a collision-free config the sensor reports. + measured = plan.atVector(X(step)).copy() + measured[0] += 0.35 + + measurement = gtsam.NonlinearFactorGraph() + measurement.add( + gtsam.PriorFactorVector( + X(step), measured, + gtsam.noiseModel.Isotropic.Sigma(self.dof, 1e-4))) + isam.update(measurement, gtsam.Values()) + updated = iterate(isam) + + # The tight measurement dominates, so the state snaps onto it. + np.testing.assert_allclose(updated.atVector(X(step)), measured, + atol=3e-2) + # The endpoints are untouched by an interior measurement. + np.testing.assert_allclose(updated.atVector(X(0)), Q_START, atol=1e-3) + np.testing.assert_allclose(updated.atVector(X(N_STATES - 1)), Q_GOAL, + atol=1e-3) + + def test_goal_factor_swapped_for_measurement(self): + """At the end, the goal factor is replaced by a pose measurement.""" + graph, goal_pos = self.build_plan_graph() + isam = gtsam.ISAM2(steap_params()) + first = isam.update(graph, self.straight_line_init()) + iterate(isam) + goal_index = first.getNewFactorsIndices()[goal_pos] + + # The robot arrived a little short of the planned goal. + arrived = Q_GOAL.copy() + arrived[0] -= 0.2 + + measurement = gtsam.NonlinearFactorGraph() + measurement.add( + gtsam.PriorFactorVector( + X(N_STATES - 1), arrived, + gtsam.noiseModel.Isotropic.Sigma(self.dof, 1e-4))) + isam.update(measurement, gtsam.Values(), [goal_index]) + updated = iterate(isam) + + # With the goal factor gone, the final state follows the measurement. + np.testing.assert_allclose(updated.atVector(X(N_STATES - 1)), arrived, + atol=3e-2) + np.testing.assert_allclose(updated.atVector(X(0)), Q_START, atol=1e-3) + + +if __name__ == "__main__": + unittest.main() From 4051f6c9fc78b839331b61982ff092f3615a362d Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Mon, 13 Jul 2026 21:40:22 -0400 Subject: [PATCH 17/27] edit fix --- gtdynamics/gpmp2/tests/testSTEAP.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtdynamics/gpmp2/tests/testSTEAP.cpp b/gtdynamics/gpmp2/tests/testSTEAP.cpp index 5ae8dac17..8b4bbbab6 100644 --- a/gtdynamics/gpmp2/tests/testSTEAP.cpp +++ b/gtdynamics/gpmp2/tests/testSTEAP.cpp @@ -167,7 +167,6 @@ static gtsam::NonlinearFactorGraph buildPlanGraph(const SteapProblem &prob, } static Values straightLineInit(const SteapProblem &prob) { - const size_t dof = prob.model.dof(); const Vector velocity = (goalConfig() - startConfig()) / (kNumStates * kDeltaT); Values init; From dcc9d86962734adad38ab98a315b6c3b86509810 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Mon, 13 Jul 2026 21:58:56 -0400 Subject: [PATCH 18/27] added 18dof factor graph test, but still need to test in RobotStudio and IRL --- python/tests/test_gpmp2_planning.py | 131 ++++++++++++++++++++++++++++ 1 file changed, 131 insertions(+) diff --git a/python/tests/test_gpmp2_planning.py b/python/tests/test_gpmp2_planning.py index a434b980c..d9ca96889 100644 --- a/python/tests/test_gpmp2_planning.py +++ b/python/tests/test_gpmp2_planning.py @@ -63,6 +63,32 @@ def sphere_sdf(center, radius, origin, cell, counts): return gtd.SignedDistanceField(positions, distances) +def spheres_sdf(centers, radius, origin, cell, counts): + """Signed distance to the nearest of several spheres, on a uniform grid.""" + positions = grid_positions(origin, cell, counts) + distances = np.min([ + np.linalg.norm(positions - c.reshape(3, 1), axis=0) - radius + for c in centers + ], axis=0) + return gtd.SignedDistanceField(positions, distances) + + +# All eighteen joints of bar_lab, robot1's nine then robot2's nine. The tree +# branches at the shared rail, so one model spanning all of them reaches both +# arms from the same base. +BOTH_ARM_JOINTS = ROBOT1_JOINTS + [ + "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" +] + +# The two arms sit on opposite sides of the rail, robot1 near x = 2 and robot2 +# near x = 9, so their workspaces are disjoint and no arm-vs-arm cost is needed. +_ARM = [0.0, -0.5, -1.0, 0.0, 0.5, 0.0] +Q_START_18 = np.array([2.0, 2.0, 1.0] + _ARM + [9.0, 3.0, 1.0] + _ARM) +Q_GOAL_18 = np.array([3.0, 2.0, 1.0] + _ARM + [10.0, 3.0, 1.0] + _ARM) + + class TestSignedDistanceField(GtsamTestCase): """Test the signed distance field built from positions and distances.""" @@ -313,5 +339,110 @@ def test_plan_under_limits(self): self.assertGreater(abs(result.atVector(V(n_states // 2))[0]), 0.3) +class TestPlanning18Dof(GtsamTestCase): + """Plan both arms of bar_lab at once on a single 18-DOF factor graph.""" + + 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] + + # One query point on each arm's wrist. + points = gtd.PointOnLinks() + points.append( + gtd.PointOnLink(self.robot.link("robot1_link_6"), np.zeros(3))) + points.append( + gtd.PointOnLink(self.robot.link("robot2_link_6"), np.zeros(3))) + + self.model = gtd.RobotQueryPoints(self.robot, "columns", joints, points) + self.dof = self.model.dof() + + def clearances(self, q, centers): + """Distance from each wrist to its own sphere centre.""" + points = self.model.worldPoints(q) + return np.array([ + np.linalg.norm(points[:, i] - centers[i]) + for i in range(len(centers)) + ]) + + def test_query_points(self): + """The model spans eighteen joints and carries two query points.""" + self.assertEqual(self.model.dof(), 18) + self.assertEqual(self.model.nrPoints(), 2) + self.assertEqual(self.model.worldPoints(Q_START_18).shape, (3, 2)) + + def test_plan_both_arms(self): + """Both arms detour their own sphere in a single optimisation.""" + dof, n_states = self.dof, 5 + total_time = 2.0 + delta_t = total_time / (n_states - 1) + velocity = (Q_GOAL_18 - Q_START_18) / total_time + + line = [ + Q_START_18 + (k / (n_states - 1)) * (Q_GOAL_18 - Q_START_18) + for k in range(n_states) + ] + swept = np.hstack([self.model.worldPoints(q) for q in line]) + + # One sphere on each arm's wrist at the midpoint of its straight line. + mid = self.model.worldPoints(line[n_states // 2]) + centers = [mid[:, 0], mid[:, 1]] + + pad = 0.7 + origin = swept.min(axis=1) - pad + extent = (swept.max(axis=1) + pad) - origin + counts = np.ceil(extent / CELL).astype(int) + 1 + sdf = spheres_sdf(centers, RADIUS, origin, CELL, counts) + + # Both arms start in collision at the midpoint, both endpoints clear. + self.assertTrue( + np.all(self.clearances(line[n_states // 2], centers) < RADIUS + + EPSILON)) + self.assertTrue( + np.all(self.clearances(Q_START_18, centers) > RADIUS + EPSILON)) + self.assertTrue( + np.all(self.clearances(Q_GOAL_18, centers) > RADIUS + EPSILON)) + + graph = gtsam.NonlinearFactorGraph() + qc_model = gtsam.noiseModel.Isotropic.Sigma(dof, 1.0) + endpoint_model = gtsam.noiseModel.Isotropic.Sigma(dof, 1e-4) + + graph.add(gtsam.PriorFactorVector(X(0), Q_START_18, endpoint_model)) + graph.add(gtsam.PriorFactorVector(V(0), np.zeros(dof), endpoint_model)) + graph.add( + gtsam.PriorFactorVector(X(n_states - 1), Q_GOAL_18, endpoint_model)) + graph.add( + gtsam.PriorFactorVector(V(n_states - 1), np.zeros(dof), + endpoint_model)) + for k in range(n_states): + graph.add( + gtd.ObstacleSDFFactor(X(k), self.model, sdf, COST_SIGMA, + EPSILON)) + for k in range(n_states - 1): + graph.add( + gtd.GPLinearPrior(X(k), V(k), X(k + 1), V(k + 1), delta_t, + qc_model)) + + initial = gtsam.Values() + for k in range(n_states): + initial.insert(X(k), line[k]) + initial.insert(V(k), velocity) + + params = gtsam.LevenbergMarquardtParams() + params.setMaxIterations(100) + result = gtsam.LevenbergMarquardtOptimizer(graph, initial, + params).optimize() + + np.testing.assert_allclose(result.atVector(X(0)), Q_START_18, atol=1e-3) + np.testing.assert_allclose(result.atVector(X(n_states - 1)), Q_GOAL_18, + atol=1e-3) + + # Each arm's wrist has been pushed clear of its own sphere at every + # state, checked against the analytic spheres. + for k in range(n_states): + clear = self.clearances(result.atVector(X(k)), centers) + self.assertTrue(np.all(clear > RADIUS + EPSILON - 0.02)) + + if __name__ == "__main__": unittest.main() From e91687cabbdce1b5618c016bd8e8523df90d875d Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 14 Jul 2026 10:30:19 -0400 Subject: [PATCH 19/27] more tests for handling original platforms thickness --- gtdynamics.i | 5 +++ gtdynamics/factors/ObstacleSDFFactor.h | 45 +++++++++++++++++++++---- python/tests/test_gpmp2_planning.py | 46 ++++++++++++++++++++++++++ 3 files changed, 89 insertions(+), 7 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index bd198d97c..aaa028a80 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -1087,8 +1087,13 @@ class ObstacleSDFFactor : gtsam::NoiseModelFactor { ObstacleSDFFactor(gtsam::Key q_key, const gtdynamics::RobotQueryPoints &robot, const gtdynamics::SignedDistanceField *sdf, double cost_sigma, double epsilon); + ObstacleSDFFactor(gtsam::Key q_key, const gtdynamics::RobotQueryPoints &robot, + const gtdynamics::SignedDistanceField *sdf, + double cost_sigma, double epsilon, + const gtsam::Vector &radii); double epsilon() const; + gtsam::Vector radii() const; void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter = gtdynamics::GTDKeyFormatter); }; diff --git a/gtdynamics/factors/ObstacleSDFFactor.h b/gtdynamics/factors/ObstacleSDFFactor.h index 1ac172791..adf85983d 100644 --- a/gtdynamics/factors/ObstacleSDFFactor.h +++ b/gtdynamics/factors/ObstacleSDFFactor.h @@ -24,15 +24,15 @@ #include #include +#include #include #include namespace gtdynamics { /** - * Unary factor keeping every query point on the robot at least epsilon away - * from the obstacles encoded in a world frame signed distance field. The error - * has one row per query point, each the hinge loss epsilon - d(wP). + * Unary factor keeping every query point clear of the obstacles in a world + * frame signed distance field, each point standing off by its radius + epsilon. */ class ObstacleSDFFactor : public gtsam::NoiseModelFactorN { private: @@ -40,12 +40,13 @@ class ObstacleSDFFactor : public gtsam::NoiseModelFactorN { using Base = gtsam::NoiseModelFactorN; double epsilon_; + gtsam::Vector radii_; ///< per query point radius, zero if unspecified RobotQueryPoints robot_; std::shared_ptr sdf_; public: /** - * Constructor. + * Constructor with a single standoff for every query point. * @param q_key key of the stacked joint angle vector * @param robot query point model of the robot * @param sdf signed distance field of the obstacles, in the world frame @@ -58,9 +59,35 @@ class ObstacleSDFFactor : public gtsam::NoiseModelFactorN { : Base(gtsam::noiseModel::Isotropic::Sigma(robot.nrPoints(), cost_sigma), q_key), epsilon_(epsilon), + radii_(gtsam::Vector::Zero(robot.nrPoints())), robot_(robot), sdf_(sdf) {} + /** + * Constructor with a radius per query point, added to the shared epsilon. + * @param q_key key of the stacked joint angle vector + * @param robot query point model of the robot + * @param sdf signed distance field of the obstacles, in the world frame + * @param cost_sigma cost function sigma, one per query point + * @param epsilon standoff distance added to every radius + * @param radii radius of each query point, one per point of the model + */ + ObstacleSDFFactor(gtsam::Key q_key, const RobotQueryPoints &robot, + const std::shared_ptr &sdf, + double cost_sigma, double epsilon, + const gtsam::Vector &radii) + : Base(gtsam::noiseModel::Isotropic::Sigma(robot.nrPoints(), cost_sigma), + q_key), + epsilon_(epsilon), + radii_(radii), + robot_(robot), + sdf_(sdf) { + if (static_cast(radii.size()) != robot.nrPoints()) { + throw std::invalid_argument( + "ObstacleSDFFactor: radii must have one entry per query point."); + } + } + ~ObstacleSDFFactor() override {} /// Return a deep copy of this factor. @@ -82,20 +109,24 @@ class ObstacleSDFFactor : public gtsam::NoiseModelFactorN { if (H1) *H1 = gtsam::Matrix::Zero(m, robot_.dof()); for (size_t i = 0; i < m; ++i) { + const double eps = epsilon_ + radii_(i); if (H1) { gtsam::Matrix13 Herr_point; - err(i) = hingeLossObstacleCost(wPs[i], *sdf_, epsilon_, Herr_point); + err(i) = hingeLossObstacleCost(wPs[i], *sdf_, eps, Herr_point); H1->row(i) = Herr_point * Jps[i]; } else { - err(i) = hingeLossObstacleCost(wPs[i], *sdf_, epsilon_); + err(i) = hingeLossObstacleCost(wPs[i], *sdf_, eps); } } return err; } - /// Return the standoff distance. + /// Return the shared standoff distance. double epsilon() const { return epsilon_; } + /// 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 = diff --git a/python/tests/test_gpmp2_planning.py b/python/tests/test_gpmp2_planning.py index d9ca96889..d4484ddc6 100644 --- a/python/tests/test_gpmp2_planning.py +++ b/python/tests/test_gpmp2_planning.py @@ -207,6 +207,52 @@ def test_query_points(self): self.assertEqual(self.model.nrPoints(), 2) self.assertEqual(self.model.worldPoints(Q_START).shape, (3, 2)) + def one_point_model(self): + """A model with a single wrist query point, for the radius tests.""" + joints = [self.robot.joint(name) for name in ROBOT1_JOINTS] + point = gtd.PointOnLinks() + point.append( + gtd.PointOnLink(self.robot.link("robot1_link_6"), np.zeros(3))) + return gtd.RobotQueryPoints(self.robot, "columns", joints, point) + + def test_radius_tightens_the_constraint(self): + """A query point that is clear becomes a collision once given a radius. + + The radius belongs to the robot, not the obstacle: it inflates the + query point into a sphere while the obstacle stays fixed in the field. + """ + model = self.one_point_model() + wrist = model.worldPoints(Q_START)[:, 0] + + # The sphere sits just past epsilon of the wrist: clear as a point, but + # inside once the point is inflated by a 0.1 m radius. + center = wrist + np.array([RADIUS + EPSILON + 0.05, 0.0, 0.0]) + support = np.column_stack([wrist, center]) + origin = support.min(axis=1) - 0.3 + counts = np.ceil((support.max(axis=1) + 0.3 - origin) / CELL).astype( + int) + 1 + sdf = sphere_sdf(center, RADIUS, origin, CELL, counts) + + values = gtsam.Values() + values.insert(X(0), Q_START) + point_factor = gtd.ObstacleSDFFactor(X(0), model, sdf, COST_SIGMA, + EPSILON) + radius_factor = gtd.ObstacleSDFFactor(X(0), model, sdf, COST_SIGMA, + EPSILON, np.array([0.1])) + + self.assertAlmostEqual(point_factor.error(values), 0.0, places=9) + self.assertGreater(radius_factor.error(values), 0.0) + np.testing.assert_allclose(radius_factor.radii(), [0.1], atol=1e-9) + + def test_radii_dimension_mismatch(self): + """A radii vector of the wrong length is rejected.""" + sdf = sphere_sdf(np.array([5.0, 5.0, 5.0]), RADIUS, + np.array([4.0, 4.0, 4.0]), CELL, (5, 5, 5)) + # The model carries two query points, so a length-one radii is invalid. + with self.assertRaises(ValueError): + gtd.ObstacleSDFFactor(X(0), self.model, sdf, COST_SIGMA, EPSILON, + np.array([0.1])) + def test_plan_around_sphere(self): """The trajectory bows around a sphere planted on its straight line.""" dof, n_states = self.model.dof(), 5 From cc12a5361f33c5b2a7f3f54df23c584f6c7b95f2 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 14 Jul 2026 11:41:21 -0400 Subject: [PATCH 20/27] collision factor addition --- gtdynamics/factors/SelfCollisionFactor.h | 247 ++++++++++++++ gtdynamics/gpmp2/ObstacleCost.h | 34 ++ .../gpmp2/tests/testSelfCollisionFactor.cpp | 314 ++++++++++++++++++ python/gtdynamics/preamble/gtdynamics.h | 1 + .../gtdynamics/specializations/gtdynamics.h | 1 + python/tests/test_self_collision.py | 118 +++++++ 6 files changed, 715 insertions(+) create mode 100644 gtdynamics/factors/SelfCollisionFactor.h create mode 100644 gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp create mode 100644 python/tests/test_self_collision.py diff --git a/gtdynamics/factors/SelfCollisionFactor.h b/gtdynamics/factors/SelfCollisionFactor.h new file mode 100644 index 000000000..e4e93b8e4 --- /dev/null +++ b/gtdynamics/factors/SelfCollisionFactor.h @@ -0,0 +1,247 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file SelfCollisionFactor.h + * @brief Self collision cost factor over a set of collision pairs. + * @author Karthik Shaji - Adapted from gpmp2 by Mustafa Mukadam. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +/// One self collision check: a query point vs another query point or a link's +/// field. One side is always a point. Build via PointPair / PointSDF. +struct SelfCollisionPair { + size_t a; ///< query point index, the point side + bool is_sdf; ///< false: point vs point; true: point vs a link field + size_t b; ///< point vs point: the other query point index + LinkSharedPtr link_b; ///< point vs field: link B + std::shared_ptr sdf_b; ///< field in link B's frame + double epsilon; ///< standoff between the two sides, added to their radii + + /// Point vs point pair. + static SelfCollisionPair PointPair(size_t a, size_t b, double epsilon) { + return SelfCollisionPair{a, false, b, nullptr, nullptr, epsilon}; + } + + /// Point vs link field pair. + static SelfCollisionPair PointSDF( + size_t a, const LinkSharedPtr &link_b, + const std::shared_ptr &sdf_b, double epsilon) { + return SelfCollisionPair{a, true, 0, link_b, sdf_b, epsilon}; + } +}; + +using SelfCollisionPairs = std::vector; + +/** + * Unary factor keeping the robot clear of itself over a set of collision pairs, + * one hinge loss row per pair. Both sides 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. + * + * The caller registers only meaningful pairs: adjacent links sit at near constant + * separation and would fire permanently, and two points that can coincide give a + * non-finite distance gradient. The factor does not auto exclude either. + */ +class SelfCollisionFactor : public gtsam::NoiseModelFactorN { + private: + using This = SelfCollisionFactor; + using Base = gtsam::NoiseModelFactorN; + + RobotQueryPoints robot_; + gtsam::Vector radii_; ///< one radius per query point, zero if unspecified + std::vector pairs_; + + /// Reject pairs whose indices, radii or fields are inconsistent. + void validate() const { + if (static_cast(radii_.size()) != robot_.nrPoints()) { + throw std::invalid_argument( + "SelfCollisionFactor: radii must have one entry per query point."); + } + for (const auto &p : pairs_) { + if (p.a >= robot_.nrPoints()) { + throw std::invalid_argument( + "SelfCollisionFactor: pair point index out of range."); + } + if (p.is_sdf) { + if (!p.link_b || !p.sdf_b) { + throw std::invalid_argument( + "SelfCollisionFactor: a point vs field pair needs a link and a " + "field."); + } + } else if (p.b >= robot_.nrPoints()) { + throw std::invalid_argument( + "SelfCollisionFactor: pair point index out of range."); + } + } + } + + public: + /** + * Constructor with a single sigma across every pair. + * @param q_key key of the stacked joint angle vector + * @param robot query point model, spanning every joint involved + * @param pairs collision pairs to check + * @param radii radius of each query point, one per point of the model + * @param cost_sigma cost function sigma, shared by every pair + */ + SelfCollisionFactor(gtsam::Key q_key, const RobotQueryPoints &robot, + const std::vector &pairs, + const gtsam::Vector &radii, double cost_sigma) + : Base(gtsam::noiseModel::Isotropic::Sigma(pairs.size(), cost_sigma), + q_key), + robot_(robot), + radii_(radii), + pairs_(pairs) { + validate(); + } + + /** + * Constructor with a sigma per pair. + * @param q_key key of the stacked joint angle vector + * @param robot query point model, spanning every joint involved + * @param pairs collision 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 + */ + SelfCollisionFactor(gtsam::Key q_key, const RobotQueryPoints &robot, + const std::vector &pairs, + const gtsam::Vector &radii, const gtsam::Vector &sigmas) + : Base(gtsam::noiseModel::Diagonal::Sigmas(sigmas), q_key), + robot_(robot), + radii_(radii), + pairs_(pairs) { + if (static_cast(sigmas.size()) != pairs.size()) { + throw std::invalid_argument( + "SelfCollisionFactor: sigmas must have one entry per pair."); + } + validate(); + } + + ~SelfCollisionFactor() 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 { + const size_t dof = robot_.dof(); + const size_t n = pairs_.size(); + gtsam::Vector err(n); + + // One FK pass supplies both the query points and the link poses. + std::map wTl; + std::map Jl; + robot_.forwardKinematics(q, &wTl, H1 ? &Jl : nullptr); + + // Transform every query point to the world, keeping the pose map for the + // field sides (queryPoints would discard it). + const auto &pts = robot_.points(); + std::vector wPs(robot_.nrPoints()); + std::vector Jps; + if (H1) Jps.resize(robot_.nrPoints()); + for (size_t i = 0; i < robot_.nrPoints(); ++i) { + const uint8_t id = pts[i].link->id(); + auto it = wTl.find(id); + if (it == wTl.end()) { + throw std::runtime_error( + "SelfCollisionFactor: query point on a link not reachable from the " + "base."); + } + if (H1) { + gtsam::Matrix36 H_pose; + wPs[i] = it->second.transformFrom(pts[i].point, H_pose); + Jps[i] = H_pose * Jl.at(id); + } else { + wPs[i] = it->second.transformFrom(pts[i].point); + } + } + + if (H1) *H1 = gtsam::Matrix::Zero(n, dof); + + for (size_t r = 0; r < n; ++r) { + const SelfCollisionPair &p = pairs_[r]; + if (!p.is_sdf) { + // Point vs point: standoff folds in both radii. + const double eps = p.epsilon + radii_(p.a) + radii_(p.b); + if (H1) { + gtsam::Matrix13 H_pA, H_pB; + err(r) = hingeLossSelfCollisionCost(wPs[p.a], wPs[p.b], eps, H_pA, + H_pB); + H1->row(r) = H_pA * Jps[p.a] + H_pB * Jps[p.b]; + } else { + err(r) = hingeLossSelfCollisionCost(wPs[p.a], wPs[p.b], eps); + } + } else { + // Point vs field: the field encodes link_b's shape, so only the point + // side carries a radius. + const uint8_t bid = p.link_b->id(); + auto itB = wTl.find(bid); + if (itB == wTl.end()) { + throw std::runtime_error( + "SelfCollisionFactor: field link not reachable from the base."); + } + const double eps = p.epsilon + radii_(p.a); + if (H1) { + gtsam::Matrix16 H_pose; + gtsam::Matrix13 H_point; + err(r) = hingeLossObstacleCost(itB->second, wPs[p.a], *p.sdf_b, eps, + H_pose, H_point); + // Both the point and link_b's frame move with q. + H1->row(r) = H_point * Jps[p.a] + H_pose * Jl.at(bid); + } else { + err(r) = hingeLossObstacleCost(itB->second, wPs[p.a], *p.sdf_b, eps); + } + } + } + return err; + } + + /// 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 << "SelfCollisionFactor with " << pairs_.size() << " pairs" + << std::endl; + Base::print("", keyFormatter); + } +}; // \class SelfCollisionFactor + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/ObstacleCost.h b/gtdynamics/gpmp2/ObstacleCost.h index 588aee8bc..1869f04fa 100644 --- a/gtdynamics/gpmp2/ObstacleCost.h +++ b/gtdynamics/gpmp2/ObstacleCost.h @@ -91,4 +91,38 @@ inline double hingeLossObstacleCost( return cost; } +/** + * 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. + * + * The two points must never coincide: the distance gradient divides by dist, + * so a zero distance yields a non-finite Jacobian. + * + * @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 H_pA optional Jacobian of the cost with respect to pA + * @param H_pB optional Jacobian of the cost with respect to pB + * @return the hinge loss cost + */ +inline double hingeLossSelfCollisionCost( + const gtsam::Point3 &pA, const gtsam::Point3 &pB, double epsilon, + gtsam::OptionalJacobian<1, 3> H_pA = {}, + gtsam::OptionalJacobian<1, 3> H_pB = {}) { + gtsam::Matrix13 H_A, H_B; + const double dist = gtsam::distance3(pA, pB, H_A, H_B); + + if (dist > epsilon) { + if (H_pA) *H_pA = gtsam::Matrix13::Zero(); + if (H_pB) *H_pB = gtsam::Matrix13::Zero(); + return 0.0; + } + // Closer than epsilon: cost falls as the points separate. + if (H_pA) *H_pA = -H_A; + if (H_pB) *H_pB = -H_B; + return epsilon - dist; +} + } // namespace gtdynamics diff --git a/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp b/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp new file mode 100644 index 000000000..dff248230 --- /dev/null +++ b/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp @@ -0,0 +1,314 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testSelfCollisionFactor.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 +#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 double kCell = 0.05; + +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 H_far; + EXPECT_DOUBLES_EQUAL( + 0.0, hingeLossSelfCollisionCost(pA, Point3(3.0, 0.5, 0.2), eps, H_far), + 1e-9); + EXPECT(assert_equal(Matrix(Matrix13::Zero()), Matrix(H_far), 1e-9)); + + Matrix13 H_pA, H_pB; + hingeLossSelfCollisionCost(pA, pB, eps, H_pA, H_pB); + 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(H_pA), 1e-5)); + EXPECT(assert_equal( + Matrix(gtsam::numericalDerivative11(fb, pB)), + Matrix(H_pB), 1e-5)); +} + +/* ************************ 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 SDF +// 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; +} + +// Sphere signed distance field centered at a frame origin, spanning +/- half m. +static std::shared_ptr sphereSDF(double radius, + double half) { + const size_t n = static_cast(2 * half / kCell) + 1; + const Point3 origin(-half, -half, -half); + std::vector data(n); + for (size_t k = 0; k < n; ++k) { + Matrix layer(n, n); + for (size_t i = 0; i < n; ++i) { + for (size_t j = 0; j < n; ++j) { + const Point3 p = origin + Point3(j * kCell, i * kCell, k * kCell); + layer(i, j) = p.norm() - radius; + } + } + data[k] = layer; + } + return std::make_shared(origin, kCell, data); +} + +/* ************************ factor Jacobians *************************** */ + +// Point-to-point across the two arms. A large epsilon keeps the hinge active +// regardless of pose, so the numerical check exercises the full chain. +TEST(SelfCollisionFactor, pointPointJacobians) { + RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); + const std::vector pairs = { + SelfCollisionPair::PointPair(0, 1, 3.0)}; // wrist1 vs wrist2 + SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); + + const Vector q = configApart(); + 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); +} + +// Point vs a link's field, same arm (forearm point vs wrist field). This is the +// test that both the point and the moving-frame Jacobian terms are present. +TEST(SelfCollisionFactor, pointSDFJacobians) { + RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); + auto sdf = sphereSDF(0.2, 1.0); + const std::vector pairs = { + SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, 1.0)}; + SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); + + const Vector q = configApart(); + 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); +} + +// One factor holding both a point-point and a point-SDF pair. +TEST(SelfCollisionFactor, mixedPairs) { + RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); + auto sdf = sphereSDF(0.2, 1.0); + const std::vector pairs = { + SelfCollisionPair::PointPair(0, 1, 3.0), + SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, 1.0)}; + SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); + + const Vector q = configApart(); + EXPECT_LONGS_EQUAL(2, factor.evaluateError(q).size()); + + 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. The +// bases move with the gantry X joints only, so the pair is reliably close. +TEST(SelfCollisionFactor, pushesPointsApart) { + RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); + const double eps = 0.4; + const std::vector pairs = { + SelfCollisionPair::PointPair(3, 4, eps)}; // base1 vs base2 + SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.01); + + const Vector q0 = configClose(); + std::vector ps0; + model.queryPoints(q0, &ps0); + const double start_dist = (ps0[3] - ps0[4]).norm(); + EXPECT(start_dist < eps); // precondition: starts in collision + + 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 ps; + model.queryPoints(result.at(X(0)), &ps); + EXPECT((ps[3] - ps[4]).norm() > eps - 0.02); // separated to the standoff +} + +/* ************************ 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 d, turning the pair into a collision. +TEST(SelfCollisionFactor, radiiPointPoint) { + RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); + const Vector q = configApart(); + std::vector wPs; + model.queryPoints(q, &wPs); + const double d = (wPs[0] - wPs[1]).norm(); + + const double eps = d - 0.05; + const std::vector pairs = { + SelfCollisionPair::PointPair(0, 1, eps)}; + + SelfCollisionFactor 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; + SelfCollisionFactor inflated(X(0), model, pairs, radii, 0.1); + EXPECT_DOUBLES_EQUAL(eps + 0.2 - d, inflated.evaluateError(q)(0), 1e-9); + + // The same total radius on either point gives the same standoff. + Vector on_a = Vector::Zero(5), on_b = Vector::Zero(5); + on_a(0) = 0.2; + on_b(1) = 0.2; + SelfCollisionFactor f_a(X(0), model, pairs, on_a, 0.1); + SelfCollisionFactor f_b(X(0), model, pairs, on_b, 0.1); + EXPECT_DOUBLES_EQUAL(f_a.evaluateError(q)(0), f_b.evaluateError(q)(0), 1e-9); + + // A radius on an uninvolved point is ignored. + Vector other = Vector::Zero(5); + other(2) = 0.2; + SelfCollisionFactor f_other(X(0), model, pairs, other, 0.1); + EXPECT_DOUBLES_EQUAL(0.0, f_other.evaluateError(q)(0), 1e-9); +} + +// A point vs field pair carries a radius only on the point side, and in the +// active branch it adds directly to the cost. +TEST(SelfCollisionFactor, radiiPointSDF) { + RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); + auto sdf = sphereSDF(0.2, 1.0); + const std::vector pairs = { + SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, 1.0)}; + const Vector q = configApart(); + + SelfCollisionFactor base(X(0), model, pairs, Vector::Zero(5), 0.1); + const double e0 = base.evaluateError(q)(0); + EXPECT(e0 > 0.0); // active branch + + Vector radii = Vector::Zero(5); + radii(2) = 0.1; + SelfCollisionFactor with_radius(X(0), model, pairs, radii, 0.1); + EXPECT_DOUBLES_EQUAL(e0 + 0.1, with_radius.evaluateError(q)(0), 1e-9); + + // A radius on a point that is not the pair's point side is ignored. + Vector other = Vector::Zero(5); + other(0) = 0.1; + SelfCollisionFactor f_other(X(0), model, pairs, other, 0.1); + EXPECT_DOUBLES_EQUAL(e0, f_other.evaluateError(q)(0), 1e-9); +} + +/* ************************ input validation *************************** */ + +TEST(SelfCollisionFactor, rejectsBadInput) { + RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); + const std::vector pairs = { + SelfCollisionPair::PointPair(0, 1, 1.0)}; + + // radii length must equal the number of query points. + CHECK_EXCEPTION( + SelfCollisionFactor(X(0), model, pairs, Vector::Zero(3), 0.1), + std::invalid_argument); + // a point index out of range. + const std::vector bad = { + SelfCollisionPair::PointPair(0, 99, 1.0)}; + CHECK_EXCEPTION( + SelfCollisionFactor(X(0), model, bad, Vector::Zero(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..9e351d052 --- /dev/null +++ b/python/tests/test_self_collision.py @@ -0,0 +1,118 @@ +""" +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/testSelfCollisionFactor.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.PointPair(0, 1, EPSILON)) + return gtd.SelfCollisionFactor(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.PointPair(0, 1, eps)) + + values = gtsam.Values() + values.insert(X(0), Q_CLOSE) + clear = gtd.SelfCollisionFactor(X(0), self.model, pairs, np.zeros(2), + 0.1) + inflated = gtd.SelfCollisionFactor(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.PointPair(0, 1, EPSILON)) + with self.assertRaises(ValueError): + gtd.SelfCollisionFactor(X(0), self.model, pairs, np.zeros(3), 0.01) + + +if __name__ == "__main__": + unittest.main() From 23ef85ddb1264ed5d82d875a807e10a9c4e7561a Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 14 Jul 2026 11:45:21 -0400 Subject: [PATCH 21/27] exposed python bindings --- gtdynamics.i | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/gtdynamics.i b/gtdynamics.i index aaa028a80..1e16f3541 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -1138,6 +1138,35 @@ class ObstacleSDFFactorGP : gtsam::NoiseModelFactor { gtdynamics::GTDKeyFormatter); }; +#include +class SelfCollisionPair { + size_t a; + bool is_sdf; + size_t b; + double epsilon; + + static gtdynamics::SelfCollisionPair PointPair(size_t a, size_t b, + double epsilon); + static gtdynamics::SelfCollisionPair PointSDF( + size_t a, const gtdynamics::LinkSharedPtr &link_b, + const gtdynamics::SignedDistanceField *sdf_b, double epsilon); +}; +// SelfCollisionPairs defined in specializations.h + +class SelfCollisionFactor : gtsam::NoiseModelFactor { + SelfCollisionFactor(gtsam::Key q_key, const gtdynamics::RobotQueryPoints &robot, + const gtdynamics::SelfCollisionPairs &pairs, + const gtsam::Vector &radii, double cost_sigma); + SelfCollisionFactor(gtsam::Key q_key, 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); +}; + /********************** Utilities **********************/ #include string GtdFormat(const gtsam::Values &t, const string &s = ""); From 418afc4aa9f5a44cb5a67c6fa8526281dac2b5bf Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 14 Jul 2026 13:24:59 -0400 Subject: [PATCH 22/27] fix unit test --- gtdynamics/factors/GPLiePriorFactor.h | 1 + gtdynamics/factors/GPLinearPriorFactor.h | 5 ++- gtdynamics/factors/ObstacleSDFFactor.h | 26 ++++++++--- gtdynamics/factors/ObstacleSDFFactorGP.h | 11 ++++- gtdynamics/factors/SelfCollisionFactor.h | 19 +++++--- gtdynamics/gpmp2/GPLieInterpolator.h | 1 + gtdynamics/gpmp2/GPLinearInterpolator.h | 1 + gtdynamics/gpmp2/GPutils.h | 16 +++++++ .../gpmp2/tests/testSelfCollisionFactor.cpp | 44 ++++++++++++------- 9 files changed, 94 insertions(+), 30 deletions(-) diff --git a/gtdynamics/factors/GPLiePriorFactor.h b/gtdynamics/factors/GPLiePriorFactor.h index f02ab0f8d..78bef8ded 100644 --- a/gtdynamics/factors/GPLiePriorFactor.h +++ b/gtdynamics/factors/GPLiePriorFactor.h @@ -75,6 +75,7 @@ class GPLiePrior pose_key1, vel_key1, pose_key2, vel_key2), dof_(Qc_model->dim()), delta_t_(delta_t) { + checkGPDeltaT(delta_t_); // 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::dimension != Eigen::Dynamic && diff --git a/gtdynamics/factors/GPLinearPriorFactor.h b/gtdynamics/factors/GPLinearPriorFactor.h index 9fc707f43..e4756b6e9 100644 --- a/gtdynamics/factors/GPLinearPriorFactor.h +++ b/gtdynamics/factors/GPLinearPriorFactor.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #ifdef GTDYNAMICS_ENABLE_BOOST_SERIALIZATION @@ -72,7 +73,9 @@ class GPLinearPrior calcQAccel(getQc(Qc_model), delta_t)), pose_key1, vel_key1, pose_key2, vel_key2), dof_(Qc_model->dim()), - delta_t_(delta_t) {} + delta_t_(delta_t) { + checkGPDeltaT(delta_t_); + } ~GPLinearPrior() override {} diff --git a/gtdynamics/factors/ObstacleSDFFactor.h b/gtdynamics/factors/ObstacleSDFFactor.h index adf85983d..c509ee1b6 100644 --- a/gtdynamics/factors/ObstacleSDFFactor.h +++ b/gtdynamics/factors/ObstacleSDFFactor.h @@ -44,6 +44,23 @@ class ObstacleSDFFactor : public gtsam::NoiseModelFactorN { RobotQueryPoints robot_; std::shared_ptr sdf_; + /// Reject a null field, a negative standoff, or bad radii. + void validate() const { + if (!sdf_) { + throw std::invalid_argument("ObstacleSDFFactor: sdf must not be null."); + } + if (epsilon_ < 0.0) { + throw std::invalid_argument("ObstacleSDFFactor: epsilon must be >= 0."); + } + if (static_cast(radii_.size()) != robot_.nrPoints()) { + throw std::invalid_argument( + "ObstacleSDFFactor: radii must have one entry per query point."); + } + if ((radii_.array() < 0.0).any()) { + throw std::invalid_argument("ObstacleSDFFactor: radii must be >= 0."); + } + } + public: /** * Constructor with a single standoff for every query point. @@ -61,7 +78,9 @@ class ObstacleSDFFactor : public gtsam::NoiseModelFactorN { epsilon_(epsilon), radii_(gtsam::Vector::Zero(robot.nrPoints())), robot_(robot), - sdf_(sdf) {} + sdf_(sdf) { + validate(); + } /** * Constructor with a radius per query point, added to the shared epsilon. @@ -82,10 +101,7 @@ class ObstacleSDFFactor : public gtsam::NoiseModelFactorN { radii_(radii), robot_(robot), sdf_(sdf) { - if (static_cast(radii.size()) != robot.nrPoints()) { - throw std::invalid_argument( - "ObstacleSDFFactor: radii must have one entry per query point."); - } + validate(); } ~ObstacleSDFFactor() override {} diff --git a/gtdynamics/factors/ObstacleSDFFactorGP.h b/gtdynamics/factors/ObstacleSDFFactorGP.h index 766370462..a6b784e67 100644 --- a/gtdynamics/factors/ObstacleSDFFactorGP.h +++ b/gtdynamics/factors/ObstacleSDFFactorGP.h @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -77,7 +78,15 @@ class ObstacleSDFFactorGP epsilon_(epsilon), robot_(robot), sdf_(sdf), - GPbase_(Qc_model, delta_t, tau) {} + GPbase_(Qc_model, delta_t, tau) { + // delta_t and tau are checked by the interpolator constructor. + if (!sdf_) { + throw std::invalid_argument("ObstacleSDFFactorGP: sdf must not be null."); + } + if (epsilon_ < 0.0) { + throw std::invalid_argument("ObstacleSDFFactorGP: epsilon must be >= 0."); + } + } ~ObstacleSDFFactorGP() override {} diff --git a/gtdynamics/factors/SelfCollisionFactor.h b/gtdynamics/factors/SelfCollisionFactor.h index e4e93b8e4..17b4d9c8d 100644 --- a/gtdynamics/factors/SelfCollisionFactor.h +++ b/gtdynamics/factors/SelfCollisionFactor.h @@ -41,8 +41,8 @@ struct SelfCollisionPair { bool is_sdf; ///< false: point vs point; true: point vs a link field size_t b; ///< point vs point: the other query point index LinkSharedPtr link_b; ///< point vs field: link B - std::shared_ptr sdf_b; ///< field in link B's frame - double epsilon; ///< standoff between the two sides, added to their radii + std::shared_ptr sdf_b; ///< field, in B's frame + double epsilon; ///< standoff between the sides, added to their radii /// Point vs point pair. static SelfCollisionPair PointPair(size_t a, size_t b, double epsilon) { @@ -65,9 +65,9 @@ using SelfCollisionPairs = std::vector; * 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. * - * The caller registers only meaningful pairs: adjacent links sit at near constant - * separation and would fire permanently, and two points that can coincide give a - * non-finite distance gradient. The factor does not auto exclude either. + * The caller registers only meaningful pairs: adjacent links sit at a near + * constant separation and would fire permanently, and two points that can + * coincide give a non-finite distance gradient. The factor excludes neither. */ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { private: @@ -78,13 +78,20 @@ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { gtsam::Vector radii_; ///< one radius per query point, zero if unspecified std::vector pairs_; - /// Reject pairs whose indices, radii or fields are inconsistent. + /// Reject pairs whose indices, radii, fields or standoffs are inconsistent. void validate() const { if (static_cast(radii_.size()) != robot_.nrPoints()) { throw std::invalid_argument( "SelfCollisionFactor: radii must have one entry per query point."); } + if ((radii_.array() < 0.0).any()) { + throw std::invalid_argument("SelfCollisionFactor: radii must be >= 0."); + } for (const auto &p : pairs_) { + if (p.epsilon < 0.0) { + throw std::invalid_argument( + "SelfCollisionFactor: a pair epsilon must be >= 0."); + } if (p.a >= robot_.nrPoints()) { throw std::invalid_argument( "SelfCollisionFactor: pair point index out of range."); diff --git a/gtdynamics/gpmp2/GPLieInterpolator.h b/gtdynamics/gpmp2/GPLieInterpolator.h index ce65c60f7..436cabcf4 100644 --- a/gtdynamics/gpmp2/GPLieInterpolator.h +++ b/gtdynamics/gpmp2/GPLieInterpolator.h @@ -66,6 +66,7 @@ class GPLieInterpolator { GPLieInterpolator(const gtsam::SharedNoiseModel &Qc_model, double delta_t, double tau) : dof_(Qc_model->dim()), delta_t_(delta_t), tau_(tau) { + checkGPInterval(delta_t_, tau_); // A mismatched Qc dimension silently mis-slices Lambda and Psi, so reject // it up front. Only checkable when T has a fixed dimension. if (gtsam::traits::dimension != Eigen::Dynamic && diff --git a/gtdynamics/gpmp2/GPLinearInterpolator.h b/gtdynamics/gpmp2/GPLinearInterpolator.h index 10ff7a307..0260d477a 100644 --- a/gtdynamics/gpmp2/GPLinearInterpolator.h +++ b/gtdynamics/gpmp2/GPLinearInterpolator.h @@ -60,6 +60,7 @@ class GPLinearInterpolator { GPLinearInterpolator(const gtsam::SharedNoiseModel &Qc_model, double delta_t, double tau) : dof_(Qc_model->dim()), delta_t_(delta_t), tau_(tau) { + checkGPInterval(delta_t_, tau_); Qc_ = getQc(Qc_model); Lambda_ = calcLambdaAccel(Qc_, delta_t_, tau_); Psi_ = calcPsiAccel(Qc_, delta_t_, tau_); diff --git a/gtdynamics/gpmp2/GPutils.h b/gtdynamics/gpmp2/GPutils.h index 948d340a4..699eb9ecc 100644 --- a/gtdynamics/gpmp2/GPutils.h +++ b/gtdynamics/gpmp2/GPutils.h @@ -20,9 +20,25 @@ #include #include +#include namespace gtdynamics { +/// Reject a non-positive support interval. +inline void checkGPDeltaT(double delta_t) { + if (delta_t <= 0.0) { + throw std::invalid_argument("GP: delta_t must be > 0."); + } +} + +/// Reject a non-positive support interval or an interpolation time outside it. +inline void checkGPInterval(double delta_t, double tau) { + checkGPDeltaT(delta_t); + if (tau < 0.0 || tau > delta_t) { + throw std::invalid_argument("GP: tau must be in [0, delta_t]."); + } +} + /* * These implement the white noise on acceleration prior of Barfoot14rss, used * by GPMP2, whose state stacks a pose and a velocity as [p; v]. The white noise diff --git a/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp b/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp index dff248230..f9726adf1 100644 --- a/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp +++ b/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp @@ -143,15 +143,19 @@ static std::shared_ptr sphereSDF(double radius, /* ************************ factor Jacobians *************************** */ -// Point-to-point across the two arms. A large epsilon keeps the hinge active -// regardless of pose, so the numerical check exercises the full chain. +// 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(SelfCollisionFactor, pointPointJacobians) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); + const Vector q = configApart(); + std::vector wPs; + model.queryPoints(q, &wPs); + const double eps = (wPs[0] - wPs[1]).norm() + 1.0; + const std::vector pairs = { - SelfCollisionPair::PointPair(0, 1, 3.0)}; // wrist1 vs wrist2 + SelfCollisionPair::PointPair(0, 1, eps)}; // wrist1 vs wrist2 SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); - const Vector q = configApart(); EXPECT(factor.evaluateError(q)(0) > 0.0); // active branch Values values; @@ -163,7 +167,7 @@ TEST(SelfCollisionFactor, pointPointJacobians) { // test that both the point and the moving-frame Jacobian terms are present. TEST(SelfCollisionFactor, pointSDFJacobians) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); - auto sdf = sphereSDF(0.2, 1.0); + auto sdf = sphereSDF(0.2, 1.5); const std::vector pairs = { SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, 1.0)}; SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); @@ -179,13 +183,17 @@ TEST(SelfCollisionFactor, pointSDFJacobians) { // One factor holding both a point-point and a point-SDF pair. TEST(SelfCollisionFactor, mixedPairs) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); - auto sdf = sphereSDF(0.2, 1.0); + const Vector q = configApart(); + std::vector wPs; + model.queryPoints(q, &wPs); + const double eps = (wPs[0] - wPs[1]).norm() + 1.0; + + auto sdf = sphereSDF(0.2, 1.5); const std::vector pairs = { - SelfCollisionPair::PointPair(0, 1, 3.0), + SelfCollisionPair::PointPair(0, 1, eps), SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, 1.0)}; SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); - const Vector q = configApart(); EXPECT_LONGS_EQUAL(2, factor.evaluateError(q).size()); Values values; @@ -195,20 +203,20 @@ TEST(SelfCollisionFactor, mixedPairs) { /* ************************ behavioral push-apart ********************** */ -// The two arm bases start within epsilon; the factor drives them apart. The -// bases move with the gantry X joints only, so the pair is reliably close. +// 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(SelfCollisionFactor, pushesPointsApart) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); - const double eps = 0.4; - const std::vector pairs = { - SelfCollisionPair::PointPair(3, 4, eps)}; // base1 vs base2 - SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.01); const Vector q0 = configClose(); std::vector ps0; model.queryPoints(q0, &ps0); const double start_dist = (ps0[3] - ps0[4]).norm(); - EXPECT(start_dist < eps); // precondition: starts in collision + const double eps = start_dist + 0.3; + + const std::vector pairs = { + SelfCollisionPair::PointPair(3, 4, eps)}; // base1 vs base2 + SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.01); gtsam::NonlinearFactorGraph graph; graph.add(factor); @@ -222,7 +230,9 @@ TEST(SelfCollisionFactor, pushesPointsApart) { std::vector ps; model.queryPoints(result.at(X(0)), &ps); - EXPECT((ps[3] - ps[4]).norm() > eps - 0.02); // separated to the standoff + // The weak prior pulls back slightly, so equilibrium sits just under eps. + EXPECT((ps[3] - ps[4]).norm() > eps - 0.05); + EXPECT((ps[3] - ps[4]).norm() > start_dist + 0.15); // clearly separated } /* ************************ per-point radii *************************** */ @@ -268,7 +278,7 @@ TEST(SelfCollisionFactor, radiiPointPoint) { // active branch it adds directly to the cost. TEST(SelfCollisionFactor, radiiPointSDF) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); - auto sdf = sphereSDF(0.2, 1.0); + auto sdf = sphereSDF(0.2, 1.5); const std::vector pairs = { SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, 1.0)}; const Vector q = configApart(); From 68e1bc148b9cc8f26d15a039b23d3e9101dca0cf Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 14 Jul 2026 14:19:37 -0400 Subject: [PATCH 23/27] fixed unit tests I believe --- .../gpmp2/tests/testSelfCollisionFactor.cpp | 33 ++++++++++++++----- 1 file changed, 24 insertions(+), 9 deletions(-) diff --git a/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp b/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp index f9726adf1..8c3330117 100644 --- a/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp +++ b/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp @@ -164,15 +164,23 @@ TEST(SelfCollisionFactor, pointPointJacobians) { } // Point vs a link's field, same arm (forearm point vs wrist field). This is the -// test that both the point and the moving-frame Jacobian terms are present. +// test that both the point and the moving-frame Jacobian terms are present. The +// grid and epsilon are sized from the measured distance so the hinge is active. +// Query point 0 is link_6's CoM, the field origin, so ||wPs[2]-wPs[0]|| is the +// distance of the forearm point into the field. TEST(SelfCollisionFactor, pointSDFJacobians) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); - auto sdf = sphereSDF(0.2, 1.5); + const Vector q = configApart(); + std::vector wPs; + model.queryPoints(q, &wPs); + const double dist = (wPs[2] - wPs[0]).norm(); + + auto sdf = sphereSDF(0.2, dist + 0.3); // grid contains the forearm point const std::vector pairs = { - SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, 1.0)}; + SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, + dist + 0.1)}; // active: cost ~= 0.3 SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); - const Vector q = configApart(); EXPECT(factor.evaluateError(q)(0) > 0.0); // active branch Values values; @@ -188,10 +196,12 @@ TEST(SelfCollisionFactor, mixedPairs) { model.queryPoints(q, &wPs); const double eps = (wPs[0] - wPs[1]).norm() + 1.0; - auto sdf = sphereSDF(0.2, 1.5); + const double dist = (wPs[2] - wPs[0]).norm(); + auto sdf = sphereSDF(0.2, dist + 0.3); const std::vector pairs = { SelfCollisionPair::PointPair(0, 1, eps), - SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, 1.0)}; + SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, + dist + 0.1)}; SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); EXPECT_LONGS_EQUAL(2, factor.evaluateError(q).size()); @@ -278,10 +288,15 @@ TEST(SelfCollisionFactor, radiiPointPoint) { // active branch it adds directly to the cost. TEST(SelfCollisionFactor, radiiPointSDF) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); - auto sdf = sphereSDF(0.2, 1.5); - const std::vector pairs = { - SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, 1.0)}; const Vector q = configApart(); + std::vector wPs; + model.queryPoints(q, &wPs); + const double dist = (wPs[2] - wPs[0]).norm(); + + auto sdf = sphereSDF(0.2, dist + 0.3); + const std::vector pairs = { + SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, + dist + 0.1)}; SelfCollisionFactor base(X(0), model, pairs, Vector::Zero(5), 0.1); const double e0 = base.evaluateError(q)(0); From 4750bf3f65b19cdfb57c51263aafb8ac55178117 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Mon, 20 Jul 2026 14:18:42 -0400 Subject: [PATCH 24/27] removed non-sphere-based methods --- gtdynamics.i | 10 +- gtdynamics/factors/SelfCollisionFactor.h | 134 +++++------------- .../gpmp2/tests/testSelfCollisionFactor.cpp | 103 ++++---------- python/tests/test_self_collision.py | 6 +- 4 files changed, 65 insertions(+), 188 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index 1e16f3541..b92d535db 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -1140,16 +1140,12 @@ class ObstacleSDFFactorGP : gtsam::NoiseModelFactor { #include class SelfCollisionPair { + SelfCollisionPair(); + SelfCollisionPair(size_t a, size_t b, double epsilon); + size_t a; - bool is_sdf; size_t b; double epsilon; - - static gtdynamics::SelfCollisionPair PointPair(size_t a, size_t b, - double epsilon); - static gtdynamics::SelfCollisionPair PointSDF( - size_t a, const gtdynamics::LinkSharedPtr &link_b, - const gtdynamics::SignedDistanceField *sdf_b, double epsilon); }; // SelfCollisionPairs defined in specializations.h diff --git a/gtdynamics/factors/SelfCollisionFactor.h b/gtdynamics/factors/SelfCollisionFactor.h index 17b4d9c8d..412dfcabf 100644 --- a/gtdynamics/factors/SelfCollisionFactor.h +++ b/gtdynamics/factors/SelfCollisionFactor.h @@ -7,7 +7,7 @@ /** * @file SelfCollisionFactor.h - * @brief Self collision cost factor over a set of collision pairs. + * @brief Self collision cost factor over a set of query point pairs. * @author Karthik Shaji - Adapted from gpmp2 by Mustafa Mukadam. */ @@ -15,18 +15,13 @@ #include #include -#include -#include #include #include #include -#include #include #include -#include #include -#include #include #include #include @@ -34,39 +29,28 @@ namespace gtdynamics { -/// One self collision check: a query point vs another query point or a link's -/// field. One side is always a point. Build via PointPair / PointSDF. +/// 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; ///< query point index, the point side - bool is_sdf; ///< false: point vs point; true: point vs a link field - size_t b; ///< point vs point: the other query point index - LinkSharedPtr link_b; ///< point vs field: link B - std::shared_ptr sdf_b; ///< field, in B's frame - double epsilon; ///< standoff between the sides, added to their radii - - /// Point vs point pair. - static SelfCollisionPair PointPair(size_t a, size_t b, double epsilon) { - return SelfCollisionPair{a, false, b, nullptr, nullptr, epsilon}; - } + size_t a; ///< first query point index + size_t b; ///< second query point index + double epsilon; ///< standoff between the two, added to their radii - /// Point vs link field pair. - static SelfCollisionPair PointSDF( - size_t a, const LinkSharedPtr &link_b, - const std::shared_ptr &sdf_b, double epsilon) { - return SelfCollisionPair{a, true, 0, link_b, sdf_b, epsilon}; - } + 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; /** - * Unary factor keeping the robot clear of itself over a set of collision pairs, - * one hinge loss row per pair. Both sides 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. + * 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. * - * The caller registers only meaningful pairs: adjacent links sit at a near - * constant separation and would fire permanently, and two points that can + * The caller registers only meaningful pairs: points on adjacent links sit at a + * near constant separation and would fire permanently, and two points that can * coincide give a non-finite distance gradient. The factor excludes neither. */ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { @@ -78,7 +62,7 @@ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { gtsam::Vector radii_; ///< one radius per query point, zero if unspecified std::vector pairs_; - /// Reject pairs whose indices, radii, fields or standoffs are inconsistent. + /// Reject inconsistent indices, radii or standoffs. void validate() const { if (static_cast(radii_.size()) != robot_.nrPoints()) { throw std::invalid_argument( @@ -92,19 +76,13 @@ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { throw std::invalid_argument( "SelfCollisionFactor: a pair epsilon must be >= 0."); } - if (p.a >= robot_.nrPoints()) { + if (p.a >= robot_.nrPoints() || p.b >= robot_.nrPoints()) { throw std::invalid_argument( "SelfCollisionFactor: pair point index out of range."); } - if (p.is_sdf) { - if (!p.link_b || !p.sdf_b) { - throw std::invalid_argument( - "SelfCollisionFactor: a point vs field pair needs a link and a " - "field."); - } - } else if (p.b >= robot_.nrPoints()) { + if (p.a == p.b) { throw std::invalid_argument( - "SelfCollisionFactor: pair point index out of range."); + "SelfCollisionFactor: a pair must use two distinct points."); } } } @@ -114,7 +92,7 @@ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { * Constructor with a single sigma across every pair. * @param q_key key of the stacked joint angle vector * @param robot query point model, spanning every joint involved - * @param pairs collision pairs to check + * @param pairs query point pairs to check * @param radii radius of each query point, one per point of the model * @param cost_sigma cost function sigma, shared by every pair */ @@ -133,7 +111,7 @@ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { * Constructor with a sigma per pair. * @param q_key key of the stacked joint angle vector * @param robot query point model, spanning every joint involved - * @param pairs collision pairs to check + * @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 */ @@ -166,73 +144,25 @@ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { gtsam::Vector evaluateError( const gtsam::Vector &q, gtsam::OptionalMatrixType H1 = nullptr) const override { - const size_t dof = robot_.dof(); const size_t n = pairs_.size(); gtsam::Vector err(n); - // One FK pass supplies both the query points and the link poses. - std::map wTl; - std::map Jl; - robot_.forwardKinematics(q, &wTl, H1 ? &Jl : nullptr); - - // Transform every query point to the world, keeping the pose map for the - // field sides (queryPoints would discard it). - const auto &pts = robot_.points(); - std::vector wPs(robot_.nrPoints()); + std::vector wPs; std::vector Jps; - if (H1) Jps.resize(robot_.nrPoints()); - for (size_t i = 0; i < robot_.nrPoints(); ++i) { - const uint8_t id = pts[i].link->id(); - auto it = wTl.find(id); - if (it == wTl.end()) { - throw std::runtime_error( - "SelfCollisionFactor: query point on a link not reachable from the " - "base."); - } - if (H1) { - gtsam::Matrix36 H_pose; - wPs[i] = it->second.transformFrom(pts[i].point, H_pose); - Jps[i] = H_pose * Jl.at(id); - } else { - wPs[i] = it->second.transformFrom(pts[i].point); - } - } - - if (H1) *H1 = gtsam::Matrix::Zero(n, dof); + robot_.queryPoints(q, &wPs, H1 ? &Jps : nullptr); + if (H1) *H1 = gtsam::Matrix::Zero(n, robot_.dof()); for (size_t r = 0; r < n; ++r) { const SelfCollisionPair &p = pairs_[r]; - if (!p.is_sdf) { - // Point vs point: standoff folds in both radii. - const double eps = p.epsilon + radii_(p.a) + radii_(p.b); - if (H1) { - gtsam::Matrix13 H_pA, H_pB; - err(r) = hingeLossSelfCollisionCost(wPs[p.a], wPs[p.b], eps, H_pA, - H_pB); - H1->row(r) = H_pA * Jps[p.a] + H_pB * Jps[p.b]; - } else { - err(r) = hingeLossSelfCollisionCost(wPs[p.a], wPs[p.b], eps); - } + // The standoff folds in both spheres' radii. + const double eps = p.epsilon + radii_(p.a) + radii_(p.b); + if (H1) { + gtsam::Matrix13 H_pA, H_pB; + err(r) = + hingeLossSelfCollisionCost(wPs[p.a], wPs[p.b], eps, H_pA, H_pB); + H1->row(r) = H_pA * Jps[p.a] + H_pB * Jps[p.b]; } else { - // Point vs field: the field encodes link_b's shape, so only the point - // side carries a radius. - const uint8_t bid = p.link_b->id(); - auto itB = wTl.find(bid); - if (itB == wTl.end()) { - throw std::runtime_error( - "SelfCollisionFactor: field link not reachable from the base."); - } - const double eps = p.epsilon + radii_(p.a); - if (H1) { - gtsam::Matrix16 H_pose; - gtsam::Matrix13 H_point; - err(r) = hingeLossObstacleCost(itB->second, wPs[p.a], *p.sdf_b, eps, - H_pose, H_point); - // Both the point and link_b's frame move with q. - H1->row(r) = H_point * Jps[p.a] + H_pose * Jl.at(bid); - } else { - err(r) = hingeLossObstacleCost(itB->second, wPs[p.a], *p.sdf_b, eps); - } + err(r) = hingeLossSelfCollisionCost(wPs[p.a], wPs[p.b], eps); } } return err; diff --git a/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp b/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp index 8c3330117..97561876a 100644 --- a/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp +++ b/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include #include #include @@ -42,8 +41,6 @@ using gtsam::Vector3; using gtsam::noiseModel::Isotropic; using gtsam::symbol_shorthand::X; -static const double kCell = 0.05; - static const Robot kRobot = CreateRobotFromFile(kUrdfPath + std::string("bar_lab.urdf")); @@ -96,7 +93,7 @@ static std::vector bothArmJoints() { return joints; } -// Query points: two wrists (cross-arm pairs), robot1's forearm (same-arm SDF +// 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)), @@ -122,25 +119,6 @@ static Vector configClose() { return q; } -// Sphere signed distance field centered at a frame origin, spanning +/- half m. -static std::shared_ptr sphereSDF(double radius, - double half) { - const size_t n = static_cast(2 * half / kCell) + 1; - const Point3 origin(-half, -half, -half); - std::vector data(n); - for (size_t k = 0; k < n; ++k) { - Matrix layer(n, n); - for (size_t i = 0; i < n; ++i) { - for (size_t j = 0; j < n; ++j) { - const Point3 p = origin + Point3(j * kCell, i * kCell, k * kCell); - layer(i, j) = p.norm() - radius; - } - } - data[k] = layer; - } - return std::make_shared(origin, kCell, data); -} - /* ************************ factor Jacobians *************************** */ // Point-to-point across the two arms. Epsilon is set a metre past the measured @@ -153,32 +131,7 @@ TEST(SelfCollisionFactor, pointPointJacobians) { const double eps = (wPs[0] - wPs[1]).norm() + 1.0; const std::vector pairs = { - SelfCollisionPair::PointPair(0, 1, eps)}; // wrist1 vs wrist2 - SelfCollisionFactor 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); -} - -// Point vs a link's field, same arm (forearm point vs wrist field). This is the -// test that both the point and the moving-frame Jacobian terms are present. The -// grid and epsilon are sized from the measured distance so the hinge is active. -// Query point 0 is link_6's CoM, the field origin, so ||wPs[2]-wPs[0]|| is the -// distance of the forearm point into the field. -TEST(SelfCollisionFactor, pointSDFJacobians) { - RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); - const Vector q = configApart(); - std::vector wPs; - model.queryPoints(q, &wPs); - const double dist = (wPs[2] - wPs[0]).norm(); - - auto sdf = sphereSDF(0.2, dist + 0.3); // grid contains the forearm point - const std::vector pairs = { - SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, - dist + 0.1)}; // active: cost ~= 0.3 + SelfCollisionPair(0, 1, eps)}; // wrist1 vs wrist2 SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); EXPECT(factor.evaluateError(q)(0) > 0.0); // active branch @@ -188,23 +141,23 @@ TEST(SelfCollisionFactor, pointSDFJacobians) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } -// One factor holding both a point-point and a point-SDF pair. -TEST(SelfCollisionFactor, mixedPairs) { +// 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(SelfCollisionFactor, multiplePairs) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); const Vector q = configApart(); std::vector wPs; model.queryPoints(q, &wPs); - const double eps = (wPs[0] - wPs[1]).norm() + 1.0; - const double dist = (wPs[2] - wPs[0]).norm(); - auto sdf = sphereSDF(0.2, dist + 0.3); const std::vector pairs = { - SelfCollisionPair::PointPair(0, 1, eps), - SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, - dist + 0.1)}; + SelfCollisionPair(0, 1, (wPs[0] - wPs[1]).norm() + 1.0), // cross-arm + SelfCollisionPair(2, 0, (wPs[2] - wPs[0]).norm() + 1.0)}; // same arm SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); - EXPECT_LONGS_EQUAL(2, factor.evaluateError(q).size()); + 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); @@ -225,7 +178,7 @@ TEST(SelfCollisionFactor, pushesPointsApart) { const double eps = start_dist + 0.3; const std::vector pairs = { - SelfCollisionPair::PointPair(3, 4, eps)}; // base1 vs base2 + SelfCollisionPair(3, 4, eps)}; // base1 vs base2 SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.01); gtsam::NonlinearFactorGraph graph; @@ -258,7 +211,7 @@ TEST(SelfCollisionFactor, radiiPointPoint) { const double eps = d - 0.05; const std::vector pairs = { - SelfCollisionPair::PointPair(0, 1, eps)}; + SelfCollisionPair(0, 1, eps)}; SelfCollisionFactor clear(X(0), model, pairs, Vector::Zero(5), 0.1); EXPECT_DOUBLES_EQUAL(0.0, clear.evaluateError(q)(0), 1e-9); @@ -284,32 +237,30 @@ TEST(SelfCollisionFactor, radiiPointPoint) { EXPECT_DOUBLES_EQUAL(0.0, f_other.evaluateError(q)(0), 1e-9); } -// A point vs field pair carries a radius only on the point side, and in the -// active branch it adds directly to the cost. -TEST(SelfCollisionFactor, radiiPointSDF) { +// In the active branch a radius adds directly to the cost, once per sphere. +TEST(SelfCollisionFactor, radiiAddToTheStandoff) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); const Vector q = configApart(); std::vector wPs; model.queryPoints(q, &wPs); - const double dist = (wPs[2] - wPs[0]).norm(); - auto sdf = sphereSDF(0.2, dist + 0.3); - const std::vector pairs = { - SelfCollisionPair::PointSDF(2, kRobot.link("robot1_link_6"), sdf, - dist + 0.1)}; + const double eps = (wPs[0] - wPs[1]).norm() + 1.0; // active by construction + const std::vector pairs = {SelfCollisionPair(0, 1, eps)}; SelfCollisionFactor base(X(0), model, pairs, Vector::Zero(5), 0.1); const double e0 = base.evaluateError(q)(0); - EXPECT(e0 > 0.0); // active branch + EXPECT(e0 > 0.0); + // Each sphere's radius adds to the standoff, so the cost rises by their sum. Vector radii = Vector::Zero(5); - radii(2) = 0.1; - SelfCollisionFactor with_radius(X(0), model, pairs, radii, 0.1); - EXPECT_DOUBLES_EQUAL(e0 + 0.1, with_radius.evaluateError(q)(0), 1e-9); + radii(0) = 0.1; + radii(1) = 0.2; + SelfCollisionFactor inflated(X(0), model, pairs, radii, 0.1); + EXPECT_DOUBLES_EQUAL(e0 + 0.3, inflated.evaluateError(q)(0), 1e-9); - // A radius on a point that is not the pair's point side is ignored. + // A radius on an uninvolved point is ignored. Vector other = Vector::Zero(5); - other(0) = 0.1; + other(2) = 0.2; SelfCollisionFactor f_other(X(0), model, pairs, other, 0.1); EXPECT_DOUBLES_EQUAL(e0, f_other.evaluateError(q)(0), 1e-9); } @@ -319,7 +270,7 @@ TEST(SelfCollisionFactor, radiiPointSDF) { TEST(SelfCollisionFactor, rejectsBadInput) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); const std::vector pairs = { - SelfCollisionPair::PointPair(0, 1, 1.0)}; + SelfCollisionPair(0, 1, 1.0)}; // radii length must equal the number of query points. CHECK_EXCEPTION( @@ -327,7 +278,7 @@ TEST(SelfCollisionFactor, rejectsBadInput) { std::invalid_argument); // a point index out of range. const std::vector bad = { - SelfCollisionPair::PointPair(0, 99, 1.0)}; + SelfCollisionPair(0, 99, 1.0)}; CHECK_EXCEPTION( SelfCollisionFactor(X(0), model, bad, Vector::Zero(5), 0.1), std::invalid_argument); diff --git a/python/tests/test_self_collision.py b/python/tests/test_self_collision.py index 9e351d052..64f6de3f5 100644 --- a/python/tests/test_self_collision.py +++ b/python/tests/test_self_collision.py @@ -57,7 +57,7 @@ def setUp(self): def make_factor(self, sigma=0.01): pairs = gtd.SelfCollisionPairs() - pairs.append(gtd.SelfCollisionPair.PointPair(0, 1, EPSILON)) + pairs.append(gtd.SelfCollisionPair(0, 1, EPSILON)) return gtd.SelfCollisionFactor(X(0), self.model, pairs, np.zeros(2), sigma) @@ -94,7 +94,7 @@ def test_radii_inflate_the_points(self): d = self.separation(Q_CLOSE) eps = d - 0.05 # bases clear by 0.05 without any radius pairs = gtd.SelfCollisionPairs() - pairs.append(gtd.SelfCollisionPair.PointPair(0, 1, eps)) + pairs.append(gtd.SelfCollisionPair(0, 1, eps)) values = gtsam.Values() values.insert(X(0), Q_CLOSE) @@ -109,7 +109,7 @@ def test_radii_inflate_the_points(self): def test_rejects_bad_radii(self): """A radii vector of the wrong length is rejected.""" pairs = gtd.SelfCollisionPairs() - pairs.append(gtd.SelfCollisionPair.PointPair(0, 1, EPSILON)) + pairs.append(gtd.SelfCollisionPair(0, 1, EPSILON)) with self.assertRaises(ValueError): gtd.SelfCollisionFactor(X(0), self.model, pairs, np.zeros(3), 0.01) From 0c2dc8e49010cd7de5c35b57062a80b5e41589a4 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Mon, 20 Jul 2026 17:19:06 -0400 Subject: [PATCH 25/27] made it explicitly a sphere factor --- ...onFactor.h => SelfCollisionSphereFactor.h} | 32 +++++++------- ....cpp => testSelfCollisionSphereFactor.cpp} | 42 +++++++++---------- 2 files changed, 38 insertions(+), 36 deletions(-) rename gtdynamics/factors/{SelfCollisionFactor.h => SelfCollisionSphereFactor.h} (85%) rename gtdynamics/gpmp2/tests/{testSelfCollisionFactor.cpp => testSelfCollisionSphereFactor.cpp} (87%) diff --git a/gtdynamics/factors/SelfCollisionFactor.h b/gtdynamics/factors/SelfCollisionSphereFactor.h similarity index 85% rename from gtdynamics/factors/SelfCollisionFactor.h rename to gtdynamics/factors/SelfCollisionSphereFactor.h index 412dfcabf..0914e7984 100644 --- a/gtdynamics/factors/SelfCollisionFactor.h +++ b/gtdynamics/factors/SelfCollisionSphereFactor.h @@ -6,7 +6,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SelfCollisionFactor.h + * @file SelfCollisionSphereFactor.h * @brief Self collision cost factor over a set of query point pairs. * @author Karthik Shaji - Adapted from gpmp2 by Mustafa Mukadam. */ @@ -53,9 +53,10 @@ using SelfCollisionPairs = std::vector; * near constant separation and would fire permanently, and two points that can * coincide give a non-finite distance gradient. The factor excludes neither. */ -class SelfCollisionFactor : public gtsam::NoiseModelFactorN { +class SelfCollisionSphereFactor + : public gtsam::NoiseModelFactorN { private: - using This = SelfCollisionFactor; + using This = SelfCollisionSphereFactor; using Base = gtsam::NoiseModelFactorN; RobotQueryPoints robot_; @@ -66,23 +67,24 @@ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { void validate() const { if (static_cast(radii_.size()) != robot_.nrPoints()) { throw std::invalid_argument( - "SelfCollisionFactor: radii must have one entry per query point."); + "SelfCollisionSphereFactor: radii must have one entry per point."); } if ((radii_.array() < 0.0).any()) { - throw std::invalid_argument("SelfCollisionFactor: radii must be >= 0."); + throw std::invalid_argument( + "SelfCollisionSphereFactor: radii must be >= 0."); } for (const auto &p : pairs_) { if (p.epsilon < 0.0) { throw std::invalid_argument( - "SelfCollisionFactor: a pair epsilon must be >= 0."); + "SelfCollisionSphereFactor: a pair epsilon must be >= 0."); } if (p.a >= robot_.nrPoints() || p.b >= robot_.nrPoints()) { throw std::invalid_argument( - "SelfCollisionFactor: pair point index out of range."); + "SelfCollisionSphereFactor: pair point index out of range."); } if (p.a == p.b) { throw std::invalid_argument( - "SelfCollisionFactor: a pair must use two distinct points."); + "SelfCollisionSphereFactor: a pair must use two distinct points."); } } } @@ -96,7 +98,7 @@ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { * @param radii radius of each query point, one per point of the model * @param cost_sigma cost function sigma, shared by every pair */ - SelfCollisionFactor(gtsam::Key q_key, const RobotQueryPoints &robot, + SelfCollisionSphereFactor(gtsam::Key q_key, const RobotQueryPoints &robot, const std::vector &pairs, const gtsam::Vector &radii, double cost_sigma) : Base(gtsam::noiseModel::Isotropic::Sigma(pairs.size(), cost_sigma), @@ -115,7 +117,7 @@ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { * @param radii radius of each query point, one per point of the model * @param sigmas cost function sigma of each pair, one per pair */ - SelfCollisionFactor(gtsam::Key q_key, const RobotQueryPoints &robot, + SelfCollisionSphereFactor(gtsam::Key q_key, const RobotQueryPoints &robot, const std::vector &pairs, const gtsam::Vector &radii, const gtsam::Vector &sigmas) : Base(gtsam::noiseModel::Diagonal::Sigmas(sigmas), q_key), @@ -124,12 +126,12 @@ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { pairs_(pairs) { if (static_cast(sigmas.size()) != pairs.size()) { throw std::invalid_argument( - "SelfCollisionFactor: sigmas must have one entry per pair."); + "SelfCollisionSphereFactor: sigmas must have one entry per pair."); } validate(); } - ~SelfCollisionFactor() override {} + ~SelfCollisionSphereFactor() override {} /// Return a deep copy of this factor. gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -175,10 +177,10 @@ class SelfCollisionFactor : public gtsam::NoiseModelFactorN { void print(const std::string &s = "", const gtsam::KeyFormatter &keyFormatter = gtsam::DefaultKeyFormatter) const override { - std::cout << s << "SelfCollisionFactor with " << pairs_.size() << " pairs" - << std::endl; + std::cout << s << "SelfCollisionSphereFactor with " << pairs_.size() + << " pairs" << std::endl; Base::print("", keyFormatter); } -}; // \class SelfCollisionFactor +}; // \class SelfCollisionSphereFactor } // namespace gtdynamics diff --git a/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp b/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp similarity index 87% rename from gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp rename to gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp index 97561876a..ae4bcf17f 100644 --- a/gtdynamics/gpmp2/tests/testSelfCollisionFactor.cpp +++ b/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp @@ -6,14 +6,14 @@ * -------------------------------------------------------------------------- */ /** - * @file testSelfCollisionFactor.cpp + * @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 @@ -123,7 +123,7 @@ static Vector configClose() { // 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(SelfCollisionFactor, pointPointJacobians) { +TEST(SelfCollisionSphereFactor, pointPointJacobians) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); const Vector q = configApart(); std::vector wPs; @@ -132,7 +132,7 @@ TEST(SelfCollisionFactor, pointPointJacobians) { const std::vector pairs = { SelfCollisionPair(0, 1, eps)}; // wrist1 vs wrist2 - SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); + SelfCollisionSphereFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); EXPECT(factor.evaluateError(q)(0) > 0.0); // active branch @@ -143,7 +143,7 @@ TEST(SelfCollisionFactor, pointPointJacobians) { // 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(SelfCollisionFactor, multiplePairs) { +TEST(SelfCollisionSphereFactor, multiplePairs) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); const Vector q = configApart(); std::vector wPs; @@ -152,7 +152,7 @@ TEST(SelfCollisionFactor, multiplePairs) { const std::vector pairs = { SelfCollisionPair(0, 1, (wPs[0] - wPs[1]).norm() + 1.0), // cross-arm SelfCollisionPair(2, 0, (wPs[2] - wPs[0]).norm() + 1.0)}; // same arm - SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); + SelfCollisionSphereFactor factor(X(0), model, pairs, Vector::Zero(5), 0.1); const Vector err = factor.evaluateError(q); EXPECT_LONGS_EQUAL(2, err.size()); @@ -168,7 +168,7 @@ TEST(SelfCollisionFactor, multiplePairs) { // 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(SelfCollisionFactor, pushesPointsApart) { +TEST(SelfCollisionSphereFactor, pushesPointsApart) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); const Vector q0 = configClose(); @@ -179,7 +179,7 @@ TEST(SelfCollisionFactor, pushesPointsApart) { const std::vector pairs = { SelfCollisionPair(3, 4, eps)}; // base1 vs base2 - SelfCollisionFactor factor(X(0), model, pairs, Vector::Zero(5), 0.01); + SelfCollisionSphereFactor factor(X(0), model, pairs, Vector::Zero(5), 0.01); gtsam::NonlinearFactorGraph graph; graph.add(factor); @@ -202,7 +202,7 @@ TEST(SelfCollisionFactor, pushesPointsApart) { // eps is set 0.05 below the measured distance so the points are clear, then two // 0.1 radii push eps + rA + rB past d, turning the pair into a collision. -TEST(SelfCollisionFactor, radiiPointPoint) { +TEST(SelfCollisionSphereFactor, radiiPointPoint) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); const Vector q = configApart(); std::vector wPs; @@ -213,32 +213,32 @@ TEST(SelfCollisionFactor, radiiPointPoint) { const std::vector pairs = { SelfCollisionPair(0, 1, eps)}; - SelfCollisionFactor clear(X(0), model, pairs, Vector::Zero(5), 0.1); + 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; - SelfCollisionFactor inflated(X(0), model, pairs, radii, 0.1); + SelfCollisionSphereFactor inflated(X(0), model, pairs, radii, 0.1); EXPECT_DOUBLES_EQUAL(eps + 0.2 - d, inflated.evaluateError(q)(0), 1e-9); // The same total radius on either point gives the same standoff. Vector on_a = Vector::Zero(5), on_b = Vector::Zero(5); on_a(0) = 0.2; on_b(1) = 0.2; - SelfCollisionFactor f_a(X(0), model, pairs, on_a, 0.1); - SelfCollisionFactor f_b(X(0), model, pairs, on_b, 0.1); + SelfCollisionSphereFactor f_a(X(0), model, pairs, on_a, 0.1); + SelfCollisionSphereFactor f_b(X(0), model, pairs, on_b, 0.1); EXPECT_DOUBLES_EQUAL(f_a.evaluateError(q)(0), f_b.evaluateError(q)(0), 1e-9); // A radius on an uninvolved point is ignored. Vector other = Vector::Zero(5); other(2) = 0.2; - SelfCollisionFactor f_other(X(0), model, pairs, other, 0.1); + SelfCollisionSphereFactor f_other(X(0), model, pairs, other, 0.1); EXPECT_DOUBLES_EQUAL(0.0, f_other.evaluateError(q)(0), 1e-9); } // In the active branch a radius adds directly to the cost, once per sphere. -TEST(SelfCollisionFactor, radiiAddToTheStandoff) { +TEST(SelfCollisionSphereFactor, radiiAddToTheStandoff) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); const Vector q = configApart(); std::vector wPs; @@ -247,7 +247,7 @@ TEST(SelfCollisionFactor, radiiAddToTheStandoff) { const double eps = (wPs[0] - wPs[1]).norm() + 1.0; // active by construction const std::vector pairs = {SelfCollisionPair(0, 1, eps)}; - SelfCollisionFactor base(X(0), model, pairs, Vector::Zero(5), 0.1); + SelfCollisionSphereFactor base(X(0), model, pairs, Vector::Zero(5), 0.1); const double e0 = base.evaluateError(q)(0); EXPECT(e0 > 0.0); @@ -255,32 +255,32 @@ TEST(SelfCollisionFactor, radiiAddToTheStandoff) { Vector radii = Vector::Zero(5); radii(0) = 0.1; radii(1) = 0.2; - SelfCollisionFactor inflated(X(0), model, pairs, radii, 0.1); + SelfCollisionSphereFactor inflated(X(0), model, pairs, radii, 0.1); EXPECT_DOUBLES_EQUAL(e0 + 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; - SelfCollisionFactor f_other(X(0), model, pairs, other, 0.1); + SelfCollisionSphereFactor f_other(X(0), model, pairs, other, 0.1); EXPECT_DOUBLES_EQUAL(e0, f_other.evaluateError(q)(0), 1e-9); } /* ************************ input validation *************************** */ -TEST(SelfCollisionFactor, rejectsBadInput) { +TEST(SelfCollisionSphereFactor, rejectsBadInput) { RobotQueryPoints model(kRobot, "columns", bothArmJoints(), queryPoints()); const std::vector pairs = { SelfCollisionPair(0, 1, 1.0)}; // radii length must equal the number of query points. CHECK_EXCEPTION( - SelfCollisionFactor(X(0), model, pairs, Vector::Zero(3), 0.1), + 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( - SelfCollisionFactor(X(0), model, bad, Vector::Zero(5), 0.1), + SelfCollisionSphereFactor(X(0), model, bad, Vector::Zero(5), 0.1), std::invalid_argument); } From f3beab734cfb69c5e1e59f625fc2bcc409b69107 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Mon, 20 Jul 2026 17:32:26 -0400 Subject: [PATCH 26/27] reject same link pairs --- gtdynamics.i | 19 +++++++++++-------- .../factors/SelfCollisionSphereFactor.h | 12 +++++++++--- .../tests/testSelfCollisionSphereFactor.cpp | 15 +++++++++++++++ python/tests/test_self_collision.py | 17 +++++++++-------- 4 files changed, 44 insertions(+), 19 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index b92d535db..309081efc 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -1138,7 +1138,7 @@ class ObstacleSDFFactorGP : gtsam::NoiseModelFactor { gtdynamics::GTDKeyFormatter); }; -#include +#include class SelfCollisionPair { SelfCollisionPair(); SelfCollisionPair(size_t a, size_t b, double epsilon); @@ -1149,13 +1149,16 @@ class SelfCollisionPair { }; // SelfCollisionPairs defined in specializations.h -class SelfCollisionFactor : gtsam::NoiseModelFactor { - SelfCollisionFactor(gtsam::Key q_key, const gtdynamics::RobotQueryPoints &robot, - const gtdynamics::SelfCollisionPairs &pairs, - const gtsam::Vector &radii, double cost_sigma); - SelfCollisionFactor(gtsam::Key q_key, const gtdynamics::RobotQueryPoints &robot, - const gtdynamics::SelfCollisionPairs &pairs, - const gtsam::Vector &radii, const gtsam::Vector &sigmas); +class SelfCollisionSphereFactor : gtsam::NoiseModelFactor { + SelfCollisionSphereFactor(gtsam::Key q_key, + const gtdynamics::RobotQueryPoints &robot, + const gtdynamics::SelfCollisionPairs &pairs, + const gtsam::Vector &radii, double cost_sigma); + SelfCollisionSphereFactor(gtsam::Key q_key, + const gtdynamics::RobotQueryPoints &robot, + const gtdynamics::SelfCollisionPairs &pairs, + const gtsam::Vector &radii, + const gtsam::Vector &sigmas); size_t nrPairs() const; gtsam::Vector radii() const; diff --git a/gtdynamics/factors/SelfCollisionSphereFactor.h b/gtdynamics/factors/SelfCollisionSphereFactor.h index 0914e7984..6766db4e6 100644 --- a/gtdynamics/factors/SelfCollisionSphereFactor.h +++ b/gtdynamics/factors/SelfCollisionSphereFactor.h @@ -49,9 +49,8 @@ using SelfCollisionPairs = std::vector; * 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. * - * The caller registers only meaningful pairs: points on adjacent links sit at a - * near constant separation and would fire permanently, and two points that can - * coincide give a non-finite distance gradient. The factor excludes neither. + * Same-link pairs are rejected; the caller must also avoid adjacent-link pairs + * (constant separation) and points that can coincide (non-finite gradient). */ class SelfCollisionSphereFactor : public gtsam::NoiseModelFactorN { @@ -86,6 +85,13 @@ class SelfCollisionSphereFactor throw std::invalid_argument( "SelfCollisionSphereFactor: 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()[p.a].link->id() == robot_.points()[p.b].link->id()) { + throw std::invalid_argument( + "SelfCollisionSphereFactor: a pair must use points on different " + "links."); + } } } diff --git a/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp b/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp index ae4bcf17f..be19d3466 100644 --- a/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp +++ b/gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp @@ -284,6 +284,21 @@ TEST(SelfCollisionSphereFactor, rejectsBadInput) { 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 + RobotQueryPoints model(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/python/tests/test_self_collision.py b/python/tests/test_self_collision.py index 64f6de3f5..f835a0a8a 100644 --- a/python/tests/test_self_collision.py +++ b/python/tests/test_self_collision.py @@ -6,7 +6,7 @@ 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/testSelfCollisionFactor.cpp. +gtdynamics/gpmp2/tests/testSelfCollisionSphereFactor.cpp. Author: Karthik Shaji """ @@ -58,8 +58,8 @@ def setUp(self): def make_factor(self, sigma=0.01): pairs = gtd.SelfCollisionPairs() pairs.append(gtd.SelfCollisionPair(0, 1, EPSILON)) - return gtd.SelfCollisionFactor(X(0), self.model, pairs, np.zeros(2), - sigma) + return gtd.SelfCollisionSphereFactor(X(0), self.model, pairs, + np.zeros(2), sigma) def separation(self, q): pts = self.model.worldPoints(q) @@ -98,10 +98,10 @@ def test_radii_inflate_the_points(self): values = gtsam.Values() values.insert(X(0), Q_CLOSE) - clear = gtd.SelfCollisionFactor(X(0), self.model, pairs, np.zeros(2), - 0.1) - inflated = gtd.SelfCollisionFactor(X(0), self.model, pairs, - np.array([0.1, 0.1]), 0.1) + 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) @@ -111,7 +111,8 @@ def test_rejects_bad_radii(self): pairs = gtd.SelfCollisionPairs() pairs.append(gtd.SelfCollisionPair(0, 1, EPSILON)) with self.assertRaises(ValueError): - gtd.SelfCollisionFactor(X(0), self.model, pairs, np.zeros(3), 0.01) + gtd.SelfCollisionSphereFactor(X(0), self.model, pairs, np.zeros(3), + 0.01) if __name__ == "__main__": From 20712cdd0c89de0b1e02993c5b75d47e5524ef3e Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Mon, 20 Jul 2026 17:58:49 -0400 Subject: [PATCH 27/27] added test to reject poiints that have different radii --- gtdynamics/factors/ObstacleSDFFactor.h | 24 +++++++++++++ .../gpmp2/tests/testObstacleFactors.cpp | 36 +++++++++++++++++++ 2 files changed, 60 insertions(+) diff --git a/gtdynamics/factors/ObstacleSDFFactor.h b/gtdynamics/factors/ObstacleSDFFactor.h index c509ee1b6..6b0b41887 100644 --- a/gtdynamics/factors/ObstacleSDFFactor.h +++ b/gtdynamics/factors/ObstacleSDFFactor.h @@ -22,7 +22,10 @@ #include #include +#include +#include #include +#include #include #include #include @@ -59,6 +62,27 @@ class ObstacleSDFFactor : public gtsam::NoiseModelFactorN { if ((radii_.array() < 0.0).any()) { throw std::invalid_argument("ObstacleSDFFactor: radii must be >= 0."); } + // Overlapping spheres on a link are fine, but the same point registered + // twice with different radii is a contradiction. Group by link so only + // same-link points are compared, not every pair. + const auto &pts = robot_.points(); + std::map> by_link; + for (size_t i = 0; i < pts.size(); ++i) { + by_link[pts[i].link->id()].push_back(i); + } + for (const auto &group : by_link) { + const std::vector &idx = group.second; + for (size_t a = 0; a < idx.size(); ++a) { + for (size_t b = a + 1; b < idx.size(); ++b) { + if ((pts[idx[a]].point - pts[idx[b]].point).norm() < 1e-9 && + std::fabs(radii_(idx[a]) - radii_(idx[b])) > 1e-9) { + throw std::invalid_argument( + "ObstacleSDFFactor: two points at the same location have " + "conflicting radii."); + } + } + } + } } public: diff --git a/gtdynamics/gpmp2/tests/testObstacleFactors.cpp b/gtdynamics/gpmp2/tests/testObstacleFactors.cpp index 2f46d79e8..d8030ef9c 100644 --- a/gtdynamics/gpmp2/tests/testObstacleFactors.cpp +++ b/gtdynamics/gpmp2/tests/testObstacleFactors.cpp @@ -236,6 +236,42 @@ static Vector goalConfig() { .finished(); } +// Two points at the same location on a link with different radii contradict; +// distinct points that merely overlap are deliberate coverage and allowed. +TEST(ObstacleSDFFactor, rejectsConflictingRadii) { + const LinkSharedPtr link = kRobot.link("robot1_link_6"); + auto sdf = std::make_shared( + makeSphereSDF(Point3(5, 5, 5), 0.1, Point3(4, 4, 4), kCell, 5, 5, 5)); + + // Same location on the link, conflicting radii: rejected. + const std::vector duplicate = { + PointOnLink(link, Point3(0.0, 0.0, 0.0)), + PointOnLink(link, Point3(0.0, 0.0, 0.0))}; + RobotQueryPoints dup_model(kRobot, "columns", robot1Joints(), duplicate); + CHECK_EXCEPTION( + ObstacleSDFFactor(X(0), dup_model, sdf, 0.01, 0.1, + (Vector(2) << 0.1, 0.2).finished()), + std::invalid_argument); + + // Same location, same radius: redundant but not a contradiction, allowed. + ObstacleSDFFactor same(X(0), dup_model, sdf, 0.01, 0.1, + (Vector(2) << 0.1, 0.1).finished()); + EXPECT(assert_equal(gtsam::Vector((Vector(2) << 0.1, 0.1).finished()), + same.radii(), 1e-9)); + + // Distinct, overlapping locations with different radii: allowed coverage. + const std::vector overlap = { + PointOnLink(link, Point3(0.0, 0.0, 0.0)), + PointOnLink(link, Point3(0.05, 0.0, 0.0))}; + RobotQueryPoints overlap_model(kRobot, "columns", robot1Joints(), overlap); + ObstacleSDFFactor allowed(X(0), overlap_model, sdf, 0.01, 0.1, + (Vector(2) << 0.1, 0.2).finished()); + EXPECT_LONGS_EQUAL(2, allowed.evaluateError((Vector(9) << 2.0, 2.0, 1.0, 0.0, + -0.5, -1.0, 0.0, 0.5, 0.0) + .finished()) + .size()); +} + // Only robot1's joints are given to the model, so the bridge2 subtree is never // traversed and robot2 is absent from the query point model entirely. TEST(RobotQueryPoints, jacobiansAgainstNumerical) {