diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 2af247094..4a5a7000e 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -373,7 +373,7 @@ LCPSolver* ConstraintSolver::getLCPSolver() const } //============================================================================== -void ConstraintSolver::solve() +void ConstraintSolver::runEnforceContactAndJointAndCustomConstraintsFn() { mEnforceContactAndJointAndCustomConstraintsFn(); } @@ -388,12 +388,18 @@ 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; } //============================================================================== -void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithLcp() +void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithFrictionlessLcp() +{ + enforceContactAndJointAndCustomConstraintsWithLcp(true); +} + +//============================================================================== +void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithLcp(bool ignoreFrictionConstraints) { for (auto& skeleton : mSkeletons) { @@ -404,7 +410,7 @@ void ConstraintSolver::enforceContactAndJointAndCustomConstraintsWithLcp() } // Update constraints and collect active constraints - updateConstraints(); + updateConstraints(ignoreFrictionConstraints); // Build constrained groups buildConstrainedGroups(); @@ -539,7 +545,7 @@ bool ConstraintSolver::checkAndAddConstraint( } //============================================================================== -void ConstraintSolver::updateConstraints() +void ConstraintSolver::updateConstraints(bool ignoreFrictionConstraints) { // Clear previous active constraint list mActiveConstraints.clear(); @@ -608,7 +614,7 @@ void ConstraintSolver::updateConstraints() 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..d3dd347b0 100644 --- a/dart/constraint/ConstraintSolver.hpp +++ b/dart/constraint/ConstraintSolver.hpp @@ -187,17 +187,20 @@ class ConstraintSolver LCPSolver* getLCPSolver() const; /// Solve constraint impulses and apply them to the skeletons - void solve(); + void runEnforceContactAndJointAndCustomConstraintsFn(); /// Enforce contact, joint, and custom constraints using LCP. - void enforceContactAndJointAndCustomConstraintsWithLcp(); + 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); /// Update constraints - void updateConstraints(); + void updateConstraints(bool ignoreFrictionConstraints = false); /// Build constrained groupsContact void buildConstrainedGroups(); diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 8a571473e..e254c0ad2 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,28 @@ 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(); + } + else + { + mIsFrictionOn = false; + } } assert(mBodyNodeA->getSkeleton()); 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 2eb808b65..cae39e913 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); }) { @@ -244,7 +244,7 @@ void World::step(bool _resetCommand) mConstraintSolver->setContactClippingDepth(mContactClippingDepth); mConstraintSolver->setFallbackConstraintForceMixingConstant( mFallbackConstraintForceMixingConstant); - runConstraintEngine(_resetCommand); + runRegisteredConstraintEngine(_resetCommand); integratePositions(initialVelocity); mTime += mTimeStep; @@ -252,15 +252,27 @@ void World::step(bool _resetCommand) } //============================================================================== -void World::runConstraintEngine(bool _resetCommand) +void World::runRegisteredConstraintEngine(bool _resetCommand) { - mConstraintEngineFn(_resetCommand); + mRegisteredConstraintEngine(_resetCommand); } //============================================================================== void World::runLcpConstraintEngine(bool _resetCommand) { - mConstraintSolver->solve(); + mConstraintSolver->runEnforceContactAndJointAndCustomConstraintsFn(); + integrateVelocitiesFromImpulses(_resetCommand); +} + +//============================================================================== +void World::runFrictionlessLcpConstraintEngine(bool _resetCommand) +{ + // Replace with frictionless version of enforce constraints function. + mConstraintSolver->replaceEnforceContactAndJointAndCustomConstraintsFn( + [this]() { + return mConstraintSolver->enforceContactAndJointAndCustomConstraintsWithFrictionlessLcp(); + }); + mConstraintSolver->runEnforceContactAndJointAndCustomConstraintsFn(); integrateVelocitiesFromImpulses(_resetCommand); } @@ -274,7 +286,7 @@ void World::replaceConstraintEngineFn(const constraintEngineFnType& engineFn) << "called `replaceConstraintEngineFn()` to " "customize the constraint engine function.\n"; - mConstraintEngineFn = engineFn; + mRegisteredConstraintEngine = engineFn; } //============================================================================== diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index bb68caba5..97b56e695 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -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 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 = true); /// Replace the default constraint engine with a custom one. void replaceConstraintEngineFn(const constraintEngineFnType& engineFn); @@ -714,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 diff --git a/python/_nimblephysics/constraint/ConstraintSolver.cpp b/python/_nimblephysics/constraint/ConstraintSolver.cpp index 913187311..e57d09d61 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) @@ -198,12 +199,20 @@ 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, bool ignoreFrictionConstraints) { + self->enforceContactAndJointAndCustomConstraintsWithLcp(ignoreFrictionConstraints); + }, + ::py::arg("ignoreFrictionConstraints") = false) + .def( + "enforceContactAndJointAndCustomConstraintsWithFrictionlessLcp", +[](dart::constraint::ConstraintSolver* self) { - self->enforceContactAndJointAndCustomConstraintsWithLcp(); + self->enforceContactAndJointAndCustomConstraintsWithFrictionlessLcp(); }) .def( "replaceEnforceContactAndJointAndCustomConstraintsFn", 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/_nimblephysics/simulation/World.cpp b/python/_nimblephysics/simulation/World.cpp index ae47f679a..62c8dbd00 100644 --- a/python/_nimblephysics/simulation/World.cpp +++ b/python/_nimblephysics/simulation/World.cpp @@ -293,15 +293,23 @@ 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( "runLcpConstraintEngine", +[](dart::simulation::World* self, bool _resetCommand) -> void { return self->runLcpConstraintEngine(_resetCommand); - }) + }, + ::py::arg("resetCommand") = true) + .def( + "runFrictionlessLcpConstraintEngine", + +[](dart::simulation::World* self, bool _resetCommand) -> void { + return self->runFrictionlessLcpConstraintEngine(_resetCommand); + }, + ::py::arg("resetCommand") = true) .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 ef48364f1..4df956413 100644 --- a/python/new_examples/custom_constraint_engine.py +++ b/python/new_examples/custom_constraint_engine.py @@ -3,38 +3,48 @@ import torch -def runDummyConstraintEngine(reset_command): +def runDummyConstraintEngine(resetCommand): 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") + skel = world.getSkeleton("box skeleton") + cube = skel.getBodyNode("box") state = torch.tensor(world.getState()) action = torch.zeros((world.getNumDofs())) solver = world.getConstraintSolver() + + 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 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) + 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: 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)); diff --git a/unittests/unit/test_ConstraintSolver.cpp b/unittests/unit/test_ConstraintSolver.cpp index ab0cd3e52..bc8c4e586 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->runRegisteredConstraintEngine(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); +}