add getFootPlacementConstraint

.gitignore

@@ -0,0 +1 @@
+.vscode

.vscode/settings.json

@@ -10,6 +10,74 @@
     "queue": "cpp",
     "string": "cpp",
     "string_view": "cpp",
-    "vector": "cpp"
+    "vector": "cpp",
+    "__bit_reference": "cpp",
+    "__config": "cpp",
+    "__debug": "cpp",
+    "__errc": "cpp",
+    "__functional_base": "cpp",
+    "__hash_table": "cpp",
+    "__mutex_base": "cpp",
+    "__node_handle": "cpp",
+    "__nullptr": "cpp",
+    "__string": "cpp",
+    "__threading_support": "cpp",
+    "__tree": "cpp",
+    "__tuple": "cpp",
+    "algorithm": "cpp",
+    "atomic": "cpp",
+    "bit": "cpp",
+    "bitset": "cpp",
+    "cctype": "cpp",
+    "chrono": "cpp",
+    "clocale": "cpp",
+    "cmath": "cpp",
+    "compare": "cpp",
+    "complex": "cpp",
+    "concepts": "cpp",
+    "condition_variable": "cpp",
+    "cstdarg": "cpp",
+    "cstddef": "cpp",
+    "cstdint": "cpp",
+    "cstdio": "cpp",
+    "cstdlib": "cpp",
+    "cstring": "cpp",
+    "ctime": "cpp",
+    "cwchar": "cpp",
+    "cwctype": "cpp",
+    "exception": "cpp",
+    "functional": "cpp",
+    "initializer_list": "cpp",
+    "ios": "cpp",
+    "iosfwd": "cpp",
+    "iostream": "cpp",
+    "istream": "cpp",
+    "iterator": "cpp",
+    "limits": "cpp",
+    "locale": "cpp",
+    "map": "cpp",
+    "memory": "cpp",
+    "mutex": "cpp",
+    "new": "cpp",
+    "numbers": "cpp",
+    "numeric": "cpp",
+    "optional": "cpp",
+    "ostream": "cpp",
+    "random": "cpp",
+    "ratio": "cpp",
+    "semaphore": "cpp",
+    "sstream": "cpp",
+    "stdexcept": "cpp",
+    "streambuf": "cpp",
+    "system_error": "cpp",
+    "thread": "cpp",
+    "tuple": "cpp",
+    "type_traits": "cpp",
+    "typeinfo": "cpp",
+    "unordered_map": "cpp",
+    "utility": "cpp",
+    "*.tcc": "cpp",
+    "memory_resource": "cpp",
+    "stop_token": "cpp"
   }
 }

CMakeLists.txt

@@ -61,6 +61,7 @@ add_library(
   src/common/ModelSettings.cpp
   src/dynamics/LeggedRobotDynamicsAD.cpp
   src/constraint/EndEffectorLinearConstraint.cpp
+  src/constraint/FootPlacementConstraint.cpp
   src/constraint/FrictionConeConstraint.cpp
   src/constraint/ZeroForceConstraint.cpp
   src/constraint/NormalVelocityConstraintCppAd.cpp

include/ocs2_legged_robot/LeggedRobotInterface.h

@@ -108,6 +108,9 @@ class LeggedRobotInterface final : public RobotInterface {
 
   std::pair<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings(
       const std::string& taskFile) const;
+  std::unique_ptr<StateConstraint> getFootPlacementConstraint(
+      const EndEffectorKinematics<scalar_t>& eeKinematics,
+      size_t contactPointIndex);
   std::unique_ptr<StateInputCost> getFrictionConeConstraint(
       size_t contactPointIndex, scalar_t frictionCoefficient,
       const RelaxedBarrierPenalty::Config& barrierPenaltyConfig);

src/LeggedRobotInterface.cpp

@@ -46,7 +46,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <string>
 
 #include "ocs2_legged_robot/LeggedRobotPreComputation.h"
-#include "ocs2_legged_robot/constraint/FrictionConeConstraint.h"
+#include "ocs2_legged_robot/constraint/FootPlace.h"
+#include "ocs2_legged_robot/constraint/FootPlacementConstraint.h"
 #include "ocs2_legged_robot/constraint/NormalVelocityConstraintCppAd.h"
 #include "ocs2_legged_robot/constraint/ZeroForceConstraint.h"
 #include "ocs2_legged_robot/constraint/ZeroVelocityConstraintCppAd.h"
@@ -217,6 +218,13 @@ void LeggedRobotInterface::setupOptimalConrolProblem(
         footName + "_normalVelocity",
         getNormalVelocityConstraint(*eeKinematicsPtr, i,
                                     useAnalyticalGradientsConstraints));
+
+    const bool addFootPlacementConstraint = true;
+    if (addFootPlacementConstraint) {
+      problemPtr_->inequalityConstraintPtr->add(
+          footName + "_footPlacement",
+          getFootPlacementConstraint(*eeKinematicsPtr, i));
+    }
   }
 
   // Pre-computation
@@ -322,6 +330,15 @@ LeggedRobotInterface::loadFrictionConeSettings(
   return {frictionCoefficient, std::move(barrierPenaltyConfig)};
 }
 
+std::unique_ptr<StateConstraint>
+LeggedRobotInterface::getFootPlacementConstraint(
+    const EndEffectorKinematics<scalar_t>& eeKinematics,
+    size_t contactPointIndex) {
+  // TODO
+  return std::unique_ptr<StateConstraint>(new FootPlacementConstraint(
+      *referenceManagerPtr_, eeKinematics, contactPointIndex));
+}
+
 std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeConstraint(
     size_t contactPointIndex, scalar_t frictionCoefficient,
     const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) {

src/constraint/FootPlacementConstraint.cpp

@@ -15,7 +15,13 @@ FootPlacementConstraint::FootPlacementConstraint(
       contactPointIndex_(contactPointIndex),
       terrainGap_(0.3),
       terrainSizeX_(0.2),
-      terrainSizeY_(1.0) {}
+      terrainSizeY_(1.0) {
+  if (endEffectorKinematicsPtr_->getIds().size() != 1) {
+    throw std::runtime_error(
+        "[FootPlacementConstraint] this class only accepts a single "
+        "end-effector!");
+  }
+}
 
 FootPlacementConstraint::isActive(scalar_t time) const {
   const scalar_t trotGaitWholeCycle = 0.7;