From ca8bc464e98f20f21322623671f04b35e473740f Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Sat, 15 Aug 2026 10:32:50 -0700 Subject: [PATCH 01/11] initial nn factor --- gtdynamics.i | 42 +++ gtdynamics/dynamics/MLP.cpp | 272 +++++++++++++++ gtdynamics/dynamics/MLP.h | 100 ++++++ gtdynamics/dynamics/tests/testMLP.cpp | 241 ++++++++++++++ gtdynamics/factors/NNCableFactor.h | 121 +++++++ .../factors/internal/CollisionFactorUtils.h | 33 ++ gtdynamics/gpmp2/NNCableSpline.cpp | 194 +++++++++++ gtdynamics/gpmp2/NNCableSpline.h | 117 +++++++ gtdynamics/gpmp2/RobotQueryPoints.cpp | 19 ++ gtdynamics/gpmp2/RobotQueryPoints.h | 11 + gtdynamics/gpmp2/tests/testNNCableFactor.cpp | 310 ++++++++++++++++++ .../gpmp2/tests/testObstacleFactors.cpp | 29 ++ 12 files changed, 1489 insertions(+) create mode 100644 gtdynamics/dynamics/MLP.cpp create mode 100644 gtdynamics/dynamics/MLP.h create mode 100644 gtdynamics/dynamics/tests/testMLP.cpp create mode 100644 gtdynamics/factors/NNCableFactor.h create mode 100644 gtdynamics/gpmp2/NNCableSpline.cpp create mode 100644 gtdynamics/gpmp2/NNCableSpline.h create mode 100644 gtdynamics/gpmp2/tests/testNNCableFactor.cpp diff --git a/gtdynamics.i b/gtdynamics.i index 9dc57279..6d6a0029 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -1152,6 +1152,48 @@ class ObstacleSDFFactorGP : gtsam::NoiseModelFactor { gtdynamics::GTDKeyFormatter); }; +#include +class MLP { + MLP(const string &filename); + + size_t inputDim() const; + size_t outputDim() const; + size_t nrLayers() const; + gtsam::Vector forward(const gtsam::Vector &x) const; +}; + +#include +class NNCableSpline { + NNCableSpline(const gtdynamics::Robot &robot, const string &baseLinkName, + const std::vector &joints, + const gtdynamics::PointOnLink &attachment0, + const gtdynamics::PointOnLink &attachment1, + const gtdynamics::Link *referenceLink, + const gtdynamics::MLP *mlp, + const std::vector &inputIndices, size_t numChebNodes, + size_t numSamples); + + size_t dof() const; + size_t numSamples() const; + size_t numChebNodes() const; + gtsam::Matrix worldPoints(const gtsam::Vector &q) const; +}; + +#include +class NNCableFactor : gtsam::NoiseModelFactor { + NNCableFactor(gtsam::Key qKey, const gtdynamics::NNCableSpline *cable, + const gtdynamics::SignedDistanceField *sdf, double costSigma, + double epsilon, double cableRadius); + NNCableFactor(gtsam::Key qKey, const gtdynamics::NNCableSpline *cable, + const gtdynamics::SignedDistanceField *sdf, double costSigma, + double epsilon, const gtsam::Vector &radii); + + double epsilon() const; + gtsam::Vector radii() const; + void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter = + gtdynamics::GTDKeyFormatter); +}; + #include class SelfCollisionPair { SelfCollisionPair(); diff --git a/gtdynamics/dynamics/MLP.cpp b/gtdynamics/dynamics/MLP.cpp new file mode 100644 index 00000000..0b686470 --- /dev/null +++ b/gtdynamics/dynamics/MLP.cpp @@ -0,0 +1,272 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file MLP.cpp + * @brief Linear multi-layer perceptron with analytic Jacobians. + * @author Karthik Shaji + */ + +#include + +#include +#include +#include +#include +#include +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +/// Activation value and derivative at z. +static double activate(MLP::Activation activation, double leakySlope, double z, + double *derivative) { + switch (activation) { + case MLP::Activation::kRelu: + *derivative = z > 0.0 ? 1.0 : 0.0; + return z > 0.0 ? z : 0.0; + case MLP::Activation::kLeakyRelu: + *derivative = z > 0.0 ? 1.0 : leakySlope; + return z > 0.0 ? z : leakySlope * z; + case MLP::Activation::kTanh: + default: { + const double t = std::tanh(z); + *derivative = 1.0 - t * t; + return t; + } + } +} + +/* ************************************************************************* */ +void MLP::validate() const { + if (weights_.empty() || weights_.size() != biases_.size()) { + throw std::invalid_argument( + "MLP: weights and biases must be non-empty and the same length."); + } + for (size_t k = 0; k < weights_.size(); ++k) { + if (biases_[k].size() != weights_[k].rows()) { + throw std::invalid_argument( + "MLP: a bias size does not match its weight's rows."); + } + if (k > 0 && weights_[k].cols() != weights_[k - 1].rows()) { + throw std::invalid_argument( + "MLP: consecutive layer dimensions do not chain."); + } + } +} + +/* ************************************************************************* */ +MLP::MLP(const std::vector &weights, + const std::vector &biases, Activation activation, + double leakySlope) + : weights_(weights), + biases_(biases), + activation_(activation), + leakySlope_(leakySlope) { + validate(); +} + +/* ************************************************************************* */ +/// Parse a whitespace-separated list of doubles into a vector. +static gtsam::Vector parseVector(const std::string &text) { + std::istringstream stream(text); + std::vector values; + double value; + while (stream >> value) values.push_back(value); + return Eigen::Map(values.data(), values.size()); +} + +/// n doubles from stream, or throw naming what was being read. +static gtsam::Vector readValues(std::istream &stream, size_t n, + const std::string &what) { + gtsam::Vector values(n); + for (size_t i = 0; i < n; ++i) { + if (!(stream >> values(i))) { + throw std::runtime_error("MLP: failed to read values of " + what + "."); + } + } + return values; +} + +/* ************************************************************************* */ +MLP::MLP(const std::string &filename) { + std::ifstream file(filename); + if (!file) { + throw std::runtime_error("MLP: cannot open " + filename + "."); + } + + // Header phase: "key value..." lines up to and including "layers K". + size_t inputDim = 0, outputDim = 0, nrLayers = 0; + std::vector hiddenDims; + bool sawActivation = false; + gtsam::Vector inputLower, inputUpper; + std::string line; + while (std::getline(file, line)) { + std::istringstream tokens(line); + std::string key; + if (!(tokens >> key) || key[0] == '#') continue; + std::string rest; + std::getline(tokens, rest); + if (key == "layers") { + std::istringstream(rest) >> nrLayers; + break; + } else if (key == "input_dim") { + std::istringstream(rest) >> inputDim; + } else if (key == "output_dim") { + std::istringstream(rest) >> outputDim; + } else if (key == "hidden_dims") { + std::istringstream dims(rest); + size_t dim; + while (dims >> dim) hiddenDims.push_back(dim); + } else if (key == "activation") { + std::string name; + std::istringstream(rest) >> name; + if (name == "relu") activation_ = Activation::kRelu; + else if (name == "tanh") activation_ = Activation::kTanh; + else if (name == "leaky_relu") activation_ = Activation::kLeakyRelu; + else throw std::runtime_error("MLP: unknown activation " + name + "."); + sawActivation = true; + } else if (key == "leaky_slope") { + std::istringstream(rest) >> leakySlope_; + } else if (key == "input_lower") { + inputLower = parseVector(rest); + } else if (key == "input_upper") { + inputUpper = parseVector(rest); + } else if (key == "output_mean") { + outputMean_ = parseVector(rest); + } else if (key == "output_std") { + outputStd_ = parseVector(rest); + } else { + // Trim both ends so CRLF endings do not pollute the stored value. + const size_t start = rest.find_first_not_of(" \t\r"); + const size_t end = rest.find_last_not_of(" \t\r"); + metadata_[key] = + start == std::string::npos ? "" : rest.substr(start, end - start + 1); + } + } + if (nrLayers == 0) { + throw std::runtime_error("MLP: missing or zero layers header in " + + filename + "."); + } + if (!sawActivation) { + throw std::runtime_error("MLP: missing activation header in " + filename + + "."); + } + + // Inputs mapped to [-1, 1] over the given bounds, as in training. + if (inputLower.size() > 0 || inputUpper.size() > 0) { + if (inputLower.size() != inputUpper.size()) { + throw std::runtime_error( + "MLP: input_lower and input_upper sizes differ."); + } + const gtsam::Vector range = inputUpper - inputLower; + inputScale_ = 2.0 * range.cwiseInverse(); + inputShift_ = -(inputUpper + inputLower).cwiseQuotient(range); + normalizeInput_ = true; + } + if (outputMean_.size() > 0 || outputStd_.size() > 0) { + if (outputMean_.size() != outputStd_.size()) { + throw std::runtime_error("MLP: output_mean and output_std sizes differ."); + } + denormalizeOutput_ = true; + } + + // Layer phase: "layerK_weight OUT IN" then values, "layerK_bias N" then + // values, whitespace-agnostic. + for (size_t k = 0; k < nrLayers; ++k) { + std::string label; + size_t rows, cols; + if (!(file >> label >> rows >> cols) || + label != "layer" + std::to_string(k) + "_weight") { + throw std::runtime_error("MLP: expected layer" + std::to_string(k) + + "_weight block in " + filename + "."); + } + // Values are row-major, matching PyTorch Linear.weight [out, in]. + const gtsam::Vector flat = readValues(file, rows * cols, label); + weights_.push_back(Eigen::Map>( + flat.data(), rows, cols)); + + size_t biasSize; + if (!(file >> label >> biasSize) || + label != "layer" + std::to_string(k) + "_bias") { + throw std::runtime_error("MLP: expected layer" + std::to_string(k) + + "_bias block in " + filename + "."); + } + biases_.push_back(readValues(file, biasSize, label)); + } + + try { + validate(); + } catch (const std::invalid_argument &e) { + throw std::runtime_error(std::string(e.what()) + " (" + filename + ")"); + } + if (inputDim != 0 && inputDim != this->inputDim()) { + throw std::runtime_error("MLP: input_dim header does not match layer 0."); + } + if (outputDim != 0 && outputDim != this->outputDim()) { + throw std::runtime_error( + "MLP: output_dim header does not match the last layer."); + } + for (size_t k = 0; k < hiddenDims.size(); ++k) { + if (k + 1 >= weights_.size() || + hiddenDims[k] != static_cast(weights_[k].rows())) { + throw std::runtime_error( + "MLP: hidden_dims header does not match the layers."); + } + } + if (normalizeInput_ && + static_cast(inputScale_.size()) != this->inputDim()) { + throw std::runtime_error( + "MLP: input bounds must have one entry per input."); + } + if (denormalizeOutput_ && + static_cast(outputStd_.size()) != this->outputDim()) { + throw std::runtime_error( + "MLP: output_mean/std must have one entry per output."); + } +} + +/* ************************************************************************* */ +gtsam::Vector MLP::forward(const gtsam::Vector &x, gtsam::Matrix *H) const { + if (static_cast(x.size()) != inputDim()) { + throw std::invalid_argument("MLP: input has the wrong size."); + } + + gtsam::Vector h = x; + gtsam::Matrix J; + if (H) J = gtsam::Matrix::Identity(inputDim(), inputDim()); + + if (normalizeInput_) { + h = inputScale_.cwiseProduct(h) + inputShift_; + if (H) J = inputScale_.asDiagonal() * J; + } + + const size_t nrLayers = weights_.size(); + for (size_t k = 0; k < nrLayers; ++k) { + h = weights_[k] * h + biases_[k]; + if (H) J = weights_[k] * J; + if (k + 1 < nrLayers) { + gtsam::Vector derivative(h.size()); + for (Eigen::Index i = 0; i < h.size(); ++i) { + h(i) = activate(activation_, leakySlope_, h(i), &derivative(i)); + } + if (H) J = derivative.asDiagonal() * J; + } + } + + if (denormalizeOutput_) { + h = outputStd_.cwiseProduct(h) + outputMean_; + if (H) J = outputStd_.asDiagonal() * J; + } + + if (H) *H = J; + return h; +} + +} // namespace gtdynamics diff --git a/gtdynamics/dynamics/MLP.h b/gtdynamics/dynamics/MLP.h new file mode 100644 index 00000000..d9e67044 --- /dev/null +++ b/gtdynamics/dynamics/MLP.h @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file MLP.h + * @brief Linear multi-layer perceptron with analytic Jacobians. + * @author Karthik Shaji + */ + +#pragma once + +#include +#include + +#include +#include +#include + +namespace gtdynamics { + +/** + * A linear multi-layer perceptron with an arbitrary number of hidden layers, + * loaded from an ASCII weight file or built from explicit weights, evaluated + * with an analytic Jacobian. Immutable after construction. + */ +class GTSAM_EXPORT MLP { + public: + enum class Activation { kRelu, kTanh, kLeakyRelu }; + + private: + std::vector weights_; ///< one [out x in] matrix per layer + std::vector biases_; ///< one [out] vector per layer + Activation activation_; + double leakySlope_ = 0.01; + + /// Optional affine normalization, applied iff the file provided it. + bool normalizeInput_ = false, denormalizeOutput_ = false; + gtsam::Vector inputScale_, inputShift_; ///< x' = scale .* x + shift + gtsam::Vector outputStd_, outputMean_; ///< y = std .* y' + mean + + std::map metadata_; ///< unrecognized header lines + + /// Reject empty or dimension-inconsistent layers. + void validate() const; + + public: + /** + * Load a network from an ASCII weight file: '#' comments, "key value..." + * header lines, then per-layer "layerK_weight OUT IN" / "layerK_bias N" + * blocks of row-major values. + * @param filename path of the weight file + * @throw std::runtime_error on a missing file or malformed or inconsistent + * header or layer blocks + */ + explicit MLP(const std::string &filename); + + /** + * Construct from explicit weights. + * @param weights weight matrix of each layer, [out x in] + * @param biases bias vector of each layer + * @param activation activation applied after every layer but the last + * @param leakySlope negative-side slope for kLeakyRelu + * @throw std::invalid_argument on empty or dimension-inconsistent layers + */ + MLP(const std::vector &weights, + const std::vector &biases, Activation activation, + double leakySlope = 0.01); + + /// Return the input dimension. + size_t inputDim() const { return weights_.front().cols(); } + + /// Return the output dimension. + size_t outputDim() const { return weights_.back().rows(); } + + /// Return the number of layers. + size_t nrLayers() const { return weights_.size(); } + + /// Return the activation applied after every layer but the last. + Activation activation() const { return activation_; } + + /// Return unrecognized header lines of the weight file, key to remainder. + const std::map &metadata() const { + return metadata_; + } + + /** + * Evaluate the network. + * @param x input vector, size inputDim + * @param H if non-null, filled with the outputDim x inputDim Jacobian + * @return the output vector + */ + gtsam::Vector forward(const gtsam::Vector &x, + gtsam::Matrix *H = nullptr) const; +}; // \class MLP + +} // namespace gtdynamics diff --git a/gtdynamics/dynamics/tests/testMLP.cpp b/gtdynamics/dynamics/tests/testMLP.cpp new file mode 100644 index 00000000..7d1a2d41 --- /dev/null +++ b/gtdynamics/dynamics/tests/testMLP.cpp @@ -0,0 +1,241 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testMLP.cpp + * @brief test the linear MLP forward pass, Jacobian, and file loader. + * @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::Vector; +using gtsam::Vector2; + +// A fixed 2 -> 2 -> 1 network, small enough to check by hand. +static std::vector smallWeights() { + Matrix W0(2, 2), W1(1, 2); + W0 << 1.0, 2.0, -1.0, 0.5; + W1 << 1.0, -1.0; + return {W0, W1}; +} +static std::vector smallBiases() { + return {Vector2(0.1, -0.2), Vector::Constant(1, 0.05)}; +} + +/* ************************* forward pass ******************************** */ + +// Hand arithmetic for each activation, at an input where both hidden +// pre-activations are negative so relu and leaky relu differ. +TEST(MLP, forwardKnownValues) { + const Vector x = Vector2(1.0, -1.0); // z0 = (-0.9, -1.7), both negative + + MLP relu(smallWeights(), smallBiases(), MLP::Activation::kRelu); + EXPECT_DOUBLES_EQUAL(0.05, relu.forward(x)(0), 1e-12); + + MLP leaky(smallWeights(), smallBiases(), MLP::Activation::kLeakyRelu, 0.01); + EXPECT_DOUBLES_EQUAL(-0.009 - (-0.017) + 0.05, leaky.forward(x)(0), 1e-12); + + MLP tanhNet(smallWeights(), smallBiases(), MLP::Activation::kTanh); + EXPECT_DOUBLES_EQUAL(std::tanh(-0.9) - std::tanh(-1.7) + 0.05, + tanhNet.forward(x)(0), 1e-12); + + EXPECT_LONGS_EQUAL(2, relu.inputDim()); + EXPECT_LONGS_EQUAL(1, relu.outputDim()); + EXPECT_LONGS_EQUAL(2, relu.nrLayers()); +} + +/* ************************* Jacobian ************************************ */ + +// The analytic Jacobian must match the numerical one; relu is checked away +// from its kinks, tanh at several points. +TEST(MLP, jacobianAgainstNumerical) { + MLP relu(smallWeights(), smallBiases(), MLP::Activation::kRelu); + MLP tanhNet(smallWeights(), smallBiases(), MLP::Activation::kTanh); + + auto check = [](const MLP &net, const Vector &x) { + Matrix H; + net.forward(x, &H); + std::function f = [&](const Vector &v) { + return net.forward(v); + }; + EXPECT(assert_equal(Matrix(gtsam::numericalDerivative11( + f, x)), + H, 1e-6)); + }; + + check(relu, Vector2(1.0, 1.0)); // z0 = (3.1, -0.7), away from kinks + check(relu, Vector2(-2.0, 0.3)); // mixed signs, still away from kinks + check(tanhNet, Vector2(0.4, -0.7)); + check(tanhNet, Vector2(-1.2, 2.0)); +} + +/* ************************* file loader ********************************* */ + +static std::string tempFile(const std::string &name) { + return (std::filesystem::temp_directory_path() / name).string(); +} + +// A 3-layer weight file with comments and unknown keys, as the cable model +// files have. +static std::string threeLayerFileText() { + return + "# synthetic test model, row major\n" + "input_dim 2\n" + "hidden_dims 3 2\n" + "activation leaky_relu\n" + "leaky_slope 0.02\n" + "cheb_nodes 16\n" + "endpoint_links link_3 link_6\n" + "layers 3\n" + "layer0_weight 3 2\n" + "0.5\n-0.3\n1.0\n0.2\n-0.7\n0.4\n" + "layer0_bias 3\n" + "0.1\n-0.1\n0.05\n" + "layer1_weight 2 3\n" + "1.0\n0.5\n-0.5\n-1.0\n0.25\n0.75\n" + "layer1_bias 2\n" + "0.0\n0.2\n" + "layer2_weight 1 2\n" + "2.0\n-1.5\n" + "layer2_bias 1\n" + "-0.05\n"; +} + +// The same network built with the explicit constructor. +static MLP threeLayerTwin() { + Matrix W0(3, 2), W1(2, 3), W2(1, 2); + W0 << 0.5, -0.3, 1.0, 0.2, -0.7, 0.4; + W1 << 1.0, 0.5, -0.5, -1.0, 0.25, 0.75; + W2 << 2.0, -1.5; + Vector b0(3), b1(2), b2(1); + b0 << 0.1, -0.1, 0.05; + b1 << 0.0, 0.2; + b2 << -0.05; + return MLP({W0, W1, W2}, {b0, b1, b2}, MLP::Activation::kLeakyRelu, 0.02); +} + +// Loading must reproduce the explicit twin, keep unknown keys as metadata, +// and reject a truncated file. +TEST(MLP, loadRoundTrip) { + const std::string path = tempFile("gtd_testMLP_weights.txt"); + std::ofstream(path) << threeLayerFileText(); + + MLP loaded(path); + const MLP twin = threeLayerTwin(); + EXPECT_LONGS_EQUAL(2, loaded.inputDim()); + EXPECT_LONGS_EQUAL(1, loaded.outputDim()); + EXPECT_LONGS_EQUAL(3, loaded.nrLayers()); + EXPECT(loaded.activation() == MLP::Activation::kLeakyRelu); + EXPECT(loaded.metadata().at("cheb_nodes") == "16"); + EXPECT(loaded.metadata().at("endpoint_links") == "link_3 link_6"); + + for (const Vector &x : {Vector2(0.3, -0.8), Vector2(-1.1, 0.6)}) { + Matrix Hloaded, Htwin; + EXPECT(assert_equal(twin.forward(x, &Htwin), + loaded.forward(x, &Hloaded), 1e-12)); + EXPECT(assert_equal(Htwin, Hloaded, 1e-12)); + } + + // Truncated values must be rejected, not silently zero-padded. + const std::string bad = tempFile("gtd_testMLP_truncated.txt"); + const std::string text = threeLayerFileText(); + std::ofstream(bad) << text.substr(0, text.size() - 20); + CHECK_EXCEPTION(MLP{bad}, std::runtime_error); + + std::filesystem::remove(path); + std::filesystem::remove(bad); +} + +// Normalization headers must map inputs to [-1, 1] over the bounds and +// de-standardize the outputs, in value and in Jacobian. +TEST(MLP, optionalNormalization) { + const std::string path = tempFile("gtd_testMLP_normalized.txt"); + std::string text = threeLayerFileText(); + text.insert(text.find("layers 3"), + "input_lower -1.0 0.0\n" + "input_upper 3.0 2.0\n" + "output_mean 1.5\n" + "output_std 0.5\n"); + std::ofstream(path) << text; + + MLP loaded(path); + const MLP twin = threeLayerTwin(); + const Vector x = Vector2(0.5, 1.7); + // x' maps (-1,3) -> (-1,1) and (0,2) -> (-1,1). + const Vector xNorm = Vector2((0.5 - 1.0) / 2.0, (1.7 - 1.0) / 1.0); + EXPECT(assert_equal(Vector(0.5 * twin.forward(xNorm).array() + 1.5), + loaded.forward(x), 1e-12)); + + Matrix H; + loaded.forward(x, &H); + std::function f = [&](const Vector &v) { + return loaded.forward(v); + }; + EXPECT(assert_equal( + Matrix(gtsam::numericalDerivative11(f, x)), H, 1e-6)); + + std::filesystem::remove(path); +} + +/* ************************* validation ********************************** */ + +// Dimension-inconsistent layer stacks are rejected at construction. +TEST(MLP, rejectsInconsistentDims) { + auto W = smallWeights(); + auto b = smallBiases(); + + CHECK_EXCEPTION(MLP({}, {}, MLP::Activation::kRelu), std::invalid_argument); + CHECK_EXCEPTION(MLP(W, {b[0]}, MLP::Activation::kRelu), + std::invalid_argument); + + auto badBias = b; + badBias[0] = Vector::Zero(3); // layer 0 has 2 outputs + CHECK_EXCEPTION(MLP(W, badBias, MLP::Activation::kRelu), + std::invalid_argument); + + auto badChain = W; + badChain[1] = Matrix::Zero(1, 3); // layer 0 outputs 2, not 3 + CHECK_EXCEPTION(MLP(badChain, b, MLP::Activation::kRelu), + std::invalid_argument); + + MLP net(W, b, MLP::Activation::kRelu); + CHECK_EXCEPTION(net.forward(Vector::Zero(3)), std::invalid_argument); +} + +/* ************************* real model ********************************** */ + +// Loads the trained cable model when GTD_CABLE_MODEL_FILE points at it, so +// the real-file path is exercised without committing the weights. +TEST(MLP, loadRealModelIfPresent) { + const char *path = std::getenv("GTD_CABLE_MODEL_FILE"); + if (!path) return; + + MLP model{std::string(path)}; + EXPECT_LONGS_EQUAL(5, model.inputDim()); + EXPECT_LONGS_EQUAL(42, model.outputDim()); + const Vector y = model.forward(Vector::Zero(5)); + EXPECT(y.allFinite()); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtdynamics/factors/NNCableFactor.h b/gtdynamics/factors/NNCableFactor.h new file mode 100644 index 00000000..dec1b0c7 --- /dev/null +++ b/gtdynamics/factors/NNCableFactor.h @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file NNCableFactor.h + * @brief Obstacle avoidance cost factor for a neural-network-predicted cable. + * @author Karthik Shaji + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gtdynamics { + +/** + * Unary factor that keeps a neural-network-predicted cable clear of a signed + * distance field, by applying a hinge loss at points sampled along the + * predicted cable curve. The connected variable is q (stacked joint angles), + * as ordered in the NNCableSpline model. + */ +class NNCableFactor : public gtsam::NoiseModelFactorN { + private: + using This = NNCableFactor; + using Base = gtsam::NoiseModelFactorN; + + double epsilon_; + gtsam::Vector radii_; ///< per sample standoff radius, e.g. the cable radius + std::shared_ptr cable_; + std::shared_ptr sdf_; + + public: + /** + * Constructor with a single cable radius for every sample. + * @param qKey key of the stacked joint angle vector + * @param cable cable shape model + * @param sdf signed distance field of the obstacles, in the world frame + * @param costSigma cost function sigma, one per sample + * @param epsilon standoff distance kept from every obstacle + * @param cableRadius radius of the cable, added to epsilon at every sample + */ + NNCableFactor(gtsam::Key qKey, + const std::shared_ptr &cable, + const std::shared_ptr &sdf, + double costSigma, double epsilon, double cableRadius = 0.0) + : NNCableFactor(qKey, cable, sdf, costSigma, epsilon, + gtsam::Vector::Constant( + internal::checkedNumSamples(cable, "NNCableFactor"), + cableRadius)) {} + + /** + * Constructor with a radius per sample, added to the shared epsilon. + * @param qKey key of the stacked joint angle vector + * @param cable cable shape model + * @param sdf signed distance field of the obstacles, in the world frame + * @param costSigma cost function sigma, one per sample + * @param epsilon standoff distance added to every radius + * @param radii standoff radius of each sample, one per sample + */ + NNCableFactor(gtsam::Key qKey, + const std::shared_ptr &cable, + const std::shared_ptr &sdf, + double costSigma, double epsilon, const gtsam::Vector &radii) + : Base(gtsam::noiseModel::Isotropic::Sigma( + internal::checkedNumSamples(cable, "NNCableFactor"), + costSigma), + qKey), + epsilon_(epsilon), + radii_(radii), + cable_(cable), + sdf_(sdf) { + internal::validateNNCableFactorArgs(*cable_, sdf_, epsilon_, radii_, + "NNCableFactor"); + } + + ~NNCableFactor() 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 cable sample, and its Jacobian. + gtsam::Vector evaluateError( + const gtsam::Vector &q, + gtsam::OptionalMatrixType H1 = nullptr) const override { + return nnCableSDFError(q, *cable_, *sdf_, epsilon_, radii_, H1); + } + + /// Return the shared standoff distance. + double epsilon() const { return epsilon_; } + + /// Return the per sample 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 << "NNCableFactor with " << cable_->numSamples() + << " cable samples" << std::endl; + Base::print("", keyFormatter); + } +}; // \class NNCableFactor + +} // namespace gtdynamics diff --git a/gtdynamics/factors/internal/CollisionFactorUtils.h b/gtdynamics/factors/internal/CollisionFactorUtils.h index 6667c453..21039e78 100644 --- a/gtdynamics/factors/internal/CollisionFactorUtils.h +++ b/gtdynamics/factors/internal/CollisionFactorUtils.h @@ -14,6 +14,7 @@ #pragma once +#include #include #include #include @@ -149,6 +150,38 @@ inline std::shared_ptr restrictToReferencedPoints( return std::make_shared(robot, uniqueIndices); } +/// numSamples of a cable model that must not be null, for factor +/// initializer lists. +inline size_t checkedNumSamples( + const std::shared_ptr &cable, + const std::string &factorName) { + if (!cable) { + throw std::invalid_argument(factorName + ": cable must not be null."); + } + return cable->numSamples(); +} + +/// Reject a null field, a negative standoff, or radii that are mis-sized or +/// negative. factorName prefixes the error messages. +inline void validateNNCableFactorArgs( + const NNCableSpline &cable, + const std::shared_ptr &sdf, double epsilon, + const gtsam::Vector &radii, const std::string &factorName) { + if (!sdf) { + throw std::invalid_argument(factorName + ": sdf must not be null."); + } + if (epsilon < 0.0) { + throw std::invalid_argument(factorName + ": epsilon must be >= 0."); + } + if (static_cast(radii.size()) != cable.numSamples()) { + throw std::invalid_argument( + factorName + ": radii must have one entry per cable sample."); + } + if ((radii.array() < 0.0).any()) { + throw std::invalid_argument(factorName + ": radii must be >= 0."); + } +} + /// Reject a null model, inconsistent pairs/radii, or, if sigmas is given, a /// sigma count not matching the pairs. Then return the model restricted to /// just the points pairs references, remapping *pairs and *radii in place. diff --git a/gtdynamics/gpmp2/NNCableSpline.cpp b/gtdynamics/gpmp2/NNCableSpline.cpp new file mode 100644 index 00000000..ccc16645 --- /dev/null +++ b/gtdynamics/gpmp2/NNCableSpline.cpp @@ -0,0 +1,194 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file NNCableSpline.cpp + * @brief Neural-network-predicted cable shape between two robot attachments. + * @author Karthik Shaji + */ + +#include +#include +#include + +#include +#include +#include + +namespace gtdynamics { + +/* ************************************************************************* */ +PointOnLinks NNCableSpline::checkedPoints(const PointOnLink &attachment0, + const PointOnLink &attachment1, + const LinkSharedPtr &referenceLink) { + // Attachment links are checked by the RobotQueryPoints constructor. + if (!referenceLink) { + throw std::invalid_argument( + "NNCableSpline: referenceLink must not be null."); + } + return {attachment0, attachment1, + PointOnLink(referenceLink, gtsam::Point3(0.0, 0.0, 0.0))}; +} + +/* ************************************************************************* */ +NNCableSpline::NNCableSpline( + const Robot &robot, const std::string &baseLinkName, + const std::vector &joints, const PointOnLink &attachment0, + const PointOnLink &attachment1, const LinkSharedPtr &referenceLink, + const std::shared_ptr &mlp, + const std::vector &inputIndices, size_t numChebNodes, + size_t numSamples, const gtsam::Pose3 &wTbase) + : fk_(robot, baseLinkName, joints, + checkedPoints(attachment0, attachment1, referenceLink), wTbase), + mlp_(mlp), + inputIndices_(inputIndices), + numChebNodes_(numChebNodes) { + if (!mlp_) { + throw std::invalid_argument("NNCableSpline: mlp must not be null."); + } + if (numChebNodes_ < 3) { + throw std::invalid_argument("NNCableSpline: numChebNodes must be >= 3."); + } + if (numSamples < 2) { + throw std::invalid_argument("NNCableSpline: numSamples must be >= 2."); + } + if (inputIndices_.size() != mlp_->inputDim()) { + throw std::invalid_argument( + "NNCableSpline: inputIndices must have one entry per network input."); + } + std::set unique; + for (size_t index : inputIndices_) { + if (index >= dof()) { + throw std::invalid_argument( + "NNCableSpline: an input index is out of range of q."); + } + if (!unique.insert(index).second) { + throw std::invalid_argument( + "NNCableSpline: input indices must be distinct."); + } + } + if (mlp_->outputDim() != 3 * (numChebNodes_ - 2)) { + throw std::invalid_argument( + "NNCableSpline: the network output size must be " + "3 * (numChebNodes - 2) interior nodal residuals."); + } + + // The evaluation is linear in the nodal residuals, so these barycentric + // weights are also the exact spline Jacobian; the endpoint columns multiply + // the identically-zero endpoint residuals and are dropped. + sampleParams_ = gtsam::Vector(numSamples); + interiorWeights_ = gtsam::Matrix(numSamples, numChebNodes_ - 2); + for (size_t m = 0; m < numSamples; ++m) { + const double s = static_cast(m) / (numSamples - 1); + sampleParams_(m) = s; + const gtsam::Matrix weights = + gtsam::Chebyshev2::CalculateWeights(numChebNodes_, s, 0.0, 1.0); + interiorWeights_.row(m) = weights.row(0).segment(1, numChebNodes_ - 2); + } +} + +/* ************************************************************************* */ +void NNCableSpline::samplePoints(const gtsam::Vector &q, + std::vector *wPts, + std::vector *ptJacobians) const { + const bool computeJacobians = (ptJacobians != nullptr); + + std::vector wTls; + std::vector poseJacobians; + fk_.queryPoses(q, &wTls, computeJacobians ? &poseJacobians : nullptr); + + // Attachment world points, chained through the link pose Jacobians. + gtsam::Point3 p[2]; + gtsam::Matrix Jp[2]; + for (int i = 0; i < 2; ++i) { + if (computeJacobians) { + gtsam::Matrix36 Hpose; + p[i] = wTls[i].transformFrom(fk_.points()[i].point, Hpose); + Jp[i] = Hpose * poseJacobians[i]; + } else { + p[i] = wTls[i].transformFrom(fk_.points()[i].point); + } + } + const gtsam::Matrix3 R = wTls[2].rotation().matrix(); + + // Network residuals from the selected q entries. + gtsam::Vector x(inputIndices_.size()); + for (size_t j = 0; j < inputIndices_.size(); ++j) x(j) = q(inputIndices_[j]); + gtsam::Matrix Jmlp; + const gtsam::Vector y = mlp_->forward(x, computeJacobians ? &Jmlp : nullptr); + const size_t nrInterior = numChebNodes_ - 2; + // Row-major reshape: interior node j's residual is y.segment(3j, 3). + const Eigen::Map> + interiorResiduals(y.data(), nrInterior, 3); + + gtsam::Matrix JmlpFull, Jw; + if (computeJacobians) { + // Scatter the network Jacobian columns into the full q dimension. + JmlpFull = gtsam::Matrix::Zero(3 * nrInterior, dof()); + for (size_t j = 0; j < inputIndices_.size(); ++j) { + JmlpFull.col(inputIndices_[j]) = Jmlp.col(j); + } + Jw = poseJacobians[2].topRows(3); // body-frame angular Jacobian + } + + wPts->resize(numSamples()); + if (computeJacobians) ptJacobians->resize(numSamples()); + for (size_t m = 0; m < numSamples(); ++m) { + const double s = sampleParams_(m); + const gtsam::Vector3 v = + interiorResiduals.transpose() * interiorWeights_.row(m).transpose(); + (*wPts)[m] = (1.0 - s) * p[0] + s * p[1] + R * v; + if (computeJacobians) { + gtsam::Matrix J = (1.0 - s) * Jp[0] + s * Jp[1]; + // The rotation moves as R * Exp(w), so d(R v)/dq = -R [v]x Jw. + J.noalias() -= R * gtsam::skewSymmetric(v) * Jw; + for (size_t j = 0; j < nrInterior; ++j) { + J.noalias() += + interiorWeights_(m, j) * (R * JmlpFull.middleRows(3 * j, 3)); + } + (*ptJacobians)[m] = J; + } + } +} + +/* ************************************************************************* */ +gtsam::Matrix NNCableSpline::worldPoints(const gtsam::Vector &q) const { + std::vector wPts; + samplePoints(q, &wPts); + gtsam::Matrix pts(3, numSamples()); + for (size_t m = 0; m < numSamples(); ++m) pts.col(m) = wPts[m]; + return pts; +} + +/* ************************************************************************* */ +gtsam::Vector nnCableSDFError(const gtsam::Vector &q, + const NNCableSpline &cable, + const SignedDistanceField &sdf, double epsilon, + const gtsam::Vector &radii, gtsam::Matrix *Hq) { + const size_t nrSamples = cable.numSamples(); + + std::vector wPts; + std::vector ptJacobians; + cable.samplePoints(q, &wPts, Hq ? &ptJacobians : nullptr); + if (Hq) *Hq = gtsam::Matrix::Zero(nrSamples, cable.dof()); + + gtsam::Vector err(nrSamples); + for (size_t m = 0; m < nrSamples; ++m) { + const double eps = epsilon + radii(m); + if (Hq) { + gtsam::Matrix13 Hpt; + err(m) = hingeLossObstacleCost(wPts[m], sdf, eps, Hpt); + Hq->row(m) = Hpt * ptJacobians[m]; + } else { + err(m) = hingeLossObstacleCost(wPts[m], sdf, eps); + } + } + return err; +} + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/NNCableSpline.h b/gtdynamics/gpmp2/NNCableSpline.h new file mode 100644 index 00000000..83f6d1cf --- /dev/null +++ b/gtdynamics/gpmp2/NNCableSpline.h @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file NNCableSpline.h + * @brief Neural-network-predicted cable shape between two robot attachments. + * @author Karthik Shaji + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gtdynamics { + +/** + * Predicts the shape of a cable strung between two attachment points on a + * robot, as the chord between the forward-kinematics endpoints plus + * MLP-predicted interior Chebyshev nodal residuals, and samples world points + * along it with Jacobians with respect to the stacked joint angle vector q. + * The residuals are predicted in the reference link's frame, as trained. + * Immutable after construction. + */ +class GTSAM_EXPORT NNCableSpline { + private: + RobotQueryPoints fk_; ///< the two attachments, then the reference link + std::shared_ptr mlp_; + std::vector inputIndices_; ///< entries of q the network reads + size_t numChebNodes_; ///< N, endpoints included + gtsam::Vector sampleParams_; ///< s_m, uniform on [0, 1] + gtsam::Matrix interiorWeights_; ///< numSamples x (N-2) Chebyshev weights + + /// The FK query points, with the reference link checked against null. + static PointOnLinks checkedPoints(const PointOnLink &attachment0, + const PointOnLink &attachment1, + const LinkSharedPtr &referenceLink); + + public: + /** + * Constructor. + * @param robot the robot model + * @param baseLinkName the link the kinematic tree is rooted at + * @param joints the joints spanned by q, in the order q indexes them + * @param attachment0 first cable attachment, in its link's CoM frame + * @param attachment1 second cable attachment, in its link's CoM frame + * @param referenceLink link whose world rotation maps the network's + * residuals into the world frame + * @param mlp network mapping the selected q entries to the interior + * Chebyshev nodal residuals, row-major (N-2) x 3 + * @param inputIndices entries of q fed to the network, in network order + * @param numChebNodes number N of Chebyshev-Lobatto nodes on [0, 1] + * @param numSamples number of points sampled uniformly along the cable + * @param wTbase pose of the base link in the world frame + * @throw std::invalid_argument on a null mlp or reference link, on + * inputIndices out of range, duplicated, or not matching the + * network input size, on a network output size other than + * 3 * (numChebNodes - 2), on numChebNodes < 3 or numSamples < 2, + * or on FK inputs the RobotQueryPoints constructor rejects + */ + NNCableSpline(const Robot &robot, const std::string &baseLinkName, + const std::vector &joints, + const PointOnLink &attachment0, const PointOnLink &attachment1, + const LinkSharedPtr &referenceLink, + const std::shared_ptr &mlp, + const std::vector &inputIndices, size_t numChebNodes, + size_t numSamples, const gtsam::Pose3 &wTbase = gtsam::Pose3()); + + /// Return the number of joints spanned by q. + size_t dof() const { return fk_.dof(); } + + /// Return the number of samples along the cable. + size_t numSamples() const { return sampleParams_.size(); } + + /// Return the number N of Chebyshev-Lobatto nodes, endpoints included. + size_t numChebNodes() const { return numChebNodes_; } + + /// Return the entries of q fed to the network. + const std::vector &inputIndices() const { return inputIndices_; } + + /** + * World positions of the cable samples. + * @param q stacked joint angles + * @param wPts filled with the world position of each sample + * @param ptJacobians if non-null, filled with a 3 x dof matrix per sample + */ + void samplePoints(const gtsam::Vector &q, std::vector *wPts, + std::vector *ptJacobians = nullptr) const; + + /// World positions of the cable samples, one per column (3 x numSamples). + gtsam::Matrix worldPoints(const gtsam::Vector &q) const; +}; // \class NNCableSpline + +/// Hinge loss of every cable sample at configuration q, using +/// epsilon + radii(m) as each sample's standoff. If Hq is non-null, it is +/// filled with the numSamples x dof Jacobian with respect to q. +GTSAM_EXPORT gtsam::Vector nnCableSDFError(const gtsam::Vector &q, + const NNCableSpline &cable, + const SignedDistanceField &sdf, + double epsilon, + const gtsam::Vector &radii, + gtsam::Matrix *Hq = nullptr); + +} // namespace gtdynamics diff --git a/gtdynamics/gpmp2/RobotQueryPoints.cpp b/gtdynamics/gpmp2/RobotQueryPoints.cpp index 97b3f394..79b0e30d 100644 --- a/gtdynamics/gpmp2/RobotQueryPoints.cpp +++ b/gtdynamics/gpmp2/RobotQueryPoints.cpp @@ -216,4 +216,23 @@ gtsam::Matrix RobotQueryPoints::worldPoints(const gtsam::Vector &q) const { return pts; } +/* ************************************************************************* */ +void RobotQueryPoints::queryPoses( + const gtsam::Vector &q, std::vector *wTls, + std::vector *poseJacobians) const { + std::vector poses(nrLinks_); + gtsam::Matrix linkJacobians; + if (poseJacobians) linkJacobians = gtsam::Matrix::Zero(6 * nrLinks_, dof()); + computeForwardKinematics(q, &poses, poseJacobians ? &linkJacobians : nullptr); + + wTls->resize(nrPoints()); + if (poseJacobians) poseJacobians->resize(nrPoints()); + for (size_t i = 0; i < nrPoints(); ++i) { + (*wTls)[i] = poses[pointSlots_[i]]; + if (poseJacobians) { + (*poseJacobians)[i] = linkJacobians.middleRows(6 * pointSlots_[i], 6); + } + } +} + } // namespace gtdynamics diff --git a/gtdynamics/gpmp2/RobotQueryPoints.h b/gtdynamics/gpmp2/RobotQueryPoints.h index f877fd02..087cc6a4 100644 --- a/gtdynamics/gpmp2/RobotQueryPoints.h +++ b/gtdynamics/gpmp2/RobotQueryPoints.h @@ -108,6 +108,17 @@ class GTSAM_EXPORT RobotQueryPoints { * @returns a 3 x nrPoints matrix of world positions */ gtsam::Matrix worldPoints(const gtsam::Vector &q) const; + + /** + * World poses of the query points' links, with Jacobians. + * @param q stacked joint angles + * @param wTls filled with the world pose of each query point's link + * @param poseJacobians if non-null, filled with a 6 x dof matrix per point, + * in the gtsam Pose3 tangent convention (body-frame twist, rows 0-2 + * rotation, rows 3-5 translation) + */ + void queryPoses(const gtsam::Vector &q, std::vector *wTls, + std::vector *poseJacobians = nullptr) const; }; // \class RobotQueryPoints } // namespace gtdynamics diff --git a/gtdynamics/gpmp2/tests/testNNCableFactor.cpp b/gtdynamics/gpmp2/tests/testNNCableFactor.cpp new file mode 100644 index 00000000..9c56987f --- /dev/null +++ b/gtdynamics/gpmp2/tests/testNNCableFactor.cpp @@ -0,0 +1,310 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testNNCableFactor.cpp + * @brief test the NN cable spline and its obstacle 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 "barLabFixtures.h" +#include "makeSphereSDF.h" + +using namespace gtdynamics; +using gtsam::assert_equal; +using gtsam::Matrix; +using gtsam::Point3; +using gtsam::Pose3; +using gtsam::Values; +using gtsam::Vector; +using gtsam::Vector3; +using gtsam::symbol_shorthand::X; + +static const Robot &kRobot = barLabRobot(); + +// The network reads robot1's five distal arm joints, as the trained cable +// model does; q is the 9-dof robot1 configuration (gantry then joints 1-6). +static const std::vector kInputIndices = {4, 5, 6, 7, 8}; + +static PointOnLink attachment0() { + return PointOnLink(kRobot.link("robot1_link_3"), Point3(0.1, 0.0, 0.05)); +} +static PointOnLink attachment1() { + return PointOnLink(kRobot.link("robot1_link_6"), Point3(0.0, 0.0, 0.1)); +} + +// A 5 -> 4 -> 3*(N-2) network with all-zero weights and the given last bias. +static std::shared_ptr biasOnlyMLP(size_t numChebNodes, + const Vector &lastBias) { + const size_t nOut = 3 * (numChebNodes - 2); + return std::make_shared( + std::vector{Matrix::Zero(4, 5), Matrix::Zero(nOut, 4)}, + std::vector{Vector::Zero(4), lastBias}, MLP::Activation::kRelu); +} + +// A smooth deterministic 5 -> 8 -> 3*(N-2) tanh network with small outputs, +// so numerical differentiation through it is clean. +static std::shared_ptr smoothMLP(size_t numChebNodes) { + const size_t nOut = 3 * (numChebNodes - 2); + Matrix W0(8, 5), W1(nOut, 8); + Vector b0(8), b1(nOut); + for (Eigen::Index i = 0; i < W0.rows(); ++i) + for (Eigen::Index j = 0; j < W0.cols(); ++j) + W0(i, j) = 0.4 * std::sin(1.3 * i + 2.1 * j + 0.5); + for (Eigen::Index i = 0; i < W1.rows(); ++i) + for (Eigen::Index j = 0; j < W1.cols(); ++j) + W1(i, j) = 0.05 * std::sin(0.7 * i + 1.9 * j + 1.1); + for (Eigen::Index i = 0; i < 8; ++i) b0(i) = 0.3 * std::sin(2.3 * i); + for (Eigen::Index i = 0; i < b1.size(); ++i) b1(i) = 0.02 * std::sin(1.7 * i); + return std::make_shared(std::vector{W0, W1}, + std::vector{b0, b1}, + MLP::Activation::kTanh); +} + +static std::shared_ptr makeCable( + const std::shared_ptr &mlp, size_t numChebNodes, + size_t numSamples) { + return std::make_shared( + kRobot, "columns", robot1Joints(), attachment0(), attachment1(), + kRobot.link("robot1_link_1"), mlp, kInputIndices, numChebNodes, + numSamples); +} + +/* ************************ spline reconstruction ************************ */ + +// With zero residuals every sample lies on the chord, whose endpoints are +// exactly the FK attachment points. +TEST(NNCableSpline, chordWhenResidualZero) { + const size_t N = 6, M = 5; + const auto cable = makeCable(biasOnlyMLP(N, Vector::Zero(3 * (N - 2))), N, M); + const Vector q = startConfig(); + + // Independent endpoints from a plain query point model. + const auto endpointModel = std::make_shared( + kRobot, "columns", robot1Joints(), + PointOnLinks{attachment0(), attachment1()}); + std::vector ends; + endpointModel->queryPoints(q, &ends); + + std::vector wPts; + cable->samplePoints(q, &wPts); + EXPECT_LONGS_EQUAL(M, wPts.size()); + for (size_t m = 0; m < M; ++m) { + const double s = static_cast(m) / (M - 1); + EXPECT(assert_equal(Point3((1.0 - s) * ends[0] + s * ends[1]), wPts[m], + 1e-9)); + } +} + +// A constant nodal residual must be interpolated with the Chebyshev weights +// and rotated by the reference link's world rotation. Catches both the +// row-major reshape and the residual frame. +TEST(NNCableSpline, constantResidualInReferenceFrame) { + const size_t N = 6, M = 7; + Vector bias(3 * (N - 2)); + for (Eigen::Index i = 0; i < bias.size(); ++i) + bias(i) = 0.05 * std::sin(1.1 * i + 0.3); + const auto cable = makeCable(biasOnlyMLP(N, bias), N, M); + + Vector q = startConfig(); + q(3) = 0.7; // rotate joint 1, so the reference frame is not trivial + + // Expected pieces, each from independent public machinery. + const auto endpointModel = std::make_shared( + kRobot, "columns", robot1Joints(), + PointOnLinks{attachment0(), attachment1(), + PointOnLink(kRobot.link("robot1_link_1"), + Point3(0.0, 0.0, 0.0))}); + std::vector ends; + endpointModel->queryPoints(q, &ends); + std::vector wTls; + endpointModel->queryPoses(q, &wTls); + const Matrix R = wTls[2].rotation().matrix(); + + std::vector wPts; + cable->samplePoints(q, &wPts); + for (size_t m = 0; m < M; ++m) { + const double s = static_cast(m) / (M - 1); + const Matrix weights = gtsam::Chebyshev2::CalculateWeights(N, s, 0.0, 1.0); + Vector3 v = Vector3::Zero(); + for (size_t j = 0; j < N - 2; ++j) { + v += weights(0, j + 1) * Vector3(bias.segment(3 * j, 3)); + } + const Point3 expected = + (1.0 - s) * ends[0] + s * ends[1] + Point3(R * v); + EXPECT(assert_equal(expected, wPts[m], 1e-9)); + } +} + +// The analytic sample Jacobians must match the numerical ones, at a config +// where the residual and the reference rotation are both non-trivial. +TEST(NNCableSpline, sampleJacobiansAgainstNumerical) { + const size_t N = 8, M = 5; + const auto cable = makeCable(smoothMLP(N), N, M); + + Vector q = startConfig(); + q(3) = 0.6; + q(5) = -0.8; + + std::vector wPts; + std::vector ptJacobians; + cable->samplePoints(q, &wPts, &ptJacobians); + + for (size_t m = 0; m < M; ++m) { + std::function f = [&](const Vector &v) { + std::vector pts; + cable->samplePoints(v, &pts); + return pts[m]; + }; + EXPECT(assert_equal( + Matrix(gtsam::numericalDerivative11(f, q)), + ptJacobians[m], 1e-5)); + } +} + +/* ************************ factor *************************************** */ + +// A sphere in the cable's path: the factor error and Jacobians must match +// the numerical ones on the active branch. +TEST(NNCableFactor, jacobianWhenActive) { + const size_t N = 8, M = 9; + const auto cable = makeCable(smoothMLP(N), N, M); + const Vector q = startConfig(); + + // Sphere centred just off the middle cable sample, grid offset by half a + // cell so samples avoid the trilinear gradient's node discontinuities. + const Matrix pts = cable->worldPoints(q); + const Point3 center = Point3(pts.col(M / 2)) + Point3(0.3 * kCell, + 0.2 * kCell, 0.0); + const Point3 origin = center - Point3::Constant(1.0 - kHalfCell); + auto sdf = std::make_shared( + makeSphereSDF(center, kRadius, origin, kCell, 41, 41, 41)); + + NNCableFactor factor(X(0), cable, sdf, 0.01, kEpsilon, 0.02); + const Vector err = factor.evaluateError(q); + EXPECT(err(M / 2) > 0.0); // active branch + + Values values; + values.insert(X(0), q); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +// Only samples within the sphere's inflated band are active; with the +// obstacle far from the cable the whole error and Jacobian vanish. +TEST(NNCableFactor, activeAndInactiveSamples) { + const size_t N = 8, M = 9; + const auto cable = makeCable(smoothMLP(N), N, M); + const Vector q = startConfig(); + const Matrix pts = cable->worldPoints(q); + + const Point3 center = Point3(pts.col(M / 2)) + Point3(0.3 * kCell, + 0.2 * kCell, 0.0); + const Point3 origin = center - Point3::Constant(1.0 - kHalfCell); + auto sdf = std::make_shared( + makeSphereSDF(center, kRadius, origin, kCell, 41, 41, 41)); + NNCableFactor factor(X(0), cable, sdf, 0.01, kEpsilon, 0.02); + const Vector err = factor.evaluateError(q); + EXPECT(err(M / 2) > 0.0); + EXPECT_DOUBLES_EQUAL(0.0, err(0), 1e-9); + EXPECT_DOUBLES_EQUAL(0.0, err(M - 1), 1e-9); + + // A grid that covers the cable with the sphere far away inside it: every + // sample is in free space, so the error and Jacobian are identically zero. + Vector3 lo = pts.rowwise().minCoeff(), hi = pts.rowwise().maxCoeff(); + const double pad = 2.0, cell = 0.1; + const Point3 farOrigin(lo.x() - pad, lo.y() - pad, lo.z() - pad); + const Vector3 extent = (hi - lo) + Vector3::Constant(2.0 * pad); + const Point3 farCenter = Point3(hi) + Point3(1.2, 1.2, 1.2); + auto farSdf = std::make_shared(makeSphereSDF( + farCenter, 0.1, farOrigin, cell, + static_cast(std::ceil(extent.x() / cell)) + 1, + static_cast(std::ceil(extent.y() / cell)) + 1, + static_cast(std::ceil(extent.z() / cell)) + 1)); + NNCableFactor farFactor(X(0), cable, farSdf, 0.01, kEpsilon, 0.02); + Matrix H; + const Vector farErr = farFactor.evaluateError(q, &H); + EXPECT_DOUBLES_EQUAL(0.0, farErr.norm(), 1e-9); + EXPECT_DOUBLES_EQUAL(0.0, H.norm(), 1e-9); +} + +/* ************************ validation *********************************** */ + +// Inconsistent construction inputs are rejected with clear errors. +TEST(NNCableSpline, rejectsBadInputs) { + const size_t N = 6, M = 5; + const auto mlp = biasOnlyMLP(N, Vector::Zero(3 * (N - 2))); + + CHECK_EXCEPTION(makeCable(nullptr, N, M), std::invalid_argument); + CHECK_EXCEPTION( + NNCableSpline(kRobot, "columns", robot1Joints(), attachment0(), + attachment1(), LinkSharedPtr(), mlp, kInputIndices, N, M), + std::invalid_argument); + CHECK_EXCEPTION( + NNCableSpline(kRobot, "columns", robot1Joints(), attachment0(), + attachment1(), kRobot.link("robot1_link_1"), mlp, + {4, 5, 6, 7, 9}, N, M), // 9 is out of range of q + std::invalid_argument); + CHECK_EXCEPTION( + NNCableSpline(kRobot, "columns", robot1Joints(), attachment0(), + attachment1(), kRobot.link("robot1_link_1"), mlp, + {4, 5, 6, 7, 7}, N, M), // duplicated index + std::invalid_argument); + CHECK_EXCEPTION( + NNCableSpline(kRobot, "columns", robot1Joints(), attachment0(), + attachment1(), kRobot.link("robot1_link_1"), mlp, + {4, 5, 6, 7}, N, M), // one index too few + std::invalid_argument); + CHECK_EXCEPTION(makeCable(mlp, 8, M), std::invalid_argument); // 3(N-2) off + CHECK_EXCEPTION(makeCable(mlp, N, 1), std::invalid_argument); + CHECK_EXCEPTION(makeCable(biasOnlyMLP(3, Vector::Zero(3)), 2, M), + std::invalid_argument); +} + +// Inconsistent factor arguments are rejected with clear errors. +TEST(NNCableFactor, rejectsBadArgs) { + const size_t N = 6, M = 5; + const auto cable = makeCable(biasOnlyMLP(N, Vector::Zero(3 * (N - 2))), N, M); + auto sdf = std::make_shared( + makeSphereSDF(Point3(5, 5, 5), 0.1, Point3(4, 4, 4), kCell, 5, 5, 5)); + + CHECK_EXCEPTION(NNCableFactor(X(0), nullptr, sdf, 0.01, kEpsilon), + std::invalid_argument); + CHECK_EXCEPTION(NNCableFactor(X(0), cable, nullptr, 0.01, kEpsilon), + std::invalid_argument); + CHECK_EXCEPTION(NNCableFactor(X(0), cable, sdf, 0.01, -0.1), + std::invalid_argument); + CHECK_EXCEPTION(NNCableFactor(X(0), cable, sdf, 0.01, kEpsilon, + Vector::Zero(M - 1)), + std::invalid_argument); + CHECK_EXCEPTION(NNCableFactor(X(0), cable, sdf, 0.01, kEpsilon, + Vector::Constant(M, -0.01)), + 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 index 127be902..aa9d8e83 100644 --- a/gtdynamics/gpmp2/tests/testObstacleFactors.cpp +++ b/gtdynamics/gpmp2/tests/testObstacleFactors.cpp @@ -288,6 +288,35 @@ TEST(RobotQueryPoints, rejectsBadKinematicInputs) { std::invalid_argument); } +// queryPoses must agree with queryPoints on the transformed points, and its +// 6 x dof Jacobians must match the numerical ones. +TEST(RobotQueryPoints, queryPosesAgainstNumerical) { + const auto model = std::make_shared( + kRobot, "columns", robot1Joints(), wristPoints()); + const Vector q = startConfig(); + + std::vector wTls; + std::vector poseJacobians; + model->queryPoses(q, &wTls, &poseJacobians); + + std::vector wPts; + model->queryPoints(q, &wPts); + for (size_t i = 0; i < model->nrPoints(); ++i) { + EXPECT(assert_equal( + wPts[i], Point3(wTls[i].transformFrom(model->points()[i].point)), + 1e-9)); + + std::function f = [&](const Vector &v) { + std::vector poses; + model->queryPoses(v, &poses); + return poses[i]; + }; + EXPECT(assert_equal( + Matrix(gtsam::numericalDerivative11(f, q)), + poseJacobians[i], 1e-5)); + } +} + // 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) { From 292d80b6b4202b7ee3431c92826428cd85e16861 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Sat, 15 Aug 2026 10:41:55 -0700 Subject: [PATCH 02/11] cleaning out of information, removal of normalized pipeelines --- gtdynamics/dynamics/MLP.cpp | 56 --------- gtdynamics/dynamics/MLP.h | 5 - gtdynamics/dynamics/tests/testMLP.cpp | 23 +--- python/tests/test_nn_cable.py | 168 ++++++++++++++++++++++++++ 4 files changed, 174 insertions(+), 78 deletions(-) create mode 100644 python/tests/test_nn_cable.py diff --git a/gtdynamics/dynamics/MLP.cpp b/gtdynamics/dynamics/MLP.cpp index 0b686470..1d699507 100644 --- a/gtdynamics/dynamics/MLP.cpp +++ b/gtdynamics/dynamics/MLP.cpp @@ -72,15 +72,6 @@ MLP::MLP(const std::vector &weights, } /* ************************************************************************* */ -/// Parse a whitespace-separated list of doubles into a vector. -static gtsam::Vector parseVector(const std::string &text) { - std::istringstream stream(text); - std::vector values; - double value; - while (stream >> value) values.push_back(value); - return Eigen::Map(values.data(), values.size()); -} - /// n doubles from stream, or throw naming what was being read. static gtsam::Vector readValues(std::istream &stream, size_t n, const std::string &what) { @@ -104,7 +95,6 @@ MLP::MLP(const std::string &filename) { size_t inputDim = 0, outputDim = 0, nrLayers = 0; std::vector hiddenDims; bool sawActivation = false; - gtsam::Vector inputLower, inputUpper; std::string line; while (std::getline(file, line)) { std::istringstream tokens(line); @@ -133,14 +123,6 @@ MLP::MLP(const std::string &filename) { sawActivation = true; } else if (key == "leaky_slope") { std::istringstream(rest) >> leakySlope_; - } else if (key == "input_lower") { - inputLower = parseVector(rest); - } else if (key == "input_upper") { - inputUpper = parseVector(rest); - } else if (key == "output_mean") { - outputMean_ = parseVector(rest); - } else if (key == "output_std") { - outputStd_ = parseVector(rest); } else { // Trim both ends so CRLF endings do not pollute the stored value. const size_t start = rest.find_first_not_of(" \t\r"); @@ -158,24 +140,6 @@ MLP::MLP(const std::string &filename) { "."); } - // Inputs mapped to [-1, 1] over the given bounds, as in training. - if (inputLower.size() > 0 || inputUpper.size() > 0) { - if (inputLower.size() != inputUpper.size()) { - throw std::runtime_error( - "MLP: input_lower and input_upper sizes differ."); - } - const gtsam::Vector range = inputUpper - inputLower; - inputScale_ = 2.0 * range.cwiseInverse(); - inputShift_ = -(inputUpper + inputLower).cwiseQuotient(range); - normalizeInput_ = true; - } - if (outputMean_.size() > 0 || outputStd_.size() > 0) { - if (outputMean_.size() != outputStd_.size()) { - throw std::runtime_error("MLP: output_mean and output_std sizes differ."); - } - denormalizeOutput_ = true; - } - // Layer phase: "layerK_weight OUT IN" then values, "layerK_bias N" then // values, whitespace-agnostic. for (size_t k = 0; k < nrLayers; ++k) { @@ -220,16 +184,6 @@ MLP::MLP(const std::string &filename) { "MLP: hidden_dims header does not match the layers."); } } - if (normalizeInput_ && - static_cast(inputScale_.size()) != this->inputDim()) { - throw std::runtime_error( - "MLP: input bounds must have one entry per input."); - } - if (denormalizeOutput_ && - static_cast(outputStd_.size()) != this->outputDim()) { - throw std::runtime_error( - "MLP: output_mean/std must have one entry per output."); - } } /* ************************************************************************* */ @@ -242,11 +196,6 @@ gtsam::Vector MLP::forward(const gtsam::Vector &x, gtsam::Matrix *H) const { gtsam::Matrix J; if (H) J = gtsam::Matrix::Identity(inputDim(), inputDim()); - if (normalizeInput_) { - h = inputScale_.cwiseProduct(h) + inputShift_; - if (H) J = inputScale_.asDiagonal() * J; - } - const size_t nrLayers = weights_.size(); for (size_t k = 0; k < nrLayers; ++k) { h = weights_[k] * h + biases_[k]; @@ -260,11 +209,6 @@ gtsam::Vector MLP::forward(const gtsam::Vector &x, gtsam::Matrix *H) const { } } - if (denormalizeOutput_) { - h = outputStd_.cwiseProduct(h) + outputMean_; - if (H) J = outputStd_.asDiagonal() * J; - } - if (H) *H = J; return h; } diff --git a/gtdynamics/dynamics/MLP.h b/gtdynamics/dynamics/MLP.h index d9e67044..73fc1cd1 100644 --- a/gtdynamics/dynamics/MLP.h +++ b/gtdynamics/dynamics/MLP.h @@ -37,11 +37,6 @@ class GTSAM_EXPORT MLP { Activation activation_; double leakySlope_ = 0.01; - /// Optional affine normalization, applied iff the file provided it. - bool normalizeInput_ = false, denormalizeOutput_ = false; - gtsam::Vector inputScale_, inputShift_; ///< x' = scale .* x + shift - gtsam::Vector outputStd_, outputMean_; ///< y = std .* y' + mean - std::map metadata_; ///< unrecognized header lines /// Reject empty or dimension-inconsistent layers. diff --git a/gtdynamics/dynamics/tests/testMLP.cpp b/gtdynamics/dynamics/tests/testMLP.cpp index 7d1a2d41..8bc42c46 100644 --- a/gtdynamics/dynamics/tests/testMLP.cpp +++ b/gtdynamics/dynamics/tests/testMLP.cpp @@ -164,10 +164,10 @@ TEST(MLP, loadRoundTrip) { std::filesystem::remove(bad); } -// Normalization headers must map inputs to [-1, 1] over the bounds and -// de-standardize the outputs, in value and in Jacobian. -TEST(MLP, optionalNormalization) { - const std::string path = tempFile("gtd_testMLP_normalized.txt"); +// Normalization headers from older model files are tolerated as metadata +// and never applied: the network runs on raw values. +TEST(MLP, normalizationHeadersIgnored) { + const std::string path = tempFile("gtd_testMLP_rawvalues.txt"); std::string text = threeLayerFileText(); text.insert(text.find("layers 3"), "input_lower -1.0 0.0\n" @@ -177,20 +177,9 @@ TEST(MLP, optionalNormalization) { std::ofstream(path) << text; MLP loaded(path); - const MLP twin = threeLayerTwin(); const Vector x = Vector2(0.5, 1.7); - // x' maps (-1,3) -> (-1,1) and (0,2) -> (-1,1). - const Vector xNorm = Vector2((0.5 - 1.0) / 2.0, (1.7 - 1.0) / 1.0); - EXPECT(assert_equal(Vector(0.5 * twin.forward(xNorm).array() + 1.5), - loaded.forward(x), 1e-12)); - - Matrix H; - loaded.forward(x, &H); - std::function f = [&](const Vector &v) { - return loaded.forward(v); - }; - EXPECT(assert_equal( - Matrix(gtsam::numericalDerivative11(f, x)), H, 1e-6)); + EXPECT(assert_equal(threeLayerTwin().forward(x), loaded.forward(x), 1e-12)); + EXPECT(loaded.metadata().at("output_std") == "0.5"); std::filesystem::remove(path); } diff --git a/python/tests/test_nn_cable.py b/python/tests/test_nn_cable.py new file mode 100644 index 00000000..510ab1ce --- /dev/null +++ b/python/tests/test_nn_cable.py @@ -0,0 +1,168 @@ +""" +GTDynamics Copyright 2020, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +See LICENSE for the license information + +Neural-network cable factor on the bar_lab platform: an MLP loaded from a +weight file predicts the cable shape between two robot1 links, and one factor +keeps the sampled cable clear of a sphere obstacle. Mirrors +gtdynamics/gpmp2/tests/testNNCableFactor.cpp. +Author: Karthik Shaji +""" + +import os +import tempfile +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 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]) + +# The network reads the five distal arm joints, as the trained model does. +INPUT_INDICES = [4, 5, 6, 7, 8] +CHEB_NODES = 6 +NUM_SAMPLES = 9 +EPSILON = 0.10 +CABLE_RADIUS = 0.02 + + +def write_zero_weight_file(path, bias): + """A 5 -> 4 -> len(bias) network with zero weights and the given bias.""" + n_out = len(bias) + lines = [ + "# synthetic test model, row major", + "input_dim 5", + "hidden_dims 4", + "activation relu", + f"output_dim {n_out}", + f"cheb_nodes {CHEB_NODES}", + "layers 2", + "layer0_weight 4 5", + ] + lines += ["0"] * 20 + lines += ["layer0_bias 4"] + ["0"] * 4 + lines += [f"layer1_weight {n_out} 4"] + ["0"] * (n_out * 4) + lines += [f"layer1_bias {n_out}"] + [repr(value) for value in bias] + with open(path, "w", encoding="ascii") as handle: + handle.write("\n".join(lines) + "\n") + + +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) + + +class TestNNCable(GtsamTestCase): + """An MLP-predicted cable between robot1's link 3 and link 6.""" + + def setUp(self): + self.robot = gtd.CreateRobotFromFile( + str(Path(gtd.URDF_PATH) / "bar_lab.urdf")) + self.joints = [self.robot.joint(name) for name in ROBOT1_JOINTS] + + n_out = 3 * (CHEB_NODES - 2) + self.weight_file = os.path.join(tempfile.gettempdir(), + "gtd_test_nn_cable.txt") + write_zero_weight_file(self.weight_file, np.zeros(n_out)) + self.addCleanup(os.remove, self.weight_file) + self.mlp = gtd.MLP(self.weight_file) + + self.attachment0 = gtd.PointOnLink(self.robot.link("robot1_link_3"), + np.array([0.1, 0.0, 0.05])) + self.attachment1 = gtd.PointOnLink(self.robot.link("robot1_link_6"), + np.array([0.0, 0.0, 0.1])) + self.cable = gtd.NNCableSpline(self.robot, "columns", self.joints, + self.attachment0, self.attachment1, + self.robot.link("robot1_link_1"), + self.mlp, INPUT_INDICES, CHEB_NODES, + NUM_SAMPLES) + + def test_mlp(self): + """The loaded network has the declared shape and runs on raw inputs.""" + self.assertEqual(self.mlp.inputDim(), 5) + self.assertEqual(self.mlp.outputDim(), 3 * (CHEB_NODES - 2)) + self.assertEqual(self.mlp.nrLayers(), 2) + np.testing.assert_allclose(self.mlp.forward(np.ones(5)), + np.zeros(3 * (CHEB_NODES - 2)), atol=1e-12) + + def test_chord_when_residual_zero(self): + """With zero residuals every sample lies on the endpoint chord.""" + self.assertEqual(self.cable.dof(), 9) + self.assertEqual(self.cable.numSamples(), NUM_SAMPLES) + + points = gtd.PointOnLinks() + points.append(self.attachment0) + points.append(self.attachment1) + endpoints = gtd.RobotQueryPoints(self.robot, "columns", self.joints, + points).worldPoints(Q_START) + + pts = self.cable.worldPoints(Q_START) + self.assertEqual(pts.shape, (3, NUM_SAMPLES)) + for m in range(NUM_SAMPLES): + s = m / (NUM_SAMPLES - 1) + chord = (1.0 - s) * endpoints[:, 0] + s * endpoints[:, 1] + np.testing.assert_allclose(pts[:, m], chord, atol=1e-9) + + def test_factor_pushes_cable_off_obstacle(self): + """A sphere on the cable is in collision; optimizing clears it.""" + pts = self.cable.worldPoints(Q_START) + center = pts[:, NUM_SAMPLES // 2] + np.array([0.015, 0.01, 0.0]) + origin = center - 1.0 + 0.5 * 0.05 + sdf = sphere_sdf(center, 0.15, origin, 0.05, (41, 41, 41)) + + factor = gtd.NNCableFactor(X(0), self.cable, sdf, 0.01, EPSILON, + CABLE_RADIUS) + values = gtsam.Values() + values.insert(X(0), Q_START) + initial_error = factor.error(values) + self.assertGreater(initial_error, 0.0) + + graph = gtsam.NonlinearFactorGraph() + graph.add(factor) + # A weak prior keeps the configuration near its start. + graph.add( + gtsam.PriorFactorVector(X(0), Q_START, + gtsam.noiseModel.Isotropic.Sigma(9, 0.5))) + result = gtsam.LevenbergMarquardtOptimizer(graph, values).optimize() + + final = gtsam.Values() + final.insert(X(0), result.atVector(X(0))) + self.assertLess(factor.error(final), initial_error) + + def test_rejects_bad_radii(self): + """A radii vector of the wrong length is rejected.""" + pts = self.cable.worldPoints(Q_START) + center = pts[:, NUM_SAMPLES // 2] + sdf = sphere_sdf(center, 0.15, center - 1.0, 0.05, (41, 41, 41)) + with self.assertRaises(ValueError): + gtd.NNCableFactor(X(0), self.cable, sdf, 0.01, EPSILON, + np.zeros(NUM_SAMPLES - 1)) + + +if __name__ == "__main__": + unittest.main() From 4ba18de5902cbad0ce4f9d6534ca1cdb41f3ad35 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Sat, 15 Aug 2026 11:09:56 -0700 Subject: [PATCH 03/11] fixes for unit tests --- gtdynamics/dynamics/tests/testMLP.cpp | 11 ++++++----- gtdynamics/gpmp2/tests/testNNCableFactor.cpp | 2 +- gtdynamics/gpmp2/tests/testObstacleFactors.cpp | 2 +- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/gtdynamics/dynamics/tests/testMLP.cpp b/gtdynamics/dynamics/tests/testMLP.cpp index 8bc42c46..fb4263ca 100644 --- a/gtdynamics/dynamics/tests/testMLP.cpp +++ b/gtdynamics/dynamics/tests/testMLP.cpp @@ -70,15 +70,15 @@ TEST(MLP, jacobianAgainstNumerical) { MLP relu(smallWeights(), smallBiases(), MLP::Activation::kRelu); MLP tanhNet(smallWeights(), smallBiases(), MLP::Activation::kTanh); - auto check = [](const MLP &net, const Vector &x) { + auto check = [&](const MLP &net, const Vector &x) { Matrix H; net.forward(x, &H); std::function f = [&](const Vector &v) { return net.forward(v); }; - EXPECT(assert_equal(Matrix(gtsam::numericalDerivative11( - f, x)), - H, 1e-6)); + EXPECT(assert_equal( + Matrix(gtsam::numericalDerivative11(f, x)), H, + 1e-6)); }; check(relu, Vector2(1.0, 1.0)); // z0 = (3.1, -0.7), away from kinks @@ -147,7 +147,8 @@ TEST(MLP, loadRoundTrip) { EXPECT(loaded.metadata().at("cheb_nodes") == "16"); EXPECT(loaded.metadata().at("endpoint_links") == "link_3 link_6"); - for (const Vector &x : {Vector2(0.3, -0.8), Vector2(-1.1, 0.6)}) { + for (const Vector &x : + std::vector{Vector2(0.3, -0.8), Vector2(-1.1, 0.6)}) { Matrix Hloaded, Htwin; EXPECT(assert_equal(twin.forward(x, &Htwin), loaded.forward(x, &Hloaded), 1e-12)); diff --git a/gtdynamics/gpmp2/tests/testNNCableFactor.cpp b/gtdynamics/gpmp2/tests/testNNCableFactor.cpp index 9c56987f..f22e529e 100644 --- a/gtdynamics/gpmp2/tests/testNNCableFactor.cpp +++ b/gtdynamics/gpmp2/tests/testNNCableFactor.cpp @@ -180,7 +180,7 @@ TEST(NNCableSpline, sampleJacobiansAgainstNumerical) { return pts[m]; }; EXPECT(assert_equal( - Matrix(gtsam::numericalDerivative11(f, q)), + Matrix(gtsam::numericalDerivative11(f, q)), ptJacobians[m], 1e-5)); } } diff --git a/gtdynamics/gpmp2/tests/testObstacleFactors.cpp b/gtdynamics/gpmp2/tests/testObstacleFactors.cpp index aa9d8e83..7dca7447 100644 --- a/gtdynamics/gpmp2/tests/testObstacleFactors.cpp +++ b/gtdynamics/gpmp2/tests/testObstacleFactors.cpp @@ -312,7 +312,7 @@ TEST(RobotQueryPoints, queryPosesAgainstNumerical) { return poses[i]; }; EXPECT(assert_equal( - Matrix(gtsam::numericalDerivative11(f, q)), + Matrix(gtsam::numericalDerivative11(f, q)), poseJacobians[i], 1e-5)); } } From d46870aed1ed069dc05d12d24829ccf6950e0c0c Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Sat, 15 Aug 2026 11:44:08 -0700 Subject: [PATCH 04/11] slight updates to cmake --- python/CMakeLists.txt | 3 +++ python/templates/pyproject.toml.in | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 44345bb8..a5688848 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -15,6 +15,9 @@ file(TO_NATIVE_PATH "${PROJECT_BINARY_DIR}/python" GTD_PYTHON_BINARY_DIR) configure_file(${GTDYNAMICS_PYTHON_PATH}/templates/pyproject.toml.in ${GTD_PYTHON_BINARY_DIR}/pyproject.toml) +# Newer hatchling requires the readme to live inside the project directory. +configure_file(${PROJECT_SOURCE_DIR}/README.md + ${GTD_PYTHON_BINARY_DIR}/README.md COPYONLY) configure_file(${GTDYNAMICS_PYTHON_PATH}/templates/${PROJECT_NAME}.tpl ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.tpl) file(COPY ${GTDYNAMICS_PYTHON_PATH}/${PROJECT_NAME} diff --git a/python/templates/pyproject.toml.in b/python/templates/pyproject.toml.in index bf6077e0..f652b58e 100644 --- a/python/templates/pyproject.toml.in +++ b/python/templates/pyproject.toml.in @@ -6,7 +6,7 @@ authors = [ { name = "Varun Agrawal", email = "varunagrawal@gatech.edu" }, { name="Frank Dellaert", email="dellaert@gatech.edu" } ] -readme = "${PROJECT_SOURCE_DIR}/README.md" +readme = "README.md" license = "BSD-3-Clause" keywords=["robotics", "kinematics", "dynamics", "factor graphs"] From 02896b7e3e66fb648215f860c1b29b6343442364 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 18 Aug 2026 14:59:43 -0700 Subject: [PATCH 05/11] fixed up unit test issues --- gtdynamics.i | 21 ++++++++ gtdynamics/gpmp2/tests/barLabFixtures.h | 3 ++ gtdynamics/gpmp2/tests/testNNCableFactor.cpp | 51 +++++++++++++++++++ .../gpmp2/tests/testObstacleFactors.cpp | 4 -- 4 files changed, 75 insertions(+), 4 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index 6d6a0029..1f374cd5 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -1194,6 +1194,27 @@ class NNCableFactor : gtsam::NoiseModelFactor { gtdynamics::GTDKeyFormatter); }; +#include +class NNCableFactorGP : gtsam::NoiseModelFactor { + NNCableFactorGP(gtsam::Key qKey1, gtsam::Key vKey1, gtsam::Key qKey2, + gtsam::Key vKey2, const gtdynamics::NNCableSpline *cable, + const gtdynamics::SignedDistanceField *sdf, double costSigma, + double epsilon, double cableRadius, + const gtsam::noiseModel::Base *QcModel, double deltaT, + double tau); + NNCableFactorGP(gtsam::Key qKey1, gtsam::Key vKey1, gtsam::Key qKey2, + gtsam::Key vKey2, const gtdynamics::NNCableSpline *cable, + const gtdynamics::SignedDistanceField *sdf, double costSigma, + double epsilon, const gtsam::Vector &radii, + const gtsam::noiseModel::Base *QcModel, double deltaT, + double tau); + + double epsilon() const; + gtsam::Vector radii() const; + void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter = + gtdynamics::GTDKeyFormatter); +}; + #include class SelfCollisionPair { SelfCollisionPair(); diff --git a/gtdynamics/gpmp2/tests/barLabFixtures.h b/gtdynamics/gpmp2/tests/barLabFixtures.h index e3799dd3..5aadb1b2 100644 --- a/gtdynamics/gpmp2/tests/barLabFixtures.h +++ b/gtdynamics/gpmp2/tests/barLabFixtures.h @@ -30,6 +30,9 @@ inline constexpr double kCell = 0.05; inline constexpr double kRadius = 0.15; inline constexpr double kEpsilon = 0.10; +/// Half-cell grid offset: the trilinear gradient is discontinuous on nodes. +inline constexpr double kHalfCell = 0.5 * kCell; + /// The bar_lab workspace, loaded once per test binary. inline const Robot &barLabRobot() { static const Robot robot = diff --git a/gtdynamics/gpmp2/tests/testNNCableFactor.cpp b/gtdynamics/gpmp2/tests/testNNCableFactor.cpp index f22e529e..d2155b00 100644 --- a/gtdynamics/gpmp2/tests/testNNCableFactor.cpp +++ b/gtdynamics/gpmp2/tests/testNNCableFactor.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,8 @@ using gtsam::Pose3; 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 Robot &kRobot = barLabRobot(); @@ -250,6 +253,54 @@ TEST(NNCableFactor, activeAndInactiveSamples) { EXPECT_DOUBLES_EQUAL(0.0, H.norm(), 1e-9); } +// At tau = 0 the GP factor must reproduce the unary error and Jacobian at q1. +TEST(NNCableFactorGP, agreesWithUnaryFactorAtTauZero) { + const size_t N = 8, M = 9; + const auto cable = makeCable(smoothMLP(N), N, M); + const Vector q1 = startConfig(); + Vector q2 = startConfig(); + q2(0) += 1.0; // the bridge has moved along the rail + const Vector v1 = Vector::Zero(9), v2 = Vector::Zero(9); + + const Matrix pts = cable->worldPoints(q1); + const Point3 center = Point3(pts.col(M / 2)) + Point3(0.3 * kCell, + 0.2 * kCell, 0.0); + const Point3 origin = center - Point3::Constant(1.0 - kHalfCell); + auto sdf = std::make_shared( + makeSphereSDF(center, kRadius, origin, kCell, 41, 41, 41)); + + const double deltaT = 0.5, costSigma = 0.01; + NNCableFactor unary(X(0), cable, sdf, costSigma, kEpsilon, 0.02); + NNCableFactorGP interpolated(X(0), V(0), X(1), V(1), cable, sdf, costSigma, + kEpsilon, 0.02, Isotropic::Sigma(9, 1.0), + deltaT, 0.0); + + // q1 passes through with unit weight, so H1 is the unary Jacobian and the + // other support states get zero. + Matrix Hu, H1, H2, H3, H4; + EXPECT(assert_equal(unary.evaluateError(q1, &Hu), + interpolated.evaluateError(q1, v1, q2, v2, &H1, &H2, &H3, + &H4), + 1e-9)); + EXPECT(assert_equal(Hu, H1, 1e-9)); + const Matrix zero = Matrix::Zero(Hu.rows(), Hu.cols()); + EXPECT(assert_equal(zero, H2, 1e-9)); + EXPECT(assert_equal(zero, H3, 1e-9)); + EXPECT(assert_equal(zero, H4, 1e-9)); + + // At an interior tau the analytic Jacobians must match the numerical ones. + Values values; + values.insert(X(0), q1); + values.insert(V(0), v1); + values.insert(X(1), q2); + values.insert(V(1), v2); + NNCableFactorGP atTau(X(0), V(0), X(1), V(1), cable, sdf, costSigma, + kEpsilon, 0.02, Isotropic::Sigma(9, 1.0), deltaT, + 0.1 * deltaT); + EXPECT(atTau.evaluateError(q1, v1, q2, v2).norm() > 0.0); // active branch + EXPECT_CORRECT_FACTOR_JACOBIANS(atTau, values, 1e-7, 1e-5); +} + /* ************************ validation *********************************** */ // Inconsistent construction inputs are rejected with clear errors. diff --git a/gtdynamics/gpmp2/tests/testObstacleFactors.cpp b/gtdynamics/gpmp2/tests/testObstacleFactors.cpp index 7dca7447..c557297b 100644 --- a/gtdynamics/gpmp2/tests/testObstacleFactors.cpp +++ b/gtdynamics/gpmp2/tests/testObstacleFactors.cpp @@ -54,10 +54,6 @@ using gtsam::noiseModel::Isotropic; using gtsam::symbol_shorthand::V; using gtsam::symbol_shorthand::X; -// 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; - /* ********************** signed distance field ************************** */ // Trilinear interpolation of an exact sphere field must recover the distance, From 20367d55dc409b9959c38390b8e12baa255bad25 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 18 Aug 2026 15:18:25 -0700 Subject: [PATCH 06/11] consolidation into helpers --- gtdynamics/gpmp2/NNCableSpline.cpp | 18 ++-------------- gtdynamics/gpmp2/ObstacleCost.cpp | 33 +++++++++++++++++++----------- gtdynamics/gpmp2/ObstacleCost.h | 14 +++++++++++++ 3 files changed, 37 insertions(+), 28 deletions(-) diff --git a/gtdynamics/gpmp2/NNCableSpline.cpp b/gtdynamics/gpmp2/NNCableSpline.cpp index ccc16645..32a53adb 100644 --- a/gtdynamics/gpmp2/NNCableSpline.cpp +++ b/gtdynamics/gpmp2/NNCableSpline.cpp @@ -170,25 +170,11 @@ gtsam::Vector nnCableSDFError(const gtsam::Vector &q, const NNCableSpline &cable, const SignedDistanceField &sdf, double epsilon, const gtsam::Vector &radii, gtsam::Matrix *Hq) { - const size_t nrSamples = cable.numSamples(); - std::vector wPts; std::vector ptJacobians; cable.samplePoints(q, &wPts, Hq ? &ptJacobians : nullptr); - if (Hq) *Hq = gtsam::Matrix::Zero(nrSamples, cable.dof()); - - gtsam::Vector err(nrSamples); - for (size_t m = 0; m < nrSamples; ++m) { - const double eps = epsilon + radii(m); - if (Hq) { - gtsam::Matrix13 Hpt; - err(m) = hingeLossObstacleCost(wPts[m], sdf, eps, Hpt); - Hq->row(m) = Hpt * ptJacobians[m]; - } else { - err(m) = hingeLossObstacleCost(wPts[m], sdf, eps); - } - } - return err; + return internal::hingeLossOverPoints(wPts, ptJacobians, sdf, epsilon, radii, + Hq); } } // namespace gtdynamics diff --git a/gtdynamics/gpmp2/ObstacleCost.cpp b/gtdynamics/gpmp2/ObstacleCost.cpp index f59ee27c..b94fee05 100644 --- a/gtdynamics/gpmp2/ObstacleCost.cpp +++ b/gtdynamics/gpmp2/ObstacleCost.cpp @@ -81,29 +81,38 @@ double hingeLossObstacleCost(const gtsam::Pose3 &wTs, } /* ************************************************************************* */ -gtsam::Vector obstacleSDFError(const gtsam::Vector &q, - const RobotQueryPoints &robot, - const SignedDistanceField &sdf, double epsilon, - const gtsam::Vector &radii, gtsam::Matrix *Hq) { - const size_t nrPts = robot.nrPoints(); - - std::vector wPts; - std::vector ptJacobians; - robot.queryPoints(q, &wPts, Hq ? &ptJacobians : nullptr); - if (Hq) *Hq = gtsam::Matrix::Zero(nrPts, robot.dof()); +gtsam::Vector internal::hingeLossOverPoints( + const std::vector &wPts, + const std::vector &ptJacobians, + const SignedDistanceField &sdf, double epsilon, const gtsam::Vector &radii, + gtsam::Matrix *Hq) { + const size_t nrPts = wPts.size(); + if (Hq) *Hq = gtsam::Matrix::Zero(nrPts, ptJacobians.front().cols()); gtsam::Vector err(nrPts); for (size_t i = 0; i < nrPts; ++i) { const double eps = epsilon + radii(i); if (Hq) { gtsam::Matrix13 Hpt; - err(i) = hingeLossObstacleCost(wPts[i], sdf, eps, Hpt); + err(i) = gtdynamics::hingeLossObstacleCost(wPts[i], sdf, eps, Hpt); Hq->row(i) = Hpt * ptJacobians[i]; } else { - err(i) = hingeLossObstacleCost(wPts[i], sdf, eps); + err(i) = gtdynamics::hingeLossObstacleCost(wPts[i], sdf, eps); } } return err; } +/* ************************************************************************* */ +gtsam::Vector obstacleSDFError(const gtsam::Vector &q, + const RobotQueryPoints &robot, + const SignedDistanceField &sdf, double epsilon, + const gtsam::Vector &radii, gtsam::Matrix *Hq) { + std::vector wPts; + std::vector ptJacobians; + robot.queryPoints(q, &wPts, Hq ? &ptJacobians : nullptr); + return internal::hingeLossOverPoints(wPts, ptJacobians, sdf, epsilon, radii, + Hq); +} + } // namespace gtdynamics diff --git a/gtdynamics/gpmp2/ObstacleCost.h b/gtdynamics/gpmp2/ObstacleCost.h index 53210a75..cc00f38a 100644 --- a/gtdynamics/gpmp2/ObstacleCost.h +++ b/gtdynamics/gpmp2/ObstacleCost.h @@ -22,6 +22,8 @@ #include #include +#include + namespace gtdynamics { /** @@ -77,4 +79,16 @@ GTSAM_EXPORT gtsam::Vector obstacleSDFError(const gtsam::Vector &q, const gtsam::Vector &radii, gtsam::Matrix *Hq = nullptr); +namespace internal { + +/// Hinge loss of each point against sdf, with standoff epsilon + radii(i). +/// If Hq is non-null, row i is filled with Hpt_i * ptJacobians[i]. +gtsam::Vector hingeLossOverPoints(const std::vector &wPts, + const std::vector &ptJacobians, + const SignedDistanceField &sdf, + double epsilon, const gtsam::Vector &radii, + gtsam::Matrix *Hq); + +} // namespace internal + } // namespace gtdynamics From 096a65f1d99c0985af0999dd1bc2dceb5f92553e Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 18 Aug 2026 15:19:07 -0700 Subject: [PATCH 07/11] gp factor --- gtdynamics/factors/NNCableFactorGP.h | 161 +++++++++++++++++++++++++++ 1 file changed, 161 insertions(+) create mode 100644 gtdynamics/factors/NNCableFactorGP.h diff --git a/gtdynamics/factors/NNCableFactorGP.h b/gtdynamics/factors/NNCableFactorGP.h new file mode 100644 index 00000000..74a36e06 --- /dev/null +++ b/gtdynamics/factors/NNCableFactorGP.h @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file NNCableFactorGP.h + * @brief Obstacle avoidance cost factor for a neural-network-predicted cable + * at a Gaussian process interpolated state. + * @author Karthik Shaji + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gtdynamics { + +/** + * Cable obstacle avoidance cost evaluated at a state interpolated between two + * support states, so that the predicted cable 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 QcModel and deltaT connects the same two support states in + * the graph. + */ +class NNCableFactorGP + : public gtsam::NoiseModelFactorN { + private: + using This = NNCableFactorGP; + using Base = gtsam::NoiseModelFactorN; + + double epsilon_; + gtsam::Vector radii_; ///< per sample standoff radius, e.g. the cable radius + std::shared_ptr cable_; + std::shared_ptr sdf_; + GPLinearInterpolator interpolator_; + + public: + /** + * Constructor with a single cable radius for every sample. + * @param qKey1 key of the joint angles of the first support state + * @param vKey1 key of the joint velocities of the first support state + * @param qKey2 key of the joint angles of the second support state + * @param vKey2 key of the joint velocities of the second support state + * @param cable cable shape model + * @param sdf signed distance field of the obstacles, in the world frame + * @param costSigma cost function sigma, one per sample + * @param epsilon standoff distance kept from every obstacle + * @param cableRadius radius of the cable, added to epsilon at every sample + * @param QcModel Gaussian noise model whose covariance is Qc + * @param deltaT time between the two support states + * @param tau time from the first support state to the interpolated state + */ + NNCableFactorGP(gtsam::Key qKey1, gtsam::Key vKey1, gtsam::Key qKey2, + gtsam::Key vKey2, + const std::shared_ptr &cable, + const std::shared_ptr &sdf, + double costSigma, double epsilon, double cableRadius, + const gtsam::SharedNoiseModel &QcModel, double deltaT, + double tau) + : NNCableFactorGP(qKey1, vKey1, qKey2, vKey2, cable, sdf, costSigma, + epsilon, + gtsam::Vector::Constant( + internal::checkedNumSamples(cable, + "NNCableFactorGP"), + cableRadius), + QcModel, deltaT, tau) {} + + /** + * Constructor with a radius per sample, added to the shared epsilon. + * @param qKey1 key of the joint angles of the first support state + * @param vKey1 key of the joint velocities of the first support state + * @param qKey2 key of the joint angles of the second support state + * @param vKey2 key of the joint velocities of the second support state + * @param cable cable shape model + * @param sdf signed distance field of the obstacles, in the world frame + * @param costSigma cost function sigma, one per sample + * @param epsilon standoff distance added to every radius + * @param radii standoff radius of each sample, one per sample + * @param QcModel Gaussian noise model whose covariance is Qc + * @param deltaT time between the two support states + * @param tau time from the first support state to the interpolated state + */ + NNCableFactorGP(gtsam::Key qKey1, gtsam::Key vKey1, gtsam::Key qKey2, + gtsam::Key vKey2, + const std::shared_ptr &cable, + const std::shared_ptr &sdf, + double costSigma, double epsilon, const gtsam::Vector &radii, + const gtsam::SharedNoiseModel &QcModel, double deltaT, + double tau) + : Base(gtsam::noiseModel::Isotropic::Sigma( + internal::checkedNumSamples(cable, "NNCableFactorGP"), + costSigma), + qKey1, vKey1, qKey2, vKey2), + epsilon_(epsilon), + radii_(radii), + cable_(cable), + sdf_(sdf), + interpolator_(QcModel, deltaT, tau) { + // deltaT and tau are checked by the interpolator constructor. + internal::validateNNCableFactorArgs(*cable_, sdf_, epsilon_, radii_, + "NNCableFactorGP"); + } + + ~NNCableFactorGP() 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 cable sample at the interpolated state, + /// and its Jacobians with respect to the support states. + gtsam::Vector evaluateError( + const gtsam::Vector &q1, const gtsam::Vector &v1, const gtsam::Vector &q2, + const gtsam::Vector &v2, gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr, + gtsam::OptionalMatrixType H3 = nullptr, + gtsam::OptionalMatrixType H4 = nullptr) const override { + return interpolator_.errorAtInterpolatedPose( + q1, v1, q2, v2, + [this](const gtsam::Vector &q, gtsam::Matrix *Hq) { + return nnCableSDFError(q, *cable_, *sdf_, epsilon_, radii_, Hq); + }, + H1, H2, H3, H4); + } + + /// Return the shared standoff distance. + double epsilon() const { return epsilon_; } + + /// Return the per sample 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 << "NNCableFactorGP with " << cable_->numSamples() + << " cable samples" << std::endl; + Base::print("", keyFormatter); + } +}; // \class NNCableFactorGP + +} // namespace gtdynamics From 3baef0f841a63e59d30ad40b8e48347335f87fd8 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 18 Aug 2026 15:21:11 -0700 Subject: [PATCH 08/11] pointsToMatrix is now internal --- gtdynamics/gpmp2/NNCableSpline.cpp | 4 +--- gtdynamics/gpmp2/RobotQueryPoints.cpp | 4 +--- gtdynamics/gpmp2/RobotQueryPoints.h | 11 +++++++++++ 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/gtdynamics/gpmp2/NNCableSpline.cpp b/gtdynamics/gpmp2/NNCableSpline.cpp index 32a53adb..061387b4 100644 --- a/gtdynamics/gpmp2/NNCableSpline.cpp +++ b/gtdynamics/gpmp2/NNCableSpline.cpp @@ -160,9 +160,7 @@ void NNCableSpline::samplePoints(const gtsam::Vector &q, gtsam::Matrix NNCableSpline::worldPoints(const gtsam::Vector &q) const { std::vector wPts; samplePoints(q, &wPts); - gtsam::Matrix pts(3, numSamples()); - for (size_t m = 0; m < numSamples(); ++m) pts.col(m) = wPts[m]; - return pts; + return internal::pointsToMatrix(wPts); } /* ************************************************************************* */ diff --git a/gtdynamics/gpmp2/RobotQueryPoints.cpp b/gtdynamics/gpmp2/RobotQueryPoints.cpp index 79b0e30d..499589a9 100644 --- a/gtdynamics/gpmp2/RobotQueryPoints.cpp +++ b/gtdynamics/gpmp2/RobotQueryPoints.cpp @@ -211,9 +211,7 @@ void RobotQueryPoints::queryPoints( gtsam::Matrix RobotQueryPoints::worldPoints(const gtsam::Vector &q) const { std::vector wPts; queryPoints(q, &wPts); - gtsam::Matrix pts(3, nrPoints()); - for (size_t i = 0; i < nrPoints(); ++i) pts.col(i) = wPts[i]; - return pts; + return internal::pointsToMatrix(wPts); } /* ************************************************************************* */ diff --git a/gtdynamics/gpmp2/RobotQueryPoints.h b/gtdynamics/gpmp2/RobotQueryPoints.h index 087cc6a4..08cf9b87 100644 --- a/gtdynamics/gpmp2/RobotQueryPoints.h +++ b/gtdynamics/gpmp2/RobotQueryPoints.h @@ -26,6 +26,17 @@ namespace gtdynamics { +namespace internal { + +/// The points as columns of a 3 x n matrix. +inline gtsam::Matrix pointsToMatrix(const std::vector &pts) { + gtsam::Matrix matrix(3, pts.size()); + for (size_t i = 0; i < pts.size(); ++i) matrix.col(i) = pts[i]; + return matrix; +} + +} // namespace internal + /** * Maps a stacked joint angle vector q to the world positions of a fixed set of * query points on the robot, with Jacobians with respect to q. The joints From d37fc7d9d981766ef8792d5910d7e9400b2618e3 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 18 Aug 2026 15:23:23 -0700 Subject: [PATCH 09/11] fixed samplePoints --- gtdynamics/gpmp2/NNCableSpline.cpp | 4 +--- gtdynamics/gpmp2/NNCableSpline.h | 4 ++-- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/gtdynamics/gpmp2/NNCableSpline.cpp b/gtdynamics/gpmp2/NNCableSpline.cpp index 061387b4..4eb59864 100644 --- a/gtdynamics/gpmp2/NNCableSpline.cpp +++ b/gtdynamics/gpmp2/NNCableSpline.cpp @@ -80,11 +80,9 @@ NNCableSpline::NNCableSpline( // The evaluation is linear in the nodal residuals, so these barycentric // weights are also the exact spline Jacobian; the endpoint columns multiply // the identically-zero endpoint residuals and are dropped. - sampleParams_ = gtsam::Vector(numSamples); interiorWeights_ = gtsam::Matrix(numSamples, numChebNodes_ - 2); for (size_t m = 0; m < numSamples; ++m) { const double s = static_cast(m) / (numSamples - 1); - sampleParams_(m) = s; const gtsam::Matrix weights = gtsam::Chebyshev2::CalculateWeights(numChebNodes_, s, 0.0, 1.0); interiorWeights_.row(m) = weights.row(0).segment(1, numChebNodes_ - 2); @@ -139,7 +137,7 @@ void NNCableSpline::samplePoints(const gtsam::Vector &q, wPts->resize(numSamples()); if (computeJacobians) ptJacobians->resize(numSamples()); for (size_t m = 0; m < numSamples(); ++m) { - const double s = sampleParams_(m); + const double s = static_cast(m) / (numSamples() - 1); const gtsam::Vector3 v = interiorResiduals.transpose() * interiorWeights_.row(m).transpose(); (*wPts)[m] = (1.0 - s) * p[0] + s * p[1] + R * v; diff --git a/gtdynamics/gpmp2/NNCableSpline.h b/gtdynamics/gpmp2/NNCableSpline.h index 83f6d1cf..40dd95d5 100644 --- a/gtdynamics/gpmp2/NNCableSpline.h +++ b/gtdynamics/gpmp2/NNCableSpline.h @@ -41,8 +41,8 @@ class GTSAM_EXPORT NNCableSpline { std::shared_ptr mlp_; std::vector inputIndices_; ///< entries of q the network reads size_t numChebNodes_; ///< N, endpoints included - gtsam::Vector sampleParams_; ///< s_m, uniform on [0, 1] gtsam::Matrix interiorWeights_; ///< numSamples x (N-2) Chebyshev weights + ///< at s_m = m / (numSamples - 1) /// The FK query points, with the reference link checked against null. static PointOnLinks checkedPoints(const PointOnLink &attachment0, @@ -83,7 +83,7 @@ class GTSAM_EXPORT NNCableSpline { size_t dof() const { return fk_.dof(); } /// Return the number of samples along the cable. - size_t numSamples() const { return sampleParams_.size(); } + size_t numSamples() const { return interiorWeights_.rows(); } /// Return the number N of Chebyshev-Lobatto nodes, endpoints included. size_t numChebNodes() const { return numChebNodes_; } From fd002eaa6b7366af6f036c0a24bdce7038d79163 Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 18 Aug 2026 15:28:08 -0700 Subject: [PATCH 10/11] fixed cable validation --- .../factors/internal/CollisionFactorUtils.h | 81 ++++++++++--------- 1 file changed, 43 insertions(+), 38 deletions(-) diff --git a/gtdynamics/factors/internal/CollisionFactorUtils.h b/gtdynamics/factors/internal/CollisionFactorUtils.h index 21039e78..48c3c9b7 100644 --- a/gtdynamics/factors/internal/CollisionFactorUtils.h +++ b/gtdynamics/factors/internal/CollisionFactorUtils.h @@ -31,14 +31,47 @@ namespace gtdynamics { namespace internal { +/// Reject a null model and return it, for factor initializer lists. +template +inline const std::shared_ptr &checkedNotNull(const std::shared_ptr &model, + const std::string &factorName, + const std::string &what) { + if (!model) { + throw std::invalid_argument(factorName + ": " + what + + " must not be null."); + } + return model; +} + /// nrPoints of a model that must not be null, for factor initializer lists. inline size_t checkedNrPoints( const std::shared_ptr &robot, const std::string &factorName) { - if (!robot) { - throw std::invalid_argument(factorName + ": robot must not be null."); + return checkedNotNull(robot, factorName, "robot")->nrPoints(); +} + +/// Reject radii that are mis-sized (one entry per `what`) or negative. +inline void validateRadii(const gtsam::Vector &radii, size_t expectedSize, + const std::string &what, + const std::string &factorName) { + if (static_cast(radii.size()) != expectedSize) { + throw std::invalid_argument(factorName + + ": radii must have one entry per " + what + + "."); + } + if ((radii.array() < 0.0).any()) { + throw std::invalid_argument(factorName + ": radii must be >= 0."); + } +} + +/// Reject a null field or a negative standoff. +inline void validateSdfStandoff( + const std::shared_ptr &sdf, double epsilon, + const std::string &factorName) { + checkedNotNull(sdf, factorName, "sdf"); + if (epsilon < 0.0) { + throw std::invalid_argument(factorName + ": epsilon must be >= 0."); } - return robot->nrPoints(); } /// Reject radii that are mis-sized, negative, or conflict at coincident @@ -46,13 +79,7 @@ inline size_t checkedNrPoints( inline void validateQueryPointRadii(const RobotQueryPoints &robot, const gtsam::Vector &radii, const std::string &factorName) { - if (static_cast(radii.size()) != robot.nrPoints()) { - throw std::invalid_argument( - factorName + ": radii must have one entry per query point."); - } - if ((radii.array() < 0.0).any()) { - throw std::invalid_argument(factorName + ": radii must be >= 0."); - } + validateRadii(radii, robot.nrPoints(), "query point", factorName); // 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. @@ -81,12 +108,7 @@ inline void validateObstacleSDFFactorArgs( const RobotQueryPoints &robot, const std::shared_ptr &sdf, double epsilon, const gtsam::Vector &radii, const std::string &factorName) { - if (!sdf) { - throw std::invalid_argument(factorName + ": sdf must not be null."); - } - if (epsilon < 0.0) { - throw std::invalid_argument(factorName + ": epsilon must be >= 0."); - } + validateSdfStandoff(sdf, epsilon, factorName); validateQueryPointRadii(robot, radii, factorName); } @@ -155,31 +177,16 @@ inline std::shared_ptr restrictToReferencedPoints( inline size_t checkedNumSamples( const std::shared_ptr &cable, const std::string &factorName) { - if (!cable) { - throw std::invalid_argument(factorName + ": cable must not be null."); - } - return cable->numSamples(); + return checkedNotNull(cable, factorName, "cable")->numSamples(); } -/// Reject a null field, a negative standoff, or radii that are mis-sized or -/// negative. factorName prefixes the error messages. +/// Reject a null field, a negative standoff, or bad radii. inline void validateNNCableFactorArgs( const NNCableSpline &cable, const std::shared_ptr &sdf, double epsilon, const gtsam::Vector &radii, const std::string &factorName) { - if (!sdf) { - throw std::invalid_argument(factorName + ": sdf must not be null."); - } - if (epsilon < 0.0) { - throw std::invalid_argument(factorName + ": epsilon must be >= 0."); - } - if (static_cast(radii.size()) != cable.numSamples()) { - throw std::invalid_argument( - factorName + ": radii must have one entry per cable sample."); - } - if ((radii.array() < 0.0).any()) { - throw std::invalid_argument(factorName + ": radii must be >= 0."); - } + validateSdfStandoff(sdf, epsilon, factorName); + validateRadii(radii, cable.numSamples(), "cable sample", factorName); } /// Reject a null model, inconsistent pairs/radii, or, if sigmas is given, a @@ -189,9 +196,7 @@ inline std::shared_ptr validateAndRestrictSelfCollision( const std::shared_ptr &robot, SelfCollisionPairs *pairs, gtsam::Vector *radii, const std::string &factorName, const gtsam::Vector *sigmas = nullptr) { - if (!robot) { - throw std::invalid_argument(factorName + ": robot must not be null."); - } + checkedNotNull(robot, factorName, "robot"); if (sigmas && static_cast(sigmas->size()) != pairs->size()) { throw std::invalid_argument( factorName + ": sigmas must have one entry per pair."); From 5e3067cb3358a2c160ab9334054f2a8a632f439b Mon Sep 17 00:00:00 2001 From: Karthik Shaji Date: Tue, 18 Aug 2026 15:29:52 -0700 Subject: [PATCH 11/11] test helper setup --- gtdynamics/gpmp2/tests/testNNCableFactor.cpp | 35 +++++++++----------- 1 file changed, 15 insertions(+), 20 deletions(-) diff --git a/gtdynamics/gpmp2/tests/testNNCableFactor.cpp b/gtdynamics/gpmp2/tests/testNNCableFactor.cpp index d2155b00..c50f768e 100644 --- a/gtdynamics/gpmp2/tests/testNNCableFactor.cpp +++ b/gtdynamics/gpmp2/tests/testNNCableFactor.cpp @@ -96,6 +96,18 @@ static std::shared_ptr makeCable( numSamples); } +// A sphere centred just off the middle cable sample at q, its grid offset by +// half a cell so samples avoid the trilinear gradient's node discontinuities. +static std::shared_ptr midCableSphereSDF( + const NNCableSpline &cable, const Vector &q) { + const Matrix pts = cable.worldPoints(q); + const Point3 center = Point3(pts.col(cable.numSamples() / 2)) + + Point3(0.3 * kCell, 0.2 * kCell, 0.0); + const Point3 origin = center - Point3::Constant(1.0 - kHalfCell); + return std::make_shared( + makeSphereSDF(center, kRadius, origin, kCell, 41, 41, 41)); +} + /* ************************ spline reconstruction ************************ */ // With zero residuals every sample lies on the chord, whose endpoints are @@ -196,15 +208,7 @@ TEST(NNCableFactor, jacobianWhenActive) { const size_t N = 8, M = 9; const auto cable = makeCable(smoothMLP(N), N, M); const Vector q = startConfig(); - - // Sphere centred just off the middle cable sample, grid offset by half a - // cell so samples avoid the trilinear gradient's node discontinuities. - const Matrix pts = cable->worldPoints(q); - const Point3 center = Point3(pts.col(M / 2)) + Point3(0.3 * kCell, - 0.2 * kCell, 0.0); - const Point3 origin = center - Point3::Constant(1.0 - kHalfCell); - auto sdf = std::make_shared( - makeSphereSDF(center, kRadius, origin, kCell, 41, 41, 41)); + const auto sdf = midCableSphereSDF(*cable, q); NNCableFactor factor(X(0), cable, sdf, 0.01, kEpsilon, 0.02); const Vector err = factor.evaluateError(q); @@ -223,11 +227,7 @@ TEST(NNCableFactor, activeAndInactiveSamples) { const Vector q = startConfig(); const Matrix pts = cable->worldPoints(q); - const Point3 center = Point3(pts.col(M / 2)) + Point3(0.3 * kCell, - 0.2 * kCell, 0.0); - const Point3 origin = center - Point3::Constant(1.0 - kHalfCell); - auto sdf = std::make_shared( - makeSphereSDF(center, kRadius, origin, kCell, 41, 41, 41)); + const auto sdf = midCableSphereSDF(*cable, q); NNCableFactor factor(X(0), cable, sdf, 0.01, kEpsilon, 0.02); const Vector err = factor.evaluateError(q); EXPECT(err(M / 2) > 0.0); @@ -262,12 +262,7 @@ TEST(NNCableFactorGP, agreesWithUnaryFactorAtTauZero) { q2(0) += 1.0; // the bridge has moved along the rail const Vector v1 = Vector::Zero(9), v2 = Vector::Zero(9); - const Matrix pts = cable->worldPoints(q1); - const Point3 center = Point3(pts.col(M / 2)) + Point3(0.3 * kCell, - 0.2 * kCell, 0.0); - const Point3 origin = center - Point3::Constant(1.0 - kHalfCell); - auto sdf = std::make_shared( - makeSphereSDF(center, kRadius, origin, kCell, 41, 41, 41)); + const auto sdf = midCableSphereSDF(*cable, q1); const double deltaT = 0.5, costSigma = 0.01; NNCableFactor unary(X(0), cable, sdf, costSigma, kEpsilon, 0.02);