[WIP] Attempt to finalize MANNTrajectoryGeneration

ami-iit · May 3, 2023 · c96c656 · c96c656
1 parent a07bd3a
commit c96c656
Show file tree

Hide file tree

Showing 2 changed files with 217 additions and 88 deletions.
diff --git a/src/ML/include/BipedalLocomotion/ML/MANNTrajectoryGeneration.h b/src/ML/include/BipedalLocomotion/ML/MANNTrajectoryGeneration.h
@@ -43,6 +43,9 @@ struct MANNTrajectoryGenerationInput
 
 struct MANNTrajectoryGenerationOutput
 {
+    Eigen::VectorXd jointsPosition;
+    Contacts::EstimatedContact leftFoot;
+    Contacts::EstimatedContact rightFoot;
 };
 
 class MANNTrajectoryGeneration
@@ -51,16 +54,17 @@ class MANNTrajectoryGeneration
     MANN m_mann;
     MANNInput m_mannInput;
     iDynTree::KinDynComputations m_kinDyn;
+    MANNTrajectoryGenerationOutput m_output;
+
+    std::chrono::nanoseconds m_currentTime{std::chrono::nanoseconds::zero()};
 
     struct AutoregressiveState
     {
-        Eigen::Vector3d currentBasePosition;
-        manif::SO2d I_R_FD;
+        MANNInput previousMannInput;
+        manif::SE2d I_H_FD{manif::SE2d::Identity()};
         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;
@@ -94,6 +98,12 @@ class MANNTrajectoryGeneration
     int m_rootIndex;
     int m_chestIndex;
 
+    void trajectoryBlending(Eigen::Ref<const Eigen::Matrix2Xd> mannOutputMatrix,
+                                                  Eigen::Ref<const Eigen::Matrix2Xd> desiredMatrix,
+                                                  const double& tau,
+                                                  Eigen::Ref<Eigen::Matrix2Xd> out);
+
+    bool m_isRobotStopped;
 public:
     bool setRobotModel(const iDynTree::Model& model);
 
@@ -103,6 +113,10 @@ class MANNTrajectoryGeneration
     bool setInput(const Input& input) override;
 
     bool advance() override;
+
+
+    bool reset(const MANNInput& input, const Contacts::EstimatedContact& leftFoot, const Contacts::EstimatedContact& rightFoot,
+                                     const manif::SE3d& basePosition, const manif::SE3Tangentd baseVelocity);
 };
 } // namespace ML
 } // namespace BipedalLocomotion