Adding support for doing an acceleration minimizing smoother with the…

… option to track accelerations at certain points

dart/utils/AccelerationTrackAndMinimize.cpp

@@ -0,0 +1,190 @@
+#include "AccelerationTrackAndMinimize.hpp"
+
+#include <iostream>
+
+// #include <Eigen/Core>
+// #include <Eigen/Dense>
+#include <Eigen/IterativeLinearSolvers>
+// #include <unsupported/Eigen/IterativeSolvers>
+
+#include "dart/math/MathTypes.hpp"
+
+namespace dart {
+namespace utils {
+
+AccelerationTrackAndMinimize::AccelerationTrackAndMinimize(
+    int numTimesteps,
+    std::vector<bool> trackAccelerationAtTimesteps,
+    s_t zeroUnobservedAccWeight,
+    s_t trackObservedAccWeight,
+    s_t regularizationWeight,
+    s_t dt,
+    int numIterations)
+  : mTimesteps(numTimesteps),
+    mDt(dt),
+    mTrackAccelerationAtTimesteps(trackAccelerationAtTimesteps),
+    mTrackObservedAccWeight(trackObservedAccWeight),
+    mZeroUnobservedAccWeight(zeroUnobservedAccWeight),
+    mRegularizationWeight(regularizationWeight),
+    mNumIterations(numIterations),
+    mNumIterationsBackoff(6),
+    mDebugIterationBackoff(false),
+    mConvergenceTolerance(1e-10)
+{
+  if (mTimesteps < 3)
+  {
+    std::cout << "AccelerationTrackAndMinimize requires at least 3 timesteps"
+              << std::endl;
+    return;
+  }
+  if (mTrackAccelerationAtTimesteps.size() != mTimesteps)
+  {
+    std::cout << "trackAccelerationAtTimesteps.size() != mTimesteps"
+              << std::endl;
+    return;
+  }
+
+  Eigen::Vector3s stamp;
+  stamp << 1, -2, 1;
+  stamp *= 1.0 / (mDt * mDt);
+
+  typedef Eigen::Triplet<s_t> T;
+  std::vector<T> tripletList;
+  const int accTimesteps = mTimesteps - 2;
+  for (int i = 0; i < accTimesteps; i++)
+  {
+    for (int j = 0; j < 3; j++)
+    {
+      if (trackAccelerationAtTimesteps[i + 1])
+      {
+        tripletList.push_back(T(i, i + j, stamp(j) * mTrackObservedAccWeight));
+      }
+      else
+      {
+        tripletList.push_back(T(i, i + j, stamp(j) * mZeroUnobservedAccWeight));
+      }
+    }
+  }
+  for (int i = 0; i < mTimesteps; i++)
+  {
+    tripletList.push_back(T(accTimesteps + i, i, mRegularizationWeight));
+  }
+  // Add one extra input (to solve for) which applies a linear offset to the
+  // tracked accelerations
+  for (int i = 0; i < accTimesteps; i++)
+  {
+    if (trackAccelerationAtTimesteps[i + 1])
+    {
+      tripletList.push_back(T(i, mTimesteps, 1.0));
+    }
+  }
+  mB_sparse
+      = Eigen::SparseMatrix<s_t>(accTimesteps + mTimesteps, mTimesteps + 1);
+  mB_sparse.setFromTriplets(tripletList.begin(), tripletList.end());
+  mB_sparse.makeCompressed();
+  mB_sparseSolver.analyzePattern(mB_sparse);
+  mB_sparseSolver.factorize(mB_sparse);
+  if (mB_sparseSolver.info() != Eigen::Success)
+  {
+    std::cout << "mB_sparseSolver.factorize(mB_sparse) error: "
+              << mB_sparseSolver.lastErrorMessage() << std::endl;
+  }
+  assert(mB_sparseSolver.info() == Eigen::Success);
+}
+
+AccelerationTrackingResult AccelerationTrackAndMinimize::minimize(
+    Eigen::VectorXs series, Eigen::VectorXs trackAcc)
+{
+  AccelerationTrackingResult result;
+  result.series = series;
+  result.accelerationOffset = 0.0;
+  if (series.size() != mTimesteps)
+  {
+    std::cout << "series.size() != mTimesteps" << std::endl;
+    return result;
+  }
+  if (trackAcc.size() != mTimesteps)
+  {
+    std::cout << "trackAcc.size() != mTimesteps" << std::endl;
+    return result;
+  }
+
+  const int accTimesteps = mTimesteps - 2;
+  Eigen::VectorXs b = Eigen::VectorXs(accTimesteps + mTimesteps);
+  b.segment(0, accTimesteps) = trackAcc.segment(1, accTimesteps);
+  for (int i = 0; i < accTimesteps; i++)
+  {
+    if (!mTrackAccelerationAtTimesteps[i + 1])
+    {
+      if (b(i) != 0)
+      {
+        std::cout << "Warning: trackAcc[" << i << "] is non-zero, but we're "
+                  << "not tracking acceleration at this timestep. Setting it "
+                  << "to zero. Check how you are formatting your input array!"
+                  << std::endl;
+      }
+      b(i) = 0;
+    }
+    else
+    {
+      b(i) *= mTrackObservedAccWeight;
+    }
+  }
+  b.segment(accTimesteps, mTimesteps) = series * mRegularizationWeight;
+
+  Eigen::VectorXs paddedSeries = Eigen::VectorXs::Zero(mTimesteps + 1);
+  paddedSeries.head(mTimesteps) = series;
+
+  int iterations = mNumIterations;
+  for (int i = 0; i < mNumIterationsBackoff; i++)
+  {
+    Eigen::LeastSquaresConjugateGradient<Eigen::SparseMatrix<s_t>> solver;
+    solver.compute(mB_sparse);
+    solver.setTolerance(mConvergenceTolerance);
+    solver.setMaxIterations(iterations);
+    paddedSeries = solver.solveWithGuess(b, paddedSeries);
+    // Check convergence
+    if (solver.info() == Eigen::Success)
+    {
+      // Converged
+      break;
+    }
+    else
+    {
+      if (mDebugIterationBackoff)
+      {
+        std::cout << "[AccelerationTrackAndMinimize] "
+                     "LeastSquaresConjugateGradient did "
+                     "not converge in "
+                  << iterations << ", with error " << solver.error()
+                  << " so doubling iteration count and trying again."
+                  << std::endl;
+      }
+      iterations *= 2;
+    }
+  }
+
+  result.series = paddedSeries.head(mTimesteps);
+  result.accelerationOffset
+      = paddedSeries(mTimesteps) / mTrackObservedAccWeight;
+
+  return result;
+}
+
+void AccelerationTrackAndMinimize::setDebugIterationBackoff(bool debug)
+{
+  mDebugIterationBackoff = debug;
+}
+
+void AccelerationTrackAndMinimize::setNumIterationsBackoff(int numIterations)
+{
+  mNumIterationsBackoff = numIterations;
+}
+
+void AccelerationTrackAndMinimize::setConvergenceTolerance(s_t tolerance)
+{
+  mConvergenceTolerance = tolerance;
+}
+
+} // namespace utils
+} // namespace dart

