Add new Advanceable exposing `UnicyclePlanner` #320

diegoferigo · 2021-06-01T16:27:01Z

This PR is the first step towards including in BLF the unicycle planner currently stored in robotology/unicycle-footstep-planner.

In order to simplify the review process, I split the porting in two steps:

This PR calls the existing UnicyclePlanner from the external repository and defines a new Advanceable with its own parameter handler, input, state, and Python bindings (and tests).
Once the first step (this PR) is finalized, I can start vendoring the external UnicyclePlanner files and perform a small refactoring. I try to minimize the changes to parameters, inputs, and outputs wrt to this PR.

Open questions:

The ::FootPrint objects are converted to BipedalLocomotion::Contacts::PlannedContacts. However, it's not clear to me how to handle the duration of the contact.
I didn't get what UnicyclePlanner::setDesiredPersonDistance does.

I based the porting on the usage implemented in the following MWE:

MWE

#include "BipedalLocomotion/Contacts/ContactList.h"

#include <Eigen/Dense>
#include <FootPrint.h>
#include <UnicyclePlanner.h>

#include <iomanip>
#include <iostream>
#include <math.h>
#include <memory>

int main()
{
    const auto deg2rad = [](const double deg) { return deg / 180.0 * M_PI; };
    const auto rad2deg = [](const double rad) { return rad * 180.0 / M_PI; };

    // ===================
    // Initialize Unicycle
    // ===================

    auto unicycle = std::make_shared<UnicyclePlanner>();

    const double dT = 0.010;
    const double dX = 0.2;
    const double dY = 0.0;

    const double timeWeight = 2.5;
    const double positionWeight = 1.0;
    const double unicycleGain = 10.0;
    const double slowWhenTurningGain = 0.0; // 0.5

    const double minStepLength = 0.05;
    const double maxStepLength = 0.30;

    const double minWidth = 0.030;
    const double nominalWidth = 0.040;

    const double minAngleVariation = deg2rad(5.0);
    const double maxAngleVariation = deg2rad(40.0);

    const double minStepDuration = 2.9;
    const double maxStepDuration = 8.0;
    const double nominalDuration = 4.0;

    const bool swingLeft = true;
    const bool startWithSameFoot = false;

    bool ok = true;
    ok = ok && unicycle->setDesiredPersonDistance(dX, dY);
    ok = ok && unicycle->setControllerGain(unicycleGain);
    ok = ok && unicycle->setMaximumIntegratorStepSize(dT);
    ok = ok && unicycle->setMaxStepLength(maxStepLength);
    ok = ok && unicycle->setWidthSetting(minWidth, nominalWidth);
    ok = ok && unicycle->setMaxAngleVariation(maxAngleVariation);
    ok = ok && unicycle->setCostWeights(positionWeight, timeWeight);
    ok = ok && unicycle->setStepTimings(minStepDuration, maxStepDuration, nominalDuration);
    ok = ok && unicycle->setPlannerPeriod(dT);
    ok = ok && unicycle->setMinimumAngleForNewSteps(minAngleVariation);
    ok = ok && unicycle->setMinimumStepLength(minStepLength);
    ok = ok && unicycle->setSlowWhenTurnGain(slowWhenTurningGain);

    unicycle->addTerminalStep(true);
    unicycle->startWithLeft(swingLeft);
    unicycle->resetStartingFootIfStill(startWithSameFoot);

    if (!ok)
    {
        std::cerr << "Failed to configure the unicycle" << std::endl;
        return 1;
    }

    // =================
    // Create trajectory
    // =================

    // xy trajectory
    const double t0 = 0.0;
    const double tf = 1.0;
    const auto t = Eigen::VectorXd::LinSpaced((tf - t0) / dT + 1, t0, tf);

    const auto x = 0.1 * t;
    const auto y = 0.5 * (0.1 * t).array().sin();

    const auto dx = 0.1 * Eigen::VectorXd::Ones(t.size());
    const auto dy = 0.5 * 0.1 * (0.1 * t).array().cos();

    for (long i = 0; i < t.size(); ++i)
    {
        auto p = iDynTree::Vector2();
        p[0] = x[i];
        p[1] = y[i];

        auto dp = iDynTree::Vector2();
        dp[0] = dx[i];
        dp[1] = dy[i];

        if (!unicycle->addDesiredTrajectoryPoint(t[i], p, dp))
        {
            std::cerr << "Failed to add trajectory" << std::endl;
            return 1;
        }
    }

    // =================
    // Compute the steps
    // =================

    auto left = std::make_shared<FootPrint>();
    left->setFootName("left");

    auto right = std::make_shared<FootPrint>();
    right->setFootName("right");

    if (!unicycle->computeNewSteps(left, right, t0, tf))
    {
        std::cerr << "Failed to compute steps" << std::endl;
        return 1;
    }

    // ===========================
    // Extract and print the steps
    // ===========================

    for (const auto& footprint : {left, right})
    {
        const StepList& steps = footprint->getSteps();
        std::cout << "==> Foot '" << footprint->getFootName() << "' has "
                  << footprint->numberOfSteps() << " steps." << std::endl
                  << std::endl;

        for (const auto& step : steps)
        {
            std::cout << std::setw(6) << std::defaultfloat //
                      << "t=" << step.impactTime << ":" //
                      << std::setprecision(6) << std::fixed //
                      << "  x=" << step.position[0] //
                      << "  y=" << step.position[1] //
                      << "  theta=" << rad2deg(step.angle) << std::endl;
            std::cout << std::defaultfloat;
        }
        std::cout << std::endl;
    }

    // ==> Foot 'left' has 2 steps.
    //
    //     t=0:  x=-0.200000  y=0.020000  theta=0.000000
    //     t=5:  x=-0.111518  y=0.027473  theta=9.655172
    //
    // ==> Foot 'right' has 2 steps.
    //
    //     t=0:  x=-0.200000  y=-0.020000  theta=0.000000
    //     t=9:  x=-0.104810  y=-0.011960  theta=9.655172

    return 0;
}

