Add optional initialization of base pose and feet pose in the Unicycle Trajectory Generator

CHANGELOG.md

@@ -1,6 +1,14 @@
 # Changelog
 All notable changes to this project are documented in this file.
 
+## [unreleased]
+### Added
+- Add the possibility to initialize the base position and the feet pose in the `unicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/887)
+
+### Changed
+### Fixed
+### Removed
+
 ## [0.19.0] - 2024-09-06
 ### Added
 - Added Vector Collection Server for publishing information for real-time users in the YARPRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/796)

src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h

@@ -243,6 +243,17 @@ class ContactList
  */
  const_iterator getPresentContact(const std::chrono::nanoseconds& time) const;
 
+ /**
+ * @brief Get the contact given the time.
+ *
+ * It returns the contact with the highest deactivation time lower than time.
+ * If no contacts have a deactivation time lower than time, it returns an iterator to the end.
+ * @param time The present time.
+ * @return an iterator to the last contact having an activation time lower than time.
+ * If no contact satisfies this condition, it returns a pointer to the end.
+ */
+ const_iterator getPreviousContact(const std::chrono::nanoseconds& time) const;
+
  /**
  * @brief Clear all the steps, except the one returned by getPresentContact
  * @param time The present time.

src/Contacts/src/ContactList.cpp

@@ -264,6 +264,27 @@ ContactList::const_iterator ContactList::getNextContact(const std::chrono::nanos
  return nextContact;
 }
 
+ContactList::const_iterator
+ContactList::getPreviousContact(const std::chrono::nanoseconds& time) const
+{
+ // With the reverse iterator we find the last step such that the deactivation time is smaller
+ // than time
+ ContactList::const_reverse_iterator presentReverse
+ = std::find_if(rbegin(), rend(), [time](const PlannedContact& a) -> bool {
+ return a.deactivationTime < time;
+ });
+
+ if (presentReverse == rend())
+ {
+ // No contact has activation time lower than the specified time.
+ return end();
+ }
+
+ return --(presentReverse.base()); // This is to convert a reverse iterator to a forward
+ // iterator. The -- is because base() returns a forward
+ // iterator to the next element.
+}
+
 bool ContactList::keepOnlyPresentContact(const std::chrono::nanoseconds& time)
 {
  ContactList::const_iterator dropPoint = getPresentContact(time);

src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h

@@ -116,6 +116,32 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryGenerator final
  */
  bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;
 
+ /**
+ * @brief Initialize the planner.
+ * @param handler Pointer to the parameter handler.
+ * @param initialBasePosition Initial position of the base.
+ * @param leftToRightTransform Transformation Matrix between the left and right foot.
+ */
+ bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const Eigen::Vector3d& initialBasePosition,
+ const manif::SE3d& leftToRightTransform);
+
+ /**
+ * @brief Initialize the planner.
+ * @param handler Pointer to the parameter handler.
+ * @param leftToRightTransform Transformation Matrix between the left and right foot.
+ */
+ bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const manif::SE3d& leftToRightTransform);
+
+ /**
+ * @brief Initialize the planner.
+ * @param handler Pointer to the parameter handler.
+ * @param initialBasePosition Initial position of the base.
+ */
+ bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const Eigen::Vector3d& initialBasePosition);
+
  /**
  * @brief Get the output of the planner.
  * @return Output of the planner.

src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h

@@ -143,6 +143,8 @@ struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerParameters
  int rightContactFrameIndex; /**< Index of the right foot contact frame */
 
  std::string rightContactFrameName; /**< Name of the right foot contact frame */
+
+ bool swingLeft; /**< Perform the first step with the left foot */
 };
 
 /**
@@ -238,6 +240,32 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryPlanner final
  bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;
  // clang-format on
 
+ /**
+ * @brief Initialize the planner.
+ * @param handler Pointer to the parameter handler.
+ * @param initialBasePosition Initial position of the base.
+ * @param leftToRightTransform Transformation Matrix between the left and right foot.
+ */
+ bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const Eigen::Vector3d& initialBasePosition,
+ const manif::SE3d& leftToRightTransform);
+
+ /**
+ * @brief Initialize the planner.
+ * @param handler Pointer to the parameter handler.
+ * @param leftToRightTransform Transformation Matrix between the left and right foot.
+ */
+ bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const manif::SE3d& leftToRightTransform);
+
+ /**
+ * @brief Initialize the planner.
+ * @param handler Pointer to the parameter handler.
+ * @param initialBasePosition Initial position of the base.
+ */
+ bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const Eigen::Vector3d& initialBasePosition);
+
  /**
  * Get the output of the planner.
  * @return The output of the planner.
@@ -291,8 +319,12 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryPlanner final
 
  /**
  * Generate the first trajectory.
+ * @param initialBasePosition Initial position of the base.
+ * @param leftToRightTransform Transformation Matrix between the left and right foot.
  */
- bool generateFirstTrajectory();
+ bool generateFirstTrajectory(const Eigen::Ref<const Eigen::Vector3d>& initialBasePosition
+ = Eigen::Vector3d::Zero(),
+ const manif::SE3d& leftToRightTransform = manif::SE3d::Identity());
 };
 
 #endif // BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_PLANNER_H

src/Planners/src/UnicycleTrajectoryGenerator.cpp

@@ -118,7 +118,6 @@ class Planners::UnicycleTrajectoryGenerator::Impl
  * @param currentContactList The current contact list, being generated.
  */
  static void resetContactList(const std::chrono::nanoseconds& time,
- const std::chrono::nanoseconds& minStepDuration,
  const Contacts::ContactList& previousContactList,
  Contacts::ContactList& currentContactList);
 };
@@ -182,8 +181,30 @@ bool Planners::UnicycleTrajectoryGenerator::setRobotContactFrames(const iDynTree
  return true;
 }
 
+bool Planners::UnicycleTrajectoryGenerator::initialize(
+ std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const manif::SE3d& leftToRightTransform)
+{
+ return initialize(handler, Eigen::Vector3d::Zero(), leftToRightTransform);
+}
+
+bool Planners::UnicycleTrajectoryGenerator::initialize(
+ std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const Eigen::Vector3d& initialBasePosition)
+{
+ return initialize(handler, initialBasePosition, manif::SE3d::Identity());
+}
+
 bool Planners::UnicycleTrajectoryGenerator::initialize(
  std::weak_ptr<const ParametersHandler::IParametersHandler> handler)
+{
+ return initialize(handler, Eigen::Vector3d::Zero(), manif::SE3d::Identity());
+}
+
+bool Planners::UnicycleTrajectoryGenerator::initialize(
+ std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const Eigen::Vector3d& initialBasePosition,
+ const manif::SE3d& leftToRightTransform)
 {
  constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::initialize]";
 
@@ -250,7 +271,10 @@ bool Planners::UnicycleTrajectoryGenerator::initialize(
  m_pImpl->newTrajectoryRequired = false;
 
  // Initialize the blf unicycle planner
- ok = ok && m_pImpl->unicycleTrajectoryPlanner.initialize(ptr);
+ ok = ok
+ && m_pImpl->unicycleTrajectoryPlanner.initialize(ptr,
+ initialBasePosition,
+ leftToRightTransform);
 
  // Initialize contact frames
  std::string leftContactFrameName, rightContactFrameName;
@@ -323,11 +347,9 @@ Planners::UnicycleTrajectoryGenerator::getOutput() const
  if (!m_pImpl->output.contactPhaseList.lists().empty())
  {
  m_pImpl->resetContactList(m_pImpl->time - m_pImpl->parameters.dt,
- m_pImpl->unicycleTrajectoryPlanner.getMinStepDuration(),
  m_pImpl->output.contactPhaseList.lists().at("left_foot"),
  leftContactList);
  m_pImpl->resetContactList(m_pImpl->time - m_pImpl->parameters.dt,
- m_pImpl->unicycleTrajectoryPlanner.getMinStepDuration(),
  m_pImpl->output.contactPhaseList.lists().at("right_foot"),
  rightContactList);
  }
@@ -808,7 +830,6 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::generateFirstTraj
 
 void Planners::UnicycleTrajectoryGenerator::Impl::resetContactList(
  const std::chrono::nanoseconds& time,
- const std::chrono::nanoseconds& minStepDuration,
  const Contacts::ContactList& previousContactList,
  Contacts::ContactList& currentContactList)
 {
@@ -825,7 +846,7 @@ void Planners::UnicycleTrajectoryGenerator::Impl::resetContactList(
  // If not, try to get the last active
  if (activeContact == previousContactList.end())
  {
- activeContact = previousContactList.getActiveContact(time - minStepDuration / 2);
+ activeContact = previousContactList.getPreviousContact(time);
  }
 
  // if any active contact found, add it to the current contact list

src/Planners/src/UnicycleTrajectoryPlanner.cpp

@@ -17,8 +17,10 @@
 #include <BipedalLocomotion/TextLogging/Logger.h>
 
 #include <FeetCubicSplineGenerator.h>
+
 #include <iDynTree/KinDynComputations.h>
 #include <iDynTree/Model.h>
+#include <iDynTree/Rotation.h>
 
 #include <chrono>
 #include <mutex>
@@ -206,8 +208,30 @@ int Planners::UnicycleTrajectoryPlanner::getRightContactFrameIndex() const
  return m_pImpl->parameters.rightContactFrameIndex;
 }
 
+bool Planners::UnicycleTrajectoryPlanner::initialize(
+ std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const manif::SE3d& leftToRightTransform)
+{
+ return initialize(handler, Eigen::Vector3d::Zero(), leftToRightTransform);
+}
+
+bool Planners::UnicycleTrajectoryPlanner::initialize(
+ std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const Eigen::Vector3d& initialBasePosition)
+{
+ return initialize(handler, initialBasePosition, manif::SE3d::Identity());
+}
+
 bool Planners::UnicycleTrajectoryPlanner::initialize(
  std::weak_ptr<const ParametersHandler::IParametersHandler> handler)
+{
+ return initialize(handler, Eigen::Vector3d::Zero(), manif::SE3d::Identity());
+}
+
+bool Planners::UnicycleTrajectoryPlanner::initialize(
+ std::weak_ptr<const ParametersHandler::IParametersHandler> handler,
+ const Eigen::Vector3d& initialBasePosition,
+ const manif::SE3d& leftToRightTransform)
 {
  constexpr auto logPrefix = "[UnicycleTrajectoryPlanner::initialize]";
 
@@ -270,7 +294,6 @@ bool Planners::UnicycleTrajectoryPlanner::initialize(
 
  Eigen::Vector2d saturationFactors;
 
- bool startWithLeft{true};
  bool startWithSameFoot{true};
  bool terminalStep{true};
 
@@ -325,7 +348,7 @@ bool Planners::UnicycleTrajectoryPlanner::initialize(
  = iDynTree::deg2rad(m_pImpl->parameters.leftYawDeltaInRad);
  m_pImpl->parameters.rightYawDeltaInRad
  = iDynTree::deg2rad(m_pImpl->parameters.rightYawDeltaInRad);
- ok = ok && loadParamWithFallback("swingLeft", startWithLeft, false);
+ ok = ok && loadParamWithFallback("swingLeft", m_pImpl->parameters.swingLeft, false);
  ok = ok && loadParamWithFallback("startAlwaysSameFoot", startWithSameFoot, true);
  ok = ok && loadParamWithFallback("terminalStep", terminalStep, true);
  ok = ok && loadParam("mergePointRatios", mergePointRatios);
@@ -365,7 +388,7 @@ bool Planners::UnicycleTrajectoryPlanner::initialize(
  unicyclePlanner->setLeftFootYawOffsetInRadians(m_pImpl->parameters.leftYawDeltaInRad);
  unicyclePlanner->setRightFootYawOffsetInRadians(m_pImpl->parameters.rightYawDeltaInRad);
  unicyclePlanner->addTerminalStep(terminalStep);
- unicyclePlanner->startWithLeft(startWithLeft);
+ unicyclePlanner->startWithLeft(m_pImpl->parameters.swingLeft);
  unicyclePlanner->resetStartingFootIfStill(startWithSameFoot);
 
  UnicycleController unicycleController;
@@ -426,7 +449,16 @@ bool Planners::UnicycleTrajectoryPlanner::initialize(
  feetCubicSplineGenerator->setStepHeight(stepHeight);
 
  // generateFirstTrajectory;
- ok = ok && generateFirstTrajectory();
+ if (leftToRightTransform == manif::SE3d::Identity())
+ {
+ // if the transform is the identity, the nominal width is used as distance between the feet
+ manif::SE3d tmpTransform = manif::SE3d::Identity();
+ tmpTransform.translation(Eigen::Vector3d(0.0, -m_pImpl->parameters.nominalWidth, 0.0));
+ ok = ok && generateFirstTrajectory(initialBasePosition, tmpTransform);
+ } else
+ {
+ ok = ok && generateFirstTrajectory(initialBasePosition, leftToRightTransform);
+ }
 
  // debug information
  auto leftSteps = m_pImpl->generator.getLeftFootPrint()->getSteps();
@@ -767,7 +799,9 @@ bool Planners::UnicycleTrajectoryPlanner::advance()
  return true;
 }
 
-bool BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::generateFirstTrajectory()
+bool BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::generateFirstTrajectory(
+ const Eigen::Ref<const Eigen::Vector3d>& initialBasePosition,
+ const manif::SE3d& leftToRightTransform)
 {
 
  constexpr auto logPrefix = "[UnicycleTrajectoryPlanner::generateFirstTrajectory]";
@@ -788,8 +822,10 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::generateFirstTrajec
 
  // at the beginning ergoCub has to stop
  Eigen::Vector2d m_personFollowingDesiredPoint;
- m_personFollowingDesiredPoint(0) = m_pImpl->parameters.referencePointDistance(0);
- m_personFollowingDesiredPoint(1) = m_pImpl->parameters.referencePointDistance(1);
+ m_personFollowingDesiredPoint(0)
+ = m_pImpl->parameters.referencePointDistance(0) + initialBasePosition(0);
+ m_personFollowingDesiredPoint(1)
+ = m_pImpl->parameters.referencePointDistance(1) + initialBasePosition(1);
 
  // add the initial point
  if (!unicyclePlanner
@@ -811,12 +847,52 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryPlanner::generateFirstTrajec
  return false;
  }
 
+ // add real position of the feet
+ std::shared_ptr<FootPrint> left, right;
+
+ left = m_pImpl->generator.getLeftFootPrint();
+ left->clearSteps();
+ right = m_pImpl->generator.getRightFootPrint();
+ right->clearSteps();
+
+ Eigen::Vector2d leftPosition, rightPosition;
+ double leftAngle, rightAngle;
+
+ // if the standing foot is the right -> the unicycle starts parallel to the right foot
+ if (m_pImpl->parameters.swingLeft)
+ {
+ rightPosition(0) = initialBasePosition(0);
+ rightPosition(1)
+ = initialBasePosition(1) - leftToRightTransform.inverse().translation()(1) / 2;
+ rightAngle = 0;
+ right->addStep(iDynTree::Vector2(rightPosition), rightAngle, 0.0);
+
+ leftPosition(0) = initialBasePosition(0) + leftToRightTransform.inverse().translation()(0);
+ leftPosition(1)
+ = initialBasePosition(1) + leftToRightTransform.inverse().translation()(1) / 2;
+ iDynTree::Rotation leftToRightRotation
+ = iDynTree::Rotation(leftToRightTransform.rotation());
+ leftAngle = leftToRightRotation.asRPY()(2);
+ left->addStep(iDynTree::Vector2(leftPosition), leftAngle, 0.0);
+ } else
+ {
+ leftPosition(0) = initialBasePosition(0);
+ leftPosition(1) = initialBasePosition(1) - leftToRightTransform.translation()(1) / 2;
+ leftAngle = 0;
+ left->addStep(iDynTree::Vector2(leftPosition), leftAngle, 0.0);
+
+ rightPosition(0) = initialBasePosition(0) + leftToRightTransform.translation()(0);
+ rightPosition(1) = initialBasePosition(1) + leftToRightTransform.translation()(1) / 2;
+ iDynTree::Rotation leftToRightRotation
+ = iDynTree::Rotation(leftToRightTransform.rotation());
+ rightAngle = leftToRightRotation.asRPY()(2);
+ right->addStep(iDynTree::Vector2(rightPosition), rightAngle, 0.0);
+ }
+
  // generate the first trajectories
  if (!m_pImpl->generator.generate(initTime, dt, endTime))
  {
-
  log()->error("{} Error while computing the first trajectories.", logPrefix);
-
  return false;
  }