diff --git a/CHANGELOG.md b/CHANGELOG.md index 943f03f7d3..9d1abd6ca1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,16 @@ # Changelog All notable changes to this project are documented in this file. +## [unreleased] +### Added +- Add `ZMPgenerator` to `UnicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/916) + +### Changed + + +### Fixed +- Fix outputs of `UnicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/916) + ## [0.20.0] - 2024-12-16 ### Added - Add `USE_SYSTEM_tiny-process-library` CMake option to use `tiny-process-library` found in system (https://github.com/ami-iit/bipedal-locomotion-framework/pull/891) @@ -27,6 +37,7 @@ All notable changes to this project are documented in this file. - Bug fix of `prepare_data` method calling in `joints-grid-position-tracking` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/895) - Fix the normal force limit constraint in the `CentroidalMPC` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/898) - Fix loading of Python bindings on Windows when installed in arbitrary directory (https://github.com/ami-iit/bipedal-locomotion-framework/pull/905) +- Fix outputs of `UnicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/916) ## [0.19.0] - 2024-09-06 ### Added diff --git a/examples/UnicycleTrajectoryGenerator/main.cpp b/examples/UnicycleTrajectoryGenerator/main.cpp index 8740c20b96..c289670808 100644 --- a/examples/UnicycleTrajectoryGenerator/main.cpp +++ b/examples/UnicycleTrajectoryGenerator/main.cpp @@ -31,6 +31,7 @@ std::shared_ptr getUnicycleParametersHand handler->setParameter("mergePointRatios", Eigen::Vector2d(0.4, 0.4)); handler->setParameter("leftContactFrameName", "l_sole"); handler->setParameter("rightContactFrameName", "r_sole"); + handler->setParameter("use_zmp_generator", false); return handler; } @@ -205,23 +206,24 @@ int main(int argc, char* argv[]) w_H_left = leftFootPlanner.getOutput().transform; w_H_right = rightFootPlanner.getOutput().transform; + // get the DCM trajectory from the unicycle trajectory generator + auto dcmPosition = output.dcmTrajectory.position; + auto dcmVelocity = output.dcmTrajectory.velocity; + if (saveResults) { positionLeftFoot.push_back(w_H_left.translation()); positionRightFoot.push_back(w_H_right.translation()); positionCOM.push_back(output.comTrajectory.position.front()); velocityCOM.push_back(output.comTrajectory.velocity.front()); - DCMposition.push_back(output.dcmTrajectory.position.front()); - DCMvelocity.push_back(output.dcmTrajectory.velocity.front()); + DCMposition.push_back(dcmPosition.front()); + DCMvelocity.push_back(dcmVelocity.front()); i++; time.push_back( std::chrono::duration_cast(currentTime - timeStart) .count()); } - // get the DCM trajectory from the unicycle trajectory generator - auto dcmPosition = output.dcmTrajectory.position; - auto dcmVelocity = output.dcmTrajectory.velocity; Eigen::VectorXd Xdcm; Xdcm.resize(dcmPosition.size()); @@ -239,8 +241,8 @@ int main(int argc, char* argv[]) Ydcm(i) = dcmPosition[i][1]; } - // log()->info("[main] DCM x: {}", Xdcm.transpose()); - // log()->info("[main] DCM y: {}", Ydcm.transpose()); + log()->debug("[main] DCM x: {}", Xdcm.transpose()); + log()->debug("[main] DCM y: {}", Ydcm.transpose()); BipedalLocomotion::clock().sleepUntil(currentTime + dtChrono); diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h index 91bf82596b..3f67fefc20 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h @@ -101,12 +101,13 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryGenerator final * Initialize the planner. * * @note The following parameters are required by the class: - * | Name | Type | Default | Example | Description | - * | :-------------------------: | :------------: | :---------------: | :-------------: | :-------------------------------------------------------: | - * | `planner_advance_time_in_s` | double | 0.08 | - | The time in advance at which the planner is called | - * | `dt` | double | 0.002 | - | The sampling time of the trajectory generator | - * | `leftContactFrameName` | string | - | "l_sole" | Name of the left foot contact frame | - * | `rightContactFrameName` | string | - | "r_sole" | Name of the right foot contact frame | + * | Name | Type | Default | Example | Description | + * | :-------------------------: | :------------: | :---------------: | :-------------: | :-----------------------------------------------------: | + * | `planner_advance_time_in_s` | double | 0.08 | - | The time in advance at which the planner is called | + * | `dt` | double | 0.002 | - | The sampling time of the trajectory generator | + * | `leftContactFrameName` | string | - | "l_sole" | Name of the left foot contact frame | + * | `rightContactFrameName` | string | - | "r_sole" | Name of the right foot contact frame | + * | `use_zmp_generator` | bool | false | - | If true enables the zmp generator | * * Implicitely, the class needs also all the parameters required by the Bipedalocotion::Planner::UnicyclePlanner class. // clang-format on @@ -182,6 +183,8 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryGenerator final * @return True in case of success, false otherwise. */ bool generateFirstTrajectory(); + + bool m_useZMPGenerator{false}; /**< True if the ZMP generator is enabled. False otherwise. */ }; #endif // BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_TRAJECTORY_GENERATOR_H diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h index d64c4e3f4d..6d6f177ac5 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h @@ -18,10 +18,12 @@ #include #include #include +#include #include #include #include +#include #include #include namespace BipedalLocomotion::Planners @@ -46,6 +48,8 @@ struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerInput DCMInitialState dcmInitialState; /**< Initial state of the DCM trajectory generator */ + InitialState zmpInitialState; /**< Initial state of the ZMP trajectory generator */ + struct COMInitialState { Eigen::Vector2d initialPlanarPosition; /**< Initial planar position of the CoM */ @@ -80,6 +84,16 @@ struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerOutput std::vector velocity; }; + struct ZMPTrajectory + { + std::vector position; + std::vector localPositionLeft; + std::vector localPositionRight; + std::vector weightInLeftFoot; + std::vector weightInRightFoot; + std::vector initialState; + }; + struct ContactStatus { std::vector leftFootInContact; /**< True if the left foot is in contact. False @@ -108,6 +122,8 @@ struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerOutput DCMTrajectory dcmTrajectory; /**< DCM trajectory */ + std::optional zmpTrajectory; /**< ZMP trajectory */ + ContactStatus contactStatus; /**< Contact status of the feet */ Steps steps; /**< List of steps and their phases */ @@ -194,45 +210,49 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryPlanner final * * @note The following parameters are required by the class: * - * | Name | Type | Default | Example | Description | - * | :-----------------------: | :---------------: | :---------------: | :-------------: | :-------------------------------------------------------------: | - * | `referencePosition` | list of 2 doubles | - | (0.1 0.0) | The reference position of the unicycle controller | - * | `controlType` | string | "direct" | - | The control mode used by the unicycle controller | - * | `unicycleGain` | double | 10.0 | - | The main gain of the unicycle controller | - * | `slowWhenTurningGain` | double | 2.0 | - | The turning gain of the unicycle controller | - * | `slowWhenBackwardFactor` | double | 0.4 | - | The backward gain of the unicycle controller | - * | `slowWhenSidewaysFactor` | double | 0.2 | - | The sideways gain of the unicycle controller | - * | `dt` | double | 0.002 | - | The sampling time of the planner | - * | `plannerHorizon` | double | 20.0 | - | The planner time horizon | - * | `positionWeight` | double | 1.0 | - | The position weight of the OC problem | - * | `timeWeight` | double | 2.5 | - | The time weight of the OC problem | - * | `maxStepLength` | double | 0.32 | - | The maximum length of a step | - * | `minStepLength` | double | 0.01 | - | The minimum length of a step | - * |`maxLengthBackwardFactor` | double | 0.8 | - | The factor of maximum backward walk | - * | `nominalWidth` | double | 0.20 | - | The nominal feet distance | - * | `minWidth` | double | 0.14 | - | The minimum feet distance | - * | `minStepDuration` | double | 0.65 | - | The minimum duration of a step | - * | `maxStepDuration` | double | 1.5 | - | The maximum duration of a step | - * | `nominalDuration` | double | 0.8 | - | The nominal duration of a step | - * | `maxAngleVariation` | double | 18.0 | - | The maximum unicycle rotation | - * | `minAngleVariation` | double | 5.0 | - | The minimum unicycle rotation | - * | `saturationFactors` | list of 2 doubles | - | (0.7 0.7) | Linear and Angular velocity conservative factors | - * | `leftYawDeltaInDeg` | double | 0.0 | - | Offset for the left foot rotation around the z axis | - * | `rightYawDeltaInDeg` | double | 0.0 | - | Offset for the right foot rotation around the z axis | - * | `swingLeft` | bool | false | - | Perform the first step with the left foot | - * | `startAlwaysSameFoot` | bool | false | - | Restart with the default foot if still | - * | `terminalStep` | bool | true | - | Add a terminal step at the end of the horizon | - * | `mergePointRatios` | list of 2 doubles | - | (0.4 0.4) | The ratios of the DS phase in which it is present a merge point | - * | `switchOverSwingRatio` | double | 0.2 | - | The ratio between single and double support phases | - * | `lastStepSwitchTime` | double | 0.3 | - | Time duration of double support phase in final step | - * | `isPauseActive` | bool | true | - | If true, the planner can pause, instead of make tiny steps. | - * | `comHeight` | double | 0.70 | - | CoM height in double support phase | - * | `comHeightDelta` | double | 0.01 | - | Delta to add to CoM heinght in Single support phases | - * | `leftZMPDelta` | list of 2 doubles | - | (0.0 0.0) | Local ZMP reference: delta wrt center frame of the foot | - * | `rightZMPDelta` | list of 2 doubles | - | (0.0 0.0) | Local ZMP reference: delta wrt center frame of the foot | - * | `lastStepDCMOffset` | double | 0.5 | - | Last Step DCM Offset. If 0, DCM coincides with stance foot ZMP | - * | `leftContactFrameName` | string | - | "l_sole" | Name of the left foot contact frame | - * | `rightContactFrameName` | string | - | "r_sole" | Name of the right foot contact frame | + * | Name | Type | Default | Example | Description | + * | :-----------------------: | :---------------: | :---------------: | :-------------: | :--------------------------------------------------------------------: | + * | `referencePosition` | list of 2 doubles | - | (0.1 0.0) | The reference position of the unicycle controller | + * | `controlType` | string | "direct" | - | The control mode used by the unicycle controller | + * | `unicycleGain` | double | 10.0 | - | The main gain of the unicycle controller | + * | `slowWhenTurningGain` | double | 2.0 | - | The turning gain of the unicycle controller | + * | `slowWhenBackwardFactor` | double | 0.4 | - | The backward gain of the unicycle controller | + * | `slowWhenSidewaysFactor` | double | 0.2 | - | The sideways gain of the unicycle controller | + * | `dt` | double | 0.002 | - | The sampling time of the planner | + * | `plannerHorizon` | double | 20.0 | - | The planner time horizon | + * | `positionWeight` | double | 1.0 | - | The position weight of the OC problem | + * | `timeWeight` | double | 2.5 | - | The time weight of the OC problem | + * | `maxStepLength` | double | 0.32 | - | The maximum length of a step | + * | `minStepLength` | double | 0.01 | - | The minimum length of a step | + * |`maxLengthBackwardFactor` | double | 0.8 | - | The factor of maximum backward walk | + * | `nominalWidth` | double | 0.20 | - | The nominal feet distance | + * | `minWidth` | double | 0.14 | - | The minimum feet distance | + * | `minStepDuration` | double | 0.65 | - | The minimum duration of a step | + * | `maxStepDuration` | double | 1.5 | - | The maximum duration of a step | + * | `nominalDuration` | double | 0.8 | - | The nominal duration of a step | + * | `maxAngleVariation` | double | 18.0 | - | The maximum unicycle rotation | + * | `minAngleVariation` | double | 5.0 | - | The minimum unicycle rotation | + * | `saturationFactors` | list of 2 doubles | - | (0.7 0.7) | Linear and Angular velocity conservative factors | + * | `leftYawDeltaInDeg` | double | 0.0 | - | Offset for the left foot rotation around the z axis | + * | `rightYawDeltaInDeg` | double | 0.0 | - | Offset for the right foot rotation around the z axis | + * | `swingLeft` | bool | false | - | Perform the first step with the left foot | + * | `startAlwaysSameFoot` | bool | false | - | Restart with the default foot if still | + * | `terminalStep` | bool | true | - | Add a terminal step at the end of the horizon | + * | `mergePointRatios` | list of 2 doubles | - | (0.4 0.4) | The ratios of the DS phase in which it is present a merge point | + * | `switchOverSwingRatio` | double | 0.2 | - | The ratio between single and double support phases | + * | `lastStepSwitchTime` | double | 0.3 | - | Time duration of double support phase in final step | + * | `isPauseActive` | bool | true | - | If true, the planner can pause, instead of make tiny steps. | + * | `comHeight` | double | 0.70 | - | CoM height in double support phase | + * | `comHeightDelta` | double | 0.01 | - | Delta to add to CoM heinght in Single support phases | + * | `leftZMPDelta` | list of 2 doubles | - | (0.0 0.0) | Local ZMP reference: delta wrt center frame of the foot | + * | `rightZMPDelta` | list of 2 doubles | - | (0.0 0.0) | Local ZMP reference: delta wrt center frame of the foot | + * | `leftZMPInitSwitchDelta` | list of 2 doubles | (0.0 0.0) | (0.0 0.0) | Local ZMP reference: delta wrt center frame when the switch to the | + * | | | | | other foot begins. This parameter is only used by the ZMP generator. | + * | `rightZMPInitSwitchDelta` | list of 2 doubles | (0.0 0.0) | (0.0 0.0) | Local ZMP reference: delta wrt center frame when the switch to the | + * | | | | | other foot begins. This parameter is only used by the ZMP generator. | + * | `lastStepDCMOffset` | double | 0.5 | - | Last Step DCM Offset. If 0, DCM coincides with stance foot ZMP | + * | `leftContactFrameName` | string | - | "l_sole" | Name of the left foot contact frame | + * | `rightContactFrameName` | string | - | "r_sole" | Name of the right foot contact frame | * * @param handler Pointer to the parameter handler. * @return True in case of success, false otherwise. diff --git a/src/Planners/src/UnicycleTrajectoryGenerator.cpp b/src/Planners/src/UnicycleTrajectoryGenerator.cpp index aaa0b30c45..3f66746f25 100644 --- a/src/Planners/src/UnicycleTrajectoryGenerator.cpp +++ b/src/Planners/src/UnicycleTrajectoryGenerator.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -74,6 +75,12 @@ class Planners::UnicycleTrajectoryGenerator::Impl std::deque comPosition; std::deque comVelocity; std::deque comAcceleration; + std::deque zmpPosition; + std::deque leftFootZMPPosition; + std::deque rightFootZMPPosition; + std::deque weightInLeftFoot; + std::deque weightInRightFoot; + std::deque zmpGeneratorInitialState; std::deque leftFootinContact; std::deque rightFootinContact; std::deque isLeftFootLastSwinging; @@ -276,6 +283,11 @@ bool Planners::UnicycleTrajectoryGenerator::initialize( initialBasePosition, leftToRightTransform); + // Get the generator being used and instantiate optional outputs + ok = ok && loadParamWithFallback("use_zmp_generator", m_useZMPGenerator, false); + m_pImpl->output.zmpTrajectory + = std::make_optional(); + // Initialize contact frames std::string leftContactFrameName, rightContactFrameName; ok = ok && loadParam("leftContactFrameName", leftContactFrameName); @@ -300,7 +312,6 @@ Planners::UnicycleTrajectoryGenerator::getOutput() const Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmPosition, m_pImpl->output.dcmTrajectory.position); - Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmVelocity, m_pImpl->output.dcmTrajectory.velocity); @@ -339,6 +350,56 @@ Planners::UnicycleTrajectoryGenerator::getOutput() const m_pImpl->output.rightFootTrajectory .mixedAcceleration); + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.leftFootinContact, + m_pImpl->output.contactStatus + .leftFootInContact); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.rightFootinContact, + m_pImpl->output.contactStatus + .rightFootInContact); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.isLeftFootLastSwinging, + m_pImpl->output.contactStatus + .UsedLeftAsFixed); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.mergePoints, + m_pImpl->output.mergePoints); + + if (m_useZMPGenerator) + { + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.zmpPosition, + m_pImpl->output.zmpTrajectory + ->position); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.leftFootZMPPosition, + m_pImpl->output.zmpTrajectory + ->localPositionLeft); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory + .rightFootZMPPosition, + m_pImpl->output.zmpTrajectory + ->localPositionRight); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.weightInLeftFoot, + m_pImpl->output.zmpTrajectory + ->weightInLeftFoot); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.weightInRightFoot, + m_pImpl->output.zmpTrajectory + ->weightInRightFoot); + + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory + .zmpGeneratorInitialState, + m_pImpl->output.zmpTrajectory + ->initialState); + } else + { + m_pImpl->output.zmpTrajectory.reset(); + } + + m_pImpl->output.steps.leftSteps = m_pImpl->trajectory.leftSteps; + m_pImpl->output.steps.rightSteps = m_pImpl->trajectory.rightSteps; + // instatiate variables for the contact phase lists BipedalLocomotion::Contacts::ContactListMap contactListMap; BipedalLocomotion::Contacts::ContactList leftContactList, rightContactList; @@ -395,6 +456,8 @@ Planners::UnicycleTrajectoryGenerator::getOutput() const m_pImpl->output.contactPhaseList.setLists(contactListMap); + m_pImpl->output.isValid = true; + return m_pImpl->output; } @@ -594,6 +657,7 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::Impl::askNewTraje = trajectory.comPosition[mergePoint].head<2>(); unicycleTrajectoryPlannerInput.comInitialState.initialPlanarVelocity = trajectory.comVelocity[mergePoint].head<2>(); + unicycleTrajectoryPlannerInput.zmpInitialState = trajectory.zmpGeneratorInitialState.front(); // set the input this->unicycleTrajectoryPlanner.setInput(unicycleTrajectoryPlannerInput); @@ -674,6 +738,34 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::Impl::mergeTrajec .comTrajectory.acceleration, trajectory.comAcceleration, mergePoint); + + // get zmp trajectory + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .zmpTrajectory->position, + trajectory.zmpPosition, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .zmpTrajectory->localPositionLeft, + trajectory.leftFootZMPPosition, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .zmpTrajectory->localPositionRight, + trajectory.rightFootZMPPosition, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .zmpTrajectory->weightInLeftFoot, + trajectory.weightInLeftFoot, + mergePoint); + Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() + .zmpTrajectory->weightInRightFoot, + trajectory.weightInRightFoot, + mergePoint); + + trajectory.zmpGeneratorInitialState + .assign(unicycleTrajectoryPlanner.getOutput().zmpTrajectory->initialState.begin(), + unicycleTrajectoryPlanner.getOutput().zmpTrajectory->initialState.end()); + trajectory.zmpGeneratorInitialState.pop_front(); + // get feet cubic spline trajectory // get left foot cubic spline @@ -772,6 +864,11 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::Impl::advanceTraj trajectoryStepLambda(trajectory.rightFootTransform); trajectoryStepLambda(trajectory.rightFootMixedVelocity); trajectoryStepLambda(trajectory.rightFootMixedAcceleration); + trajectoryStepLambda(trajectory.zmpPosition); + trajectoryStepLambda(trajectory.leftFootZMPPosition); + trajectoryStepLambda(trajectory.rightFootZMPPosition); + trajectoryStepLambda(trajectory.weightInLeftFoot); + trajectoryStepLambda(trajectory.weightInRightFoot); // at each sampling time the merge points are decreased by one. // If the first merge point is equal to 0 it will be dropped. diff --git a/src/Planners/src/UnicycleTrajectoryPlanner.cpp b/src/Planners/src/UnicycleTrajectoryPlanner.cpp index 66bea056e8..7a3810841e 100644 --- a/src/Planners/src/UnicycleTrajectoryPlanner.cpp +++ b/src/Planners/src/UnicycleTrajectoryPlanner.cpp @@ -306,6 +306,8 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( double comHeightDelta; Eigen::Vector2d leftZMPDelta; Eigen::Vector2d rightZMPDelta; + Eigen::Vector2d leftZMPInitSwitchDelta; + Eigen::Vector2d rightZMPInitSwitchDelta; double lastStepDCMOffset; // parse initialization parameters @@ -359,6 +361,15 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( ok = ok && loadParamWithFallback("comHeightDelta", comHeightDelta, 0.01); ok = ok && loadParam("leftZMPDelta", leftZMPDelta); ok = ok && loadParam("rightZMPDelta", rightZMPDelta); + ok = ok + && loadParamWithFallback("leftZMPInitSwitchDelta", + leftZMPInitSwitchDelta, + Eigen::Vector2d::Zero()); + ok = ok + && loadParamWithFallback("rightZMPInitSwitchDelta", + rightZMPInitSwitchDelta, + Eigen::Vector2d::Zero()); + ok = ok && loadParamWithFallback("lastStepDCMOffset", lastStepDCMOffset, 0.5); ok = ok && loadParam("leftContactFrameName", m_pImpl->parameters.leftContactFrameName); ok = ok && loadParam("rightContactFrameName", m_pImpl->parameters.rightContactFrameName); @@ -405,6 +416,9 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( auto comHeightGenerator = m_pImpl->generator.addCoMHeightTrajectoryGenerator(); ok = ok && comHeightGenerator->setCoMHeightSettings(comHeight, comHeightDelta); + m_pImpl->output.zmpTrajectory + = std::make_optional(); + auto dcmGenerator = m_pImpl->generator.addDCMTrajectoryGenerator(); iDynTree::Vector2 leftZMPDeltaVec{leftZMPDelta}; iDynTree::Vector2 rightZMPDeltaVec{rightZMPDelta}; @@ -414,6 +428,14 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( dcmGenerator->setFirstDCMTrajectoryMode(FirstDCMTrajectoryMode::FifthOrderPoly); ok = ok && dcmGenerator->setLastStepDCMOffsetPercentage(lastStepDCMOffset); + auto zmpGenerator = m_pImpl->generator.addZMPTrajectoryGenerator(); + iDynTree::Vector2 leftZMPInitSwitchDeltaVec{leftZMPInitSwitchDelta}; + iDynTree::Vector2 rightZMPInitSwitchDeltaVec{rightZMPInitSwitchDelta}; + ok = ok && zmpGenerator->setStanceZMPDelta(leftZMPDeltaVec, rightZMPDeltaVec); + ok = ok + && zmpGenerator->setInitialSwitchZMPDelta(leftZMPInitSwitchDeltaVec, + rightZMPInitSwitchDeltaVec); + // initialize the COM system m_pImpl->comSystem.dynamics = std::make_shared(); @@ -548,6 +570,7 @@ bool Planners::UnicycleTrajectoryPlanner::advance() auto unicyclePlanner = m_pImpl->generator.unicyclePlanner(); auto dcmGenerator = m_pImpl->generator.addDCMTrajectoryGenerator(); + auto zmpGenerator = m_pImpl->generator.addZMPTrajectoryGenerator(); double initTime{m_pImpl->input.initTime.count() * 1e-9}; m_pImpl->initTime = m_pImpl->input.initTime; @@ -651,6 +674,13 @@ bool Planners::UnicycleTrajectoryPlanner::advance() return false; } + // set the initial state of the ZMP trajectory generator + if (!zmpGenerator->setWeightInitialState(m_pImpl->input.zmpInitialState)) + { + log()->error("{} Failed to set the initial state of the zmp generator.", logPrefix); + return false; + } + // generate the new trajectory if (!(m_pImpl->generator.reGenerate(initTime, dt, @@ -695,6 +725,21 @@ bool Planners::UnicycleTrajectoryPlanner::advance() m_pImpl->output.dcmTrajectory.position); Planners::UnicycleUtilities::Conversions::convertVector(dcmGenerator->getDCMVelocity(), m_pImpl->output.dcmTrajectory.velocity); + // get the ZMP trajectory + std::vector leftZMP, rightZMP, globalZMP; + zmpGenerator->getZMPTrajectory(globalZMP); + Planners::UnicycleUtilities::Conversions::convertVector(globalZMP, + m_pImpl->output.zmpTrajectory->position); + zmpGenerator->getLocalZMPTrajectories(leftZMP, rightZMP); + Planners::UnicycleUtilities::Conversions::convertVector(leftZMP, + m_pImpl->output.zmpTrajectory + ->localPositionLeft); + Planners::UnicycleUtilities::Conversions::convertVector(rightZMP, + m_pImpl->output.zmpTrajectory + ->localPositionRight); + zmpGenerator->getWeightPercentage(m_pImpl->output.zmpTrajectory->weightInLeftFoot, + m_pImpl->output.zmpTrajectory->weightInRightFoot); + zmpGenerator->getInitialStatesAtMergePoints(m_pImpl->output.zmpTrajectory->initialState); // get the CoM planar trajectory std::chrono::nanoseconds time = m_pImpl->input.initTime; diff --git a/src/Planners/src/UnicycleUtilities.cpp b/src/Planners/src/UnicycleUtilities.cpp index 7294c587c8..edde9f9ce7 100644 --- a/src/Planners/src/UnicycleUtilities.cpp +++ b/src/Planners/src/UnicycleUtilities.cpp @@ -14,7 +14,6 @@ #include #include - namespace BipedalLocomotion::Planners::UnicycleUtilities { @@ -145,8 +144,7 @@ namespace Conversions { void convertToBLF(const iDynTree::Transform& input, manif::SE3d& output) { - output.asSO3() = iDynTree::toEigen(input.getRotation().asQuaternion()); - output.translation(iDynTree::toEigen(input.getPosition())); + output = BipedalLocomotion::Conversions::toManifPose(input); } void convertToBLF(const iDynTree::Twist& input, manif::SE3d::Tangent& output)