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

@@ -88,7 +88,7 @@ World::World(const std::string& _name)
     mWrtMass(std::make_shared<neural::WithRespectToMass>()),
     mUseFDOverride(false),
     mSlowDebugResultsAgainstFD(false),
-    mConstraintEngineFn([this](bool _resetCommand) {
+    mRegisteredConstraintEngine([this](bool _resetCommand) {
       return runLcpConstraintEngine(_resetCommand);
     })
 {
@@ -244,23 +244,35 @@ void World::step(bool _resetCommand)
   mConstraintSolver->setContactClippingDepth(mContactClippingDepth);
   mConstraintSolver->setFallbackConstraintForceMixingConstant(
       mFallbackConstraintForceMixingConstant);
-  runConstraintEngine(_resetCommand);
+  runRegisteredConstraintEngine(_resetCommand);
   integratePositions(initialVelocity);
 
   mTime += mTimeStep;
   mFrame++;
 }
 
 //==============================================================================
-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;
 }
 
 //==============================================================================

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

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

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)

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:

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