From 3af4db7075b56bda3ab38c0c353dbd9cab6b58b3 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Mon, 20 Dec 2021 22:17:50 -0800 Subject: [PATCH 01/14] draft ignoreFrictionConstraints --- dart/constraint/ConstraintSolver.cpp | 10 +++++----- dart/constraint/ContactConstraint.cpp | 28 +++++++++++++++------------ dart/simulation/World.cpp | 14 ++++++++++---- 3 files changed, 31 insertions(+), 21 deletions(-) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 2af247094..d7a71889e 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -373,9 +373,9 @@ LCPSolver* ConstraintSolver::getLCPSolver() const } //============================================================================== -void ConstraintSolver::solve() +void ConstraintSolver::runEnforceContactAndJointAndCustomConstraintsFn(bool frictionless) { - mEnforceContactAndJointAndCustomConstraintsFn(); + mEnforceContactAndJointAndCustomConstraintsFn(frictionless); } //============================================================================== @@ -393,7 +393,7 @@ void ConstraintSolver::replaceEnforceContactAndJointAndCustomConstraintsFn( } //============================================================================== -void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithLcp() +void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithLcp(bool frictionless) { for (auto& skeleton : mSkeletons) { @@ -404,7 +404,7 @@ void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithLcp() } // Update constraints and collect active constraints - updateConstraints(); + updateConstraints(frictionless); // Build constrained groups buildConstrainedGroups(); @@ -539,7 +539,7 @@ bool ConstraintSolver::checkAndAddConstraint( } //============================================================================== -void ConstraintSolver::updateConstraints() +void ConstraintSolver::updateConstraints(bool frictionless) { // Clear previous active constraint list mActiveConstraints.clear(); diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 8a571473e..9f5b8dff3 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -66,7 +66,8 @@ s_t ContactConstraint::mConstraintForceMixing = DART_CFM; ContactConstraint::ContactConstraint( collision::Contact& contact, s_t timeStep, - bool penetrationCorrectionEnabled) + bool penetrationCorrectionEnabled, + bool ignoreFrictionConstraints) : ConstraintBase(), mTimeStep(timeStep), mBodyNodeA(const_cast( @@ -102,21 +103,24 @@ ContactConstraint::ContactConstraint( //---------------------------------------------- // Friction //---------------------------------------------- - // TODO(JS): Assume the frictional coefficient can be changed during - // simulation steps. - // Update mFrictionalCoff - mFrictionCoeff = std::min( - mBodyNodeA->getFrictionCoeff(), mBodyNodeB->getFrictionCoeff()); - if (mFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD) + if (ignoreFrictionConstraints) { - mIsFrictionOn = true; - - // Update frictional direction - updateFirstFrictionalDirection(); + mIsFrictionOn = false; } else { - mIsFrictionOn = false; + // TODO(JS): Assume the frictional coefficient can be changed during + // simulation steps. + // Update mFrictionalCoff + mFrictionCoeff = std::min( + mBodyNodeA->getFrictionCoeff(), mBodyNodeB->getFrictionCoeff()); + if (mFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD) + { + mIsFrictionOn = true; + + // Update frictional direction + updateFirstFrictionalDirection(); + } } assert(mBodyNodeA->getSkeleton()); diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 0f1456686..f87a1f494 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -252,18 +252,24 @@ void World::step(bool _resetCommand) } //============================================================================== -void World::runConstraintEngine(bool _resetCommand) +void World::runConstraintEngine(bool _resetCommand, bool frictionless) { - mConstraintEngineFn(_resetCommand); + mConstraintEngineFn(_resetCommand, frictionless); } //============================================================================== -void World::runLcpConstraintEngine(bool _resetCommand) +void World::runLcpConstraintEngine(bool _resetCommand, bool frictionless) { - mConstraintSolver->solve(); + mConstraintSolver->solve(frictionless); integrateVelocitiesFromImpulses(_resetCommand); } +//============================================================================== +void World::runFrictionlessLcpConstraintEngine(bool _resetCommand) +{ + runLcpConstraintEngine(_resetCommand, true); +} + //============================================================================== void World::replaceConstraintEngineFn(const constraintEngineFnType& engineFn) { From 145cbe46bfc2ab748574d74407a7e31892370757 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Mon, 20 Dec 2021 22:34:19 -0800 Subject: [PATCH 02/14] fix compile errors --- dart/constraint/ConstraintSolver.cpp | 16 ++++++++-------- dart/constraint/ConstraintSolver.hpp | 8 ++++---- dart/constraint/ContactConstraint.hpp | 3 ++- dart/simulation/World.cpp | 12 ++++++------ dart/simulation/World.hpp | 9 ++++++--- python/new_examples/custom_constraint_engine.py | 12 +++++++++++- 6 files changed, 37 insertions(+), 23 deletions(-) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index d7a71889e..65e502bfb 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -88,8 +88,8 @@ ConstraintSolver::ConstraintSolver() // gradients mContactClippingDepth( 0.03), // Default to clipping only after fairly deep penetration - mEnforceContactAndJointAndCustomConstraintsFn([this]() { - return enforceContactAndJointAndCustomConstraintsWithLcp(); + mEnforceContactAndJointAndCustomConstraintsFn([this](bool ignoreFrictionConstraints) { + return enforceContactAndJointAndCustomConstraintsWithLcp(ignoreFrictionConstraints); }) { } @@ -373,9 +373,9 @@ LCPSolver* ConstraintSolver::getLCPSolver() const } //============================================================================== -void ConstraintSolver::runEnforceContactAndJointAndCustomConstraintsFn(bool frictionless) +void ConstraintSolver::solve(bool ignoreFrictionConstraints) { - mEnforceContactAndJointAndCustomConstraintsFn(frictionless); + mEnforceContactAndJointAndCustomConstraintsFn(ignoreFrictionConstraints); } //============================================================================== @@ -393,7 +393,7 @@ void ConstraintSolver::replaceEnforceContactAndJointAndCustomConstraintsFn( } //============================================================================== -void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithLcp(bool frictionless) +void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithLcp(bool ignoreFrictionConstraints) { for (auto& skeleton : mSkeletons) { @@ -404,7 +404,7 @@ void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithLcp(bool fr } // Update constraints and collect active constraints - updateConstraints(frictionless); + updateConstraints(ignoreFrictionConstraints); // Build constrained groups buildConstrainedGroups(); @@ -539,7 +539,7 @@ bool ConstraintSolver::checkAndAddConstraint( } //============================================================================== -void ConstraintSolver::updateConstraints(bool frictionless) +void ConstraintSolver::updateConstraints(bool ignoreFrictionConstraints) { // Clear previous active constraint list mActiveConstraints.clear(); @@ -608,7 +608,7 @@ void ConstraintSolver::updateConstraints(bool frictionless) else { mContactConstraints.push_back(std::make_shared( - contact, mTimeStep, mPenetrationCorrectionEnabled)); + contact, mTimeStep, mPenetrationCorrectionEnabled, ignoreFrictionConstraints)); } } diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp index 92b48171f..8dfd83fd4 100644 --- a/dart/constraint/ConstraintSolver.hpp +++ b/dart/constraint/ConstraintSolver.hpp @@ -63,7 +63,7 @@ class ConstraintSolver { public: using enforceContactAndJointAndCustomConstraintsFnType - = std::function; + = std::function; /// Constructor /// @@ -187,17 +187,17 @@ class ConstraintSolver LCPSolver* getLCPSolver() const; /// Solve constraint impulses and apply them to the skeletons - void solve(); + void solve(bool ignoreFrictionConstraints = false); /// Enforce contact, joint, and custom constraints using LCP. - void enforceContactAndJointAndCustomConstraintsWithLcp(); + void enforceContactAndJointAndCustomConstraintsWithLcp(bool ignoreFrictionConstraints = false); /// Replace the default solve callback function. void replaceEnforceContactAndJointAndCustomConstraintsFn( const enforceContactAndJointAndCustomConstraintsFnType& f); /// Update constraints - void updateConstraints(); + void updateConstraints(bool ignoreFrictionConstraints = false); /// Build constrained groupsContact void buildConstrainedGroups(); diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp index 860bd374c..2f11f65cc 100644 --- a/dart/constraint/ContactConstraint.hpp +++ b/dart/constraint/ContactConstraint.hpp @@ -54,7 +54,8 @@ class ContactConstraint : public ConstraintBase ContactConstraint( collision::Contact& contact, s_t timeStep, - bool penetrationCorrectionEnabled); + bool penetrationCorrectionEnabled, + bool ignoreFrictionConstraints); /// Destructor ~ContactConstraint() override = default; diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index f87a1f494..17188d5b9 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -88,8 +88,8 @@ World::World(const std::string& _name) mWrtMass(std::make_shared()), mUseFDOverride(false), mSlowDebugResultsAgainstFD(false), - mConstraintEngineFn([this](bool _resetCommand) { - return runLcpConstraintEngine(_resetCommand); + mConstraintEngineFn([this](bool _resetCommand, bool ignoreFrictionConstraints) { + return runLcpConstraintEngine(_resetCommand, ignoreFrictionConstraints); }) { mIndices.push_back(0); @@ -252,15 +252,15 @@ void World::step(bool _resetCommand) } //============================================================================== -void World::runConstraintEngine(bool _resetCommand, bool frictionless) +void World::runConstraintEngine(bool _resetCommand, bool ignoreFrictionConstraints) { - mConstraintEngineFn(_resetCommand, frictionless); + mConstraintEngineFn(_resetCommand, ignoreFrictionConstraints); } //============================================================================== -void World::runLcpConstraintEngine(bool _resetCommand, bool frictionless) +void World::runLcpConstraintEngine(bool _resetCommand, bool ignoreFrictionConstraints) { - mConstraintSolver->solve(frictionless); + mConstraintSolver->solve(ignoreFrictionConstraints); integrateVelocitiesFromImpulses(_resetCommand); } diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index bb68caba5..371ee699d 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -93,7 +93,7 @@ class World : public virtual common::Subject, using NameChangedSignal = common::Signal; - using constraintEngineFnType = std::function; + using constraintEngineFnType = std::function; /// Creates World as shared_ptr template @@ -481,10 +481,13 @@ class World : public virtual common::Subject, /// Run the constraint engine which solves for constraint impulses and /// integrates velocities given these constraint impulses. - void runConstraintEngine(bool _resetCommand); + void runConstraintEngine(bool _resetCommand, bool ignoreFrictionConstraints = false); /// The default constraint engine which runs an LCP. - void runLcpConstraintEngine(bool _resetCommand); + void runLcpConstraintEngine(bool _resetCommand, bool ignoreFrictionConstraints = false); + + /// A constraint engine that runs a frictionless LCP. + void runFrictionlessLcpConstraintEngine(bool _resetCommand); /// Replace the default constraint engine with a custom one. void replaceConstraintEngineFn(const constraintEngineFnType& engineFn); diff --git a/python/new_examples/custom_constraint_engine.py b/python/new_examples/custom_constraint_engine.py index ef48364f1..1409b4ac9 100644 --- a/python/new_examples/custom_constraint_engine.py +++ b/python/new_examples/custom_constraint_engine.py @@ -3,7 +3,7 @@ import torch -def runDummyConstraintEngine(reset_command): +def runDummyConstraintEngine(reset): pass @@ -31,10 +31,20 @@ def main(): action = torch.zeros((world.getNumDofs())) solver = world.getConstraintSolver() + def full_frictionless_lcp_engine(reset_command): + world.runLcpConstraintEngine(reset_command) + world.runFrictionlessLcpConstraintEngine(reset_command) + + def frictionless_full_lcp_engine(reset_command): + world.runFrictionlessLcpConstraintEngine(reset_command) + world.runLcpConstraintEngine(reset_command) + engines = [ None, # Use default (don't replace) runDummyConstraintEngine, # Replace with dummy engine world.runLcpConstraintEngine, # Replace with LCP engine (same as default) + full_frictionless_lcp_engine, # LCP + FrictionlessLCP + frictionless_full_lcp_engine, # FrictionlessLCP + LCP ] for engine in engines: if engine is not None: From f978001035c0710fb6139ba1f455ca5575465cdc Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Mon, 20 Dec 2021 22:37:55 -0800 Subject: [PATCH 03/14] rename solve to runEnforceContactAndJointAndCustomConstraintsFn --- dart/constraint/ConstraintSolver.cpp | 2 +- dart/constraint/ConstraintSolver.hpp | 2 +- dart/simulation/World.cpp | 2 +- .../constraint/ConstraintSolver.cpp | 4 ++-- .../new_examples/custom_constraint_engine.py | 18 ------------------ unittests/GradientTestUtils.hpp | 2 +- unittests/comprehensive/test_Gradients.cpp | 2 +- 7 files changed, 7 insertions(+), 25 deletions(-) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 65e502bfb..6aedd39bf 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -373,7 +373,7 @@ LCPSolver* ConstraintSolver::getLCPSolver() const } //============================================================================== -void ConstraintSolver::solve(bool ignoreFrictionConstraints) +void ConstraintSolver::runEnforceContactAndJointAndCustomConstraintsFn(bool ignoreFrictionConstraints) { mEnforceContactAndJointAndCustomConstraintsFn(ignoreFrictionConstraints); } diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp index 8dfd83fd4..c90faadae 100644 --- a/dart/constraint/ConstraintSolver.hpp +++ b/dart/constraint/ConstraintSolver.hpp @@ -187,7 +187,7 @@ class ConstraintSolver LCPSolver* getLCPSolver() const; /// Solve constraint impulses and apply them to the skeletons - void solve(bool ignoreFrictionConstraints = false); + void runEnforceContactAndJointAndCustomConstraintsFn(bool ignoreFrictionConstraints = false); /// Enforce contact, joint, and custom constraints using LCP. void enforceContactAndJointAndCustomConstraintsWithLcp(bool ignoreFrictionConstraints = false); diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 17188d5b9..0e37720d7 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -260,7 +260,7 @@ void World::runConstraintEngine(bool _resetCommand, bool ignoreFrictionConstrain //============================================================================== void World::runLcpConstraintEngine(bool _resetCommand, bool ignoreFrictionConstraints) { - mConstraintSolver->solve(ignoreFrictionConstraints); + mConstraintSolver->runEnforceContactAndJointAndCustomConstraintsFn(ignoreFrictionConstraints); integrateVelocitiesFromImpulses(_resetCommand); } diff --git a/python/_nimblephysics/constraint/ConstraintSolver.cpp b/python/_nimblephysics/constraint/ConstraintSolver.cpp index 913187311..d8c11f87a 100644 --- a/python/_nimblephysics/constraint/ConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/ConstraintSolver.cpp @@ -198,8 +198,8 @@ void ConstraintSolver(py::module& m) self->solveConstrainedGroups(); }) .def( - "solve", - +[](dart::constraint::ConstraintSolver* self) { self->solve(); }) + "runEnforceContactAndJointAndCustomConstraintsFn", + +[](dart::constraint::ConstraintSolver* self) { self->runEnforceContactAndJointAndCustomConstraintsFn(); }) .def( "enforceContactAndJointAndCustomConstraintsWithLcp", +[](dart::constraint::ConstraintSolver* self) { diff --git a/python/new_examples/custom_constraint_engine.py b/python/new_examples/custom_constraint_engine.py index 1409b4ac9..58f116726 100644 --- a/python/new_examples/custom_constraint_engine.py +++ b/python/new_examples/custom_constraint_engine.py @@ -7,24 +7,6 @@ def runDummyConstraintEngine(reset): pass -# def frictionless_lcp_callback(): -# # Backup and remove friction. -# friction_coefs = [] -# bodies = [] -# for i in range(world.getNumBodyNodes()): -# body = world.getBodyNodeIndex(i) -# bodies.append(body) -# friction_coefs.append(body.getFrictionCoeff()) -# body.setFrictionCoeff(0.0) - -# # Frictionless LCP -# lcp_callback() - -# # Restore friction. -# for friction_coef, body in zip(friction_coefs, bodies): -# body.setFrictionCoeff(friction_coef) - - def main(): world = nimble.loadWorld("../../data/skel/test/colliding_cube.skel") state = torch.tensor(world.getState()) diff --git a/unittests/GradientTestUtils.hpp b/unittests/GradientTestUtils.hpp index 137edbb04..1d3f27002 100644 --- a/unittests/GradientTestUtils.hpp +++ b/unittests/GradientTestUtils.hpp @@ -144,7 +144,7 @@ bool verifyClassicClampingConstraintMatrix( // Populate the constraint matrices, without taking a time step or integrating // velocities world->getConstraintSolver()->setGradientEnabled(true); - world->getConstraintSolver()->solve(); + world->getConstraintSolver()->runEnforceContactAndJointAndCustomConstraintsFn(); for (std::size_t i = 0; i < world->getNumSkeletons(); i++) { diff --git a/unittests/comprehensive/test_Gradients.cpp b/unittests/comprehensive/test_Gradients.cpp index 97484ea1e..83976f2b9 100644 --- a/unittests/comprehensive/test_Gradients.cpp +++ b/unittests/comprehensive/test_Gradients.cpp @@ -564,7 +564,7 @@ void testTwoBlocks( // Test the classic formulation world->getConstraintSolver()->setGradientEnabled(true); - world->getConstraintSolver()->solve(); + world->getConstraintSolver()->runEnforceContactAndJointAndCustomConstraintsFn(); EXPECT_TRUE(verifyVelGradients(world, worldVel)); EXPECT_TRUE(verifyAnalyticalBackprop(world)); From f9f921d43f00aafcda1da13cd5fe401a519ab2ad Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Mon, 20 Dec 2021 22:46:51 -0800 Subject: [PATCH 04/14] update python bindings --- .../constraint/ConstraintSolver.cpp | 19 +++++++++++------- python/_nimblephysics/simulation/World.cpp | 20 ++++++++++++++----- .../new_examples/custom_constraint_engine.py | 14 ++++++------- 3 files changed, 34 insertions(+), 19 deletions(-) diff --git a/python/_nimblephysics/constraint/ConstraintSolver.cpp b/python/_nimblephysics/constraint/ConstraintSolver.cpp index d8c11f87a..d14537a25 100644 --- a/python/_nimblephysics/constraint/ConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/ConstraintSolver.cpp @@ -172,9 +172,10 @@ void ConstraintSolver(py::module& m) }) .def( "updateConstraints", - +[](dart::constraint::ConstraintSolver* self) { - self->updateConstraints(); - }) + +[](dart::constraint::ConstraintSolver* self, bool ignoreFrictionConstraints) { + self->updateConstraints(ignoreFrictionConstraints); + }, + ::py::arg("ignoreFrictionConstraints") = false) .def( "getConstraints", +[](dart::constraint::ConstraintSolver* self) @@ -199,12 +200,16 @@ void ConstraintSolver(py::module& m) }) .def( "runEnforceContactAndJointAndCustomConstraintsFn", - +[](dart::constraint::ConstraintSolver* self) { self->runEnforceContactAndJointAndCustomConstraintsFn(); }) + +[](dart::constraint::ConstraintSolver* self, bool ignoreFrictionConstraints) { + self->runEnforceContactAndJointAndCustomConstraintsFn(ignoreFrictionConstraints); + }, + ::py::arg("ignoreFrictionConstraints") = false) .def( "enforceContactAndJointAndCustomConstraintsWithLcp", - +[](dart::constraint::ConstraintSolver* self) { - self->enforceContactAndJointAndCustomConstraintsWithLcp(); - }) + +[](dart::constraint::ConstraintSolver* self, bool ignoreFrictionConstraints) { + self->enforceContactAndJointAndCustomConstraintsWithLcp(ignoreFrictionConstraints); + }, + ::py::arg("ignoreFrictionConstraints") = false) .def( "replaceEnforceContactAndJointAndCustomConstraintsFn", &dart::constraint::ConstraintSolver:: diff --git a/python/_nimblephysics/simulation/World.cpp b/python/_nimblephysics/simulation/World.cpp index ae47f679a..2929d565e 100644 --- a/python/_nimblephysics/simulation/World.cpp +++ b/python/_nimblephysics/simulation/World.cpp @@ -294,14 +294,24 @@ void World(py::module& m) ::py::return_value_policy::reference_internal) .def( "runConstraintEngine", - +[](dart::simulation::World* self, bool _resetCommand) -> void { - return self->runConstraintEngine(_resetCommand); - }) + +[](dart::simulation::World* self, bool _resetCommand, bool ignoreFrictionConstraints) -> void { + return self->runConstraintEngine(_resetCommand, ignoreFrictionConstraints); + }, + ::py::arg("resetCommand"), + ::py::arg("ignoreFrictionConstraints") = false) .def( "runLcpConstraintEngine", + +[](dart::simulation::World* self, bool _resetCommand, bool ignoreFrictionConstraints) -> void { + return self->runLcpConstraintEngine(_resetCommand, ignoreFrictionConstraints); + }, + ::py::arg("resetCommand"), + ::py::arg("ignoreFrictionConstraints") = false) + .def( + "runFrictionlessLcpConstraintEngine", +[](dart::simulation::World* self, bool _resetCommand) -> void { - return self->runLcpConstraintEngine(_resetCommand); - }) + return self->runFrictionlessLcpConstraintEngine(_resetCommand); + }, + ::py::arg("resetCommand")) .def( "replaceConstraintEngineFn", &dart::simulation::World::replaceConstraintEngineFn) diff --git a/python/new_examples/custom_constraint_engine.py b/python/new_examples/custom_constraint_engine.py index 58f116726..7108ad673 100644 --- a/python/new_examples/custom_constraint_engine.py +++ b/python/new_examples/custom_constraint_engine.py @@ -3,7 +3,7 @@ import torch -def runDummyConstraintEngine(reset): +def runDummyConstraintEngine(resetCommand, ignoreFrictionConstraints): pass @@ -13,13 +13,13 @@ def main(): action = torch.zeros((world.getNumDofs())) solver = world.getConstraintSolver() - def full_frictionless_lcp_engine(reset_command): - world.runLcpConstraintEngine(reset_command) - world.runFrictionlessLcpConstraintEngine(reset_command) + def full_frictionless_lcp_engine(resetCommand, ignoreFrictionConstraints): + world.runLcpConstraintEngine(resetCommand, ignoreFrictionConstraints) + world.runFrictionlessLcpConstraintEngine(resetCommand, ignoreFrictionConstraints) - def frictionless_full_lcp_engine(reset_command): - world.runFrictionlessLcpConstraintEngine(reset_command) - world.runLcpConstraintEngine(reset_command) + def frictionless_full_lcp_engine(resetCommand, ignoreFrictionConstraints): + world.runFrictionlessLcpConstraintEngine(resetCommand, ignoreFrictionConstraints) + world.runLcpConstraintEngine(resetCommand, ignoreFrictionConstraints) engines = [ None, # Use default (don't replace) From 192524751fd935487e10c5cf2288de0196eea7f3 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Mon, 20 Dec 2021 22:49:24 -0800 Subject: [PATCH 05/14] get frictionless working --- python/new_examples/custom_constraint_engine.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/new_examples/custom_constraint_engine.py b/python/new_examples/custom_constraint_engine.py index 7108ad673..f17800ef4 100644 --- a/python/new_examples/custom_constraint_engine.py +++ b/python/new_examples/custom_constraint_engine.py @@ -15,10 +15,10 @@ def main(): def full_frictionless_lcp_engine(resetCommand, ignoreFrictionConstraints): world.runLcpConstraintEngine(resetCommand, ignoreFrictionConstraints) - world.runFrictionlessLcpConstraintEngine(resetCommand, ignoreFrictionConstraints) + world.runFrictionlessLcpConstraintEngine(resetCommand) def frictionless_full_lcp_engine(resetCommand, ignoreFrictionConstraints): - world.runFrictionlessLcpConstraintEngine(resetCommand, ignoreFrictionConstraints) + world.runFrictionlessLcpConstraintEngine(resetCommand) world.runLcpConstraintEngine(resetCommand, ignoreFrictionConstraints) engines = [ From 7a0982d8abf9b5a76abf60547cda38895675b486 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Mon, 20 Dec 2021 23:02:12 -0800 Subject: [PATCH 06/14] change ignoreFrictionConstraints to be only part of the enforce constraints funcions --- dart/constraint/ConstraintSolver.cpp | 14 ++++++++++---- dart/constraint/ConstraintSolver.hpp | 7 +++++-- dart/simulation/World.cpp | 20 +++++++++++++------- dart/simulation/World.hpp | 6 +++--- python/_nimblephysics/simulation/World.cpp | 14 ++++++-------- 5 files changed, 37 insertions(+), 24 deletions(-) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 6aedd39bf..9c7084bc3 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -88,8 +88,8 @@ ConstraintSolver::ConstraintSolver() // gradients mContactClippingDepth( 0.03), // Default to clipping only after fairly deep penetration - mEnforceContactAndJointAndCustomConstraintsFn([this](bool ignoreFrictionConstraints) { - return enforceContactAndJointAndCustomConstraintsWithLcp(ignoreFrictionConstraints); + mEnforceContactAndJointAndCustomConstraintsFn([this]() { + return enforceContactAndJointAndCustomConstraintsWithLcp(); }) { } @@ -373,9 +373,9 @@ LCPSolver* ConstraintSolver::getLCPSolver() const } //============================================================================== -void ConstraintSolver::runEnforceContactAndJointAndCustomConstraintsFn(bool ignoreFrictionConstraints) +void ConstraintSolver::runEnforceContactAndJointAndCustomConstraintsFn() { - mEnforceContactAndJointAndCustomConstraintsFn(ignoreFrictionConstraints); + mEnforceContactAndJointAndCustomConstraintsFn(); } //============================================================================== @@ -392,6 +392,12 @@ void ConstraintSolver::replaceEnforceContactAndJointAndCustomConstraintsFn( mEnforceContactAndJointAndCustomConstraintsFn = f; } +//============================================================================== +void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithFrictionlessLcp() +{ + enforceContactAndJointAndCustomConstraintsWithLcp(true); +} + //============================================================================== void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithLcp(bool ignoreFrictionConstraints) { diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp index c90faadae..d3dd347b0 100644 --- a/dart/constraint/ConstraintSolver.hpp +++ b/dart/constraint/ConstraintSolver.hpp @@ -63,7 +63,7 @@ class ConstraintSolver { public: using enforceContactAndJointAndCustomConstraintsFnType - = std::function; + = std::function; /// Constructor /// @@ -187,11 +187,14 @@ class ConstraintSolver LCPSolver* getLCPSolver() const; /// Solve constraint impulses and apply them to the skeletons - void runEnforceContactAndJointAndCustomConstraintsFn(bool ignoreFrictionConstraints = false); + void runEnforceContactAndJointAndCustomConstraintsFn(); /// Enforce contact, joint, and custom constraints using LCP. void enforceContactAndJointAndCustomConstraintsWithLcp(bool ignoreFrictionConstraints = false); + /// Enforce contact, joint, and custom constraints using frictionless LCP. + void enforceContactAndJointAndCustomConstraintsWithFrictionlessLcp(); + /// Replace the default solve callback function. void replaceEnforceContactAndJointAndCustomConstraintsFn( const enforceContactAndJointAndCustomConstraintsFnType& f); diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 0e37720d7..9dee23fbf 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -88,8 +88,8 @@ World::World(const std::string& _name) mWrtMass(std::make_shared()), mUseFDOverride(false), mSlowDebugResultsAgainstFD(false), - mConstraintEngineFn([this](bool _resetCommand, bool ignoreFrictionConstraints) { - return runLcpConstraintEngine(_resetCommand, ignoreFrictionConstraints); + mConstraintEngineFn([this](bool _resetCommand) { + return runLcpConstraintEngine(_resetCommand); }) { mIndices.push_back(0); @@ -252,22 +252,28 @@ void World::step(bool _resetCommand) } //============================================================================== -void World::runConstraintEngine(bool _resetCommand, bool ignoreFrictionConstraints) +void World::runConstraintEngine(bool _resetCommand) { - mConstraintEngineFn(_resetCommand, ignoreFrictionConstraints); + mConstraintEngineFn(_resetCommand); } //============================================================================== -void World::runLcpConstraintEngine(bool _resetCommand, bool ignoreFrictionConstraints) +void World::runLcpConstraintEngine(bool _resetCommand) { - mConstraintSolver->runEnforceContactAndJointAndCustomConstraintsFn(ignoreFrictionConstraints); + mConstraintSolver->runEnforceContactAndJointAndCustomConstraintsFn(); integrateVelocitiesFromImpulses(_resetCommand); } //============================================================================== void World::runFrictionlessLcpConstraintEngine(bool _resetCommand) { - runLcpConstraintEngine(_resetCommand, true); + // Replace with frictionless version of enforce constraints function. + mConstraintSolver->replaceEnforceContactAndJointAndCustomConstraintsFn( + [this]() { + return mConstraintSolver->enforceContactAndJointAndCustomConstraintsWithFrictionlessLcp(); + }); + mConstraintSolver->runEnforceContactAndJointAndCustomConstraintsFn(); + integrateVelocitiesFromImpulses(_resetCommand); } //============================================================================== diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index 371ee699d..67fa6a465 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -93,7 +93,7 @@ class World : public virtual common::Subject, using NameChangedSignal = common::Signal; - using constraintEngineFnType = std::function; + using constraintEngineFnType = std::function; /// Creates World as shared_ptr template @@ -481,10 +481,10 @@ class World : public virtual common::Subject, /// Run the constraint engine which solves for constraint impulses and /// integrates velocities given these constraint impulses. - void runConstraintEngine(bool _resetCommand, bool ignoreFrictionConstraints = false); + void runConstraintEngine(bool _resetCommand); /// The default constraint engine which runs an LCP. - void runLcpConstraintEngine(bool _resetCommand, bool ignoreFrictionConstraints = false); + void runLcpConstraintEngine(bool _resetCommand); /// A constraint engine that runs a frictionless LCP. void runFrictionlessLcpConstraintEngine(bool _resetCommand); diff --git a/python/_nimblephysics/simulation/World.cpp b/python/_nimblephysics/simulation/World.cpp index 2929d565e..fe2fa300d 100644 --- a/python/_nimblephysics/simulation/World.cpp +++ b/python/_nimblephysics/simulation/World.cpp @@ -294,18 +294,16 @@ void World(py::module& m) ::py::return_value_policy::reference_internal) .def( "runConstraintEngine", - +[](dart::simulation::World* self, bool _resetCommand, bool ignoreFrictionConstraints) -> void { - return self->runConstraintEngine(_resetCommand, ignoreFrictionConstraints); + +[](dart::simulation::World* self, bool _resetCommand) -> void { + return self->runConstraintEngine(_resetCommand); }, - ::py::arg("resetCommand"), - ::py::arg("ignoreFrictionConstraints") = false) + ::py::arg("resetCommand")) .def( "runLcpConstraintEngine", - +[](dart::simulation::World* self, bool _resetCommand, bool ignoreFrictionConstraints) -> void { - return self->runLcpConstraintEngine(_resetCommand, ignoreFrictionConstraints); + +[](dart::simulation::World* self, bool _resetCommand) -> void { + return self->runLcpConstraintEngine(_resetCommand); }, - ::py::arg("resetCommand"), - ::py::arg("ignoreFrictionConstraints") = false) + ::py::arg("resetCommand")) .def( "runFrictionlessLcpConstraintEngine", +[](dart::simulation::World* self, bool _resetCommand) -> void { From 6e5fc8dcfdc964a845e03ac3d3f6878f8897de6f Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Mon, 20 Dec 2021 23:06:25 -0800 Subject: [PATCH 07/14] remove friction arg from python code, add newline to warning message --- dart/constraint/ConstraintSolver.cpp | 2 +- .../_nimblephysics/constraint/ConstraintSolver.cpp | 12 ++++++++---- python/new_examples/custom_constraint_engine.py | 10 +++++----- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 9c7084bc3..4a5a7000e 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -388,7 +388,7 @@ void ConstraintSolver::replaceEnforceContactAndJointAndCustomConstraintsFn( << "BE INCORRECT!!!! Nimble is still under heavy development, and we " << "don't yet support differentiating through `timestep()` if you've " << "called `replaceEnforceContactAndJointAndCustomConstraintsFn()` to " - "customize the solve function."; + "customize the solve function.\n"; mEnforceContactAndJointAndCustomConstraintsFn = f; } diff --git a/python/_nimblephysics/constraint/ConstraintSolver.cpp b/python/_nimblephysics/constraint/ConstraintSolver.cpp index d14537a25..e57d09d61 100644 --- a/python/_nimblephysics/constraint/ConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/ConstraintSolver.cpp @@ -200,16 +200,20 @@ void ConstraintSolver(py::module& m) }) .def( "runEnforceContactAndJointAndCustomConstraintsFn", - +[](dart::constraint::ConstraintSolver* self, bool ignoreFrictionConstraints) { - self->runEnforceContactAndJointAndCustomConstraintsFn(ignoreFrictionConstraints); - }, - ::py::arg("ignoreFrictionConstraints") = false) + +[](dart::constraint::ConstraintSolver* self) { + self->runEnforceContactAndJointAndCustomConstraintsFn(); + }) .def( "enforceContactAndJointAndCustomConstraintsWithLcp", +[](dart::constraint::ConstraintSolver* self, bool ignoreFrictionConstraints) { self->enforceContactAndJointAndCustomConstraintsWithLcp(ignoreFrictionConstraints); }, ::py::arg("ignoreFrictionConstraints") = false) + .def( + "enforceContactAndJointAndCustomConstraintsWithFrictionlessLcp", + +[](dart::constraint::ConstraintSolver* self) { + self->enforceContactAndJointAndCustomConstraintsWithFrictionlessLcp(); + }) .def( "replaceEnforceContactAndJointAndCustomConstraintsFn", &dart::constraint::ConstraintSolver:: diff --git a/python/new_examples/custom_constraint_engine.py b/python/new_examples/custom_constraint_engine.py index f17800ef4..d0b6824fa 100644 --- a/python/new_examples/custom_constraint_engine.py +++ b/python/new_examples/custom_constraint_engine.py @@ -3,7 +3,7 @@ import torch -def runDummyConstraintEngine(resetCommand, ignoreFrictionConstraints): +def runDummyConstraintEngine(resetCommand): pass @@ -13,13 +13,13 @@ def main(): action = torch.zeros((world.getNumDofs())) solver = world.getConstraintSolver() - def full_frictionless_lcp_engine(resetCommand, ignoreFrictionConstraints): - world.runLcpConstraintEngine(resetCommand, ignoreFrictionConstraints) + def full_frictionless_lcp_engine(resetCommand): + world.runLcpConstraintEngine(resetCommand) world.runFrictionlessLcpConstraintEngine(resetCommand) - def frictionless_full_lcp_engine(resetCommand, ignoreFrictionConstraints): + def frictionless_full_lcp_engine(resetCommand): world.runFrictionlessLcpConstraintEngine(resetCommand) - world.runLcpConstraintEngine(resetCommand, ignoreFrictionConstraints) + world.runLcpConstraintEngine(resetCommand) engines = [ None, # Use default (don't replace) From b84b540c7a5197a302184f1824dcf0ef1c26073a Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Mon, 20 Dec 2021 23:20:55 -0800 Subject: [PATCH 08/14] add unit test for frictionless --- unittests/unit/test_ConstraintSolver.cpp | 33 ++++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/unittests/unit/test_ConstraintSolver.cpp b/unittests/unit/test_ConstraintSolver.cpp index ab0cd3e52..38180f5ad 100644 --- a/unittests/unit/test_ConstraintSolver.cpp +++ b/unittests/unit/test_ConstraintSolver.cpp @@ -41,7 +41,7 @@ using namespace dart; -TEST(ConstraintSolver, SIMPLE) +std::shared_ptr loadWorldAndIntegrateNonConstraintForces() { // Load a world where a cube is colliding with the ground. std::shared_ptr world @@ -59,9 +59,17 @@ TEST(ConstraintSolver, SIMPLE) skel->integrateVelocities(world->getTimeStep()); // 0, 0, 0, 0, -0.00981, 0 EXPECT_TRUE(box->getRelativeSpatialVelocity()[4] < 0); + return world; +} - // Collision detection. +TEST(ConstraintSolver, Simple) +{ + auto world = loadWorldAndIntegrateNonConstraintForces(); + auto skel = world->getSkeleton("box skeleton"); + auto box = skel->getBodyNode("box"); auto solver = world->getConstraintSolver(); + + // Collision detection. solver->updateConstraints(); solver->buildConstrainedGroups(); EXPECT_TRUE(solver->getLastCollisionResult().getNumContacts() > 0); @@ -81,3 +89,24 @@ TEST(ConstraintSolver, SIMPLE) skel->computeImpulseForwardDynamics(); EXPECT_TRUE(box->getRelativeSpatialVelocity()[4] >= 0); } + +TEST(ConstraintSolver, ReplaceConstraintEngineWithFrictionlessLcp) +{ + auto world = loadWorldAndIntegrateNonConstraintForces(); + auto skel = world->getSkeleton("box skeleton"); + auto box = skel->getBodyNode("box"); + auto solver = world->getConstraintSolver(); + + world->replaceConstraintEngineFn([world](bool _resetCommand) { + return world->runFrictionlessLcpConstraintEngine(_resetCommand); + }); + world->runConstraintEngine(false); + + // Collision detection. + EXPECT_TRUE(solver->getLastCollisionResult().getNumContacts() > 0); + EXPECT_TRUE(solver->getNumConstrainedGroups() > 0); + + // Integrate velocities from solved impulses. Should have non-negative normal + // velocity after integration. + EXPECT_TRUE(box->getRelativeSpatialVelocity()[4] >= 0); +} From 3ff7b11d414c9ebca8786bf25e06d8b5195d1a76 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 21 Dec 2021 00:59:23 -0800 Subject: [PATCH 09/14] fix missing false case for mIsFrictionOn --- dart/constraint/ContactConstraint.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 9f5b8dff3..e254c0ad2 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -121,6 +121,10 @@ ContactConstraint::ContactConstraint( // Update frictional direction updateFirstFrictionalDirection(); } + else + { + mIsFrictionOn = false; + } } assert(mBodyNodeA->getSkeleton()); From 769e26d0589e76a47ef5b3cca2aeae498b29d329 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 21 Dec 2021 03:19:46 -0800 Subject: [PATCH 10/14] add python binding for dAdInvT, implement friction + frictionless lcp in python --- python/_nimblephysics/math/Geometry.cpp | 9 +++++ .../new_examples/custom_constraint_engine.py | 34 ++++++++++++++----- 2 files changed, 35 insertions(+), 8 deletions(-) diff --git a/python/_nimblephysics/math/Geometry.cpp b/python/_nimblephysics/math/Geometry.cpp index 7cb5d7305..9a039bbf2 100644 --- a/python/_nimblephysics/math/Geometry.cpp +++ b/python/_nimblephysics/math/Geometry.cpp @@ -132,6 +132,15 @@ void Geometry(py::module& m) ::py::arg("p"), ::py::arg("S")); + m.def( + "dAdInvT", + +[](const Eigen::Isometry3s& T, + const Eigen::Vector6s& F) -> Eigen::Vector6s { + return dart::math::dAdInvT(T, F); + }, + ::py::arg("T"), + ::py::arg("F")); + m.def( "rightMultiplyInFreeJointSpace", +[](const Eigen::Matrix3s& R, diff --git a/python/new_examples/custom_constraint_engine.py b/python/new_examples/custom_constraint_engine.py index d0b6824fa..4df956413 100644 --- a/python/new_examples/custom_constraint_engine.py +++ b/python/new_examples/custom_constraint_engine.py @@ -9,24 +9,42 @@ def runDummyConstraintEngine(resetCommand): def main(): world = nimble.loadWorld("../../data/skel/test/colliding_cube.skel") + skel = world.getSkeleton("box skeleton") + cube = skel.getBodyNode("box") state = torch.tensor(world.getState()) action = torch.zeros((world.getNumDofs())) solver = world.getConstraintSolver() - - def full_frictionless_lcp_engine(resetCommand): - world.runLcpConstraintEngine(resetCommand) + + def friction_frictionless_lcp_engine(resetCommand, use_tauxz): + solver.runEnforceContactAndJointAndCustomConstraintsFn() + local_impulse = cube.getConstraintImpulse() + world_impulse = nimble.math.dAdInvT(cube.getWorldTransform(), local_impulse) + cube.clearConstraintImpulse() + taux, tauy, tauz, fx, fy, fz = world_impulse + y = skel.getPositions()[4] + tauzx, tauxz = 0, 0 + if use_tauxz: + tauzx = -y * fz + tauxz = y * fx + world_friction_impulse = [tauzx, tauy, tauxz, fx, 0, fz] + local_friction_impulse = nimble.math.dAdInvT(cube.getWorldTransform().inverse(), world_friction_impulse) + cube.addConstraintImpulse(local_friction_impulse) + world.integrateVelocitiesFromImpulses(resetCommand) world.runFrictionlessLcpConstraintEngine(resetCommand) - def frictionless_full_lcp_engine(resetCommand): - world.runFrictionlessLcpConstraintEngine(resetCommand) - world.runLcpConstraintEngine(resetCommand) + def friction_frictionless_lcp_engine_with_tauxz(resetCommand): + friction_frictionless_lcp_engine(resetCommand, use_tauxz=True) + + def friction_frictionless_lcp_engine_without_tauxz(resetCommand): + friction_frictionless_lcp_engine(resetCommand, use_tauxz=False) engines = [ None, # Use default (don't replace) runDummyConstraintEngine, # Replace with dummy engine world.runLcpConstraintEngine, # Replace with LCP engine (same as default) - full_frictionless_lcp_engine, # LCP + FrictionlessLCP - frictionless_full_lcp_engine, # FrictionlessLCP + LCP + world.runFrictionlessLcpConstraintEngine, # Replace with FrictionlessLCP + friction_frictionless_lcp_engine_with_tauxz, + friction_frictionless_lcp_engine_without_tauxz ] for engine in engines: if engine is not None: From 48f777eb2f43fc29bb9d4b57e8a90e6dc1744057 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 21 Dec 2021 15:49:46 -0800 Subject: [PATCH 11/14] default args for resetCommand --- python/_nimblephysics/simulation/World.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/_nimblephysics/simulation/World.cpp b/python/_nimblephysics/simulation/World.cpp index fe2fa300d..b9830cb3a 100644 --- a/python/_nimblephysics/simulation/World.cpp +++ b/python/_nimblephysics/simulation/World.cpp @@ -297,19 +297,19 @@ void World(py::module& m) +[](dart::simulation::World* self, bool _resetCommand) -> void { return self->runConstraintEngine(_resetCommand); }, - ::py::arg("resetCommand")) + ::py::arg("resetCommand") = true) .def( "runLcpConstraintEngine", +[](dart::simulation::World* self, bool _resetCommand) -> void { return self->runLcpConstraintEngine(_resetCommand); }, - ::py::arg("resetCommand")) + ::py::arg("resetCommand") = true) .def( "runFrictionlessLcpConstraintEngine", +[](dart::simulation::World* self, bool _resetCommand) -> void { return self->runFrictionlessLcpConstraintEngine(_resetCommand); }, - ::py::arg("resetCommand")) + ::py::arg("resetCommand") = true) .def( "replaceConstraintEngineFn", &dart::simulation::World::replaceConstraintEngineFn) From 363f1a43ace6abc745824d921cded123034ada78 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 21 Dec 2021 15:54:24 -0800 Subject: [PATCH 12/14] rename runConstraintEngine to runRegisteredConstraintEngine --- dart/simulation/World.cpp | 4 ++-- dart/simulation/World.hpp | 2 +- python/_nimblephysics/simulation/World.cpp | 4 ++-- unittests/unit/test_ConstraintSolver.cpp | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 9dee23fbf..9dbfe628a 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -244,7 +244,7 @@ void World::step(bool _resetCommand) mConstraintSolver->setContactClippingDepth(mContactClippingDepth); mConstraintSolver->setFallbackConstraintForceMixingConstant( mFallbackConstraintForceMixingConstant); - runConstraintEngine(_resetCommand); + runRegisteredConstraintEngine(_resetCommand); integratePositions(initialVelocity); mTime += mTimeStep; @@ -252,7 +252,7 @@ void World::step(bool _resetCommand) } //============================================================================== -void World::runConstraintEngine(bool _resetCommand) +void World::runRegisteredConstraintEngine(bool _resetCommand) { mConstraintEngineFn(_resetCommand); } diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index 67fa6a465..72f778168 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -481,7 +481,7 @@ class World : public virtual common::Subject, /// Run the constraint engine which solves for constraint impulses and /// integrates velocities given these constraint impulses. - void runConstraintEngine(bool _resetCommand); + void runRegisteredConstraintEngine(bool _resetCommand); /// The default constraint engine which runs an LCP. void runLcpConstraintEngine(bool _resetCommand); diff --git a/python/_nimblephysics/simulation/World.cpp b/python/_nimblephysics/simulation/World.cpp index b9830cb3a..62c8dbd00 100644 --- a/python/_nimblephysics/simulation/World.cpp +++ b/python/_nimblephysics/simulation/World.cpp @@ -293,9 +293,9 @@ void World(py::module& m) }, ::py::return_value_policy::reference_internal) .def( - "runConstraintEngine", + "runRegisteredConstraintEngine", +[](dart::simulation::World* self, bool _resetCommand) -> void { - return self->runConstraintEngine(_resetCommand); + return self->runRegisteredConstraintEngine(_resetCommand); }, ::py::arg("resetCommand") = true) .def( diff --git a/unittests/unit/test_ConstraintSolver.cpp b/unittests/unit/test_ConstraintSolver.cpp index 38180f5ad..bc8c4e586 100644 --- a/unittests/unit/test_ConstraintSolver.cpp +++ b/unittests/unit/test_ConstraintSolver.cpp @@ -100,7 +100,7 @@ TEST(ConstraintSolver, ReplaceConstraintEngineWithFrictionlessLcp) world->replaceConstraintEngineFn([world](bool _resetCommand) { return world->runFrictionlessLcpConstraintEngine(_resetCommand); }); - world->runConstraintEngine(false); + world->runRegisteredConstraintEngine(false); // Collision detection. EXPECT_TRUE(solver->getLastCollisionResult().getNumContacts() > 0); From 9765693d9ec5d713a42b150f99376621e4aaa9d2 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 21 Dec 2021 15:56:43 -0800 Subject: [PATCH 13/14] rename mConstraintEngineFn to mRegisteredConstraintEngine --- dart/simulation/World.cpp | 6 +++--- dart/simulation/World.hpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 9dbfe628a..fcab290e7 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -88,7 +88,7 @@ World::World(const std::string& _name) mWrtMass(std::make_shared()), mUseFDOverride(false), mSlowDebugResultsAgainstFD(false), - mConstraintEngineFn([this](bool _resetCommand) { + mRegisteredConstraintEngine([this](bool _resetCommand) { return runLcpConstraintEngine(_resetCommand); }) { @@ -254,7 +254,7 @@ void World::step(bool _resetCommand) //============================================================================== void World::runRegisteredConstraintEngine(bool _resetCommand) { - mConstraintEngineFn(_resetCommand); + mRegisteredConstraintEngine(_resetCommand); } //============================================================================== @@ -279,7 +279,7 @@ void World::runFrictionlessLcpConstraintEngine(bool _resetCommand) //============================================================================== void World::replaceConstraintEngineFn(const constraintEngineFnType& engineFn) { - mConstraintEngineFn = engineFn; + mRegisteredConstraintEngine = engineFn; } //============================================================================== diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index 72f778168..68ba1d4c6 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -717,7 +717,7 @@ class World : public virtual common::Subject, /// Constraint engine which solves for constraint impulses and integrates /// velocities according to the given impulses. - constraintEngineFnType mConstraintEngineFn; + constraintEngineFnType mRegisteredConstraintEngine; /// True if we want to update p_{t+1} as f(p_t, v_t), rather than the old /// f(p_t, v_{t+1}). This makes it much easier to reason about From 18b11c7c545e015f380a76d4c6be98140bb7555e Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 21 Dec 2021 15:57:12 -0800 Subject: [PATCH 14/14] add default args for constraint engines in C++ --- dart/simulation/World.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index 68ba1d4c6..97b56e695 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -481,13 +481,13 @@ class World : public virtual common::Subject, /// Run the constraint engine which solves for constraint impulses and /// integrates velocities given these constraint impulses. - void runRegisteredConstraintEngine(bool _resetCommand); + void runRegisteredConstraintEngine(bool _resetCommand = true); /// The default constraint engine which runs an LCP. - void runLcpConstraintEngine(bool _resetCommand); + void runLcpConstraintEngine(bool _resetCommand = true); /// A constraint engine that runs a frictionless LCP. - void runFrictionlessLcpConstraintEngine(bool _resetCommand); + void runFrictionlessLcpConstraintEngine(bool _resetCommand = true); /// Replace the default constraint engine with a custom one. void replaceConstraintEngineFn(const constraintEngineFnType& engineFn);