Add frictionless LCP

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<ContactConstraint>(
-          contact, mTimeStep, mPenetrationCorrectionEnabled));
+          contact, mTimeStep, mPenetrationCorrectionEnabled, ignoreFrictionConstraints));
     }
   }
 

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();

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<dynamics::ShapeFrame*>(
@@ -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());

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;

dart/simulation/World.cpp

@@ -260,7 +260,19 @@ void World::runConstraintEngine(bool _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);
 }
 

dart/simulation/World.hpp

@@ -486,6 +486,9 @@ class World : public virtual common::Subject,
   /// The default constraint engine which runs an LCP.
   void runLcpConstraintEngine(bool _resetCommand);
 
+  /// 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);
 

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",

python/_nimblephysics/simulation/World.cpp

@@ -296,12 +296,20 @@ void World(py::module& m)
           "runConstraintEngine",
           +[](dart::simulation::World* self, bool _resetCommand) -> void {
             return self->runConstraintEngine(_resetCommand);
-          })
+          },
+          ::py::arg("resetCommand"))
       .def(
           "runLcpConstraintEngine",
           +[](dart::simulation::World* self, bool _resetCommand) -> void {
             return self->runLcpConstraintEngine(_resetCommand);
-          })
+          },
+          ::py::arg("resetCommand"))
+      .def(
+          "runFrictionlessLcpConstraintEngine",
+          +[](dart::simulation::World* self, bool _resetCommand) -> void {
+            return self->runFrictionlessLcpConstraintEngine(_resetCommand);
+          },
+          ::py::arg("resetCommand"))
       .def(
           "replaceConstraintEngineFn",
           &dart::simulation::World::replaceConstraintEngineFn)

python/new_examples/custom_constraint_engine.py

@@ -3,38 +3,30 @@
 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")
     state = torch.tensor(world.getState())
     action = torch.zeros((world.getNumDofs()))
     solver = world.getConstraintSolver()
 
+    def full_frictionless_lcp_engine(resetCommand):
+        world.runLcpConstraintEngine(resetCommand)
+        world.runFrictionlessLcpConstraintEngine(resetCommand)
+
+    def frictionless_full_lcp_engine(resetCommand):
+        world.runFrictionlessLcpConstraintEngine(resetCommand)
+        world.runLcpConstraintEngine(resetCommand)
+
     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:

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++)
   {

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));

unittests/unit/test_ConstraintSolver.cpp

@@ -41,7 +41,7 @@
 
 using namespace dart;
 
-TEST(ConstraintSolver, SIMPLE)
+std::shared_ptr<simulation::World> loadWorldAndIntegrateNonConstraintForces()
 {
   // Load a world where a cube is colliding with the ground.
   std::shared_ptr<simulation::World> 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);
+}