Merge remote-tracking branch 'origin/main' into issue/465_closed_kinematic_changes

Author: jslee02

.gitignore

@@ -59,6 +59,7 @@ docs/doxygen/html/
 docs/readthedocs/site
 docs/readthedocs/_build/
 docs/readthedocs/_generated/
+docs/readthedocs/_generated_stubs/
 docs/python_api/_build/
 
 # Data files

Brewfile

@@ -37,4 +37,3 @@ brew 'urdfdom'
 
 # dartpy dependencies
 brew 'python3'
-brew 'pybind11'

CMakeLists.txt

@@ -247,7 +247,7 @@ dart_option(DART_USE_SYSTEM_IMGUI
   "Use system-installed ImGui instead of fetching from GitHub (recommended for package distributions)" OFF CATEGORY system)
 dart_option(DART_USE_SYSTEM_GOOGLEBENCHMARK "Use system GoogleBenchmark" OFF CATEGORY system)
 dart_option(DART_USE_SYSTEM_GOOGLETEST "Use system GoogleTest" OFF CATEGORY system)
-dart_option(DART_USE_SYSTEM_PYBIND11 "Use system pybind11" OFF CATEGORY system)
+dart_option(DART_USE_SYSTEM_NANOBIND "Use system nanobind" OFF CATEGORY system)
 dart_option(DART_USE_SYSTEM_TRACY "Use system Tracy" OFF CATEGORY system)
 dart_option(DART_VERBOSE "Whether print detailed information in CMake process" OFF CATEGORY diagnostics)
 dart_option(DART_BUILD_DART8 "Build experimental DART8 library" OFF CATEGORY build)
@@ -674,7 +674,10 @@ endif()
 add_subdirectory(python)
 
 # Add 'ALL' target that builds everything
-set(all_target_candidates dartpy)
+set(all_target_candidates)
+if(DART_BUILD_DARTPY)
+  list(APPEND all_target_candidates dartpy)
+endif()
 if(DART_BUILD_TESTS AND BUILD_TESTING)
   list(APPEND all_target_candidates tests_and_run pytest)
 endif()
