min_time_via_points_cost.cpp

Go to the documentation of this file.

/*********************************************************************
 *
 *  Software License Agreement
 *
 *  Copyright (c) 2020, Christoph Rösmann, All rights reserved.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <https://www.gnu.org/licenses/>.
 *
 *  Authors: Christoph Rösmann
 *********************************************************************/

#include <mpc_local_planner/optimal_control/min_time_via_points_cost.h>

#include <mpc_local_planner/optimal_control/full_discretization_grid_base_se2.h>
#include <mpc_local_planner/utils/math_utils.h>

#include <cmath>
#include <memory>

namespace mpc_local_planner {

int MinTimeViaPointsCost::getNonIntegralStateTermDimension(int k) const
{
    assert(k < _vp_association.size());
    assert(_vp_association[k].first.size() == _vp_association[k].second);
    return (int)_vp_association[k].second;
}

bool MinTimeViaPointsCost::update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/,
                                  corbo::ReferenceTrajectoryInterface* /*sref*/, bool single_dt, const Eigen::VectorXd& x0,
                                  corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector<double>& /*dts*/,
                                  const corbo::DiscretizationGridInterface* grid)
{
    if (!_via_points)
    {
        PRINT_WARNING("MinTimeViaPointsCost::update(): no via_point container specified");
        return !_vp_association.empty();
    }

    // setup minimum time cost
    _single_dt = single_dt;
    if (single_dt)
        _time_weight = n - 1;
    else
        _time_weight = 1.0;

    // Setup via points

    // we currently require a full discretization grid as we want to have fast access to
    // individual states without requiring any new simulation.
    // Alternatively, other grids could be used in combination with method getStateAndControlTimeSeries()
    const FullDiscretizationGridBaseSE2* fd_grid = static_cast<const FullDiscretizationGridBaseSE2*>(grid);

    assert(n == fd_grid->getN());

    bool new_structure = (n != _vp_association.size());
    if (new_structure)
    {
        _vp_association.resize(n, std::make_pair<std::vector<const teb_local_planner::PoseSE2*>, std::size_t>({}, 0));
    }

    // clear previous association
    for (auto& item : _vp_association)
    {
        item.first.clear();
    }

    int start_pose_idx = 0;

    for (const teb_local_planner::PoseSE2& pose : *_via_points)
    {
        int idx = fd_grid->findClosestPose(pose.x(), pose.y(), start_pose_idx);

        if (_via_points_ordered) start_pose_idx = idx + 2;  // skip a point to have a DOF inbetween for further via-points

        // check if point coincides with final state or is located behind it
        if (idx > n - 2) idx = n - 2;  // set to a pose before the goal, since it is never fixed

        // check if point coincides with start or is located before it
        if (idx < 1)
        {
            if (_via_points_ordered)
                idx = 1;  // try to connect the via point with the second pose. Grid adaptation might add new poses inbetween later if enabled.
            else
            {
                PRINT_DEBUG("TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current robot pose.");
                continue;  // skip via points really close or behind the current robot pose
            }
        }

        _vp_association[idx].first.push_back(&pose);
    }

    // check for structure changes
    for (auto& item : _vp_association)
    {
        if (item.first.size() != item.second)
        {
            item.second   = item.first.size();
            new_structure = true;
            // but continue until end for next time
        }
    }

    //  return true if structure requires a changed
    return new_structure;
}

void MinTimeViaPointsCost::computeNonIntegralDtTerm(int k, double dt, Eigen::Ref<Eigen::VectorXd> cost) const
{
    if (!_single_dt || k == 0)
        cost[0] = _time_weight * dt;
    else
    {
        PRINT_DEBUG_NAMED("this method should not be called in single_dt mode and k>0");
    }
}

void MinTimeViaPointsCost::computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const
{
    assert(_via_points);
    assert(k < _vp_association.size());
    assert(cost.size() == _vp_association[k].first.size());
    assert(_vp_association[k].second == _vp_association[k].first.size());

    for (int i = 0; i < (int)_vp_association[k].second; ++i)
    {
        cost[i] = _vp_position_weight * (_vp_association[k].first[i]->position() - x_k.head(2)).squaredNorm();
        if (_vp_orientation_weight > 0)
        {
            cost[i] += _vp_orientation_weight * normalize_theta(_vp_association[k].first[i]->theta() - x_k[2]);
        }
    }
}

}  // namespace mpc_local_planner