From 4ba9f7ec2e2bc642b69644a6b13d697693d19950 Mon Sep 17 00:00:00 2001
From: Masaki Murooka <m-murooka@aist.go.jp>
Date: Wed, 1 Nov 2023 21:40:37 +0900
Subject: [PATCH] Allow to set waypoint in walkToRelativePose.

---
 .../BaselineWalkingController/FootManager.h   |  5 +++-
 src/FootManager.cpp                           | 28 ++++++++++++-------
 src/states/ConfigWalkState.cpp                | 16 +++++++++++
 3 files changed, 38 insertions(+), 11 deletions(-)

diff --git a/include/BaselineWalkingController/FootManager.h b/include/BaselineWalkingController/FootManager.h
index 95e74644..df55114f 100644
--- a/include/BaselineWalkingController/FootManager.h
+++ b/include/BaselineWalkingController/FootManager.h
@@ -316,9 +316,12 @@ class FootManager
   /** \brief Send footstep sequence to walk to the relative target pose.
       \param targetTrans relative target pose of foot midpose (x [m], y [m], theta [rad])
       \param lastFootstepNum number of last footstep
+      \param waypointTransList waypoint pose list of foot midpose relative to current pose (x [m], y [m], theta [rad])
       \return whether footstep is successfully sent
    */
-  bool walkToRelativePose(const Eigen::Vector3d & targetTrans, int lastFootstepNum = 0);
+  bool walkToRelativePose(const Eigen::Vector3d & targetTrans,
+                          int lastFootstepNum = 0,
+                          const std::vector<Eigen::Vector3d> & waypointTransList = {});
 
   /** \brief Start velocity mode.
       \return whether it is successfully started
diff --git a/src/FootManager.cpp b/src/FootManager.cpp
index b8310515..4212c95e 100644
--- a/src/FootManager.cpp
+++ b/src/FootManager.cpp
@@ -587,7 +587,9 @@ Eigen::Vector3d FootManager::calcZmpWithOffset(const std::unordered_map<Foot, sv
   }
 }
 
-bool FootManager::walkToRelativePose(const Eigen::Vector3d & targetTrans, int lastFootstepNum)
+bool FootManager::walkToRelativePose(const Eigen::Vector3d & targetTrans,
+                                     int lastFootstepNum,
+                                     const std::vector<Eigen::Vector3d> & waypointTransList)
 {
   if(footstepQueue_.size() > 0)
   {
@@ -608,22 +610,28 @@ bool FootManager::walkToRelativePose(const Eigen::Vector3d & targetTrans, int la
   // transformation in the world frame.
   const sva::PTransformd & initialFootMidpose =
       projGround(sva::interpolate(targetFootPoses_.at(Foot::Left), targetFootPoses_.at(Foot::Right), 0.5));
-  const sva::PTransformd & goalFootMidpose = convertTo3d(targetTrans) * initialFootMidpose;
 
-  Foot foot = targetTrans.y() >= 0 ? Foot::Left : Foot::Right;
+  Foot foot = (waypointTransList.empty() ? targetTrans.y() : waypointTransList[0].y()) >= 0 ? Foot::Left : Foot::Right;
   sva::PTransformd footMidpose = initialFootMidpose;
   double startTime = ctl().t() + 1.0;
 
-  while(convertTo2d(goalFootMidpose * footMidpose.inv()).norm() > 1e-6)
+  for(size_t i = 0; i < waypointTransList.size() + 1; i++)
   {
-    Eigen::Vector3d deltaTrans = convertTo2d(goalFootMidpose * footMidpose.inv());
-    footMidpose = convertTo3d(clampDeltaTrans(deltaTrans, foot)) * footMidpose;
+    const Eigen::Vector3d & goalTrans = (i == waypointTransList.size() ? targetTrans : waypointTransList[i]);
+    const sva::PTransformd & goalFootMidpose = convertTo3d(goalTrans) * initialFootMidpose;
+    double thre = (i == waypointTransList.size() ? 1e-6 : 1e-2);
 
-    const auto & footstep = makeFootstep(foot, footMidpose, startTime);
-    appendFootstep(footstep);
+    while(convertTo2d(goalFootMidpose * footMidpose.inv()).norm() > thre)
+    {
+      Eigen::Vector3d deltaTrans = convertTo2d(goalFootMidpose * footMidpose.inv());
+      footMidpose = convertTo3d(clampDeltaTrans(deltaTrans, foot)) * footMidpose;
 
-    foot = opposite(foot);
-    startTime = footstep.transitEndTime;
+      const auto & footstep = makeFootstep(foot, footMidpose, startTime);
+      appendFootstep(footstep);
+
+      foot = opposite(foot);
+      startTime = footstep.transitEndTime;
+    }
   }
 
   for(int i = 0; i < lastFootstepNum + 1; i++)
diff --git a/src/states/ConfigWalkState.cpp b/src/states/ConfigWalkState.cpp
index ab776fa5..7e29b4eb 100644
--- a/src/states/ConfigWalkState.cpp
+++ b/src/states/ConfigWalkState.cpp
@@ -39,6 +39,22 @@ void ConfigWalkState::start(mc_control::fsm::Controller & _ctl)
     ctl().footManager_->setRelativeVel(config_("configs")("velocityMode")("velocity"));
     velModeEndTime_ = ctl().t() + static_cast<double>(config_("configs")("velocityMode")("duration"));
   }
+  else if(config_.has("configs") && config_("configs").has("footMidpose"))
+  {
+    auto convertDegToRad = [](const Eigen::Vector3d & trans) -> Eigen::Vector3d {
+      return Eigen::Vector3d(trans.x(), trans.y(), mc_rtc::constants::toRad(trans.z()));
+    };
+    Eigen::Vector3d targetTrans = convertDegToRad(config_("configs")("footMidpose")("target"));
+    std::vector<Eigen::Vector3d> waypointTransList = {};
+    if(config_("configs")("footMidpose").has("waypointList"))
+    {
+      for(const Eigen::Vector3d & waypointTrans : config_("configs")("footMidpose")("waypointList"))
+      {
+        waypointTransList.push_back(convertDegToRad(waypointTrans));
+      }
+    }
+    ctl().footManager_->walkToRelativePose(targetTrans, 0, waypointTransList);
+  }
 
   // Set reference CoM Z position
   if(config_.has("configs") && config_("configs").has("refComZList"))