add an SDF to the planar terrain model

Author: rubengrandia

common/ocs2_switched_model_interface/CMakeLists.txt

@@ -77,6 +77,7 @@ add_library(${PROJECT_NAME}
 	src/logic/DynamicsParametersSynchronizedModule.cpp
 	src/logic/SwitchedModelModeScheduleManager.cpp
 	src/ros_msg_conversions/RosMsgConversions.cpp
+	src/terrain/PlanarSignedDistanceField.cpp
 	src/terrain/PlanarTerrainModel.cpp
 	src/terrain/PlaneFitting.cpp
 	src/terrain/TerrainPlane.cpp

common/ocs2_switched_model_interface/include/ocs2_switched_model_interface/terrain/PlanarSignedDistanceField.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "ocs2_switched_model_interface/core/SwitchedModel.h"
+#include "ocs2_switched_model_interface/terrain/SignedDistanceField.h"
+#include "ocs2_switched_model_interface/terrain/TerrainModel.h"
+
+namespace switched_model {
+
+/**
+ * Implements a flat terrain signed distance field
+ */
+class PlanarSignedDistanceField : public SignedDistanceField {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit PlanarSignedDistanceField(TerrainPlane terrainPlane);
+
+  ~PlanarSignedDistanceField() override = default;
+  PlanarSignedDistanceField* clone() const override { return new PlanarSignedDistanceField(*this); };
+
+  scalar_t value(const vector3_t& position) const override;
+
+  vector3_t derivative(const vector3_t& position) const override;
+
+  std::pair<scalar_t, vector3_t> valueAndDerivative(const vector3_t& position) const override;
+
+ protected:
+  PlanarSignedDistanceField(const PlanarSignedDistanceField& other);
+
+ private:
+  TerrainPlane terrainPlane_;
+};
+
+}  // namespace switched_model

common/ocs2_switched_model_interface/include/ocs2_switched_model_interface/terrain/PlanarTerrainModel.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include "ocs2_switched_model_interface/core/SwitchedModel.h"
+#include "ocs2_switched_model_interface/terrain/PlanarSignedDistanceField.h"
 #include "ocs2_switched_model_interface/terrain/TerrainModel.h"
 
 namespace switched_model {
@@ -25,8 +26,11 @@ class PlanarTerrainModel : public TerrainModel {
 
   std::vector<vector2_t> getHeightProfileAlongLine(const vector3_t& position1InWorld, const vector3_t& position2InWorld) const override;
 
+  const SignedDistanceField* getSignedDistanceField() const override { return &sdf_; }
+
  private:
   TerrainPlane terrainPlane_;
+  PlanarSignedDistanceField sdf_;
 };
 
 }  // namespace switched_model

common/ocs2_switched_model_interface/src/terrain/PlanarSignedDistanceField.cpp

@@ -0,0 +1,25 @@
+//
+// Created by rgrandia on 08.09.22.
+//
+
+#include "ocs2_switched_model_interface/terrain/PlanarSignedDistanceField.h"
+
+namespace switched_model {
+
+PlanarSignedDistanceField::PlanarSignedDistanceField(TerrainPlane terrainPlane) : terrainPlane_(std::move(terrainPlane)) {}
+
+PlanarSignedDistanceField::PlanarSignedDistanceField(const PlanarSignedDistanceField& other) : terrainPlane_(other.terrainPlane_) {}
+
+scalar_t PlanarSignedDistanceField::value(const vector3_t& position) const {
+  return terrainSignedDistanceFromPositionInWorld(position, terrainPlane_);
+}
+
+vector3_t PlanarSignedDistanceField::derivative(const vector3_t& position) const {
+  return surfaceNormalInWorld(terrainPlane_);
+}
+
+std::pair<scalar_t, vector3_t> PlanarSignedDistanceField::valueAndDerivative(const vector3_t& position) const {
+  return {value(position), derivative(position)};
+}
+
+}  // namespace switched_model

common/ocs2_switched_model_interface/src/terrain/PlanarTerrainModel.cpp

@@ -6,7 +6,7 @@
 
 namespace switched_model {
 
-PlanarTerrainModel::PlanarTerrainModel(TerrainPlane terrainPlane) : terrainPlane_(std::move(terrainPlane)) {}
+PlanarTerrainModel::PlanarTerrainModel(TerrainPlane terrainPlane) : terrainPlane_(terrainPlane), sdf_(std::move(terrainPlane)) {}
 
 TerrainPlane PlanarTerrainModel::getLocalTerrainAtPositionInWorldAlongGravity(
     const vector3_t& positionInWorld, std::function<scalar_t(const vector3_t&)> penaltyFunction) const {