From d199517e8d7079d1cd4ba3b713af78e7c747cb74 Mon Sep 17 00:00:00 2001 From: Keenon Werling Date: Fri, 6 Sep 2024 14:56:19 -0700 Subject: [PATCH 1/4] Adding native AccelerationMinimizer support --- VERSION.txt | 2 +- dart/utils/AccelerationMinimizer.cpp | 122 ++++++++++++++++++ dart/utils/AccelerationMinimizer.hpp | 45 +++++++ .../biomechanics/MarkerLabeller.cpp | 61 +++++++++ .../utils/AccelerationMinimizer.cpp | 70 ++++++++++ python/_nimblephysics/utils/module.cpp | 2 + python/nimblephysics.egg-info/PKG-INFO | 2 +- .../biomechanics/__init__.pyi | 39 ++++++ stubs/_nimblephysics-stubs/utils/__init__.pyi | 8 ++ unittests/comprehensive/CMakeLists.txt | 7 +- unittests/unit/CMakeLists.txt | 3 + unittests/unit/test_AccelerationMinimizer.cpp | 29 +++++ 12 files changed, 385 insertions(+), 5 deletions(-) create mode 100644 dart/utils/AccelerationMinimizer.cpp create mode 100644 dart/utils/AccelerationMinimizer.hpp create mode 100644 python/_nimblephysics/utils/AccelerationMinimizer.cpp create mode 100644 unittests/unit/test_AccelerationMinimizer.cpp diff --git a/VERSION.txt b/VERSION.txt index a9099fcb9..213d18328 100644 --- a/VERSION.txt +++ b/VERSION.txt @@ -1 +1 @@ -0.10.44 +0.10.45 diff --git a/dart/utils/AccelerationMinimizer.cpp b/dart/utils/AccelerationMinimizer.cpp new file mode 100644 index 000000000..8b3b53e38 --- /dev/null +++ b/dart/utils/AccelerationMinimizer.cpp @@ -0,0 +1,122 @@ +#include "AccelerationMinimizer.hpp" + +#include + +// #include +// #include +#include +// #include + +#include "dart/math/MathTypes.hpp" + +namespace dart { +namespace utils { + +/** + * Create (and pre-factor) a smoother that can remove the "jerk" from a time + * seriese of data. + * + * The alpha value will determine how much smoothing to apply. A value of 0 + * corresponds to no smoothing. + */ +AccelerationMinimizer::AccelerationMinimizer( + int timesteps, + s_t smoothingWeight, + s_t regularizationWeight, + int numIterations) + : mTimesteps(timesteps), + mSmoothingWeight(smoothingWeight), + mRegularizationWeight(regularizationWeight), + mNumIterations(numIterations), + mNumIterationsBackoff(6), + mDebugIterationBackoff(false), + mConvergenceTolerance(1e-10) +{ + Eigen::Vector3s stamp; + stamp << -1, 2, -1; + stamp *= mSmoothingWeight; + + typedef Eigen::Triplet T; + std::vector tripletList; + const int accTimesteps = mTimesteps - 2; + for (int i = 0; i < accTimesteps; i++) + { + for (int j = 0; j < 3; j++) + { + tripletList.push_back(T(i, i + j, stamp(j))); + } + } + for (int i = 0; i < mTimesteps; i++) + { + tripletList.push_back(T(accTimesteps + i, i, mRegularizationWeight)); + } + mB_sparse = Eigen::SparseMatrix(accTimesteps + mTimesteps, mTimesteps); + 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); +} + +Eigen::VectorXs AccelerationMinimizer::minimize(Eigen::VectorXs series) +{ + const int accTimesteps = mTimesteps - 2; + Eigen::VectorXs b = Eigen::VectorXs(accTimesteps + mTimesteps); + b.segment(0, accTimesteps).setZero(); + b.segment(accTimesteps, mTimesteps) = series * mRegularizationWeight; + + Eigen::VectorXs x = series; + + int iterations = mNumIterations; + for (int i = 0; i < mNumIterationsBackoff; i++) + { + Eigen::LeastSquaresConjugateGradient> solver; + solver.compute(mB_sparse); + solver.setTolerance(mConvergenceTolerance); + solver.setMaxIterations(iterations); + x = solver.solveWithGuess(b, series); + // Check convergence + if (solver.info() == Eigen::Success) + { + // Converged + break; + } + else + { + if (mDebugIterationBackoff) + { + std::cout + << "[AccelerationMinimizer] LeastSquaresConjugateGradient did " + "not converge in " + << iterations << ", with error " << solver.error() + << " so doubling iteration count and trying again." << std::endl; + } + iterations *= 2; + } + } + + return x; +} + +void AccelerationMinimizer::setDebugIterationBackoff(bool debug) +{ + mDebugIterationBackoff = debug; +} + +void AccelerationMinimizer::setNumIterationsBackoff(int numIterations) +{ + mNumIterationsBackoff = numIterations; +} + +void AccelerationMinimizer::setConvergenceTolerance(s_t tolerance) +{ + mConvergenceTolerance = tolerance; +} + +} // namespace utils +} // namespace dart \ No newline at end of file diff --git a/dart/utils/AccelerationMinimizer.hpp b/dart/utils/AccelerationMinimizer.hpp new file mode 100644 index 000000000..9d110a280 --- /dev/null +++ b/dart/utils/AccelerationMinimizer.hpp @@ -0,0 +1,45 @@ +#ifndef UTILS_ACC_MINIMIZER +#define UTILS_ACC_MINIMIZER + +#include +#include + +#include "dart/math/MathTypes.hpp" + +namespace dart { +namespace utils { + +class AccelerationMinimizer +{ +public: + AccelerationMinimizer( + int numTimesteps, + s_t smoothingWeight = 1.0, + s_t regularizationWeight = 0.01, + int numIterations = 10000); + + Eigen::VectorXs minimize(Eigen::VectorXs series); + + void setDebugIterationBackoff(bool debug); + + void setNumIterationsBackoff(int numIterations); + + void setConvergenceTolerance(s_t tolerance); + +protected: + int mTimesteps; + s_t mSmoothingWeight; + s_t mRegularizationWeight; + int mNumIterations; + int mNumIterationsBackoff; + bool mDebugIterationBackoff; + s_t mConvergenceTolerance; + Eigen::SparseMatrix mB_sparse; + Eigen::SparseQR, Eigen::NaturalOrdering> + mB_sparseSolver; +}; + +} // namespace utils +} // namespace dart + +#endif \ No newline at end of file diff --git a/python/_nimblephysics/biomechanics/MarkerLabeller.cpp b/python/_nimblephysics/biomechanics/MarkerLabeller.cpp index fd82aa6ca..ab037d235 100644 --- a/python/_nimblephysics/biomechanics/MarkerLabeller.cpp +++ b/python/_nimblephysics/biomechanics/MarkerLabeller.cpp @@ -17,6 +17,67 @@ void MarkerLabeller(py::module& m) { (void)m; ::py::class_(m, "MarkerTrace") + .def_static( + "createRawTraces", + &dart::biomechanics::MarkerTrace::createRawTraces, + ::py::arg("pointClouds"), + ::py::arg("mergeDistance") = 0.01, + ::py::arg("mergeFrames") = 5) + .def( + "appendPoint", + &dart::biomechanics::MarkerTrace::appendPoint, + ::py::arg("time"), + ::py::arg("point"), + "Add a point to the end of the marker trace") + .def( + "pointToAppendDistance", + &dart::biomechanics::MarkerTrace::pointToAppendDistance, + ::py::arg("time"), + ::py::arg("point"), + ::py::arg("extrapolate"), + "This gives the distance from the last point (or an extrapolation at " + "this timestep of the last point, of order up to 2)") + .def( + "computeBodyMarkerStats", + &dart::biomechanics::MarkerTrace::computeBodyMarkerStats, + ::py::arg("skel"), + ::py::arg("posesOverTime"), + ::py::arg("scalesOverTime"), + "Each possible combination of (trace, body) can create a marker. So " + "we can compute some summary statistics for each body we could " + "assign this trace to.") + .def( + "computeBodyMarkerLoss", + &dart::biomechanics::MarkerTrace::computeBodyMarkerLoss, + ::py::arg("bodyName"), + "Each possible combination of (trace, body) can create a marker. " + "This returns a score for a given body, for how \"good\" of a marker " + "that body would create when combined with this trace. Lower is " + "better.") + .def( + "getBestMarker", + &dart::biomechanics::MarkerTrace::getBestMarker, + "This finds the best body to pair this trace with (using the stats " + "from computeBodyMarkerStats()) and returns the best marker") + .def( + "overlap", + &dart::biomechanics::MarkerTrace::overlap, + ::py::arg("toAppend"), + "Returns true if these traces overlap in time") + .def( + "concat", + &dart::biomechanics::MarkerTrace::concat, + ::py::arg("toAppend"), + "This merges two MarkerTrace's together, to create a new trace " + "object") + .def( + "firstTimestep", + &dart::biomechanics::MarkerTrace::firstTimestep, + "This returns when this MarkerTrace begins (inclusive)") + .def( + "lastTimestep", + &dart::biomechanics::MarkerTrace::lastTimestep, + "This returns when this MarkerTrace ends (inclusive)") .def_readonly("minTime", &dart::biomechanics::MarkerTrace::mMinTime) .def_readonly("maxTime", &dart::biomechanics::MarkerTrace::mMaxTime) .def_readonly("times", &dart::biomechanics::MarkerTrace::mTimes) diff --git a/python/_nimblephysics/utils/AccelerationMinimizer.cpp b/python/_nimblephysics/utils/AccelerationMinimizer.cpp new file mode 100644 index 000000000..92931c4e0 --- /dev/null +++ b/python/_nimblephysics/utils/AccelerationMinimizer.cpp @@ -0,0 +1,70 @@ +/* + * 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 +#include +#include + +namespace py = pybind11; + +namespace dart { +namespace python { + +void AccelerationMinimizer(py::module& m) +{ + ::py::class_(m, "AccelerationMinimizer") + .def( + ::py::init(), + ::py::arg("numTimesteps"), + ::py::arg("smoothingWeight") = 1.0, + ::py::arg("regularizationWeight") = 0.01, + ::py::arg("numIterations") = 10000) + .def( + "minimize", + &dart::utils::AccelerationMinimizer::minimize, + ::py::arg("series")) + .def( + "setDebugIterationBackoff", + &dart::utils::AccelerationMinimizer::setDebugIterationBackoff, + ::py::arg("iterations")) + .def( + "setNumIterationsBackoff", + &dart::utils::AccelerationMinimizer::setNumIterationsBackoff, + ::py::arg("series")) + .def( + "setConvergenceTolerance", + &dart::utils::AccelerationMinimizer::setConvergenceTolerance, + ::py::arg("tolerance")); +} + +} // namespace python +} // namespace dart \ No newline at end of file diff --git a/python/_nimblephysics/utils/module.cpp b/python/_nimblephysics/utils/module.cpp index 543b98d4e..b734b4dac 100644 --- a/python/_nimblephysics/utils/module.cpp +++ b/python/_nimblephysics/utils/module.cpp @@ -44,6 +44,7 @@ void StringUtils(py::module& sm); void MJCFExporter(py::module& sm); void UniversalLoader(py::module& sm); void AccelerationSmoother(py::module& sm); +void AccelerationMinimizer(py::module& sm); void dart_utils(py::module& m) { @@ -56,6 +57,7 @@ void dart_utils(py::module& m) MJCFExporter(sm); UniversalLoader(sm); AccelerationSmoother(sm); + AccelerationMinimizer(sm); } } // namespace python diff --git a/python/nimblephysics.egg-info/PKG-INFO b/python/nimblephysics.egg-info/PKG-INFO index 211466be7..73ed31ff1 100644 --- a/python/nimblephysics.egg-info/PKG-INFO +++ b/python/nimblephysics.egg-info/PKG-INFO @@ -1,6 +1,6 @@ Metadata-Version: 2.1 Name: nimblephysics -Version: 0.10.42.1 +Version: 0.10.43 Summary: A differentiable fully featured physics engine Author: Keenon Werling Author-email: keenonwerling@gmail.com diff --git a/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi b/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi index ea42f7925..015de8dc4 100644 --- a/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi +++ b/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi @@ -1949,6 +1949,44 @@ class MarkerLabellerMock(MarkerLabeller): def setMockJointLocations(self, jointsOverTime: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]]) -> None: ... pass class MarkerTrace(): + def appendPoint(self, time: int, point: numpy.ndarray[numpy.float64, _Shape[3, 1]]) -> None: + """ + Add a point to the end of the marker trace + """ + def computeBodyMarkerLoss(self, bodyName: str) -> float: + """ + Each possible combination of (trace, body) can create a marker. This returns a score for a given body, for how "good" of a marker that body would create when combined with this trace. Lower is better. + """ + def computeBodyMarkerStats(self, skel: nimblephysics_libs._nimblephysics.dynamics.Skeleton, posesOverTime: typing.List[numpy.ndarray[numpy.float64, _Shape[m, 1]]], scalesOverTime: typing.List[numpy.ndarray[numpy.float64, _Shape[m, 1]]]) -> None: + """ + Each possible combination of (trace, body) can create a marker. So we can compute some summary statistics for each body we could assign this trace to. + """ + def concat(self, toAppend: MarkerTrace) -> MarkerTrace: + """ + This merges two MarkerTrace's together, to create a new trace object + """ + @staticmethod + def createRawTraces(pointClouds: typing.List[typing.List[numpy.ndarray[numpy.float64, _Shape[3, 1]]]], mergeDistance: float = 0.01, mergeFrames: int = 5) -> typing.List[MarkerTrace]: ... + def firstTimestep(self) -> int: + """ + This returns when this MarkerTrace begins (inclusive) + """ + def getBestMarker(self) -> typing.Tuple[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]: + """ + This finds the best body to pair this trace with (using the stats from computeBodyMarkerStats()) and returns the best marker + """ + def lastTimestep(self) -> int: + """ + This returns when this MarkerTrace ends (inclusive) + """ + def overlap(self, toAppend: MarkerTrace) -> bool: + """ + Returns true if these traces overlap in time + """ + def pointToAppendDistance(self, time: int, point: numpy.ndarray[numpy.float64, _Shape[3, 1]], extrapolate: bool) -> float: + """ + This gives the distance from the last point (or an extrapolation at this timestep of the last point, of order up to 2) + """ @property def bodyClosestPointDistance(self) -> typing.Dict[str, float]: """ @@ -2733,6 +2771,7 @@ class SubjectOnDiskTrial(): pass class SubjectOnDiskTrialPass(): def __init__(self) -> None: ... + def computeKinematicValues(self, skel: nimblephysics_libs._nimblephysics.dynamics.Skeleton, timestep: float, poses: numpy.ndarray[numpy.float64, _Shape[m, n]], rootHistoryLen: int = 5, rootHistoryStride: int = 1, explicitVels: numpy.ndarray[numpy.float64, _Shape[m, n]] = array([], shape=(0, 0), dtype=float64), explicitAccs: numpy.ndarray[numpy.float64, _Shape[m, n]] = array([], shape=(0, 0), dtype=float64)) -> None: ... def computeValues(self, skel: nimblephysics_libs._nimblephysics.dynamics.Skeleton, timestep: float, poses: numpy.ndarray[numpy.float64, _Shape[m, n]], footBodyNames: typing.List[str], forces: numpy.ndarray[numpy.float64, _Shape[m, n]], moments: numpy.ndarray[numpy.float64, _Shape[m, n]], cops: numpy.ndarray[numpy.float64, _Shape[m, n]], rootHistoryLen: int = 5, rootHistoryStride: int = 1) -> None: ... def computeValuesFromForcePlates(self, skel: nimblephysics_libs._nimblephysics.dynamics.Skeleton, timestep: float, poses: numpy.ndarray[numpy.float64, _Shape[m, n]], footBodyNames: typing.List[str], forcePlates: typing.List[ForcePlate], rootHistoryLen: int = 5, rootHistoryStride: int = 1, explicitVels: numpy.ndarray[numpy.float64, _Shape[m, n]] = array([], shape=(0, 0), dtype=float64), explicitAccs: numpy.ndarray[numpy.float64, _Shape[m, n]] = array([], shape=(0, 0), dtype=float64), forcePlateZeroThresholdNewtons: float = 3.0) -> None: ... def copyValuesFrom(self, other: SubjectOnDiskTrialPass) -> None: ... diff --git a/stubs/_nimblephysics-stubs/utils/__init__.pyi b/stubs/_nimblephysics-stubs/utils/__init__.pyi index 1e31e53b7..9ec654f32 100644 --- a/stubs/_nimblephysics-stubs/utils/__init__.pyi +++ b/stubs/_nimblephysics-stubs/utils/__init__.pyi @@ -8,6 +8,7 @@ import numpy _Shape = typing.Tuple[int, ...] __all__ = [ + "AccelerationMinimizer", "AccelerationSmoother", "DartLoader", "MJCFExporter", @@ -18,6 +19,13 @@ __all__ = [ ] +class AccelerationMinimizer(): + def __init__(self, numTimesteps: int, smoothingWeight: float, regularizationWeight: float, numIterations: int = 10000) -> None: ... + def minimize(self, series: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: ... + def setConvergenceTolerance(self, tolerance: float) -> None: ... + def setDebugIterationBackoff(self, iterations: bool) -> None: ... + def setNumIterationsBackoff(self, series: int) -> None: ... + pass class AccelerationSmoother(): def __init__(self, timesteps: int, smoothingWeight: float, regularizationWeight: float, useSparse: bool = True, useIterative: bool = True) -> None: ... def debugTimeSeries(self, series: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> None: ... diff --git a/unittests/comprehensive/CMakeLists.txt b/unittests/comprehensive/CMakeLists.txt index f04944638..b451ceef3 100644 --- a/unittests/comprehensive/CMakeLists.txt +++ b/unittests/comprehensive/CMakeLists.txt @@ -26,24 +26,25 @@ dart_add_test("comprehensive" test_SpringGradient) # target_link_libraries(test_Gradients dart-collision-bullet) # target_link_libraries(test_Gradients dart-collision-ode) - if(TARGET dart-optimizer-pagmo) dart_add_test("comprehensive" test_MultiObjectiveOptimization) target_link_libraries(test_MultiObjectiveOptimization dart-optimizer-pagmo) endif() dart_add_test("comprehensive" test_Raycast) + if(TARGET dart-collision-bullet) target_link_libraries(test_Raycast dart-collision-bullet) endif() if(TARGET dart-utils) - dart_add_test("comprehensive" test_Collision) target_link_libraries(test_Collision dart-utils) + if(TARGET dart-collision-bullet) target_link_libraries(test_Collision dart-collision-bullet) endif() + if(TARGET dart-collision-ode) target_link_libraries(test_Collision dart-collision-ode) endif() @@ -70,6 +71,7 @@ if(TARGET dart-utils) dart_add_test("comprehensive" test_World) target_link_libraries(test_World dart-utils) + if(TARGET dart-collision-bullet) target_link_libraries(test_World dart-collision-bullet) endif() @@ -96,7 +98,6 @@ if(TARGET dart-utils) dart_add_test("comprehensive" test_ParallelPosAndVel) target_link_libraries(test_ParallelPosAndVel dart-utils-urdf) endif() - endif() dart_format_add( diff --git a/unittests/unit/CMakeLists.txt b/unittests/unit/CMakeLists.txt index 17843b55c..3d8f7b811 100644 --- a/unittests/unit/CMakeLists.txt +++ b/unittests/unit/CMakeLists.txt @@ -45,6 +45,9 @@ if(TARGET dart-utils) dart_add_test("unit" test_AccelerationSmoother) target_link_libraries(test_AccelerationSmoother dart-utils) + dart_add_test("unit" test_AccelerationMinimizer) + target_link_libraries(test_AccelerationMinimizer dart-utils) + dart_add_test("unit" test_IKInitializer) target_link_libraries(test_IKInitializer dart-utils) diff --git a/unittests/unit/test_AccelerationMinimizer.cpp b/unittests/unit/test_AccelerationMinimizer.cpp new file mode 100644 index 000000000..b533ae70b --- /dev/null +++ b/unittests/unit/test_AccelerationMinimizer.cpp @@ -0,0 +1,29 @@ +#include +#include + +#include + +#include "dart/math/MathTypes.hpp" +#include "dart/utils/AccelerationMinimizer.hpp" + +#include "TestHelpers.hpp" + +using namespace dart; +using namespace utils; + +#define ALL_TESTS + +#ifdef ALL_TESTS +TEST(ACCEL_MINIMIZER, DOES_NOT_CRASH) +{ + int timesteps = 50; + AccelerationMinimizer minimizer(timesteps, 1.0, 1.0); + + Eigen::VectorXs series = Eigen::VectorXs::Random(timesteps); + + Eigen::VectorXs x = minimizer.minimize(series); + + std::cout << "Finished" << std::endl; + std::cout << x << std::endl; +} +#endif \ No newline at end of file From b69eb3fa8e4769c45be7243a0029b040651f4971 Mon Sep 17 00:00:00 2001 From: Keenon Werling Date: Mon, 9 Sep 2024 14:21:19 -0700 Subject: [PATCH 2/4] Adding the ability to try to regularize the initial and terminal velocity of a segment to 0, to help with ramping when GRF data --- dart/utils/AccelerationMinimizer.cpp | 23 +++++++++++++++---- dart/utils/AccelerationMinimizer.hpp | 4 ++++ .../utils/AccelerationMinimizer.cpp | 4 +++- 3 files changed, 25 insertions(+), 6 deletions(-) diff --git a/dart/utils/AccelerationMinimizer.cpp b/dart/utils/AccelerationMinimizer.cpp index 8b3b53e38..d0e0a2e25 100644 --- a/dart/utils/AccelerationMinimizer.cpp +++ b/dart/utils/AccelerationMinimizer.cpp @@ -23,10 +23,14 @@ AccelerationMinimizer::AccelerationMinimizer( int timesteps, s_t smoothingWeight, s_t regularizationWeight, + s_t startVelocityZeroWeight, + s_t endVelocityZeroWeight, int numIterations) : mTimesteps(timesteps), mSmoothingWeight(smoothingWeight), mRegularizationWeight(regularizationWeight), + mStartVelocityZeroWeight(startVelocityZeroWeight * timesteps), + mEndVelocityZeroWeight(endVelocityZeroWeight * timesteps), mNumIterations(numIterations), mNumIterationsBackoff(6), mDebugIterationBackoff(false), @@ -46,11 +50,20 @@ AccelerationMinimizer::AccelerationMinimizer( tripletList.push_back(T(i, i + j, stamp(j))); } } + // Add start velocity zero constraint + tripletList.push_back(T(accTimesteps, 0, mStartVelocityZeroWeight)); + tripletList.push_back(T(accTimesteps, 1, -mStartVelocityZeroWeight)); + // Add end velocity zero constraint + tripletList.push_back( + T(accTimesteps + 1, mTimesteps - 2, mEndVelocityZeroWeight)); + tripletList.push_back( + T(accTimesteps + 1, mTimesteps - 1, -mEndVelocityZeroWeight)); for (int i = 0; i < mTimesteps; i++) { - tripletList.push_back(T(accTimesteps + i, i, mRegularizationWeight)); + tripletList.push_back(T(accTimesteps + 2 + i, i, mRegularizationWeight)); } - mB_sparse = Eigen::SparseMatrix(accTimesteps + mTimesteps, mTimesteps); + mB_sparse + = Eigen::SparseMatrix(accTimesteps + 2 + mTimesteps, mTimesteps); mB_sparse.setFromTriplets(tripletList.begin(), tripletList.end()); mB_sparse.makeCompressed(); mB_sparseSolver.analyzePattern(mB_sparse); @@ -66,9 +79,9 @@ AccelerationMinimizer::AccelerationMinimizer( Eigen::VectorXs AccelerationMinimizer::minimize(Eigen::VectorXs series) { const int accTimesteps = mTimesteps - 2; - Eigen::VectorXs b = Eigen::VectorXs(accTimesteps + mTimesteps); - b.segment(0, accTimesteps).setZero(); - b.segment(accTimesteps, mTimesteps) = series * mRegularizationWeight; + Eigen::VectorXs b = Eigen::VectorXs(accTimesteps + 2 + mTimesteps); + b.segment(0, accTimesteps + 2).setZero(); + b.segment(accTimesteps + 2, mTimesteps) = series * mRegularizationWeight; Eigen::VectorXs x = series; diff --git a/dart/utils/AccelerationMinimizer.hpp b/dart/utils/AccelerationMinimizer.hpp index 9d110a280..a18dfde24 100644 --- a/dart/utils/AccelerationMinimizer.hpp +++ b/dart/utils/AccelerationMinimizer.hpp @@ -16,6 +16,8 @@ class AccelerationMinimizer int numTimesteps, s_t smoothingWeight = 1.0, s_t regularizationWeight = 0.01, + s_t startVelocityZeroWeight = 0.0, + s_t endVelocityZeroWeight = 0.0, int numIterations = 10000); Eigen::VectorXs minimize(Eigen::VectorXs series); @@ -30,6 +32,8 @@ class AccelerationMinimizer int mTimesteps; s_t mSmoothingWeight; s_t mRegularizationWeight; + s_t mStartVelocityZeroWeight; + s_t mEndVelocityZeroWeight; int mNumIterations; int mNumIterationsBackoff; bool mDebugIterationBackoff; diff --git a/python/_nimblephysics/utils/AccelerationMinimizer.cpp b/python/_nimblephysics/utils/AccelerationMinimizer.cpp index 92931c4e0..64d8f3834 100644 --- a/python/_nimblephysics/utils/AccelerationMinimizer.cpp +++ b/python/_nimblephysics/utils/AccelerationMinimizer.cpp @@ -43,10 +43,12 @@ void AccelerationMinimizer(py::module& m) { ::py::class_(m, "AccelerationMinimizer") .def( - ::py::init(), + ::py::init(), ::py::arg("numTimesteps"), ::py::arg("smoothingWeight") = 1.0, ::py::arg("regularizationWeight") = 0.01, + ::py::arg("startVelocityZeroWeight") = 0.0, + ::py::arg("endVelocityZeroWeight") = 0.0, ::py::arg("numIterations") = 10000) .def( "minimize", From ad75456d5d1937dc9dc7295c92024bc9098796f9 Mon Sep 17 00:00:00 2001 From: Keenon Werling Date: Mon, 9 Sep 2024 16:23:02 -0700 Subject: [PATCH 3/4] Adding an option to regularize the start and end points to 0, in addition to the velocities --- dart/utils/AccelerationMinimizer.cpp | 19 ++++++++++++++----- dart/utils/AccelerationMinimizer.hpp | 4 ++++ .../utils/AccelerationMinimizer.cpp | 4 +++- 3 files changed, 21 insertions(+), 6 deletions(-) diff --git a/dart/utils/AccelerationMinimizer.cpp b/dart/utils/AccelerationMinimizer.cpp index d0e0a2e25..d49d73271 100644 --- a/dart/utils/AccelerationMinimizer.cpp +++ b/dart/utils/AccelerationMinimizer.cpp @@ -23,12 +23,16 @@ AccelerationMinimizer::AccelerationMinimizer( int timesteps, s_t smoothingWeight, s_t regularizationWeight, + s_t startPositionZeroWeight, + s_t endPositionZeroWeight, s_t startVelocityZeroWeight, s_t endVelocityZeroWeight, int numIterations) : mTimesteps(timesteps), mSmoothingWeight(smoothingWeight), mRegularizationWeight(regularizationWeight), + mStartPositionZeroWeight(startPositionZeroWeight * timesteps), + mEndPositionZeroWeight(endPositionZeroWeight * timesteps), mStartVelocityZeroWeight(startVelocityZeroWeight * timesteps), mEndVelocityZeroWeight(endVelocityZeroWeight * timesteps), mNumIterations(numIterations), @@ -58,12 +62,17 @@ AccelerationMinimizer::AccelerationMinimizer( T(accTimesteps + 1, mTimesteps - 2, mEndVelocityZeroWeight)); tripletList.push_back( T(accTimesteps + 1, mTimesteps - 1, -mEndVelocityZeroWeight)); + // Add start position zero constraint + tripletList.push_back(T(accTimesteps + 2, 0, mStartPositionZeroWeight)); + // Add end position zero constraint + tripletList.push_back( + T(accTimesteps + 3, mTimesteps - 1, mEndPositionZeroWeight)); for (int i = 0; i < mTimesteps; i++) { - tripletList.push_back(T(accTimesteps + 2 + i, i, mRegularizationWeight)); + tripletList.push_back(T(accTimesteps + 4 + i, i, mRegularizationWeight)); } mB_sparse - = Eigen::SparseMatrix(accTimesteps + 2 + mTimesteps, mTimesteps); + = Eigen::SparseMatrix(accTimesteps + 4 + mTimesteps, mTimesteps); mB_sparse.setFromTriplets(tripletList.begin(), tripletList.end()); mB_sparse.makeCompressed(); mB_sparseSolver.analyzePattern(mB_sparse); @@ -79,9 +88,9 @@ AccelerationMinimizer::AccelerationMinimizer( Eigen::VectorXs AccelerationMinimizer::minimize(Eigen::VectorXs series) { const int accTimesteps = mTimesteps - 2; - Eigen::VectorXs b = Eigen::VectorXs(accTimesteps + 2 + mTimesteps); - b.segment(0, accTimesteps + 2).setZero(); - b.segment(accTimesteps + 2, mTimesteps) = series * mRegularizationWeight; + Eigen::VectorXs b = Eigen::VectorXs(accTimesteps + 4 + mTimesteps); + b.segment(0, accTimesteps + 4).setZero(); + b.segment(accTimesteps + 4, mTimesteps) = series * mRegularizationWeight; Eigen::VectorXs x = series; diff --git a/dart/utils/AccelerationMinimizer.hpp b/dart/utils/AccelerationMinimizer.hpp index a18dfde24..c70b978c3 100644 --- a/dart/utils/AccelerationMinimizer.hpp +++ b/dart/utils/AccelerationMinimizer.hpp @@ -16,6 +16,8 @@ class AccelerationMinimizer int numTimesteps, s_t smoothingWeight = 1.0, s_t regularizationWeight = 0.01, + s_t startPositionZeroWeight = 0.0, + s_t endPositionZeroWeight = 0.0, s_t startVelocityZeroWeight = 0.0, s_t endVelocityZeroWeight = 0.0, int numIterations = 10000); @@ -32,6 +34,8 @@ class AccelerationMinimizer int mTimesteps; s_t mSmoothingWeight; s_t mRegularizationWeight; + s_t mStartPositionZeroWeight; + s_t mEndPositionZeroWeight; s_t mStartVelocityZeroWeight; s_t mEndVelocityZeroWeight; int mNumIterations; diff --git a/python/_nimblephysics/utils/AccelerationMinimizer.cpp b/python/_nimblephysics/utils/AccelerationMinimizer.cpp index 64d8f3834..7aa1ab71c 100644 --- a/python/_nimblephysics/utils/AccelerationMinimizer.cpp +++ b/python/_nimblephysics/utils/AccelerationMinimizer.cpp @@ -43,10 +43,12 @@ void AccelerationMinimizer(py::module& m) { ::py::class_(m, "AccelerationMinimizer") .def( - ::py::init(), + ::py::init(), ::py::arg("numTimesteps"), ::py::arg("smoothingWeight") = 1.0, ::py::arg("regularizationWeight") = 0.01, + ::py::arg("startPositionZeroWeight") = 0.0, + ::py::arg("endPositionZeroWeight") = 0.0, ::py::arg("startVelocityZeroWeight") = 0.0, ::py::arg("endVelocityZeroWeight") = 0.0, ::py::arg("numIterations") = 10000) From 8c28aa5bcc2b3cf9ca189dfb83597af8cb0b0a76 Mon Sep 17 00:00:00 2001 From: Keenon Werling Date: Wed, 11 Sep 2024 15:05:05 -0700 Subject: [PATCH 4/4] Minor fixes --- dart/utils/AccelerationMinimizer.cpp | 2 +- python/nimblephysics.egg-info/PKG-INFO | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/utils/AccelerationMinimizer.cpp b/dart/utils/AccelerationMinimizer.cpp index d49d73271..57f08d055 100644 --- a/dart/utils/AccelerationMinimizer.cpp +++ b/dart/utils/AccelerationMinimizer.cpp @@ -14,7 +14,7 @@ namespace utils { /** * Create (and pre-factor) a smoother that can remove the "jerk" from a time - * seriese of data. + * series of data. * * The alpha value will determine how much smoothing to apply. A value of 0 * corresponds to no smoothing. diff --git a/python/nimblephysics.egg-info/PKG-INFO b/python/nimblephysics.egg-info/PKG-INFO index 73ed31ff1..fca4fcae1 100644 --- a/python/nimblephysics.egg-info/PKG-INFO +++ b/python/nimblephysics.egg-info/PKG-INFO @@ -1,6 +1,6 @@ Metadata-Version: 2.1 Name: nimblephysics -Version: 0.10.43 +Version: 0.10.45 Summary: A differentiable fully featured physics engine Author: Keenon Werling Author-email: keenonwerling@gmail.com