BipedalLocomotion::Planners::UnicycleTrajectoryGenerator class final

UnicycleTrajectoryGenerator is a class that generates reference trajectories for humanoid robots.

Every time that the advance() member function is called, the Generator:

  1. First checks if it is time to generate a new trajectory. In this case, it deploys the BipedalLocomotion::Planners::UnicycleTrajectoryPlanner to plan this new trajectory.
  2. Then, it checks if it is time to merge the current trajectory with the last one computed by the UnicycleTrajectoryPlanner. In this case, it merges the two trajectories, which become the current one.
  3. Finally, it unrolls the current trajectory (i.e., it advances it over time). The getOutput() member function returns the current trajectory, which includes the CoM, DCM, and footstep ones. The Generator requires the user to set the robot model using the setRobotModel() member function, before invoking the initialize() member function, which configures the Generator. As input, which is set by the setInput() member function, the Generator requires an instance of the UnicycleTrajectoryGeneratorInput struct.

Base classes

template<class _Input, class _Output>
class BipedalLocomotion::System::Advanceable<UnicycleTrajectoryGeneratorInput, UnicycleTrajectoryGeneratorOutput>
Basic class that represents a discrete system.

Constructors, destructors, conversion operators

UnicycleTrajectoryGenerator()
~UnicycleTrajectoryGenerator() virtual

Public functions

auto setRobotContactFrames(const iDynTree::Model& model) -> bool
Set the robot contact frames.
auto initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) -> bool override
auto getOutput() const -> const UnicycleTrajectoryGeneratorOutput& override
Get the output of the planner.
auto isOutputValid() const -> bool override
Check if the output is valid.
auto setInput(const UnicycleTrajectoryGeneratorInput& input) -> bool override
Set the input of the planner.
auto advance() -> bool override
Advance the planner.
auto getSamplingTime() const -> std::chrono::nanoseconds
Get the sampling time of the planner.

Function documentation

bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::setRobotContactFrames(const iDynTree::Model& model)

Set the robot contact frames.

Parameters
model iDynTree::Model of the robot considered.

It should be called after the initialize() function. It checks if the contact frames names parsed by the initialize() function exist. If yes, it sets the related contact frames indexes and returns true. Otherwise, it sets the Impl::FSM::State back to NotInitialized and returns false.

bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override

   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                  |

   Implicitely, the class needs also all the parameters required by the Bipedalocotion::Planner::UnicyclePlanner class.

clang-format on

   @param handler Pointer to the parameter handler.
   @return True in case of success, false otherwise.

const UnicycleTrajectoryGeneratorOutput& BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::getOutput() const override

Get the output of the planner.

Returns Output of the planner.

bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::isOutputValid() const override

Check if the output is valid.

Returns True if the output is valid, false otherwise.

bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::setInput(const UnicycleTrajectoryGeneratorInput& input) override

Set the input of the planner.

Parameters
input Input of the planner.
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::advance() override

Advance the planner.

Returns True in case of success, false otherwise.

std::chrono::nanoseconds BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::getSamplingTime() const

Get the sampling time of the planner.

Returns Sampling time of the planner.