@@ -1003,8 +1006,7 @@ message(STATUS "Run '${DART_CMAKE_BUILD_CMD} --target tutorials' to build all th
 message(STATUS "Run '${DART_CMAKE_BUILD_CMD} --target view_docs' to see the API documentation")
 message(STATUS "Run '${DART_CMAKE_BUILD_CMD} --target install' to install all the C++ components")
 if(TARGET dartpy)
-  message(STATUS "Run '${DART_CMAKE_BUILD_CMD} --target dartpy' to build dartpy")
-  message(STATUS "Run '${DART_CMAKE_BUILD_CMD} --target install' to install dartpy")
+  message(STATUS "Run '${DART_CMAKE_BUILD_CMD} --target dartpy' to build the python bindings")
 endif()
 if(TARGET coverage)
   message(STATUS "- 'coverage'     : generate coverage report")

dart/constraint/CouplerConstraint.cpp

@@ -38,12 +38,21 @@
 #include "dart/dynamics/Joint.hpp"
 #include "dart/dynamics/Skeleton.hpp"
 
-#define DART_CFM 1e-9
+#include <cmath>
+
+namespace {
+
+constexpr inline double kConstraintForceMixing = 1e-6;
+constexpr inline double kDefaultForceLimit = 800.0;
+constexpr inline double kDefaultVelocityLimit = 50.0;
+constexpr inline double kDefaultErp = 0.4;
+
+} // namespace
 
 namespace dart {
 namespace constraint {
 
-double CouplerConstraint::mConstraintForceMixing = DART_CFM;
+double CouplerConstraint::mConstraintForceMixing = kConstraintForceMixing;
 
 //==============================================================================
 CouplerConstraint::CouplerConstraint(
@@ -114,20 +123,32 @@ void CouplerConstraint::update()
     const auto& mimicProp = mMimicProps[i];
 
     double timeStep = mJoint->getSkeleton()->getTimeStep();
+    double velLower = mJoint->getVelocityLowerLimit(i);
+    double velUpper = mJoint->getVelocityUpperLimit(i);
+    if (!std::isfinite(velLower))
+      velLower = -kDefaultVelocityLimit;
+    if (!std::isfinite(velUpper))
+      velUpper = kDefaultVelocityLimit;
     double qError
         = mimicProp.mReferenceJoint->getPosition(mimicProp.mReferenceDofIndex)
               * mimicProp.mMultiplier
           + mimicProp.mOffset - mJoint->getPosition(i);
-    double desiredVelocity = math::clip(
-        qError / timeStep,
-        mJoint->getVelocityLowerLimit(i),
-        mJoint->getVelocityUpperLimit(i));
+    const double erp = kDefaultErp;
+    double desiredVelocity
+        = math::clip((erp * qError) / timeStep, velLower, velUpper);
 
     mNegativeVelocityError[i] = desiredVelocity - mJoint->getVelocity(i);
 
     if (mNegativeVelocityError[i] != 0.0) {
-      mUpperBound[i] = mJoint->getForceUpperLimit(i) * timeStep;
-      mLowerBound[i] = mJoint->getForceLowerLimit(i) * timeStep;
+      double upper = mJoint->getForceUpperLimit(i);
+      double lower = mJoint->getForceLowerLimit(i);
+      if (!std::isfinite(upper))
+        upper = kDefaultForceLimit;
+      if (!std::isfinite(lower))
+        lower = -kDefaultForceLimit;
+
+      mUpperBound[i] = upper * timeStep;
+      mLowerBound[i] = lower * timeStep;
 
       if (mActive[i]) {
         ++(mLifeTime[i]);

dart/constraint/CouplerConstraint.hpp

@@ -35,6 +35,7 @@
 
 #include <dart/constraint/ConstraintBase.hpp>
 
+#include <dart/dynamics/Fwd.hpp>
 #include <dart/dynamics/MimicDofProperties.hpp>
 
 #include <dart/Export.hpp>
@@ -144,7 +145,7 @@ class DART_API CouplerConstraint : public ConstraintBase
   double mLowerBound[6];
 
   /// Global constraint force mixing parameter in the range of [1e-9, 1]. The
-  /// default is 1e-5
+  /// default is 1e-6
   /// @sa http://www.ode.org/ode-latest-userguide.html#sec_3_8_0
   static double mConstraintForceMixing;
 };

dart/constraint/MimicMotorConstraint.cpp

@@ -41,12 +41,22 @@
 
 #include <iostream>
 
-#define DART_CFM 1e-9
+#include <cmath>
+
+namespace {
+
+constexpr inline double kConstraintForceMixing = 1e-6;
+constexpr inline double kDefaultForceLimit = 800.0;
+constexpr inline double kDefaultVelocityLimit = 50.0;
+constexpr inline double kDefaultErp = 0.4;
+
+} // namespace
 
 namespace dart {
 namespace constraint {
 
-double MimicMotorConstraint::mConstraintForceMixing = DART_CFM;
+double MimicMotorConstraint::mConstraintForceMixing = kConstraintForceMixing;
+double MimicMotorConstraint::mErrorReductionParameter = kDefaultErp;
 
 //==============================================================================
 MimicMotorConstraint::MimicMotorConstraint(
@@ -117,6 +127,31 @@ double MimicMotorConstraint::getConstraintForceMixing()
   return mConstraintForceMixing;
 }
 
+//==============================================================================
+void MimicMotorConstraint::setErrorReductionParameter(double erp)
+{
+  double clamped = erp;
+  if (clamped < 0.0) {
+    DART_WARN(
+        "Error reduction parameter [{}] is lower than 0.0. It is set to 0.0.",
+        erp);
+    clamped = 0.0;
+  } else if (clamped > 1.0) {
+    DART_WARN(
+        "Error reduction parameter [{}] is greater than 1.0. It is set to 1.0.",
+        erp);
+    clamped = 1.0;
+  }
+
+  mErrorReductionParameter = clamped;
+}
+
+//==============================================================================
+double MimicMotorConstraint::getErrorReductionParameter()
+{
+  return mErrorReductionParameter;
+}
+
 //==============================================================================
 void MimicMotorConstraint::update()
 {
@@ -128,21 +163,33 @@ void MimicMotorConstraint::update()
     const auto& mimicProp = mMimicProps[i];
 
     double timeStep = mJoint->getSkeleton()->getTimeStep();
+    double velLower = mJoint->getVelocityLowerLimit(i);
+    double velUpper = mJoint->getVelocityUpperLimit(i);
+    if (!std::isfinite(velLower))
+      velLower = -kDefaultVelocityLimit;
+    if (!std::isfinite(velUpper))
+      velUpper = kDefaultVelocityLimit;
     double qError
         = mimicProp.mReferenceJoint->getPosition(mimicProp.mReferenceDofIndex)
               * mimicProp.mMultiplier
           + mimicProp.mOffset - mJoint->getPosition(i);
-    double desiredVelocity = math::clip(
-        qError / timeStep,
-        mJoint->getVelocityLowerLimit(i),
-        mJoint->getVelocityUpperLimit(i));
+    const double erp = mErrorReductionParameter;
+    double desiredVelocity
+        = math::clip((erp * qError) / timeStep, velLower, velUpper);
 
     mNegativeVelocityError[i] = desiredVelocity - mJoint->getVelocity(i);
 
     if (mNegativeVelocityError[i] != 0.0) {
       // Note that we are computing impulse not force
-      mUpperBound[i] = mJoint->getForceUpperLimit(i) * timeStep;
-      mLowerBound[i] = mJoint->getForceLowerLimit(i) * timeStep;
+      double upper = mJoint->getForceUpperLimit(i);
+      double lower = mJoint->getForceLowerLimit(i);
+      if (!std::isfinite(upper))
+        upper = kDefaultForceLimit;
+      if (!std::isfinite(lower))
+        lower = -kDefaultForceLimit;
+
+      mUpperBound[i] = upper * timeStep;
+      mLowerBound[i] = lower * timeStep;
 
       if (mActive[i]) {
         ++(mLifeTime[i]);

dart/constraint/MimicMotorConstraint.hpp

@@ -35,17 +35,12 @@
 
 #include <dart/constraint/ConstraintBase.hpp>
 
+#include <dart/dynamics/Fwd.hpp>
 #include <dart/dynamics/MimicDofProperties.hpp>
 
 #include <vector>
 
 namespace dart {
-
-namespace dynamics {
-class BodyNode;
-class Joint;
-} // namespace dynamics
-
 namespace constraint {
 
 /// MimicMotorConstraint behaves like a servo motor: it drives only the
@@ -84,6 +79,12 @@ class MimicMotorConstraint : public ConstraintBase
   /// Get global constraint force mixing parameter
   static double getConstraintForceMixing();
 
+  /// Set global error reduction parameter applied to mimic motors.
+  static void setErrorReductionParameter(double erp);
+
+  /// Get global error reduction parameter applied to mimic motors.
+  static double getErrorReductionParameter();
+
   //----------------------------------------------------------------------------
   // Friendship
   //----------------------------------------------------------------------------
@@ -157,9 +158,12 @@ class MimicMotorConstraint : public ConstraintBase
   double mLowerBound[6];
 
   /// Global constraint force mixing parameter in the range of [1e-9, 1]. The
-  /// default is 1e-5
+  /// default is 1e-6
   /// @sa http://www.ode.org/ode-latest-userguide.html#sec_3_8_0
   static double mConstraintForceMixing;
+
+  /// Global error reduction parameter that scales mimic motor position error.
+  static double mErrorReductionParameter;
 };
 
 } // namespace constraint