cc @S-Dafarra @GiulioRomualdi @traversaro

Update: the Python script to test the final version of this PR is included in #320 (comment).

src/Planners/src/UnicyclePlanner.cpp

S-Dafarra · 2021-06-01T16:38:31Z

The ::FootPrint objects are converted to BipedalLocomotion::Contacts::PlannedContacts. However, it's not clear to me how to handle the duration of the contact.

This is something I did not realize at first. The contacts planned by the unicycle only define the impact time, i.e. the instant in which the foot touches the ground. For computing when the foot lifts from the ground, you need to define the ratio between the single and double support in a step. This logic is included in the UnicycleGenerator class: https://github.com/robotology/unicycle-footstep-planner/blob/master/include/UnicycleGenerator.h#L52-L79. This class uses inside the UnicyclePlanner, but in principle it can also work with an external list of FootPrints: https://github.com/robotology/unicycle-footstep-planner/blob/master/include/UnicycleGenerator.h#L36. After setting these external lists, you can retrieve the phases for each time instant using the methods mentioned in the first link.

I didn't get what UnicyclePlanner::setDesiredPersonDistance does.

This is an internal detail of the UnicycleController. Check Section II.B of https://arxiv.org/pdf/1807.05395.pdf

diegoferigo · 2021-06-01T16:50:04Z

The ::FootPrint objects are converted to BipedalLocomotion::Contacts::PlannedContacts. However, it's not clear to me how to handle the duration of the contact.

This is something I did not realize at first. The contacts planned by the unicycle only define the impact time, i.e. the instant in which the foot touches the ground. For computing when the foot lifts from the ground, you need to define the ratio between the single and double support in a step. This logic is included in the UnicycleGenerator class: https://github.com/robotology/unicycle-footstep-planner/blob/master/include/UnicycleGenerator.h#L52-L79. This class uses inside the UnicyclePlanner, but in principle it can also work with an external list of FootPrints: https://github.com/robotology/unicycle-footstep-planner/blob/master/include/UnicycleGenerator.h#L36. After setting these external lists, you can retrieve the phases for each time instant using the methods mentioned in the first link.

Ok, thanks for the pointer! I'll check how to use these methods to finalize the contact conversion.

I didn't get what UnicyclePlanner::setDesiredPersonDistance does.