dart/utils/AccelerationTrackAndMinimize.hpp

@@ -0,0 +1,59 @@
+#ifndef UTILS_ACC_TRACK_AND_MINIMIZE
+#define UTILS_ACC_TRACK_AND_MINIMIZE
+
+#include <Eigen/Sparse>
+#include <Eigen/SparseQR>
+
+#include "dart/math/MathTypes.hpp"
+
+namespace dart {
+namespace utils {
+
+typedef struct AccelerationTrackingResult
+{
+  Eigen::VectorXs series;
+  s_t accelerationOffset;
+} AccelerationTrackingResult;
+
+class AccelerationTrackAndMinimize
+{
+public:
+  AccelerationTrackAndMinimize(
+      int numTimesteps,
+      std::vector<bool> trackAccelerationAtTimesteps,
+      s_t zeroUnobservedAccWeight = 1.0,
+      s_t trackObservedAccWeight = 1.0,
+      s_t regularizationWeight = 0.01,
+      s_t dt = 1.0,
+      int numIterations = 10000);
+
+  AccelerationTrackingResult minimize(
+      Eigen::VectorXs series, Eigen::VectorXs trackAcc);
+
+  void setDebugIterationBackoff(bool debug);
+
+  void setNumIterationsBackoff(int numIterations);
+
+  void setConvergenceTolerance(s_t tolerance);
+
+protected:
+  int mTimesteps;
+  s_t mDt;
+  s_t mZeroUnobservedAccWeight;
+  s_t mTrackObservedAccWeight;
+  s_t mRegularizationWeight;
+  std::vector<bool> mTrackAccelerationAtTimesteps;
+
+  int mNumIterations;
+  int mNumIterationsBackoff;
+  bool mDebugIterationBackoff;
+  s_t mConvergenceTolerance;
+  Eigen::SparseMatrix<s_t> mB_sparse;
+  Eigen::SparseQR<Eigen::SparseMatrix<s_t>, Eigen::NaturalOrdering<int>>
+      mB_sparseSolver;
+};
+
+} // namespace utils
+} // namespace dart
+
+#endif

