[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