This is an internal detail of the UnicycleController. Check Section II.B of https://arxiv.org/pdf/1807.05395.pdf

I quickly skimmed the paper but I couldn't find many details. Is this a mandatory feature or we can ignore it for the moment?

S-Dafarra · 2021-06-01T17:06:17Z

I quickly skimmed the paper but I couldn't find many details. Is this a mandatory feature or we can ignore it for the moment?

It is the position of the point F. This point is the one for which we provide a reference. This point has to be away from the wheel axis. In the walking we set it 10 cm forward in the x direction with respect to the center of the feet, and we never touched it (https://github.com/robotology/walking-controllers/blob/master/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/plannerParams.ini#L6). This has a role only when the size of the robot changes consistently (if the robot is 50cm tall, those 10cm make a difference in the walking "style").

diegoferigo · 2021-06-08T09:35:14Z

I quickly skimmed the paper but I couldn't find many details. Is this a mandatory feature or we can ignore it for the moment?

It is the position of the point F. This point is the one for which we provide a reference. This point has to be away from the wheel axis. In the walking we set it 10 cm forward in the x direction with respect to the center of the feet, and we never touched it (https://github.com/robotology/walking-controllers/blob/master/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/plannerParams.ini#L6). This has a role only when the size of the robot changes consistently (if the robot is 50cm tall, those 10cm make a difference in the walking "style").

Thanks for the clarification! It was not straightforward to link the point F to the UnicyclePlanner::setDesiredPersonDistance method. I'm currently having a look to the interpolators in order to understand how to extract the stance duration.

Btw, I wanted to visualize the current implementation to check that everything is in order, and it seems ok:

Code

from typing import Tuple

import bipedal_locomotion_framework.bindings as blf
import matplotlib.patches as patches
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as R


def get_unicycle_planner() -> blf.planners.UnicyclePlanner:

    parameters_handler = blf.parameters_handler.StdParametersHandler()

    # dt
    parameters_handler.set_parameter_float("sampling_time_planner", 0.010)
    parameters_handler.set_parameter_float("sampling_time_integrator", 0.010)

    # gains
    parameters_handler.set_parameter_float("gain_unicycle", 10.0)
    parameters_handler.set_parameter_float("gain_turning_speed", 5.0)

    # weights
    parameters_handler.set_parameter_float("weight_time", 2.5)
    parameters_handler.set_parameter_float("weight_position", 1.0)

    # duration
    parameters_handler.set_parameter_float("duration_min", 0.8)
    parameters_handler.set_parameter_float("duration_max", 1.0)
    parameters_handler.set_parameter_float("duration_nominal", 0.9)

    # step length
    parameters_handler.set_parameter_float("step_length_min", 0.05)
    parameters_handler.set_parameter_float("step_length_max", 0.30)

    # feet distance
    parameters_handler.set_parameter_float("feet_distance_min", 0.14)
    parameters_handler.set_parameter_float("feet_distance_nominal", 0.16)

    # angle variation
    parameters_handler.set_parameter_float("foot_angle_variation_min", np.deg2rad(8.0))
    parameters_handler.set_parameter_float("foot_angle_variation_max", np.deg2rad(15.0))

    # Create the planner
    unicycle = blf.planners.UnicyclePlanner()

    if not unicycle.initialize(handler=parameters_handler):
        raise RuntimeError

    return unicycle


class FootStep:
    def __init__(
        self,
        xy: Tuple[float, float],
        x_size: float,
        y_size: float,
        color: str,
        angle: float = 0.0,
    ):

        self.xy = xy
        self.angle = angle
        self.color = color
        self.x_size = x_size
        self.y_size = y_size

    def patch(self) -> patches.Rectangle:

        anchor = (self.xy[0] - self.x_size / 2, self.xy[1] - self.y_size / 2)

        return patches.Rectangle(
            xy=anchor,
            width=self.x_size,
            height=self.y_size,
            facecolor=self.color,
            edgecolor="k",
            angle=np.rad2deg(self.angle),
        )


def main():

    # Create the unicycle planner
    unicycle = get_unicycle_planner()

    # Create the input
    unicycle_input = blf.planners.UnicyclePlannerInput(
        t0=0.0,
        tf=20.0,
        knots=[
            blf.planners.UnicycleKnot(
                position=(0.1, 0.0), velocity=(0.0, 0.0), time=0.0
            ),
            blf.planners.UnicycleKnot(
                position=(0.5, 0.0), velocity=(0.1, 0.0), time=7.0
            ),
            blf.planners.UnicycleKnot(
                position=(1.0, 0.25), velocity=(0.1, 0.1), time=13.0
            ),
            blf.planners.UnicycleKnot(
                position=(1.1, 0.25), velocity=(0.0, 0.0), time=15.0
            ),
        ],
    )

    # Set the input
    assert unicycle.set_input(input=unicycle_input)

    # Compute the steps
    assert unicycle.advance()

    # Get the output
    unicycle_output = unicycle.get_output()

    contact_patches = []

    for contact in unicycle_output.left:

        contact_patches += [
            FootStep(
                xy=contact.pose.translation(),
                x_size=0.1,
                y_size=0.05,
                color="r",
                angle=R.from_quat(contact.pose.quat()).as_euler(seq="zxy")[0],
            ).patch()
        ]

    for contact in unicycle_output.right:

        contact_patches += [
            FootStep(
                xy=contact.pose.translation(),
                x_size=0.1,
                y_size=0.05,
                color="g",
                angle=R.from_quat(contact.pose.quat()).as_euler(seq="zxy")[0],
            ).patch()
        ]

    fig, ax = plt.subplots()
    ax.set_title("Planned Footsteps")
    ax.set_xlabel(r"$x$")
    ax.set_ylabel(r"$y$")
    ax.autoscale(True)
    ax.set_aspect("equal", "box")
    ax.grid(True, linestyle="--")
    ax.set_axisbelow(1.0)

    _ = [ax.add_patch(p) for p in contact_patches]

    plt.show()


main()

diegoferigo · 2021-06-08T11:22:35Z

It seems to me that the logic to compute the step timing is here. It is not 100% crystal clear, but in the most simple case it seems that:

Step duration is computed from impact times
The step duration is split in stance / using a fixed percentage (configurable)

This does not guarantee that there is always a long enough double stance phase, am I right @S-Dafarra?

S-Dafarra · 2021-06-08T12:22:01Z

Step duration is computed from impact times

Yes. The distance between two consecutive impact times for a given foot contains first a stance phase and then a swing phase.

2. The step duration is split in stance / using a fixed percentage (configurable)

Exactly. This determines the ratio between the stance and the swing phase between two impact times.

This does not guarantee that there is always a long enough double stance phase, am I right @S-Dafarra?

Not directly, no. There is a minimum time between two impact times, and the ratio cannot be such that there is no stance time, but there is not a parameter to directly control the length of each phase.

In any case, the logic is a bit more involved to consider the first step and pause conditions. The latter are when the robot simply stops because the references are moving too slow.

diegoferigo · 2021-06-08T13:47:38Z

Step duration is computed from impact times

Yes. The distance between two consecutive impact times for a given foot contains first a stance phase and then a swing phase.

The step duration is split in stance / using a fixed percentage (configurable)

Exactly. This determines the ratio between the stance and the swing phase between two impact times.

Perfect, this means that running the interpolator is not necessary then. This logic can be easily re-implemented in the current PR when the contact types conversion is performed.

This does not guarantee that there is always a long enough double stance phase, am I right @S-Dafarra?

Not directly, no. There is a minimum time between two impact times, and the ratio cannot be such that there is no stance time, but there is not a parameter to directly control the length of each phase.

In any case, the logic is a bit more involved to consider the first step and pause conditions. The latter are when the robot simply stops because the references are moving too slow.

Yes, there are some cases covered that at first seem advance usage. I'll proceed with a simple implementation, we can iterate on that in the future, if needed.

S-Dafarra · 2021-06-08T16:21:29Z

Step duration is computed from impact times

Yes. The distance between two consecutive impact times for a given foot contains first a stance phase and then a swing phase.

The step duration is split in stance / using a fixed percentage (configurable)

Exactly. This determines the ratio between the stance and the swing phase between two impact times.

Perfect, this means that running the interpolator is not necessary then. This logic can be easily re-implemented in the current PR when the contact types conversion is performed.

This does not guarantee that there is always a long enough double stance phase, am I right @S-Dafarra?

Not directly, no. There is a minimum time between two impact times, and the ratio cannot be such that there is no stance time, but there is not a parameter to directly control the length of each phase.
In any case, the logic is a bit more involved to consider the first step and pause conditions. The latter are when the robot simply stops because the references are moving too slow.

Yes, there are some cases covered that at first seem advance usage. I'll proceed with a simple implementation, we can iterate on that in the future if needed.

As quickly mentioned to the meeting, I would avoid duplicating the logic and exploit the one in the UnicycleGenerator class eventually. I agree in any case that most of the logic boils down to a simple ratio between the stance and swing phase.

So for a first iteration, it makes sense to use something simple, and eventually change later on.

Also, from the UnicyclePlanner side, there could be easier ways to retrieve the duration of each phase.

diegoferigo · 2021-06-10T11:18:49Z

Yes, there are some cases covered that at first seem advance usage. I'll proceed with a simple implementation, we can iterate on that in the future if needed.

As quickly mentioned to the meeting, I would avoid duplicating the logic and exploit the one in the UnicycleGenerator class eventually. I agree in any case that most of the logic boils down to a simple ratio between the stance and swing phase.

So for a first iteration, it makes sense to use something simple, and eventually change later on.

Also, from the UnicyclePlanner side, there could be easier ways to retrieve the duration of each phase.

I guess the PR now is ready for review. Currently there's a minor overlap in logic that is just a couple of lines long:

https://github.com/diegoferigo/bipedal-locomotion-framework/blob/a8e0fb76b5bbafa026450a8c5f58c4752fe29825/src/Planners/src/UnicyclePlanner.cpp#L353-L359

I guess we can get a better behaviour in the future when the unicycle gets vendored in blf.

diegoferigo · 2021-06-10T11:23:36Z

bindings/python/Planners/tests/test_unicycle_planner.py

+    for contact in unicycle_output.left:
+        print(contact)
+
+    print()
+
+    for contact in unicycle_output.right:
+        print(contact)


maybe I can comment these out

diegoferigo · 2021-06-10T11:32:47Z

Windows CI fails with:

D:\a\bipedal-locomotion-framework\bipedal-locomotion-framework\src\Planners\src\UnicyclePlanner.cpp(366,57): error C2039: 'optional': is not a member of 'std' [D:\a\bipedal-locomotion-framework\bipedal-locomotion-framework\build\src\Planners\Planners.vcxproj]

Is C++17 enabled on a per-target base on Windows?

cmake/BipedalLocomotionFrameworkDependencies.cmake

src/Planners/CMakeLists.txt

src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h

src/Planners/src/UnicyclePlanner.cpp

src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h

src/Planners/src/UnicyclePlanner.cpp

diegoferigo · 2021-06-11T09:58:02Z

Rebased on top of master in order to solve conflict

src/Planners/src/UnicyclePlanner.cpp

S-Dafarra · 2021-09-09T10:20:36Z

Edit: unfortunately the integration with the DCM planner is not working correctly since CasADI does not seem to like the infinite deactivation time of the last contacts.

Can you print the activation/deactivation times? I suspect it may be because it does not end with a double support

diegoferigo · 2021-09-10T09:30:09Z

Edit: unfortunately the integration with the DCM planner is not working correctly since CasADI does not seem to like the infinite deactivation time of the last contacts.

Can you print the activation/deactivation times? I suspect it may be because it does not end with a double support

Here below you can find the timings generated by the first planning of the script posted in #320 (comment):

=== LEFT ===
Contact (name = left, pose = SE3 (position = [0, 0.08, 0], quaternion = [0, 0, 0, 1]), activation_time = 0, deactivation_time = 2.81, type = 0)
Contact (name = left, pose = SE3 (position = [0.231943, 0.08, 0], quaternion = [0, 0, 0, 1]), activation_time = 4.06, deactivation_time = 6.81, type = 0)
Contact (name = left, pose = SE3 (position = [0.475742, 0.0885835, 0], quaternion = [0, 0, 0.117272, 0.9931]), activation_time = 8.06, deactivation_time = 10.81, type = 0)
Contact (name = left, pose = SE3 (position = [0.797768, 0.240102, 0], quaternion = [0, 0, 0.236206, 0.971703]), activation_time = 12.06, deactivation_time = 14.31, type = 0)
Contact (name = left, pose = SE3 (position = [0.984975, 0.315186, 0], quaternion = [0, 0, 0.0725488, 0.997365]), activation_time = 15, deactivation_time = 3.402823e+38, type = 0)

=== RIGHT ===
Contact (name = right, pose = SE3 (position = [0, -0.08, 0], quaternion = [0, 0, 0, 1]), activation_time = 0, deactivation_time = 1.5, type = 0)
Contact (name = right, pose = SE3 (position = [0.1148, -0.08, 0], quaternion = [0, 0, 0, 1]), activation_time = 2.06, deactivation_time = 4.81, type = 0)
Contact (name = right, pose = SE3 (position = [0.349086, -0.08, 0], quaternion = [0, 0, 0, 1]), activation_time = 6.06, deactivation_time = 8.81, type = 0)
Contact (name = right, pose = SE3 (position = [0.702869, 0.0120522, 0], quaternion = [0, 0, 0.229199, 0.97338]), activation_time = 10.06, deactivation_time = 12.75, type = 0)
Contact (name = right, pose = SE3 (position = [0.974549, 0.149471, 0], quaternion = [0, 0, 0.153582, 0.988136]), activation_time = 13.89, deactivation_time = 3.402823e+38, type = 0)

S-Dafarra · 2021-09-10T09:37:08Z

Edit: unfortunately the integration with the DCM planner is not working correctly since CasADI does not seem to like the infinite deactivation time of the last contacts.

Can you print the activation/deactivation times? I suspect it may be because it does not end with a double support

Here below you can find the timings generated by the first planning of the script posted in #320 (comment):

=== LEFT ===
Contact (name = left, pose = SE3 (position = [0, 0.08, 0], quaternion = [0, 0, 0, 1]), activation_time = 0, deactivation_time = 2.81, type = 0)
Contact (name = left, pose = SE3 (position = [0.231943, 0.08, 0], quaternion = [0, 0, 0, 1]), activation_time = 4.06, deactivation_time = 6.81, type = 0)
Contact (name = left, pose = SE3 (position = [0.475742, 0.0885835, 0], quaternion = [0, 0, 0.117272, 0.9931]), activation_time = 8.06, deactivation_time = 10.81, type = 0)
Contact (name = left, pose = SE3 (position = [0.797768, 0.240102, 0], quaternion = [0, 0, 0.236206, 0.971703]), activation_time = 12.06, deactivation_time = 14.31, type = 0)
Contact (name = left, pose = SE3 (position = [0.984975, 0.315186, 0], quaternion = [0, 0, 0.0725488, 0.997365]), activation_time = 15, deactivation_time = 3.402823e+38, type = 0)

=== RIGHT ===
Contact (name = right, pose = SE3 (position = [0, -0.08, 0], quaternion = [0, 0, 0, 1]), activation_time = 0, deactivation_time = 1.5, type = 0)
Contact (name = right, pose = SE3 (position = [0.1148, -0.08, 0], quaternion = [0, 0, 0, 1]), activation_time = 2.06, deactivation_time = 4.81, type = 0)
Contact (name = right, pose = SE3 (position = [0.349086, -0.08, 0], quaternion = [0, 0, 0, 1]), activation_time = 6.06, deactivation_time = 8.81, type = 0)
Contact (name = right, pose = SE3 (position = [0.702869, 0.0120522, 0], quaternion = [0, 0, 0.229199, 0.97338]), activation_time = 10.06, deactivation_time = 12.75, type = 0)
Contact (name = right, pose = SE3 (position = [0.974549, 0.149471, 0], quaternion = [0, 0, 0.153582, 0.988136]), activation_time = 13.89, deactivation_time = 3.402823e+38, type = 0)

They look good to me. For me we can tackle the DCM planner problem in a separate issue then! @GiulioRomualdi what do you think?

diegoferigo · 2021-09-10T09:41:04Z

As an extra comment, beyond the current problem of the DCM planner that does not like infinite contact deactivation, also appending new contacts to a pre-existing list in the case the last one has infinite deactivation will not work. This is the case of the last Python script posted above, but this case is more a visualization / storage caveat, not really a problem / bug.

S-Dafarra · 2021-09-10T09:48:24Z

As an extra comment, beyond the current problem of the DCM planner that does not like infinite contact deactivation, also appending new contacts to a pre-existing list in the case the last one has infinite deactivation will not work. This is the case of the last Python script posted above, but this case is more a visualization / storage caveat, not really a problem / bug.

You would need to edit the last planned steps. If you think about this makes sense. You don't know when the last steps are going to be deactivated, until you know better, i.e. you have some new steps to add 🤔

src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h

diegoferigo · 2021-09-10T10:40:16Z

As an extra comment, beyond the current problem of the DCM planner that does not like infinite contact deactivation, also appending new contacts to a pre-existing list in the case the last one has infinite deactivation will not work. This is the case of the last Python script posted above, but this case is more a visualization / storage caveat, not really a problem / bug.

You would need to edit the last planned steps. If you think about this makes sense. You don't know when the last steps are going to be deactivated, until you know better, i.e. you have some new steps to add thinking

Yes, this is what I did in the Python example above. If you think this need is ok, this PR is finally ready. As I mentioned, there might be some edge cases I didn't cover in my simple examples, but we can fix them as soon as this class will start being integrated in more complex pipelines.

GiulioRomualdi · 2021-09-21T14:56:47Z

I'm on it :) I check the PR

GiulioRomualdi

As written here https://github.com/ami-iit/bipedal-locomotion-framework/pull/320/files#r716716753 the only thing I'm scared is

        contacts.back().deactivationTime = std::numeric_limits<float>().max();

We should understand if the planner (DCM) is compatible with this choice.
Apart from that, the PR is good. Thank you @diegoferigo for the effort

diegoferigo force-pushed the feature/unicycle-planner branch from ce00957 to 27defa4 Compare June 1, 2021 16:31

diegoferigo commented Jun 1, 2021

View reviewed changes

src/Planners/src/UnicyclePlanner.cpp Outdated Show resolved Hide resolved

diegoferigo commented Jun 1, 2021

View reviewed changes

src/Planners/src/UnicyclePlanner.cpp Outdated Show resolved Hide resolved

diegoferigo self-assigned this Jun 8, 2021

diegoferigo force-pushed the feature/unicycle-planner branch 3 times, most recently from 679166e to a8e0fb7 Compare June 10, 2021 11:15

diegoferigo requested review from GiulioRomualdi and S-Dafarra June 10, 2021 11:15

diegoferigo commented Jun 10, 2021

View reviewed changes

diegoferigo force-pushed the feature/unicycle-planner branch 4 times, most recently from 4832f91 to 0ceaa75 Compare June 10, 2021 12:25

S-Dafarra reviewed Jun 10, 2021

View reviewed changes

cmake/BipedalLocomotionFrameworkDependencies.cmake Outdated Show resolved Hide resolved

src/Planners/CMakeLists.txt Outdated Show resolved Hide resolved

S-Dafarra requested changes Jun 10, 2021

View reviewed changes

diegoferigo force-pushed the feature/unicycle-planner branch 2 times, most recently from 4839913 to 4e90f6d Compare June 11, 2021 09:57

S-Dafarra reviewed Jun 11, 2021

View reviewed changes

src/Planners/src/UnicyclePlanner.cpp Show resolved Hide resolved

src/Planners/src/UnicyclePlanner.cpp Outdated Show resolved Hide resolved

S-Dafarra reviewed Sep 9, 2021

View reviewed changes

src/Planners/src/UnicyclePlanner.cpp Outdated Show resolved Hide resolved

src/Planners/src/UnicyclePlanner.cpp Show resolved Hide resolved

S-Dafarra reviewed Sep 10, 2021

View reviewed changes

src/Planners/include/BipedalLocomotion/Planners/UnicyclePlanner.h Show resolved Hide resolved

diegoferigo force-pushed the feature/unicycle-planner branch from a8fad53 to f0209f2 Compare September 10, 2021 09:58

diegoferigo added 14 commits September 10, 2021 12:02

Update CMakeLists.txt style

1835d07

Add UnicyclePlanner class

38b39ea

Update CMakeLists.txt style

56d9231

Add UnicyclePlanner Python bindings

e1ea778

Add UnicyclePlanner Python test

50762b2

Add equality operator to Python's ContactList

023ebaf

Install unicycle-footstep-planner in CI

ee4cf78

Print Contact in scientific notation, if needed

3f5cbc0

Add documentation

7e1a3a3

Fix warning on non-virtual Advanceable destructor

e9c785a

Update CHANGELOG.md

9e52d7a

Allow passing initial contacts state

6588116

Fix cleaning up when output is not valid

09233ad

Update gitignore

6285b8c

diegoferigo force-pushed the feature/unicycle-planner branch from f0209f2 to 6285b8c Compare September 10, 2021 10:02

S-Dafarra approved these changes Sep 10, 2021

View reviewed changes

Add __repr__ methods to pretty print objects in Python

331c919

GiulioRomualdi approved these changes Sep 27, 2021

View reviewed changes

GiulioRomualdi merged commit 9fdaf21 into ami-iit:master Sep 27, 2021

S-Dafarra mentioned this pull request Nov 3, 2021

Add UnicyclePlanner to the list of dependency to disable in CI to check if build works fine anyhow #435

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add new Advanceable exposing `UnicyclePlanner` #320

Add new Advanceable exposing `UnicyclePlanner` #320

diegoferigo commented Jun 1, 2021 •

edited

Loading

S-Dafarra commented Jun 1, 2021

diegoferigo commented Jun 1, 2021

S-Dafarra commented Jun 1, 2021

diegoferigo commented Jun 8, 2021

diegoferigo commented Jun 8, 2021

S-Dafarra commented Jun 8, 2021

diegoferigo commented Jun 8, 2021

S-Dafarra commented Jun 8, 2021

diegoferigo commented Jun 10, 2021

diegoferigo Jun 10, 2021

diegoferigo commented Jun 10, 2021 •

edited

Loading

diegoferigo commented Jun 11, 2021

S-Dafarra commented Sep 9, 2021

diegoferigo commented Sep 10, 2021

S-Dafarra commented Sep 10, 2021

diegoferigo commented Sep 10, 2021

S-Dafarra commented Sep 10, 2021

diegoferigo commented Sep 10, 2021

GiulioRomualdi commented Sep 21, 2021

GiulioRomualdi left a comment

Add new Advanceable exposing UnicyclePlanner #320

Add new Advanceable exposing UnicyclePlanner #320

Conversation

diegoferigo commented Jun 1, 2021 • edited Loading

S-Dafarra commented Jun 1, 2021

diegoferigo commented Jun 1, 2021

S-Dafarra commented Jun 1, 2021

diegoferigo commented Jun 8, 2021

diegoferigo commented Jun 8, 2021

S-Dafarra commented Jun 8, 2021

diegoferigo commented Jun 8, 2021

S-Dafarra commented Jun 8, 2021

diegoferigo commented Jun 10, 2021

diegoferigo Jun 10, 2021

Choose a reason for hiding this comment

diegoferigo commented Jun 10, 2021 • edited Loading

diegoferigo commented Jun 11, 2021

S-Dafarra commented Sep 9, 2021

diegoferigo commented Sep 10, 2021

S-Dafarra commented Sep 10, 2021

diegoferigo commented Sep 10, 2021

S-Dafarra commented Sep 10, 2021

diegoferigo commented Sep 10, 2021

GiulioRomualdi commented Sep 21, 2021

GiulioRomualdi left a comment

Choose a reason for hiding this comment

Add new Advanceable exposing `UnicyclePlanner` #320

Add new Advanceable exposing `UnicyclePlanner` #320

diegoferigo commented Jun 1, 2021 •

edited

Loading

diegoferigo commented Jun 10, 2021 •

edited

Loading