class final
UnicycleTrajectoryPlannerUnicycleTrajectoryPlanner is a class that uses UnicycleGenerator of https:/
Every time the advance() function is called, the planner generates a new trajectory, which spans the time horizon configured by the user. The getOutput() member function returns the generated trajectory, which includes the CoM, DCM, and footstep ones. The planner requires the user to set the robot model using the setRobotModel() member function, before invoking the initialize() member function, which configures the planner. As input, which is set using the setInput() member function, the planner requires an instance of the UnicycleTrajectoryPlannerInput struct.
Base classes
-
template<class _Input, class _Output>class BipedalLocomotion::System::Advanceable<UnicycleTrajectoryPlannerInput, UnicycleTrajectoryPlannerOutput>
- Basic class that represents a discrete system.
Constructors, destructors, conversion operators
- UnicycleTrajectoryPlanner()
- ~UnicycleTrajectoryPlanner() virtual
Public functions
- auto setRobotContactFrames(const iDynTree::Model& model) -> bool
- Set the robot contact frames.
- auto getRightContactFrameIndex() const -> int
- Get the index of the right foot contact frame.
- auto getLeftContactFrameIndex() const -> int
- Get the index of the left foot contact frame.
-
auto initialize(std::weak_ptr<const ParametersHandler::
IParametersHandler> handler) -> bool override - Initialize the planner.
- auto getOutput() const -> const UnicycleTrajectoryPlannerOutput& override
- Get the output of the planner.
- auto isOutputValid() const -> bool override
- Check if the output is valid.
- auto setInput(const UnicycleTrajectoryPlannerInput& input) -> bool override
- Set the input of the planner.
- auto advance() -> bool override
- Advance the planner.
-
auto getContactPhaseList() -> Contacts::
ContactPhaseList - Get the contact phase list.
- auto getMinStepDuration() const -> std::chrono::nanoseconds
- Get the minimum time duration of a step.
Function documentation
bool BipedalLocomotion:: Planners:: UnicycleTrajectoryPlanner:: 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.
int BipedalLocomotion:: Planners:: UnicycleTrajectoryPlanner:: getRightContactFrameIndex() const
Get the index of the right foot contact frame.
Returns | The index of the right foot contact frame. |
---|
int BipedalLocomotion:: Planners:: UnicycleTrajectoryPlanner:: getLeftContactFrameIndex() const
Get the index of the left foot contact frame.
Returns | The index of the left foot contact frame. |
---|
bool BipedalLocomotion:: Planners:: UnicycleTrajectoryPlanner:: initialize(std::weak_ptr<const ParametersHandler:: IParametersHandler> handler) override
Initialize the planner.
Parameters | |
---|---|
handler | Pointer to the parameter handler. |
Returns | True in case of success, false otherwise. |
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 |
const UnicycleTrajectoryPlannerOutput& BipedalLocomotion:: Planners:: UnicycleTrajectoryPlanner:: getOutput() const override
Get the output of the planner.
Returns | The output of the planner. |
---|
bool BipedalLocomotion:: Planners:: UnicycleTrajectoryPlanner:: isOutputValid() const override
Check if the output is valid.
Returns | True if the output is valid, false otherwise. |
---|
bool BipedalLocomotion:: Planners:: UnicycleTrajectoryPlanner:: setInput(const UnicycleTrajectoryPlannerInput& 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:: UnicycleTrajectoryPlanner:: advance() override
Advance the planner.
Returns | True in case of success, false otherwise. |
---|
Contacts:: ContactPhaseList BipedalLocomotion:: Planners:: UnicycleTrajectoryPlanner:: getContactPhaseList()
Get the contact phase list.
Returns | The contact phase list. |
---|
It returns the contact phase list built with the footsteps generated by the planner.
std::chrono::nanoseconds BipedalLocomotion:: Planners:: UnicycleTrajectoryPlanner:: getMinStepDuration() const
Get the minimum time duration of a step.
Returns | minimum time duration of a step. |
---|