[WIP] First iteration in MANNTrajectoryGeneration

ami-iit · Apr 28, 2023 · a07bd3a · a07bd3a
1 parent d6eb4dd
commit a07bd3a
Show file tree

Hide file tree

Showing 3 changed files with 539 additions and 3 deletions.
diff --git a/src/ML/CMakeLists.txt b/src/ML/CMakeLists.txt
@@ -8,10 +8,11 @@ if (FRAMEWORK_COMPILE_ML)
 
   add_bipedal_locomotion_library(
     NAME ML
-    PUBLIC_HEADERS ${H_PREFIX}/MANN.h
-    SOURCES        src/MANN.cpp
+    PUBLIC_HEADERS ${H_PREFIX}/MANN.h ${H_PREFIX}/MANNTrajectoryGeneration.h
+    SOURCES        src/MANN.cpp src/MANNTrajectoryGeneration.cpp
     PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System
-    PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging onnxruntime::onnxruntime
+                          BipedalLocomotion::ContinuousDynamicalSystem BipedalLocomotion::Contacts
+    PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging onnxruntime::onnxruntime BipedalLocomotion::ManifConversions
     INSTALLATION_FOLDER ML)
 
   add_subdirectory(tests)

diff --git a/src/ML/include/BipedalLocomotion/ML/MANNTrajectoryGeneration.h b/src/ML/include/BipedalLocomotion/ML/MANNTrajectoryGeneration.h
@@ -0,0 +1,110 @@
+/**
+ * @file MANNTrajecoryGeneration.h
+ * @authors Paolo Maria Viceconte, Giulio Romualdi
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_ML_MANN_TRAJECTORY_GENERATION_H
+#define BIPEDAL_LOCOMOTION_ML_MANN_TRAJECTORY_GENERATION_H
+
+#include <chrono>
+#include <deque>
+#include <memory>
+
+#include <Eigen/Dense>
+
+#include <BipedalLocomotion/Contacts/Contact.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/SO3Dynamics.h>
+#include <BipedalLocomotion/ML/MANN.h>
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+#include <BipedalLocomotion/System/Advanceable.h>
+
+#include <iDynTree/KinDynComputations.h>
+
+namespace BipedalLocomotion
+{
+namespace ML
+{
+
+// # 0-13 are quad_bezier (x,y)
+//     # 14-27 are base_velocities (x,y)
+//     # 28-41 are facing_dirs (x,y)
+//     # 42-45 are joystick inputs to be stored for future plotting (curr_x, curr_y, curr_z,
+//     curr_rz)
+
+struct MANNTrajectoryGenerationInput
+{
+    Eigen::Matrix2Xd desiredFutureBaseTrajectory;
+    Eigen::Matrix2Xd desiredFutureBaseVelocities;
+    Eigen::Matrix2Xd desiredFutureFacingDirections;
+};
+
+struct MANNTrajectoryGenerationOutput
+{
+};
+
+class MANNTrajectoryGeneration
+    : public System::Advanceable<MANNTrajectoryGenerationInput, MANNTrajectoryGenerationOutput>
+{
+    MANN m_mann;
+    MANNInput m_mannInput;
+    iDynTree::KinDynComputations m_kinDyn;
+
+    struct AutoregressiveState
+    {
+        Eigen::Vector3d currentBasePosition;
+        manif::SO2d I_R_FD;
+        std::deque<Eigen::Vector2d> pastProjectedBasePositions;
+        std::deque<Eigen::Vector2d> pastFacingDirection;
+        std::deque<Eigen::Vector2d> pastProjectedBaseVelocity;
+
+        manif::SE3d basePose;
+    };
+
+    std::shared_ptr<ContinuousDynamicalSystem::SO3Dynamics> m_baseOrientationDynamics;
+    ContinuousDynamicalSystem::ForwardEuler<ContinuousDynamicalSystem::SO3Dynamics> m_integrator;
+
+    Eigen::Vector3d m_gravity;
+
+    std::chrono::nanoseconds m_generationTimeStep;
+
+    AutoregressiveState m_state;
+
+    // TODO initialize
+    Contacts::DiscreteGeometryContact m_leftFoot;
+    Contacts::DiscreteGeometryContact m_rightFoot;
+    Contacts::DiscreteGeometryContact* m_supportFootPtr{nullptr}; /**< Pointer to the support foot.
+                                                                     It will point to an object
+                                                                     stored in the class. */
+    Eigen::MatrixXd m_supportFootJacobian;
+
+    // support_foot_prev: str
+    // support_foot: str
+    // support_vertex_prev: int
+    // support_vertex: int
+    // support_foot_pos: float = 0
+    // support_vertex_pos: float = 0
+    // support_vertex_offset: float = 0
+
+    int m_supportVertexPrev;
+    Eigen::Vector3d m_projectedContactPositionInWorldFrame{Eigen::Vector3d::Zero()};
+
+    int m_rootIndex;
+    int m_chestIndex;
+
+public:
+    bool setRobotModel(const iDynTree::Model& model);
+
+    bool
+    initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+
+    bool setInput(const Input& input) override;
+
+    bool advance() override;
+};
+} // namespace ML
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_ML_MANN_TRAJECTORY_GENERATION_H