From 4869136ea05f156b1f1ef24a555baa28c4234ab9 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 10 Dec 2024 13:07:33 +0100 Subject: [PATCH 01/10] populate all outputs of unycycle trajectory generator --- .../src/UnicycleTrajectoryGenerator.cpp | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/Planners/src/UnicycleTrajectoryGenerator.cpp b/src/Planners/src/UnicycleTrajectoryGenerator.cpp index aaa0b30c45..1d9b441c49 100644 --- a/src/Planners/src/UnicycleTrajectoryGenerator.cpp +++ b/src/Planners/src/UnicycleTrajectoryGenerator.cpp @@ -339,6 +339,24 @@ 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); + + 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 +413,8 @@ Planners::UnicycleTrajectoryGenerator::getOutput() const m_pImpl->output.contactPhaseList.setLists(contactListMap); + m_pImpl->output.isValid = true; + return m_pImpl->output; } From 2624012a891a72728d25c8f4ab2256455cc7da50 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 10 Dec 2024 13:18:09 +0100 Subject: [PATCH 02/10] update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 943f03f7d3..bc3835649a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,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 From c82f0d0429e8daa6676e18c2a02e13f1b25fa43f Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Mon, 16 Dec 2024 19:19:08 +0100 Subject: [PATCH 03/10] add zmp generator --- .../Planners/UnicycleTrajectoryPlanner.h | 95 +++++++++++-------- .../src/UnicycleTrajectoryGenerator.cpp | 62 ++++++++++++ .../src/UnicycleTrajectoryPlanner.cpp | 42 ++++++++ 3 files changed, 160 insertions(+), 39 deletions(-) diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h index d64c4e3f4d..5cefc5e83e 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -46,6 +47,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 +83,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 +121,8 @@ struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerOutput DCMTrajectory dcmTrajectory; /**< DCM trajectory */ + ZMPTrajectory zmpTrajectory; /**< ZMP trajectory */ + ContactStatus contactStatus; /**< Contact status of the feet */ Steps steps; /**< List of steps and their phases */ @@ -194,45 +209,47 @@ 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 begin | + * | `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 begin | + * | `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 1d9b441c49..6f1cc45f6b 100644 --- a/src/Planners/src/UnicycleTrajectoryGenerator.cpp +++ b/src/Planners/src/UnicycleTrajectoryGenerator.cpp @@ -74,6 +74,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; @@ -354,6 +360,29 @@ Planners::UnicycleTrajectoryGenerator::getOutput() const Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.mergePoints, m_pImpl->output.mergePoints); + 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); + m_pImpl->output.steps.leftSteps = m_pImpl->trajectory.leftSteps; m_pImpl->output.steps.rightSteps = m_pImpl->trajectory.rightSteps; @@ -614,6 +643,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); @@ -694,6 +724,33 @@ 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()); + // get feet cubic spline trajectory // get left foot cubic spline @@ -792,6 +849,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..868df19db5 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); @@ -414,6 +425,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 +567,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 +671,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 +722,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; From 2f356f65877bf53ba35616b5667c7fe86719f05b Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Mon, 16 Dec 2024 19:24:55 +0100 Subject: [PATCH 04/10] update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index bc3835649a..d36a1121fb 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,7 @@ All notable changes to this project are documented in this file. - Add KF to estimate joint and motor velocities in JoinTorqueControlDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/909) - Added option `FRAMEWORK_COMPILE_Ros1Publisher` to enable or disable the compilation of the `BipedalLocomotion::YarpUtilities::RosPublisher` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/914) - Added rpc commands to `YarpLoggerDevice` to trigger the saving of data to a local file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/915) +- Add `ZMPgenerator` to `UnicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/916) ### Changed - Change device jtcvc to use motor velocity and joint velocity in input to PINN models (https://github.com/ami-iit/bipedal-locomotion-framework/pull/903) From ac4b0f5b8d626410555b5ebf2b72fb3e12dc56f0 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Wed, 18 Dec 2024 16:27:48 +0100 Subject: [PATCH 05/10] fix merging of zmp initial state --- src/Planners/src/UnicycleTrajectoryGenerator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Planners/src/UnicycleTrajectoryGenerator.cpp b/src/Planners/src/UnicycleTrajectoryGenerator.cpp index 6f1cc45f6b..ff29c8e7f7 100644 --- a/src/Planners/src/UnicycleTrajectoryGenerator.cpp +++ b/src/Planners/src/UnicycleTrajectoryGenerator.cpp @@ -750,6 +750,7 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::Impl::mergeTrajec trajectory.zmpGeneratorInitialState .assign(unicycleTrajectoryPlanner.getOutput().zmpTrajectory.initialState.begin(), unicycleTrajectoryPlanner.getOutput().zmpTrajectory.initialState.end()); + trajectory.zmpGeneratorInitialState.pop_front(); // get feet cubic spline trajectory From 7f78303f5c6db4a09afeed4db94b4c659f38fb3b Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Wed, 18 Dec 2024 16:32:22 +0100 Subject: [PATCH 06/10] update changelog after rebase --- CHANGELOG.md | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d36a1121fb..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) @@ -14,7 +24,6 @@ All notable changes to this project are documented in this file. - Add KF to estimate joint and motor velocities in JoinTorqueControlDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/909) - Added option `FRAMEWORK_COMPILE_Ros1Publisher` to enable or disable the compilation of the `BipedalLocomotion::YarpUtilities::RosPublisher` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/914) - Added rpc commands to `YarpLoggerDevice` to trigger the saving of data to a local file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/915) -- Add `ZMPgenerator` to `UnicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/916) ### Changed - Change device jtcvc to use motor velocity and joint velocity in input to PINN models (https://github.com/ami-iit/bipedal-locomotion-framework/pull/903) From ecbd51ffa8afd51fca28cf5eca9c8b51ee7d432b Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Mon, 23 Dec 2024 12:14:13 +0100 Subject: [PATCH 07/10] add optional outputs for zmp and dcm trajectory --- examples/UnicycleTrajectoryGenerator/main.cpp | 21 ++-- .../Planners/UnicycleTrajectoryGenerator.h | 16 +-- .../Planners/UnicycleTrajectoryPlanner.h | 5 +- .../src/UnicycleTrajectoryGenerator.cpp | 97 ++++++++++++------- .../src/UnicycleTrajectoryPlanner.cpp | 33 ++++--- 5 files changed, 107 insertions(+), 65 deletions(-) diff --git a/examples/UnicycleTrajectoryGenerator/main.cpp b/examples/UnicycleTrajectoryGenerator/main.cpp index 8740c20b96..b1eefbde3e 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,29 @@ 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 + bool isDCMGeneratorUsed = output.dcmTrajectory.has_value(); + auto dcmPosition = (isDCMGeneratorUsed) + ? output.dcmTrajectory->position + : std::vector(1, Eigen::Vector2d(0.0, 0.0)); + auto dcmVelocity = (isDCMGeneratorUsed) + ? output.dcmTrajectory->velocity + : std::vector(1, Eigen::Vector2d(0.0, 0.0)); + 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 +246,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..ac701168a2 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 use the zmp generator, if false use the default one (i.e. dcm) | * * Implicitely, the class needs also all the parameters required by the Bipedalocotion::Planner::UnicyclePlanner class. // clang-format on @@ -182,6 +183,9 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryGenerator final * @return True in case of success, false otherwise. */ bool generateFirstTrajectory(); + + bool m_useDCMGenerator{true}; /**< True if the DCM generator is used. False otherwise. */ + bool m_useZMPGenerator{false}; /**< True if the ZMP generator is used. 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 5cefc5e83e..643e483214 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include namespace BipedalLocomotion::Planners @@ -119,9 +120,9 @@ struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerOutput COMTrajectory comTrajectory; /**< CoM trajectory */ - DCMTrajectory dcmTrajectory; /**< DCM trajectory */ + std::optional dcmTrajectory; /**< DCM trajectory */ - ZMPTrajectory zmpTrajectory; /**< ZMP trajectory */ + std::optional zmpTrajectory; /**< ZMP trajectory */ ContactStatus contactStatus; /**< Contact status of the feet */ diff --git a/src/Planners/src/UnicycleTrajectoryGenerator.cpp b/src/Planners/src/UnicycleTrajectoryGenerator.cpp index ff29c8e7f7..7ad3c6e219 100644 --- a/src/Planners/src/UnicycleTrajectoryGenerator.cpp +++ b/src/Planners/src/UnicycleTrajectoryGenerator.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -282,6 +283,14 @@ 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_useDCMGenerator = !m_useZMPGenerator; + m_pImpl->output.zmpTrajectory + = std::make_optional(); + m_pImpl->output.dcmTrajectory + = std::make_optional(); + // Initialize contact frames std::string leftContactFrameName, rightContactFrameName; ok = ok && loadParam("leftContactFrameName", leftContactFrameName); @@ -304,11 +313,18 @@ Planners::UnicycleTrajectoryGenerator::getOutput() const { constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::getOutput]"; - Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmPosition, - m_pImpl->output.dcmTrajectory.position); - - Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmVelocity, - m_pImpl->output.dcmTrajectory.velocity); + if (m_useDCMGenerator) + { + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmPosition, + m_pImpl->output.dcmTrajectory + ->position); + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmVelocity, + m_pImpl->output.dcmTrajectory + ->velocity); + } else + { + m_pImpl->output.dcmTrajectory.reset(); + } Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.comPosition, m_pImpl->output.comTrajectory.position); @@ -360,28 +376,37 @@ Planners::UnicycleTrajectoryGenerator::getOutput() const Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.mergePoints, m_pImpl->output.mergePoints); - 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); + 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; @@ -690,12 +715,12 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::Impl::mergeTrajec // get dcm position Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() - .dcmTrajectory.position, + .dcmTrajectory->position, trajectory.dcmPosition, mergePoint); // get dcm velocity Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() - .dcmTrajectory.velocity, + .dcmTrajectory->velocity, trajectory.dcmVelocity, mergePoint); // get feet contact status @@ -727,29 +752,29 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::Impl::mergeTrajec // get zmp trajectory Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() - .zmpTrajectory.position, + .zmpTrajectory->position, trajectory.zmpPosition, mergePoint); Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() - .zmpTrajectory.localPositionLeft, + .zmpTrajectory->localPositionLeft, trajectory.leftFootZMPPosition, mergePoint); Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() - .zmpTrajectory.localPositionRight, + .zmpTrajectory->localPositionRight, trajectory.rightFootZMPPosition, mergePoint); Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() - .zmpTrajectory.weightInLeftFoot, + .zmpTrajectory->weightInLeftFoot, trajectory.weightInLeftFoot, mergePoint); Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() - .zmpTrajectory.weightInRightFoot, + .zmpTrajectory->weightInRightFoot, trajectory.weightInRightFoot, mergePoint); trajectory.zmpGeneratorInitialState - .assign(unicycleTrajectoryPlanner.getOutput().zmpTrajectory.initialState.begin(), - unicycleTrajectoryPlanner.getOutput().zmpTrajectory.initialState.end()); + .assign(unicycleTrajectoryPlanner.getOutput().zmpTrajectory->initialState.begin(), + unicycleTrajectoryPlanner.getOutput().zmpTrajectory->initialState.end()); trajectory.zmpGeneratorInitialState.pop_front(); // get feet cubic spline trajectory diff --git a/src/Planners/src/UnicycleTrajectoryPlanner.cpp b/src/Planners/src/UnicycleTrajectoryPlanner.cpp index 868df19db5..1eceb00d86 100644 --- a/src/Planners/src/UnicycleTrajectoryPlanner.cpp +++ b/src/Planners/src/UnicycleTrajectoryPlanner.cpp @@ -416,6 +416,11 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( auto comHeightGenerator = m_pImpl->generator.addCoMHeightTrajectoryGenerator(); ok = ok && comHeightGenerator->setCoMHeightSettings(comHeight, comHeightDelta); + m_pImpl->output.zmpTrajectory + = std::make_optional(); + m_pImpl->output.dcmTrajectory + = std::make_optional(); + auto dcmGenerator = m_pImpl->generator.addDCMTrajectoryGenerator(); iDynTree::Vector2 leftZMPDeltaVec{leftZMPDelta}; iDynTree::Vector2 rightZMPDeltaVec{rightZMPDelta}; @@ -719,24 +724,24 @@ bool Planners::UnicycleTrajectoryPlanner::advance() }; Planners::UnicycleUtilities::Conversions::convertVector(dcmGenerator->getDCMPosition(), - m_pImpl->output.dcmTrajectory.position); + m_pImpl->output.dcmTrajectory->position); Planners::UnicycleUtilities::Conversions::convertVector(dcmGenerator->getDCMVelocity(), - m_pImpl->output.dcmTrajectory.velocity); + 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); + m_pImpl->output.zmpTrajectory->position); zmpGenerator->getLocalZMPTrajectories(leftZMP, rightZMP); Planners::UnicycleUtilities::Conversions::convertVector(leftZMP, m_pImpl->output.zmpTrajectory - .localPositionLeft); + ->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); + ->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; @@ -749,19 +754,19 @@ bool Planners::UnicycleTrajectoryPlanner::advance() BipedalLocomotion::GenericContainer::named_param<"dx"_h, Eigen::VectorXd>()); Eigen::Vector4d controlInput; - m_pImpl->output.comTrajectory.position.resize(m_pImpl->output.dcmTrajectory.position.size()); - m_pImpl->output.comTrajectory.velocity.resize(m_pImpl->output.dcmTrajectory.position.size()); + m_pImpl->output.comTrajectory.position.resize(m_pImpl->output.dcmTrajectory->position.size()); + m_pImpl->output.comTrajectory.velocity.resize(m_pImpl->output.dcmTrajectory->position.size()); m_pImpl->output.comTrajectory.acceleration.resize( - m_pImpl->output.dcmTrajectory.position.size()); + m_pImpl->output.dcmTrajectory->position.size()); - for (size_t i = 0; i < m_pImpl->output.dcmTrajectory.position.size(); i++) + for (size_t i = 0; i < m_pImpl->output.dcmTrajectory->position.size(); i++) { // populate CoM planar position m_pImpl->output.comTrajectory.position[i].head<2>() = state.head<2>(); // set the control input, u - controlInput << m_pImpl->output.dcmTrajectory.position.at(i), - m_pImpl->output.dcmTrajectory.velocity.at(i); + controlInput << m_pImpl->output.dcmTrajectory->position.at(i), + m_pImpl->output.dcmTrajectory->velocity.at(i); m_pImpl->comSystem.dynamics->setControlInput({controlInput}); // compute the state derivative xdot = Ax + Bu From 4e1a9a5a14c2fb267a3b37847c5cc92ca25276ce Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Mon, 23 Dec 2024 13:11:57 +0100 Subject: [PATCH 08/10] only zmpGenerator outputs as optional --- examples/UnicycleTrajectoryGenerator/main.cpp | 9 ++------ .../Planners/UnicycleTrajectoryGenerator.h | 17 +++++++------- .../Planners/UnicycleTrajectoryPlanner.h | 2 +- .../src/UnicycleTrajectoryGenerator.cpp | 23 +++++-------------- .../src/UnicycleTrajectoryPlanner.cpp | 18 +++++++-------- 5 files changed, 25 insertions(+), 44 deletions(-) diff --git a/examples/UnicycleTrajectoryGenerator/main.cpp b/examples/UnicycleTrajectoryGenerator/main.cpp index b1eefbde3e..c289670808 100644 --- a/examples/UnicycleTrajectoryGenerator/main.cpp +++ b/examples/UnicycleTrajectoryGenerator/main.cpp @@ -207,13 +207,8 @@ int main(int argc, char* argv[]) w_H_right = rightFootPlanner.getOutput().transform; // get the DCM trajectory from the unicycle trajectory generator - bool isDCMGeneratorUsed = output.dcmTrajectory.has_value(); - auto dcmPosition = (isDCMGeneratorUsed) - ? output.dcmTrajectory->position - : std::vector(1, Eigen::Vector2d(0.0, 0.0)); - auto dcmVelocity = (isDCMGeneratorUsed) - ? output.dcmTrajectory->velocity - : std::vector(1, Eigen::Vector2d(0.0, 0.0)); + auto dcmPosition = output.dcmTrajectory.position; + auto dcmVelocity = output.dcmTrajectory.velocity; if (saveResults) { diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h index ac701168a2..3f67fefc20 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h @@ -101,13 +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 | - * | `use_zmp_generator` | bool | false | - | If true use the zmp generator, if false use the default one (i.e. dcm) | + * | 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 @@ -184,8 +184,7 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryGenerator final */ bool generateFirstTrajectory(); - bool m_useDCMGenerator{true}; /**< True if the DCM generator is used. False otherwise. */ - bool m_useZMPGenerator{false}; /**< True if the ZMP generator is used. False otherwise. */ + 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 643e483214..ec03aa098f 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h @@ -120,7 +120,7 @@ struct BipedalLocomotion::Planners::UnicycleTrajectoryPlannerOutput COMTrajectory comTrajectory; /**< CoM trajectory */ - std::optional dcmTrajectory; /**< DCM trajectory */ + DCMTrajectory dcmTrajectory; /**< DCM trajectory */ std::optional zmpTrajectory; /**< ZMP trajectory */ diff --git a/src/Planners/src/UnicycleTrajectoryGenerator.cpp b/src/Planners/src/UnicycleTrajectoryGenerator.cpp index 7ad3c6e219..3f66746f25 100644 --- a/src/Planners/src/UnicycleTrajectoryGenerator.cpp +++ b/src/Planners/src/UnicycleTrajectoryGenerator.cpp @@ -285,11 +285,8 @@ bool Planners::UnicycleTrajectoryGenerator::initialize( // Get the generator being used and instantiate optional outputs ok = ok && loadParamWithFallback("use_zmp_generator", m_useZMPGenerator, false); - m_useDCMGenerator = !m_useZMPGenerator; m_pImpl->output.zmpTrajectory = std::make_optional(); - m_pImpl->output.dcmTrajectory - = std::make_optional(); // Initialize contact frames std::string leftContactFrameName, rightContactFrameName; @@ -313,18 +310,10 @@ Planners::UnicycleTrajectoryGenerator::getOutput() const { constexpr auto logPrefix = "[UnicycleTrajectoryGenerator::getOutput]"; - if (m_useDCMGenerator) - { - Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmPosition, - m_pImpl->output.dcmTrajectory - ->position); - Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmVelocity, - m_pImpl->output.dcmTrajectory - ->velocity); - } else - { - m_pImpl->output.dcmTrajectory.reset(); - } + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmPosition, + m_pImpl->output.dcmTrajectory.position); + Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.dcmVelocity, + m_pImpl->output.dcmTrajectory.velocity); Planners::UnicycleUtilities::populateVectorFromDeque(m_pImpl->trajectory.comPosition, m_pImpl->output.comTrajectory.position); @@ -715,12 +704,12 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::Impl::mergeTrajec // get dcm position Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() - .dcmTrajectory->position, + .dcmTrajectory.position, trajectory.dcmPosition, mergePoint); // get dcm velocity Planners::UnicycleUtilities::appendVectorToDeque(unicycleTrajectoryPlanner.getOutput() - .dcmTrajectory->velocity, + .dcmTrajectory.velocity, trajectory.dcmVelocity, mergePoint); // get feet contact status diff --git a/src/Planners/src/UnicycleTrajectoryPlanner.cpp b/src/Planners/src/UnicycleTrajectoryPlanner.cpp index 1eceb00d86..7a3810841e 100644 --- a/src/Planners/src/UnicycleTrajectoryPlanner.cpp +++ b/src/Planners/src/UnicycleTrajectoryPlanner.cpp @@ -418,8 +418,6 @@ bool Planners::UnicycleTrajectoryPlanner::initialize( m_pImpl->output.zmpTrajectory = std::make_optional(); - m_pImpl->output.dcmTrajectory - = std::make_optional(); auto dcmGenerator = m_pImpl->generator.addDCMTrajectoryGenerator(); iDynTree::Vector2 leftZMPDeltaVec{leftZMPDelta}; @@ -724,9 +722,9 @@ bool Planners::UnicycleTrajectoryPlanner::advance() }; Planners::UnicycleUtilities::Conversions::convertVector(dcmGenerator->getDCMPosition(), - m_pImpl->output.dcmTrajectory->position); + m_pImpl->output.dcmTrajectory.position); Planners::UnicycleUtilities::Conversions::convertVector(dcmGenerator->getDCMVelocity(), - m_pImpl->output.dcmTrajectory->velocity); + m_pImpl->output.dcmTrajectory.velocity); // get the ZMP trajectory std::vector leftZMP, rightZMP, globalZMP; zmpGenerator->getZMPTrajectory(globalZMP); @@ -754,19 +752,19 @@ bool Planners::UnicycleTrajectoryPlanner::advance() BipedalLocomotion::GenericContainer::named_param<"dx"_h, Eigen::VectorXd>()); Eigen::Vector4d controlInput; - m_pImpl->output.comTrajectory.position.resize(m_pImpl->output.dcmTrajectory->position.size()); - m_pImpl->output.comTrajectory.velocity.resize(m_pImpl->output.dcmTrajectory->position.size()); + m_pImpl->output.comTrajectory.position.resize(m_pImpl->output.dcmTrajectory.position.size()); + m_pImpl->output.comTrajectory.velocity.resize(m_pImpl->output.dcmTrajectory.position.size()); m_pImpl->output.comTrajectory.acceleration.resize( - m_pImpl->output.dcmTrajectory->position.size()); + m_pImpl->output.dcmTrajectory.position.size()); - for (size_t i = 0; i < m_pImpl->output.dcmTrajectory->position.size(); i++) + for (size_t i = 0; i < m_pImpl->output.dcmTrajectory.position.size(); i++) { // populate CoM planar position m_pImpl->output.comTrajectory.position[i].head<2>() = state.head<2>(); // set the control input, u - controlInput << m_pImpl->output.dcmTrajectory->position.at(i), - m_pImpl->output.dcmTrajectory->velocity.at(i); + controlInput << m_pImpl->output.dcmTrajectory.position.at(i), + m_pImpl->output.dcmTrajectory.velocity.at(i); m_pImpl->comSystem.dynamics->setControlInput({controlInput}); // compute the state derivative xdot = Ax + Bu From 52e42e31f0dc10775f1160cf3707d907dce23f4e Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Mon, 23 Dec 2024 15:19:07 +0100 Subject: [PATCH 09/10] update description of planner parameters --- .../Planners/UnicycleTrajectoryPlanner.h | 84 ++++++++++--------- 1 file changed, 43 insertions(+), 41 deletions(-) diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h index ec03aa098f..6d6f177ac5 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryPlanner.h @@ -210,47 +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 | - * | `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 begin | - * | `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 begin | - * | `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. From ae3f129bb14818fe8981e7847e11f52f48474909 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Thu, 9 Jan 2025 14:44:11 +0100 Subject: [PATCH 10/10] fix conversion from idyntree pose to manif one --- src/Planners/src/UnicycleUtilities.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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)