add perceptive interface, fix compile problems

README.md

@@ -10,8 +10,9 @@ This repository contains the ros2-control based controllers for the quadruped ro
 Todo List:
 
 - [x] **[2025-02-23]** Add Gazebo Playground
-- [ ] OCS2 controller for Gazebo Simulation
+  - [x] OCS2 controller for Gazebo Simulation
-- [ ] Refactor FSM and Unitree Guide Controller
+  - [x] Refactor FSM and Unitree Guide Controller
+- [ ] OCS2 Perceptive locomotion demo
 
 Video for Unitree Guide Controller:
 [![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/)

controllers/ocs2_quadruped_controller/CMakeLists.txt

@@ -21,6 +21,11 @@ set(CONTROLLER_INCLUDE_DEPENDS
         nav_msgs
         qpoases_colcon
         ament_index_cpp
+
+        convex_plane_decomposition_msgs
+        convex_plane_decomposition_ros
+        grid_map_sdf
+        ocs2_sphere_approximation
 )
 
 # find dependencies
@@ -58,6 +63,13 @@ add_library(${PROJECT_NAME} SHARED
         src/control/TargetManager.cpp
         src/control/CtrlComponent.cpp
 
+        src/perceptive_interface/constraint/FootCollisionConstraint.cpp
+        src/perceptive_interface/constraint/FootPlacementConstraint.cpp
+        src/perceptive_interface/constraint/SphereSdfConstraint.cpp
+        src/perceptive_interface/ConvexRegionSelector.cpp
+        src/perceptive_interface/PerceptiveLeggedInterface.cpp
+        src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp
+        src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp
 )
 target_include_directories(${PROJECT_NAME}
         PUBLIC

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h

@@ -22,7 +22,7 @@
 
 namespace ocs2::legged_robot
 {
-    class LeggedInterface final : public RobotInterface
+    class LeggedInterface : public RobotInterface
     {
     public:
         LeggedInterface(const std::string& task_file,
@@ -35,7 +35,7 @@ namespace ocs2::legged_robot
         void setupJointNames(const std::vector<std::string>& joint_names,
                              const std::vector<std::string>& foot_names);
 
-        void setupOptimalControlProblem(const std::string& task_file,
+        virtual void setupOptimalControlProblem(const std::string& task_file,
                                                 const std::string& urdf_file,
                                                 const std::string& reference_file,
                                                 bool verbose);
@@ -66,7 +66,7 @@ namespace ocs2::legged_robot
         void setupModel(const std::string& task_file, const std::string& urdf_file,
                         const std::string& reference_file);
 
-        void setupReferenceManager(const std::string& taskFile, const std::string& urdfFile,
+        virtual void setupReferenceManager(const std::string& taskFile, const std::string& urdfFile,
                                            const std::string& referenceFile,
                                            bool verbose);
 

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h

@@ -0,0 +1,71 @@
+//
+// Created by qiayuan on 23-1-2.
+//
+
+#pragma once
+
+#include <ocs2_core/reference/ModeSchedule.h>
+
+#include <convex_plane_decomposition/PlanarRegion.h>
+#include <convex_plane_decomposition/PolygonTypes.h>
+#include <convex_plane_decomposition/SegmentedPlaneProjection.h>
+#include <ocs2_centroidal_model/CentroidalModelInfo.h>
+#include <ocs2_core/reference/TargetTrajectories.h>
+#include <ocs2_legged_robot/common/Types.h>
+#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
+
+namespace ocs2::legged_robot
+{
+    class ConvexRegionSelector
+    {
+    public:
+        ConvexRegionSelector(CentroidalModelInfo info,
+                             std::shared_ptr<convex_plane_decomposition::PlanarTerrain> PlanarTerrainPtr,
+                             const EndEffectorKinematics<scalar_t>& endEffectorKinematics, size_t numVertices);
+
+        void update(const ModeSchedule& modeSchedule, scalar_t initTime, const vector_t& initState,
+                    TargetTrajectories& targetTrajectories);
+
+        convex_plane_decomposition::PlanarTerrainProjection getProjection(size_t leg, scalar_t time) const;
+
+        convex_plane_decomposition::CgalPolygon2d getConvexPolygon(size_t leg, scalar_t time) const;
+
+        vector3_t getNominalFootholds(size_t leg, scalar_t time) const;
+
+        std::vector<scalar_t> getMiddleTimes(size_t leg) const { return middleTimes_[leg]; }
+
+        std::vector<convex_plane_decomposition::PlanarTerrainProjection> getProjections(size_t leg)
+        {
+            return feetProjections_[leg];
+        }
+
+        std::shared_ptr<convex_plane_decomposition::PlanarTerrain> getPlanarTerrainPtr() { return planarTerrainPtr_; }
+
+        feet_array_t<scalar_t> getInitStandFinalTimes() { return initStandFinalTime_; }
+
+        feet_array_t<std::vector<bool>> extractContactFlags(const std::vector<size_t>& phaseIDsStock) const;
+
+    private:
+        static std::pair<int, int> findIndex(size_t index, const std::vector<bool>& contactFlagStock);
+
+        vector3_t getNominalFoothold(size_t leg, scalar_t time, const vector_t& initState,
+                                     TargetTrajectories& targetTrajectories);
+
+        feet_array_t<std::vector<convex_plane_decomposition::PlanarTerrainProjection>> feetProjections_;
+        feet_array_t<std::vector<convex_plane_decomposition::CgalPolygon2d>> convexPolygons_;
+
+        feet_array_t<std::vector<vector3_t>> nominalFootholds_;
+        feet_array_t<std::vector<scalar_t>> middleTimes_;
+
+        feet_array_t<scalar_t> initStandFinalTime_;
+
+        feet_array_t<std::vector<scalar_t>> timeEvents_;
+
+        const CentroidalModelInfo info_;
+        size_t numVertices_;
+
+        convex_plane_decomposition::PlanarTerrain planarTerrain_;
+        std::shared_ptr<convex_plane_decomposition::PlanarTerrain> planarTerrainPtr_;
+        std::unique_ptr<EndEffectorKinematics<scalar_t>> endEffectorKinematicsPtr_;
+    };
+} // namespace ocs2::legged_robot

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h

@@ -0,0 +1,56 @@
+//
+// Created by qiayuan on 22-12-27.
+//
+
+#pragma once
+
+#include <convex_plane_decomposition/PlanarRegion.h>
+#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
+#include <ocs2_sphere_approximation/PinocchioSphereInterface.h>
+#include <grid_map_sdf/SignedDistanceField.hpp>
+
+namespace ocs2::legged_robot
+{
+    class PerceptiveLeggedInterface final : public LeggedInterface
+    {
+    public:
+        using LeggedInterface::LeggedInterface;
+
+        void setupOptimalControlProblem(const std::string& taskFile,
+                                        const std::string& urdfFile,
+                                        const std::string& referenceFile,
+                                        bool verbose) override;
+
+        void setupReferenceManager(const std::string& taskFile, const std::string& urdfFile,
+                                   const std::string& referenceFile,
+                                   bool verbose) override;
+
+        void setupPreComputation(const std::string& taskFile, const std::string& urdfFile,
+                                 const std::string& referenceFile,
+                                 bool verbose);
+
+        std::shared_ptr<grid_map::SignedDistanceField> getSignedDistanceFieldPtr() const
+        {
+            return signedDistanceFieldPtr_;
+        }
+
+        std::shared_ptr<convex_plane_decomposition::PlanarTerrain> getPlanarTerrainPtr() const
+        {
+            return planarTerrainPtr_;
+        }
+
+        std::shared_ptr<PinocchioSphereInterface> getPinocchioSphereInterfacePtr() const
+        {
+            return pinocchioSphereInterfacePtr_;
+        }
+
+        size_t getNumVertices() const { return numVertices_; }
+
+    protected:
+        size_t numVertices_ = 16;
+
+        std::shared_ptr<convex_plane_decomposition::PlanarTerrain> planarTerrainPtr_;
+        std::shared_ptr<grid_map::SignedDistanceField> signedDistanceFieldPtr_;
+        std::shared_ptr<PinocchioSphereInterface> pinocchioSphereInterfacePtr_;
+    };
+} // namespace ocs2::legged_robot

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h

@@ -0,0 +1,45 @@
+//
+// Created by qiayuan on 23-1-1.
+//
+
+#pragma once
+
+#include <ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h>
+
+#include <convex_plane_decomposition/PlanarRegion.h>
+#include <convex_plane_decomposition/PolygonTypes.h>
+
+#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h"
+#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h"
+
+namespace ocs2::legged_robot
+{
+    /** Callback for caching and reference update */
+    class PerceptiveLeggedPrecomputation : public LeggedRobotPreComputation
+    {
+    public:
+        PerceptiveLeggedPrecomputation(PinocchioInterface pinocchioInterface, const CentroidalModelInfo& info,
+                                       const SwingTrajectoryPlanner& swingTrajectoryPlanner, ModelSettings settings,
+                                       const ConvexRegionSelector& convexRegionSelector);
+        ~PerceptiveLeggedPrecomputation() override = default;
+
+        PerceptiveLeggedPrecomputation* clone() const override { return new PerceptiveLeggedPrecomputation(*this); }
+
+        void request(RequestSet request, scalar_t t, const vector_t& x, const vector_t& u) override;
+
+        const std::vector<FootPlacementConstraint::Parameter>& getFootPlacementConParameters() const
+        {
+            return footPlacementConParameters_;
+        }
+
+        PerceptiveLeggedPrecomputation(const PerceptiveLeggedPrecomputation& rhs);
+
+    private:
+        std::pair<matrix_t, vector_t> getPolygonConstraint(
+            const convex_plane_decomposition::CgalPolygon2d& polygon) const;
+
+        const ConvexRegionSelector* convexRegionSelectorPtr_;
+
+        std::vector<FootPlacementConstraint::Parameter> footPlacementConParameters_;
+    };
+} // namespace ocs2::legged_robot

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h

@@ -0,0 +1,52 @@
+//
+// Created by qiayuan on 23-1-3.
+//
+#pragma once
+
+#include <memory>
+
+#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h"
+
+#include <ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h>
+
+namespace ocs2::legged_robot
+{
+    class PerceptiveLeggedReferenceManager : public SwitchedModelReferenceManager
+    {
+    public:
+        PerceptiveLeggedReferenceManager(CentroidalModelInfo info, std::shared_ptr<GaitSchedule> gaitSchedulePtr,
+                                         std::shared_ptr<SwingTrajectoryPlanner> swingTrajectoryPtr,
+                                         std::shared_ptr<ConvexRegionSelector> convexRegionSelectorPtr,
+                                         const EndEffectorKinematics<scalar_t>& endEffectorKinematics,
+                                         scalar_t comHeight);
+
+        const std::shared_ptr<ConvexRegionSelector>& getConvexRegionSelectorPtr() { return convexRegionSelectorPtr_; }
+
+        contact_flag_t getFootPlacementFlags(scalar_t time) const;
+
+    protected:
+        void modifyReferences(scalar_t initTime, scalar_t finalTime, const vector_t& initState,
+                              TargetTrajectories& targetTrajectories,
+                              ModeSchedule& modeSchedule) override;
+
+        virtual void updateSwingTrajectoryPlanner(scalar_t initTime, const vector_t& initState,
+                                                  ModeSchedule& modeSchedule);
+
+        void modifyProjections(scalar_t initTime, const vector_t& initState, size_t leg, size_t initIndex,
+                               const std::vector<bool>& contactFlagStocks,
+                               std::vector<convex_plane_decomposition::PlanarTerrainProjection>& projections);
+
+        std::pair<scalar_array_t, scalar_array_t> getHeights(const std::vector<bool>& contactFlagStocks,
+                                                             const std::vector<
+                                                                 convex_plane_decomposition::PlanarTerrainProjection>&
+                                                             projections);
+
+        const CentroidalModelInfo info_;
+        feet_array_t<vector3_t> lastLiftoffPos_;
+
+        std::shared_ptr<ConvexRegionSelector> convexRegionSelectorPtr_;
+        std::unique_ptr<EndEffectorKinematics<scalar_t>> endEffectorKinematicsPtr_;
+
+        scalar_t comHeight_;
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h

@@ -0,0 +1,41 @@
+//
+// Created by qiayuan on 23-1-1.
+//
+
+#pragma once
+
+#include <ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h>
+#include <ocs2_core/constraint/StateConstraint.h>
+#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h>
+#include <grid_map_sdf/SignedDistanceField.hpp>
+
+namespace ocs2::legged_robot
+{
+    class FootCollisionConstraint final : public StateConstraint
+    {
+    public:
+        FootCollisionConstraint(const SwitchedModelReferenceManager& referenceManager,
+                                const EndEffectorKinematics<scalar_t>& endEffectorKinematics,
+                                std::shared_ptr<grid_map::SignedDistanceField> sdfPtr, size_t contactPointIndex,
+                                scalar_t clearance);
+
+        ~FootCollisionConstraint() override = default;
+        FootCollisionConstraint* clone() const override { return new FootCollisionConstraint(*this); }
+
+        bool isActive(scalar_t time) const override;
+        size_t getNumConstraints(scalar_t /*time*/) const override { return 1; }
+        vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override;
+        VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state,
+                                                                 const PreComputation& preComp) const override;
+
+    private:
+        FootCollisionConstraint(const FootCollisionConstraint& rhs);
+
+        const SwitchedModelReferenceManager* referenceManagerPtr_;
+        std::unique_ptr<EndEffectorKinematics<scalar_t>> endEffectorKinematicsPtr_;
+        std::shared_ptr<grid_map::SignedDistanceField> sdfPtr_;
+
+        const size_t contactPointIndex_;
+        const scalar_t clearance_;
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h

@@ -0,0 +1,44 @@
+//
+// Created by qiayuan on 23-1-1.
+//
+
+#pragma once
+
+#include <ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h>
+#include <ocs2_core/constraint/StateConstraint.h>
+#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h>
+
+namespace ocs2::legged_robot
+{
+    class FootPlacementConstraint final : public StateConstraint
+    {
+    public:
+        struct Parameter
+        {
+            matrix_t a;
+            vector_t b;
+        };
+
+        FootPlacementConstraint(const SwitchedModelReferenceManager& referenceManager,
+                                const EndEffectorKinematics<scalar_t>& endEffectorKinematics, size_t contactPointIndex,
+                                size_t numVertices);
+
+        ~FootPlacementConstraint() override = default;
+        FootPlacementConstraint* clone() const override { return new FootPlacementConstraint(*this); }
+
+        bool isActive(scalar_t time) const override;
+        size_t getNumConstraints(scalar_t /*time*/) const override { return numVertices_; }
+        vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override;
+        VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state,
+                                                                 const PreComputation& preComp) const override;
+
+    private:
+        FootPlacementConstraint(const FootPlacementConstraint& rhs);
+
+        const SwitchedModelReferenceManager* referenceManagerPtr_;
+        std::unique_ptr<EndEffectorKinematics<scalar_t>> endEffectorKinematicsPtr_;
+
+        const size_t contactPointIndex_;
+        const size_t numVertices_;
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h

@@ -0,0 +1,39 @@
+//
+// Created by qiayuan on 22-12-27.
+//
+
+#pragma once
+
+#include <memory>
+
+#include <ocs2_core/constraint/StateConstraint.h>
+#include <ocs2_sphere_approximation/PinocchioSphereKinematics.h>
+#include <grid_map_sdf/SignedDistanceField.hpp>
+
+namespace ocs2::legged_robot
+{
+    class SphereSdfConstraint final : public StateConstraint
+    {
+    public:
+        SphereSdfConstraint(const PinocchioSphereKinematics& sphereKinematics,
+                            std::shared_ptr<grid_map::SignedDistanceField> sdfPtr);
+
+        /** Default destructor */
+        ~SphereSdfConstraint() override = default;
+
+        SphereSdfConstraint* clone() const override { return new SphereSdfConstraint(*this); }
+
+        size_t getNumConstraints(scalar_t /*time*/) const override { return numConstraints_; }
+
+        vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override;
+        VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state,
+                                                                 const PreComputation& preComp) const override;
+
+    private:
+        SphereSdfConstraint(const SphereSdfConstraint& rhs);
+
+        std::unique_ptr<PinocchioSphereKinematics> sphereKinematicsPtr_;
+        std::shared_ptr<grid_map::SignedDistanceField> sdfPtr_;
+        size_t numConstraints_{};
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/package.xml

@@ -19,6 +19,12 @@
   <depend>ocs2_legged_robot_ros</depend>
   <depend>angles</depend>
 
+  <!--/ Dependencies for Perceptive Mode -->
+  <depend>convex_plane_decomposition_msgs</depend>
+  <depend>convex_plane_decomposition_ros</depend>
+  <depend>grid_map_sdf</depend>
+  <depend>ocs2_sphere_approximation</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

controllers/ocs2_quadruped_controller/src/perceptive_interface/ConvexRegionSelector.cpp

@@ -0,0 +1,206 @@
+//
+// Created by qiayuan on 23-1-2.
+//
+#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h"
+
+#include <ocs2_centroidal_model/AccessHelperFunctions.h>
+#include <ocs2_core/misc/Lookup.h>
+#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
+
+#include <convex_plane_decomposition/ConvexRegionGrowing.h>
+
+namespace ocs2::legged_robot
+{
+    ConvexRegionSelector::ConvexRegionSelector(CentroidalModelInfo info,
+                                               std::shared_ptr<convex_plane_decomposition::PlanarTerrain>
+                                               planarTerrainPtr,
+                                               const EndEffectorKinematics<scalar_t>& endEffectorKinematics,
+                                               size_t numVertices)
+        : info_(std::move(info)),
+          numVertices_(numVertices),
+          planarTerrainPtr_(std::move(planarTerrainPtr)),
+          endEffectorKinematicsPtr_(endEffectorKinematics.clone())
+    {
+    }
+
+    convex_plane_decomposition::PlanarTerrainProjection ConvexRegionSelector::getProjection(
+        size_t leg, scalar_t time) const
+    {
+        const auto index = lookup::findIndexInTimeArray(timeEvents_[leg], time);
+        return feetProjections_[leg][index];
+    }
+
+    convex_plane_decomposition::CgalPolygon2d ConvexRegionSelector::getConvexPolygon(size_t leg, scalar_t time) const
+    {
+        const auto index = lookup::findIndexInTimeArray(timeEvents_[leg], time);
+        return convexPolygons_[leg][index];
+    }
+
+    vector3_t ConvexRegionSelector::getNominalFootholds(size_t leg, scalar_t time) const
+    {
+        const auto index = lookup::findIndexInTimeArray(timeEvents_[leg], time);
+        return nominalFootholds_[leg][index];
+    }
+
+    void ConvexRegionSelector::update(const ModeSchedule& modeSchedule, scalar_t initTime, const vector_t& initState,
+                                      TargetTrajectories& targetTrajectories)
+    {
+        planarTerrain_ = *planarTerrainPtr_;
+        // Need copy storage it since PlanarTerrainProjection.regionPtr is a pointer
+        const auto& modeSequence = modeSchedule.modeSequence;
+        const auto& eventTimes = modeSchedule.eventTimes;
+        const auto contactFlagStocks = extractContactFlags(modeSequence);
+        const size_t numPhases = modeSequence.size();
+
+        // Find start and final index of time for legs
+        feet_array_t<std::vector<int>> startIndices;
+        feet_array_t<std::vector<int>> finalIndices;
+        for (size_t leg = 0; leg < info_.numThreeDofContacts; leg++)
+        {
+            startIndices[leg] = std::vector<int>(numPhases, 0);
+            finalIndices[leg] = std::vector<int>(numPhases, 0);
+            // find the startTime and finalTime indices for swing feet
+            for (size_t i = 0; i < numPhases; i++)
+            {
+                // skip if it is a stance leg
+                if (contactFlagStocks[leg][i])
+                {
+                    std::tie(startIndices[leg][i], finalIndices[leg][i]) = findIndex(i, contactFlagStocks[leg]);
+                }
+            }
+        }
+
+        for (size_t leg = 0; leg < info_.numThreeDofContacts; leg++)
+        {
+            feetProjections_[leg].clear();
+            convexPolygons_[leg].clear();
+            nominalFootholds_[leg].clear();
+            feetProjections_[leg].resize(numPhases);
+            convexPolygons_[leg].resize(numPhases);
+            nominalFootholds_[leg].resize(numPhases);
+            middleTimes_[leg].clear();
+            initStandFinalTime_[leg] = 0;
+
+            scalar_t lastStandMiddleTime = NAN;
+            // Stand leg foot
+            for (size_t i = 0; i < numPhases; ++i)
+            {
+                if (contactFlagStocks[leg][i])
+                {
+                    const int standStartIndex = startIndices[leg][i];
+                    const int standFinalIndex = finalIndices[leg][i];
+                    const scalar_t standStartTime = eventTimes[standStartIndex];
+                    const scalar_t standFinalTime = eventTimes[standFinalIndex];
+                    const scalar_t standMiddleTime = standStartTime + (standFinalTime - standStartTime) / 2;
+
+                    if (!numerics::almost_eq(standMiddleTime, lastStandMiddleTime))
+                    {
+                        vector3_t footPos = getNominalFoothold(leg, standMiddleTime, initState, targetTrajectories);
+                        auto penaltyFunction = [](const vector3_t& /*projectedPoint*/) { return 0.0; };
+                        const auto projection = getBestPlanarRegionAtPositionInWorld(
+                            footPos, planarTerrain_.planarRegions, penaltyFunction);
+                        scalar_t growthFactor = 1.05;
+                        const auto convexRegion = convex_plane_decomposition::growConvexPolygonInsideShape(
+                            projection.regionPtr->boundaryWithInset.boundary, projection.positionInTerrainFrame,
+                            numVertices_, growthFactor);
+
+                        feetProjections_[leg][i] = projection;
+                        convexPolygons_[leg][i] = convexRegion;
+                        nominalFootholds_[leg][i] = footPos;
+                        middleTimes_[leg].push_back(standMiddleTime);
+                    }
+                    else
+                    {
+                        feetProjections_[leg][i] = feetProjections_[leg][i - 1];
+                        convexPolygons_[leg][i] = convexPolygons_[leg][i - 1];
+                        nominalFootholds_[leg][i] = nominalFootholds_[leg][i - 1];
+                    }
+
+                    if (standStartTime < initTime && initTime < standFinalTime)
+                    {
+                        initStandFinalTime_[leg] = standFinalTime;
+                    }
+                }
+            }
+        }
+
+        for (size_t leg = 0; leg < info_.numThreeDofContacts; leg++)
+        {
+            timeEvents_[leg] = eventTimes;
+        }
+    }
+
+    feet_array_t<std::vector<bool>> ConvexRegionSelector::extractContactFlags(
+        const std::vector<size_t>& phaseIDsStock) const
+    {
+        const size_t numPhases = phaseIDsStock.size();
+
+        feet_array_t<std::vector<bool>> contactFlagStock;
+        std::fill(contactFlagStock.begin(), contactFlagStock.end(), std::vector<bool>(numPhases));
+
+        for (size_t i = 0; i < numPhases; i++)
+        {
+            const auto contactFlag = modeNumber2StanceLeg(phaseIDsStock[i]);
+            for (size_t j = 0; j < info_.numThreeDofContacts; j++)
+            {
+                contactFlagStock[j][i] = contactFlag[j];
+            }
+        }
+        return contactFlagStock;
+    }
+
+    std::pair<int, int> ConvexRegionSelector::findIndex(size_t index, const std::vector<bool>& contactFlagStock)
+    {
+        const size_t numPhases = contactFlagStock.size();
+
+        if (!contactFlagStock[index])
+        {
+            return {0, 0};
+        }
+
+        // find the starting time
+        int startTimesIndex = 0;
+        for (int ip = index - 1; ip >= 0; ip--)
+        {
+            if (!contactFlagStock[ip])
+            {
+                startTimesIndex = ip;
+                break;
+            }
+        }
+        // find the final time
+        int finalTimesIndex = numPhases - 2;
+        for (size_t ip = index + 1; ip < numPhases; ip++)
+        {
+            if (!contactFlagStock[ip])
+            {
+                finalTimesIndex = ip - 1;
+                break;
+            }
+        }
+        return {startTimesIndex, finalTimesIndex};
+    }
+
+    vector3_t ConvexRegionSelector::getNominalFoothold(size_t leg, scalar_t time, const vector_t& initState,
+                                                       TargetTrajectories& targetTrajectories)
+    {
+        scalar_t height = 0.4;
+
+        vector_t desiredState = targetTrajectories.getDesiredState(time);
+        vector3_t desiredVel = centroidal_model::getNormalizedMomentum(desiredState, info_).head(3);
+        vector3_t measuredVel = centroidal_model::getNormalizedMomentum(initState, info_).head(3);
+
+        auto feedback = (vector3_t() << (std::sqrt(height / 9.81) * (measuredVel - desiredVel)).head(2), 0).finished();
+        vector_t zyx = centroidal_model::getBasePose(desiredState, info_).tail(3);
+        scalar_t offset = tan(-zyx(1)) * height;
+        vector3_t offsetVector(offset * cos(-zyx(1)), 0, offset * sin(-zyx(1)));
+        matrix3_t R;
+        scalar_t z = zyx(0);
+        R << cos(z), -sin(z), 0, // clang-format off
+             sin(z), cos(z), 0,
+             0, 0, 1;  // clang-format on
+        //  return endEffectorKinematicsPtr_->getPosition(targetTrajectories.getDesiredState(time))[leg] + feedback;
+        return endEffectorKinematicsPtr_->getPosition(targetTrajectories.getDesiredState(time))[leg] - R.transpose() *
+            offsetVector;
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedInterface.cpp

@@ -0,0 +1,136 @@
+//
+// Created by qiayuan on 22-12-27.
+//
+
+#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h"
+#include "ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h"
+#include <ocs2_core/misc/LoadData.h>
+
+#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h"
+#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h"
+#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h"
+#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h"
+
+#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
+#include <ocs2_core/soft_constraint/StateSoftConstraint.h>
+#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematicsCppAd.h>
+
+#include <memory>
+
+namespace ocs2::legged_robot
+{
+    void PerceptiveLeggedInterface::setupOptimalControlProblem(const std::string& taskFile, const std::string& urdfFile,
+                                                               const std::string& referenceFile, bool verbose)
+    {
+        planarTerrainPtr_ = std::make_shared<convex_plane_decomposition::PlanarTerrain>();
+
+        double width{5.0}, height{5.0};
+        convex_plane_decomposition::PlanarRegion plannerRegion;
+        plannerRegion.transformPlaneToWorld.setIdentity();
+        plannerRegion.bbox2d = convex_plane_decomposition::CgalBbox2d(-height / 2, -width / 2, +height / 2, width / 2);
+        convex_plane_decomposition::CgalPolygonWithHoles2d boundary;
+        boundary.outer_boundary().push_back(convex_plane_decomposition::CgalPoint2d(+height / 2, +width / 2));
+        boundary.outer_boundary().push_back(convex_plane_decomposition::CgalPoint2d(-height / 2, +width / 2));
+        boundary.outer_boundary().push_back(convex_plane_decomposition::CgalPoint2d(-height / 2, -width / 2));
+        boundary.outer_boundary().push_back(convex_plane_decomposition::CgalPoint2d(+height / 2, -width / 2));
+        plannerRegion.boundaryWithInset.boundary = boundary;
+        convex_plane_decomposition::CgalPolygonWithHoles2d insets;
+        insets.outer_boundary().push_back(
+            convex_plane_decomposition::CgalPoint2d(+height / 2 - 0.01, +width / 2 - 0.01));
+        insets.outer_boundary().push_back(
+            convex_plane_decomposition::CgalPoint2d(-height / 2 + 0.01, +width / 2 - 0.01));
+        insets.outer_boundary().push_back(
+            convex_plane_decomposition::CgalPoint2d(-height / 2 + 0.01, -width / 2 + 0.01));
+        insets.outer_boundary().push_back(
+            convex_plane_decomposition::CgalPoint2d(+height / 2 - 0.01, -width / 2 + 0.01));
+        plannerRegion.boundaryWithInset.insets.push_back(insets);
+        planarTerrainPtr_->planarRegions.push_back(plannerRegion);
+
+        std::string layer = "elevation_before_postprocess";
+        planarTerrainPtr_->gridMap.setGeometry(grid_map::Length(5.0, 5.0), 0.03);
+        planarTerrainPtr_->gridMap.add(layer, 0);
+        planarTerrainPtr_->gridMap.add("smooth_planar", 0);
+        signedDistanceFieldPtr_ = std::make_shared<grid_map::SignedDistanceField>();
+        signedDistanceFieldPtr_->calculateSignedDistanceField(planarTerrainPtr_->gridMap, layer, 0.1);
+
+        LeggedInterface::setupOptimalControlProblem(taskFile, urdfFile, referenceFile, verbose);
+
+        for (size_t i = 0; i < centroidal_model_info_.numThreeDofContacts; i++)
+        {
+            const std::string& footName = modelSettings().contactNames3DoF[i];
+            std::unique_ptr<EndEffectorKinematics<scalar_t>> eeKinematicsPtr = getEeKinematicsPtr({footName}, footName);
+
+            std::unique_ptr<PenaltyBase> placementPenalty(
+                new RelaxedBarrierPenalty(RelaxedBarrierPenalty::Config(1e-2, 1e-4)));
+            std::unique_ptr<PenaltyBase> collisionPenalty(
+                new RelaxedBarrierPenalty(RelaxedBarrierPenalty::Config(1e-2, 1e-3)));
+
+            // For foot placement
+            std::unique_ptr<FootPlacementConstraint> footPlacementConstraint(
+                new FootPlacementConstraint(*reference_manager_ptr_, *eeKinematicsPtr, i, numVertices_));
+            problem_ptr_->stateSoftConstraintPtr->add(
+                footName + "_footPlacement",
+                std::make_unique<StateSoftConstraint>(std::move(footPlacementConstraint), std::move(placementPenalty)));
+
+            // For foot Collision
+            std::unique_ptr<FootCollisionConstraint> footCollisionConstraint(
+                new FootCollisionConstraint(*reference_manager_ptr_, *eeKinematicsPtr, signedDistanceFieldPtr_, i,
+                                            0.03));
+            problem_ptr_->stateSoftConstraintPtr->add(
+                footName + "_footCollision",
+                std::make_unique<StateSoftConstraint>(std::move(footCollisionConstraint), std::move(collisionPenalty)));
+        }
+
+        // For collision avoidance
+        scalar_t thighExcess = 0.025;
+        scalar_t calfExcess = 0.02;
+
+        std::vector<std::string> collisionLinks = {"LF_calf", "RF_calf", "LH_calf", "RH_calf"};
+        const std::vector<scalar_t>& maxExcesses = {calfExcess, calfExcess, calfExcess, calfExcess};
+
+        pinocchioSphereInterfacePtr_ = std::make_shared<PinocchioSphereInterface>(
+            *pinocchio_interface_ptr_, collisionLinks, maxExcesses, 0.6);
+
+        CentroidalModelPinocchioMapping pinocchioMapping(centroidal_model_info_);
+        auto sphereKinematicsPtr = std::make_unique<PinocchioSphereKinematics>(
+            *pinocchioSphereInterfacePtr_, pinocchioMapping);
+
+        std::unique_ptr<SphereSdfConstraint> sphereSdfConstraint(
+            new SphereSdfConstraint(*sphereKinematicsPtr, signedDistanceFieldPtr_));
+
+        //  std::unique_ptr<PenaltyBase> penalty(new RelaxedBarrierPenalty(RelaxedBarrierPenalty::Config(1e-3, 1e-3)));
+        //  problem_ptr_->stateSoftConstraintPtr->add(
+        //      "sdfConstraint", std::unique_ptr<StateCost>(new StateSoftConstraint(std::move(sphereSdfConstraint), std::move(penalty))));
+    }
+
+    void PerceptiveLeggedInterface::setupReferenceManager(const std::string& taskFile, const std::string& /*urdfFile*/,
+                                                          const std::string& referenceFile, bool verbose)
+    {
+        auto swingTrajectoryPlanner =
+            std::make_unique<SwingTrajectoryPlanner>(
+                loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4);
+
+        std::unique_ptr<EndEffectorKinematics<scalar_t>> eeKinematicsPtr = getEeKinematicsPtr(
+            {model_settings_.contactNames3DoF}, "ALL_FOOT");
+        auto convexRegionSelector =
+            std::make_unique<ConvexRegionSelector>(centroidal_model_info_, planarTerrainPtr_, *eeKinematicsPtr,
+                                                   numVertices_);
+
+        scalar_t comHeight = 0;
+        loadData::loadCppDataType(referenceFile, "comHeight", comHeight);
+        reference_manager_ptr_.reset(new PerceptiveLeggedReferenceManager(
+            centroidal_model_info_, loadGaitSchedule(referenceFile, verbose),
+            std::move(swingTrajectoryPlanner), std::move(convexRegionSelector),
+            *eeKinematicsPtr, comHeight));
+    }
+
+    void PerceptiveLeggedInterface::setupPreComputation(const std::string& /*taskFile*/,
+                                                        const std::string& /*urdfFile*/,
+                                                        const std::string& /*referenceFile*/, bool /*verbose*/)
+    {
+        problem_ptr_->preComputationPtr = std::make_unique<PerceptiveLeggedPrecomputation>(
+            *pinocchio_interface_ptr_, centroidal_model_info_, *reference_manager_ptr_->getSwingTrajectoryPlanner(),
+            model_settings_,
+            *dynamic_cast<PerceptiveLeggedReferenceManager&>(*reference_manager_ptr_).getConvexRegionSelectorPtr());
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp

@@ -0,0 +1,96 @@
+//
+// Created by qiayuan on 23-1-1.
+//
+
+#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h"
+
+namespace ocs2::legged_robot
+{
+    PerceptiveLeggedPrecomputation::PerceptiveLeggedPrecomputation(PinocchioInterface pinocchioInterface,
+                                                                   const CentroidalModelInfo& info,
+                                                                   const SwingTrajectoryPlanner& swingTrajectoryPlanner,
+                                                                   ModelSettings settings,
+                                                                   const ConvexRegionSelector& convexRegionSelector)
+        : LeggedRobotPreComputation(std::move(pinocchioInterface), info, swingTrajectoryPlanner, std::move(settings)),
+          convexRegionSelectorPtr_(&convexRegionSelector)
+    {
+        footPlacementConParameters_.resize(info.numThreeDofContacts);
+    }
+
+    PerceptiveLeggedPrecomputation::PerceptiveLeggedPrecomputation(const PerceptiveLeggedPrecomputation& rhs)
+        : LeggedRobotPreComputation(rhs), convexRegionSelectorPtr_(rhs.convexRegionSelectorPtr_)
+    {
+        footPlacementConParameters_.resize(rhs.footPlacementConParameters_.size());
+    }
+
+    void PerceptiveLeggedPrecomputation::request(RequestSet request, scalar_t t, const vector_t& x, const vector_t& u)
+    {
+        if (!request.containsAny(Request::Cost + Request::Constraint + Request::SoftConstraint))
+        {
+            return;
+        }
+        LeggedRobotPreComputation::request(request, t, x, u);
+
+        if (request.contains(Request::Constraint))
+        {
+            for (size_t i = 0; i < footPlacementConParameters_.size(); i++)
+            {
+                FootPlacementConstraint::Parameter params;
+
+                auto projection = convexRegionSelectorPtr_->getProjection(i, t);
+                if (projection.regionPtr == nullptr)
+                {
+                    // Swing leg
+                    continue;
+                }
+
+                matrix_t polytopeA;
+                vector_t polytopeB;
+                std::tie(polytopeA, polytopeB) = getPolygonConstraint(convexRegionSelectorPtr_->getConvexPolygon(i, t));
+                matrix_t p = (matrix_t(2, 3) << // clang-format off
+                        1, 0, 0,
+                        0, 1, 0).finished();  // clang-format on
+                params.a = polytopeA * p * projection.regionPtr->transformPlaneToWorld.inverse().linear();
+                params.b = polytopeB + polytopeA * projection.regionPtr->transformPlaneToWorld.inverse().translation().
+                                                              head(2);
+
+                footPlacementConParameters_[i] = params;
+            }
+        }
+    }
+
+    std::pair<matrix_t, vector_t> PerceptiveLeggedPrecomputation::getPolygonConstraint(
+        const convex_plane_decomposition::CgalPolygon2d& polygon) const
+    {
+        size_t numVertices = polygon.size();
+        matrix_t polytopeA = matrix_t::Zero(numVertices, 2);
+        vector_t polytopeB = vector_t::Zero(numVertices);
+
+        for (size_t i = 0; i < numVertices; i++)
+        {
+            size_t j = i + 1;
+            if (j == numVertices)
+            {
+                j = 0;
+            }
+            size_t k = j + 1;
+            if (k == numVertices)
+            {
+                k = 0;
+            }
+            const auto point_a = polygon.vertex(i);
+            const auto point_b = polygon.vertex(j);
+            const auto point_c = polygon.vertex(k);
+
+            polytopeA.row(i) << point_b.y() - point_a.y(), point_a.x() - point_b.x();
+            polytopeB(i) = point_a.y() * point_b.x() - point_a.x() * point_b.y();
+            if (polytopeA.row(i) * (vector_t(2) << point_c.x(), point_c.y()).finished() + polytopeB(i) < 0)
+            {
+                polytopeA.row(i) *= -1;
+                polytopeB(i) *= -1;
+            }
+        }
+
+        return {polytopeA, polytopeB};
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp

@@ -0,0 +1,206 @@
+//
+// Created by qiayuan on 23-1-3.
+//
+
+#include <utility>
+#include <ocs2_core/misc/Lookup.h>
+#include <ocs2_centroidal_model/AccessHelperFunctions.h>
+#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h"
+
+namespace ocs2::legged_robot
+{
+    PerceptiveLeggedReferenceManager::PerceptiveLeggedReferenceManager(CentroidalModelInfo info,
+                                                                       std::shared_ptr<GaitSchedule> gaitSchedulePtr,
+                                                                       std::shared_ptr<SwingTrajectoryPlanner>
+                                                                       swingTrajectoryPtr,
+                                                                       std::shared_ptr<ConvexRegionSelector>
+                                                                       convexRegionSelectorPtr,
+                                                                       const EndEffectorKinematics<scalar_t>&
+                                                                       endEffectorKinematics,
+                                                                       scalar_t comHeight)
+        : info_(std::move(info)),
+          SwitchedModelReferenceManager(std::move(gaitSchedulePtr), std::move(swingTrajectoryPtr)),
+          convexRegionSelectorPtr_(std::move(convexRegionSelectorPtr)),
+          endEffectorKinematicsPtr_(endEffectorKinematics.clone()),
+          comHeight_(comHeight)
+    {
+    }
+
+    void PerceptiveLeggedReferenceManager::modifyReferences(scalar_t initTime, scalar_t finalTime,
+                                                            const vector_t& initState,
+                                                            TargetTrajectories& targetTrajectories,
+                                                            ModeSchedule& modeSchedule)
+    {
+        const auto timeHorizon = finalTime - initTime;
+        modeSchedule = getGaitSchedule()->getModeSchedule(initTime - timeHorizon, finalTime + timeHorizon);
+
+        TargetTrajectories newTargetTrajectories;
+        int nodeNum = 11;
+        for (size_t i = 0; i < nodeNum; ++i)
+        {
+            scalar_t time = initTime + static_cast<double>(i) * timeHorizon / (nodeNum - 1);
+            vector_t state = targetTrajectories.getDesiredState(time);
+            vector_t input = targetTrajectories.getDesiredState(time);
+
+            const auto& map = convexRegionSelectorPtr_->getPlanarTerrainPtr()->gridMap;
+            vector_t pos = centroidal_model::getBasePose(state, info_).head(3);
+
+            // Base Orientation
+            scalar_t step = 0.3;
+            grid_map::Vector3 normalVector;
+            normalVector(0) = (map.atPosition("smooth_planar", pos + grid_map::Position(-step, 0)) -
+                    map.atPosition("smooth_planar", pos + grid_map::Position(step, 0))) /
+                (2 * step);
+            normalVector(1) = (map.atPosition("smooth_planar", pos + grid_map::Position(0, -step)) -
+                    map.atPosition("smooth_planar", pos + grid_map::Position(0, step))) /
+                (2 * step);
+            normalVector(2) = 1;
+            normalVector.normalize();
+            matrix3_t R;
+            scalar_t z = centroidal_model::getBasePose(state, info_)(3);
+            R << cos(z), -sin(z), 0, // clang-format off
+             sin(z), cos(z), 0,
+             0, 0, 1;  // clang-format on
+            vector_t v = R.transpose() * normalVector;
+            centroidal_model::getBasePose(state, info_)(4) = atan(v.x() / v.z());
+
+            // Base Z Position
+            centroidal_model::getBasePose(state, info_)(2) =
+                map.atPosition("smooth_planar", pos) + comHeight_ / cos(centroidal_model::getBasePose(state, info_)(4));
+
+            newTargetTrajectories.timeTrajectory.push_back(time);
+            newTargetTrajectories.stateTrajectory.push_back(state);
+            newTargetTrajectories.inputTrajectory.push_back(input);
+        }
+        targetTrajectories = newTargetTrajectories;
+
+        // Footstep
+        convexRegionSelectorPtr_->update(modeSchedule, initTime, initState, targetTrajectories);
+
+        // Swing trajectory
+        updateSwingTrajectoryPlanner(initTime, initState, modeSchedule);
+    }
+
+    void PerceptiveLeggedReferenceManager::updateSwingTrajectoryPlanner(scalar_t initTime, const vector_t& initState,
+                                                                        ModeSchedule& modeSchedule)
+    {
+        const auto contactFlagStocks = convexRegionSelectorPtr_->extractContactFlags(modeSchedule.modeSequence);
+        feet_array_t<scalar_array_t> liftOffHeightSequence, touchDownHeightSequence;
+
+        for (size_t leg = 0; leg < info_.numThreeDofContacts; leg++)
+        {
+            size_t initIndex = lookup::findIndexInTimeArray(modeSchedule.eventTimes, initTime);
+
+            auto projections = convexRegionSelectorPtr_->getProjections(leg);
+            modifyProjections(initTime, initState, leg, initIndex, contactFlagStocks[leg], projections);
+
+            scalar_array_t liftOffHeights, touchDownHeights;
+            std::tie(liftOffHeights, touchDownHeights) = getHeights(contactFlagStocks[leg], projections);
+            liftOffHeightSequence[leg] = liftOffHeights;
+            touchDownHeightSequence[leg] = touchDownHeights;
+        }
+        swingTrajectoryPtr_->update(modeSchedule, liftOffHeightSequence, touchDownHeightSequence);
+    }
+
+    void PerceptiveLeggedReferenceManager::modifyProjections(scalar_t initTime, const vector_t& initState, size_t leg,
+                                                             size_t initIndex,
+                                                             const std::vector<bool>& contactFlagStocks,
+                                                             std::vector<
+                                                                 convex_plane_decomposition::PlanarTerrainProjection>&
+                                                             projections)
+    {
+        if (contactFlagStocks[initIndex])
+        {
+            lastLiftoffPos_[leg] = endEffectorKinematicsPtr_->getPosition(initState)[leg];
+            lastLiftoffPos_[leg].z() -= 0.02;
+            for (int i = initIndex; i < projections.size(); ++i)
+            {
+                if (!contactFlagStocks[i])
+                {
+                    break;
+                }
+                projections[i].positionInWorld = lastLiftoffPos_[leg];
+            }
+            for (int i = initIndex; i >= 0; --i)
+            {
+                if (!contactFlagStocks[i])
+                {
+                    break;
+                }
+                projections[i].positionInWorld = lastLiftoffPos_[leg];
+            }
+        }
+        if (initTime > convexRegionSelectorPtr_->getInitStandFinalTimes()[leg])
+        {
+            for (int i = initIndex; i >= 0; --i)
+            {
+                if (contactFlagStocks[i])
+                {
+                    projections[i].positionInWorld = lastLiftoffPos_[leg];
+                }
+                if (!contactFlagStocks[i] && !contactFlagStocks[i + 1])
+                {
+                    break;
+                }
+            }
+        }
+        //    for (int i = 0; i < numPhases; ++i) {
+        //      if (leg == 1) std::cerr << std::setprecision(3) << projections[i].positionInWorld.z() << "\t";
+        //    }
+        //    std::cerr << std::endl;
+    }
+
+    std::pair<scalar_array_t, scalar_array_t> PerceptiveLeggedReferenceManager::getHeights(
+        const std::vector<bool>& contactFlagStocks,
+        const std::vector<convex_plane_decomposition::PlanarTerrainProjection>& projections)
+    {
+        scalar_array_t liftOffHeights, touchDownHeights;
+        const size_t numPhases = projections.size();
+
+        liftOffHeights.clear();
+        liftOffHeights.resize(numPhases);
+        touchDownHeights.clear();
+        touchDownHeights.resize(numPhases);
+
+        for (size_t i = 1; i < numPhases; ++i)
+        {
+            if (!contactFlagStocks[i])
+            {
+                liftOffHeights[i] = contactFlagStocks[i - 1]
+                                        ? projections[i - 1].positionInWorld.z()
+                                        : liftOffHeights[i - 1];
+            }
+        }
+        for (int i = numPhases - 2; i >= 0; --i)
+        {
+            if (!contactFlagStocks[i])
+            {
+                touchDownHeights[i] = contactFlagStocks[i + 1]
+                                          ? projections[i + 1].positionInWorld.z()
+                                          : touchDownHeights[i + 1];
+            }
+        }
+
+        //  for (int i = 0; i < numPhases; ++i) {
+        //    std::cerr << std::setprecision(3) << liftOffHeights[i] << "\t";
+        //  }
+        //  std::cerr << std::endl;
+        //  for (int i = 0; i < numPhases; ++i) {
+        //    std::cerr << std::setprecision(3) << contactFlagStocks[i] << "\t";
+        //  }
+        //  std::cerr << std::endl;
+
+        return {liftOffHeights, touchDownHeights};
+    }
+
+    contact_flag_t PerceptiveLeggedReferenceManager::getFootPlacementFlags(scalar_t time) const
+    {
+        contact_flag_t flag;
+        const auto finalTime = convexRegionSelectorPtr_->getInitStandFinalTimes();
+        for (int i = 0; i < flag.size(); ++i)
+        {
+            flag[i] = getContactFlags(time)[i] && time >= finalTime[i];
+        }
+        return flag;
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootCollisionConstraint.cpp

@@ -0,0 +1,60 @@
+//
+// Created by qiayuan on 23-1-26.
+//
+#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h"
+
+namespace ocs2::legged_robot
+{
+    FootCollisionConstraint::FootCollisionConstraint(const SwitchedModelReferenceManager& referenceManager,
+                                                     const EndEffectorKinematics<scalar_t>& endEffectorKinematics,
+                                                     std::shared_ptr<grid_map::SignedDistanceField> sdfPtr,
+                                                     size_t contactPointIndex,
+                                                     scalar_t clearance)
+        : StateConstraint(ConstraintOrder::Linear),
+          referenceManagerPtr_(&referenceManager),
+          endEffectorKinematicsPtr_(endEffectorKinematics.clone()),
+          sdfPtr_(std::move(sdfPtr)),
+          contactPointIndex_(contactPointIndex),
+          clearance_(clearance)
+    {
+    }
+
+    FootCollisionConstraint::FootCollisionConstraint(const FootCollisionConstraint& rhs)
+        : StateConstraint(ConstraintOrder::Linear),
+          referenceManagerPtr_(rhs.referenceManagerPtr_),
+          endEffectorKinematicsPtr_(rhs.endEffectorKinematicsPtr_->clone()),
+          sdfPtr_(rhs.sdfPtr_),
+          contactPointIndex_(rhs.contactPointIndex_),
+          clearance_(rhs.clearance_)
+    {
+    }
+
+    bool FootCollisionConstraint::isActive(scalar_t time) const
+    {
+        scalar_t offset = 0.05;
+        return !referenceManagerPtr_->getContactFlags(time)[contactPointIndex_] &&
+            !referenceManagerPtr_->getContactFlags(time + 0.5 * offset)[contactPointIndex_] &&
+            !referenceManagerPtr_->getContactFlags(time - offset)[contactPointIndex_];
+    }
+
+    vector_t FootCollisionConstraint::getValue(scalar_t /*time*/, const vector_t& state,
+                                               const PreComputation& /*preComp*/) const
+    {
+        vector_t value(1);
+        value(0) = sdfPtr_->getDistanceAt(grid_map::Position3(endEffectorKinematicsPtr_->getPosition(state).front())) -
+            clearance_;
+        return value;
+    }
+
+    VectorFunctionLinearApproximation FootCollisionConstraint::getLinearApproximation(
+        scalar_t time, const vector_t& state,
+        const PreComputation& preComp) const
+    {
+        VectorFunctionLinearApproximation approx = VectorFunctionLinearApproximation::Zero(1, state.size(), 0);
+        approx.f = getValue(time, state, preComp);
+        approx.dfdx = sdfPtr_->getDistanceGradientAt(grid_map::Position3(endEffectorKinematicsPtr_->getPosition(state).front())).
+                               transpose() *
+            endEffectorKinematicsPtr_->getPositionLinearApproximation(state).front().dfdx;
+        return approx;
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootPlacementConstraint.cpp

@@ -0,0 +1,60 @@
+//
+// Created by qiayuan on 23-1-1.
+//
+
+#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h"
+#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h"
+#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h"
+
+namespace ocs2::legged_robot
+{
+    FootPlacementConstraint::FootPlacementConstraint(const SwitchedModelReferenceManager& referenceManager,
+                                                     const EndEffectorKinematics<scalar_t>& endEffectorKinematics,
+                                                     size_t contactPointIndex,
+                                                     size_t numVertices)
+        : StateConstraint(ConstraintOrder::Linear),
+          referenceManagerPtr_(&referenceManager),
+          endEffectorKinematicsPtr_(endEffectorKinematics.clone()),
+          contactPointIndex_(contactPointIndex),
+          numVertices_(numVertices)
+    {
+    }
+
+    FootPlacementConstraint::FootPlacementConstraint(const FootPlacementConstraint& rhs)
+        : StateConstraint(ConstraintOrder::Linear),
+          referenceManagerPtr_(rhs.referenceManagerPtr_),
+          endEffectorKinematicsPtr_(rhs.endEffectorKinematicsPtr_->clone()),
+          contactPointIndex_(rhs.contactPointIndex_),
+          numVertices_(rhs.numVertices_)
+    {
+    }
+
+    bool FootPlacementConstraint::isActive(scalar_t time) const
+    {
+        return dynamic_cast<const PerceptiveLeggedReferenceManager&>(*referenceManagerPtr_).getFootPlacementFlags(time)[
+            contactPointIndex_];
+    }
+
+    vector_t FootPlacementConstraint::getValue(scalar_t /*time*/, const vector_t& state,
+                                               const PreComputation& preComp) const
+    {
+        const auto param = cast<PerceptiveLeggedPrecomputation>(preComp).getFootPlacementConParameters()[
+            contactPointIndex_];
+        return param.a * endEffectorKinematicsPtr_->getPosition(state).front() + param.b;
+    }
+
+    VectorFunctionLinearApproximation FootPlacementConstraint::getLinearApproximation(
+        scalar_t /*time*/, const vector_t& state,
+        const PreComputation& preComp) const
+    {
+        VectorFunctionLinearApproximation approx = VectorFunctionLinearApproximation::Zero(
+            numVertices_, state.size(), 0);
+        const auto param = cast<PerceptiveLeggedPrecomputation>(preComp).getFootPlacementConParameters()[
+            contactPointIndex_];
+
+        const auto positionApprox = endEffectorKinematicsPtr_->getPositionLinearApproximation(state).front();
+        approx.f = param.a * positionApprox.f + param.b;
+        approx.dfdx = param.a * positionApprox.dfdx;
+        return approx;
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/SphereSdfConstraint.cpp

@@ -0,0 +1,60 @@
+//
+// Created by qiayuan on 22-12-27.
+//
+
+#include "ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h"
+#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h"
+
+namespace ocs2::legged_robot
+{
+    SphereSdfConstraint::SphereSdfConstraint(const PinocchioSphereKinematics& sphereKinematics,
+                                             std::shared_ptr<grid_map::SignedDistanceField> sdfPtr)
+        : StateConstraint(ConstraintOrder::Linear),
+          sphereKinematicsPtr_(sphereKinematics.clone()),
+          sdfPtr_(std::move(sdfPtr)),
+          numConstraints_(sphereKinematicsPtr_->getPinocchioSphereInterface().getNumSpheresInTotal())
+    {
+    }
+
+    vector_t SphereSdfConstraint::getValue(scalar_t /*time*/, const vector_t& state,
+                                           const PreComputation& preComp) const
+    {
+        vector_t value(numConstraints_);
+
+        sphereKinematicsPtr_->setPinocchioInterface(
+            cast<PerceptiveLeggedPrecomputation>(preComp).getPinocchioInterface());
+        auto position = sphereKinematicsPtr_->getPosition(state);
+        auto radius = sphereKinematicsPtr_->getPinocchioSphereInterface().getSphereRadii();
+        for (int i = 0; i < numConstraints_; ++i)
+        {
+            value(i) = sdfPtr_->getDistanceAt(grid_map::Position3(position[i])) - radius[i];
+        }
+        return value;
+    }
+
+    VectorFunctionLinearApproximation SphereSdfConstraint::getLinearApproximation(scalar_t time, const vector_t& state,
+        const PreComputation& preComp) const
+    {
+        VectorFunctionLinearApproximation approx = VectorFunctionLinearApproximation::Zero(
+            numConstraints_, state.size(), 0);
+        approx.f = getValue(time, state, preComp);
+
+        auto position = sphereKinematicsPtr_->getPosition(state);
+        auto sphereApprox = sphereKinematicsPtr_->getPositionLinearApproximation(state);
+
+        for (int i = 0; i < numConstraints_; ++i)
+        {
+            vector_t sdfGradient = sdfPtr_->getDistanceGradientAt(grid_map::Position3(position[i]));
+            approx.dfdx.row(i) = sdfGradient.transpose() * sphereApprox[i].dfdx;
+        }
+        return approx;
+    }
+
+    SphereSdfConstraint::SphereSdfConstraint(const SphereSdfConstraint& rhs)
+        : StateConstraint(rhs),
+          sphereKinematicsPtr_(rhs.sphereKinematicsPtr_->clone()),
+          sdfPtr_(rhs.sdfPtr_),
+          numConstraints_(rhs.numConstraints_)
+    {
+    }
+} // namespace legged

libraries/controller_common/include/controller_common/CtrlInterfaces.h

@@ -10,7 +10,8 @@
 #include <hardware_interface/loaned_state_interface.hpp>
 #include <control_input_msgs/msg/inputs.hpp>
 
-struct CtrlInterfaces {
+struct CtrlInterfaces
+{
     std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
     joint_torque_command_interface_;
     std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
@@ -45,7 +46,8 @@ struct CtrlInterfaces {
 
     CtrlInterfaces() = default;
 
-    void clear() {
+    void clear()
+    {
         joint_torque_command_interface_.clear();
         joint_position_command_interface_.clear();
         joint_velocity_command_interface_.clear();

libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h

@@ -1,13 +1,12 @@
 //
 // Created by biao on 24-9-10.
 //
-
+#pragma once
-#ifndef BASEFIXEDSTAND_H
-#define BASEFIXEDSTAND_H
 
 #include "FSMState.h"
 
-class BaseFixedStand : public FSMState {
+class BaseFixedStand : public FSMState
+{
 public:
     BaseFixedStand(CtrlInterfaces& ctrl_interfaces,
                    const std::vector<double>& target_pos,
@@ -34,6 +33,3 @@ protected:
     double percent_ = 0; //%
     double phase = 0.0;
 };
-
-
-#endif //BASEFIXEDSTAND_H

libraries/controller_common/include/controller_common/FSM/FSMState.h

@@ -11,14 +11,16 @@
 #include <controller_common/CtrlInterfaces.h>
 #include <rclcpp/time.hpp>
 
-class FSMState {
+class FSMState
+{
 public:
     virtual ~FSMState() = default;
 
     FSMState(const FSMStateName& state_name, std::string state_name_string, CtrlInterfaces& ctrl_interfaces)
         : state_name(state_name),
           state_name_string(std::move(state_name_string)),
-          ctrl_interfaces_(ctrl_interfaces) {
+          ctrl_interfaces_(ctrl_interfaces)
+    {
     }
 
     virtual void enter() = 0;

libraries/controller_common/include/controller_common/FSM/StateFixedDown.h

@@ -7,7 +7,8 @@
 
 #include "FSMState.h"
 
-class StateFixedDown final : public FSMState {
+class StateFixedDown final : public FSMState
+{
 public:
     explicit StateFixedDown(CtrlInterfaces& ctrl_interfaces,
                             const std::vector<double>& target_pos,

libraries/controller_common/include/controller_common/FSM/StatePassive.h

@@ -6,7 +6,8 @@
 #define STATEPASSIVE_H
 #include "FSMState.h"
 
-class StatePassive final : public FSMState {
+class StatePassive final : public FSMState
+{
 public:
     explicit StatePassive(CtrlInterfaces& ctrl_interfaces);
 

libraries/controller_common/include/controller_common/common/enumClass.h

@@ -5,7 +5,8 @@
 #ifndef ENUMCLASS_H
 #define ENUMCLASS_H
 
-enum class FSMStateName {
+enum class FSMStateName
+{
     // EXIT,
     INVALID,
     PASSIVE,
@@ -21,18 +22,21 @@ enum class FSMStateName {
     RL
 };
 
-enum class FSMMode {
+enum class FSMMode
+{
     NORMAL,
     CHANGE
 };
 
-enum class FrameType {
+enum class FrameType
+{
     BODY,
     HIP,
     GLOBAL
 };
 
-enum class WaveStatus {
+enum class WaveStatus
+{
     STANCE_ALL,
     SWING_ALL,
     WAVE_ALL

libraries/controller_common/src/FSM/BaseFixedStand.cpp

@@ -10,18 +10,23 @@ BaseFixedStand::BaseFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vecto
                                const double kp,
                                const double kd)
     : FSMState(FSMStateName::FIXEDSTAND, "fixed stand", ctrl_interfaces),
-      kp_(kp), kd_(kd) {
+      kp_(kp), kd_(kd)
+{
     duration_ = ctrl_interfaces_.frequency_ * 1.2;
-    for (int i = 0; i < 12; i++) {
+    for (int i = 0; i < 12; i++)
+    {
         target_pos_[i] = target_pos[i];
     }
 }
 
-void BaseFixedStand::enter() {
+void BaseFixedStand::enter()
-    for (int i = 0; i < 12; i++) {
+{
-        start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value();
+    for (int i = 0; i < 12; i++)
+    {
+        start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_optional().value();
     }
-    for (int i = 0; i < 12; i++) {
+    for (int i = 0; i < 12; i++)
+    {
         std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
         std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0);
         std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0);
@@ -31,24 +36,30 @@ void BaseFixedStand::enter() {
     ctrl_interfaces_.control_inputs_.command = 0;
 }
 
-void BaseFixedStand::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) {
+void BaseFixedStand::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*period*/)
+{
     percent_ += 1 / duration_;
     phase = std::tanh(percent_);
-    for (int i = 0; i < 12; i++) {
+    for (int i = 0; i < 12; i++)
+    {
         std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(
             phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
     }
 }
 
-void BaseFixedStand::exit() {
+void BaseFixedStand::exit()
+{
     percent_ = 0;
 }
 
-FSMStateName BaseFixedStand::checkChange() {
+FSMStateName BaseFixedStand::checkChange()
-    if (percent_ < 1.5) {
+{
+    if (percent_ < 1.5)
+    {
         return FSMStateName::FIXEDSTAND;
     }
-    switch (ctrl_interfaces_.control_inputs_.command) {
+    switch (ctrl_interfaces_.control_inputs_.command)
+    {
     case 1:
         return FSMStateName::PASSIVE;
     case 2:

libraries/controller_common/src/FSM/StateFixedDown.cpp

@@ -11,19 +11,24 @@ StateFixedDown::StateFixedDown(CtrlInterfaces &ctrl_interfaces,
                                const double kp,
                                const double kd)
     : FSMState(FSMStateName::FIXEDDOWN, "fixed down", ctrl_interfaces),
-      kp_(kp), kd_(kd) {
+      kp_(kp), kd_(kd)
+{
     duration_ = ctrl_interfaces_.frequency_ * 1.2;
-    for (int i = 0; i < 12; i++) {
+    for (int i = 0; i < 12; i++)
+    {
         target_pos_[i] = target_pos[i];
     }
 }
 
-void StateFixedDown::enter() {
+void StateFixedDown::enter()
-    for (int i = 0; i < 12; i++) {
+{
-        start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value();
+    for (int i = 0; i < 12; i++)
+    {
+        start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_optional().value();
     }
     ctrl_interfaces_.control_inputs_.command = 0;
-    for (int i = 0; i < 12; i++) {
+    for (int i = 0; i < 12; i++)
+    {
         std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
         std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0);
         std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0);
@@ -32,24 +37,30 @@ void StateFixedDown::enter() {
     }
 }
 
-void StateFixedDown::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) {
+void StateFixedDown::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*period*/)
+{
     percent_ += 1 / duration_;
     phase = std::tanh(percent_);
-    for (int i = 0; i < 12; i++) {
+    for (int i = 0; i < 12; i++)
+    {
         std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(
             phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
     }
 }
 
-void StateFixedDown::exit() {
+void StateFixedDown::exit()
+{
     percent_ = 0;
 }
 
-FSMStateName StateFixedDown::checkChange() {
+FSMStateName StateFixedDown::checkChange()
-    if (percent_ < 1.5) {
+{
+    if (percent_ < 1.5)
+    {
         return FSMStateName::FIXEDDOWN;
     }
-    switch (ctrl_interfaces_.control_inputs_.command) {
+    switch (ctrl_interfaces_.control_inputs_.command)
+    {
     case 1:
         return FSMStateName::PASSIVE;
     case 2:

libraries/controller_common/src/FSM/StatePassive.cpp

@@ -7,36 +7,47 @@
 #include <iostream>
 
 StatePassive::StatePassive(CtrlInterfaces& ctrl_interfaces) : FSMState(
-    FSMStateName::PASSIVE, "passive", ctrl_interfaces) {
+    FSMStateName::PASSIVE, "passive", ctrl_interfaces)
+{
 }
 
-void StatePassive::enter() {
+void StatePassive::enter()
-    for (auto i: ctrl_interfaces_.joint_torque_command_interface_) {
+{
+    for (auto i : ctrl_interfaces_.joint_torque_command_interface_)
+    {
         std::ignore = i.get().set_value(0);
     }
-    for (auto i: ctrl_interfaces_.joint_position_command_interface_) {
+    for (auto i : ctrl_interfaces_.joint_position_command_interface_)
+    {
         std::ignore = i.get().set_value(0);
     }
-    for (auto i: ctrl_interfaces_.joint_velocity_command_interface_) {
+    for (auto i : ctrl_interfaces_.joint_velocity_command_interface_)
+    {
         std::ignore = i.get().set_value(0);
     }
-    for (auto i: ctrl_interfaces_.joint_kp_command_interface_) {
+    for (auto i : ctrl_interfaces_.joint_kp_command_interface_)
+    {
         std::ignore = i.get().set_value(0);
     }
-    for (auto i: ctrl_interfaces_.joint_kd_command_interface_) {
+    for (auto i : ctrl_interfaces_.joint_kd_command_interface_)
+    {
         std::ignore = i.get().set_value(1);
     }
     ctrl_interfaces_.control_inputs_.command = 0;
 }
 
-void StatePassive::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) {
+void StatePassive::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*period*/)
+{
 }
 
-void StatePassive::exit() {
+void StatePassive::exit()
+{
 }
 
-FSMStateName StatePassive::checkChange() {
+FSMStateName StatePassive::checkChange()
-    if (ctrl_interfaces_.control_inputs_.command == 2) {
+{
+    if (ctrl_interfaces_.control_inputs_.command == 2)
+    {
         return FSMStateName::FIXEDDOWN;
     }
     return FSMStateName::PASSIVE;