python/_nimblephysics/utils/AccelerationTrackAndMinimize.cpp

@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2011-2019, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/master/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dart/utils/AccelerationTrackAndMinimize.hpp>
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+namespace py = pybind11;
+
+namespace dart {
+namespace python {
+
+void AccelerationTrackAndMinimize(py::module& m)
+{
+  py::class_<dart::utils::AccelerationTrackingResult>(
+      m, "AccelerationTrackingResult")
+      .def_readwrite("series", &dart::utils::AccelerationTrackingResult::series)
+      .def_readwrite(
+          "accelerationOffset",
+          &dart::utils::AccelerationTrackingResult::accelerationOffset);
+
+  ::py::class_<dart::utils::AccelerationTrackAndMinimize>(
+      m, "AccelerationTrackAndMinimize")
+      .def(
+          ::py::init<int, std::vector<bool>, s_t, s_t, s_t, s_t, int>(),
+          ::py::arg("numTimesteps"),
+          ::py::arg("trackAccelerationAtTimesteps"),
+          ::py::arg("zeroUnobservedAccWeight") = 1.0,
+          ::py::arg("trackObservedAccWeight") = 1.0,
+          ::py::arg("regularizationWeight") = 0.01,
+          ::py::arg("dt") = 1.0,
+          ::py::arg("numIterations") = 10000)
+      .def(
+          "minimize",
+          &dart::utils::AccelerationTrackAndMinimize::minimize,
+          ::py::arg("series"),
+          ::py::arg("trackAcc"))
+      .def(
+          "setDebugIterationBackoff",
+          &dart::utils::AccelerationTrackAndMinimize::setDebugIterationBackoff,
+          ::py::arg("iterations"))
+      .def(
+          "setNumIterationsBackoff",
+          &dart::utils::AccelerationTrackAndMinimize::setNumIterationsBackoff,
+          ::py::arg("series"))
+      .def(
+          "setConvergenceTolerance",
+          &dart::utils::AccelerationTrackAndMinimize::setConvergenceTolerance,
+          ::py::arg("tolerance"));
+}
+
+} // namespace python
+} // namespace dart

python/_nimblephysics/utils/module.cpp

@@ -45,6 +45,7 @@ void MJCFExporter(py::module& sm);
 void UniversalLoader(py::module& sm);
 void AccelerationSmoother(py::module& sm);
 void AccelerationMinimizer(py::module& sm);
+void AccelerationTrackAndMinimize(py::module& sm);
 
 void dart_utils(py::module& m)
 {
@@ -58,6 +59,7 @@ void dart_utils(py::module& m)
   UniversalLoader(sm);
   AccelerationSmoother(sm);
   AccelerationMinimizer(sm);
+  AccelerationTrackAndMinimize(sm);
 }
 
 } // namespace python

unittests/unit/CMakeLists.txt

@@ -48,6 +48,9 @@ if(TARGET dart-utils)
   dart_add_test("unit" test_AccelerationMinimizer)
   target_link_libraries(test_AccelerationMinimizer dart-utils)
 
+  dart_add_test("unit" test_AccelerationTrackAndMinimize)
+  target_link_libraries(test_AccelerationTrackAndMinimize dart-utils)
+
   dart_add_test("unit" test_IKInitializer)
   target_link_libraries(test_IKInitializer dart-utils)