Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add zmp generator and populate all outputs of the unycycle trajectory generator #916

Open
wants to merge 9 commits into
base: master
Choose a base branch
from
+231 −53
Open

Add zmp generator and populate all outputs of the unycycle trajectory generator #916

Show file tree
Hide file tree
Changes from 8 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
4869136
populate all outputs of unycycle trajectory generator
LoreMoretti Dec 10, 2024
2624012
update changelog
LoreMoretti Dec 10, 2024
c82f0d0
add zmp generator
LoreMoretti Dec 16, 2024
2f356f6
update changelog
LoreMoretti Dec 16, 2024
ac4b0f5
fix merging of zmp initial state
LoreMoretti Dec 18, 2024
7f78303
update changelog after rebase
LoreMoretti Dec 18, 2024
ecbd51f
add optional outputs for zmp and dcm trajectory
LoreMoretti Dec 23, 2024
4e1a9a5
only zmpGenerator outputs as optional
LoreMoretti Dec 23, 2024
52e42e3
update description of planner parameters
LoreMoretti Dec 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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)
Expand All @@ -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
Expand Down
16 changes: 9 additions & 7 deletions examples/UnicycleTrajectoryGenerator/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ std::shared_ptr<ParametersHandler::IParametersHandler> 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;
}
Expand Down Expand Up @@ -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<std::chrono::milliseconds>(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());
Expand All @@ -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);

Expand Down
15 changes: 9 additions & 6 deletions src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Loading
Loading