LinearPath.hpp

Go to the documentation of this file.

/*
 * Copyright (C) Cross The Road Electronics.  All rights reserved.
 * License information can be found in CTRE_LICENSE.txt
 * For support and suggestions contact support@ctr-electronics.com or file
 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
 */
#pragma once
 
#include "ctre/phoenix6/swerve/utility/Kinematics.hpp"
#include <wpi/math/trajectory/TrapezoidProfile.hpp>
 
namespace ctre {
namespace phoenix6 {
namespace swerve {
 
/**
 * \brief A linear path for a holonomic drivetrain (i.e. swerve or mechanum).
 * This generates profiled setpoints for the robot along a straight line
 * based on trapezoidal velocity constraints (using wpi#math#TrapezoidProfile).
 *
 * Initialization:
 * \code{.cpp}
 * LinearPath path{
 *     TrapezoidProfile::Constraints{kMaxV, kMaxA},
 *     TrapezoidProfile::Constraints{kMaxOmega, kMaxAlpha}
 * };
 * LinearPath::State current{initialPose, initialFieldSpeeds};
 * \endcode
 *
 * Run on update:
 * \code{.cpp}
 * current = path.Calculate(timeSincePreviousUpdate, current, targetPose);
 * \endcode
 */
class LinearPath {
public:
    /** \brief Path state. */
    struct State {
        /** \brief The pose at this state. */
        Pose2d pose{};
        /** \brief The field-centric velocity at this state. */
        ChassisVelocities velocity{};
 
        constexpr bool operator==(State const &) const = default;
    };
 
private:
    wpi::math::TrapezoidProfile<wpi::units::meters> linearProfile;
    wpi::math::TrapezoidProfile<wpi::units::radians> angularProfile;
 
    Pose2d initialPose{};
    Rotation2d heading{};
 
    wpi::math::TrapezoidProfile<wpi::units::meters>::State linearGoal{};
    wpi::math::TrapezoidProfile<wpi::units::meters>::State linearStart{};
 
    wpi::math::TrapezoidProfile<wpi::units::radians>::State angularGoal{};
    wpi::math::TrapezoidProfile<wpi::units::radians>::State angularStart{};
 
    /**
     * \brief Calculates the component of the velocity in the direction of travel.
     *
     * \param velocity The field-centric chassis velocity
     * \param heading The heading of the direction of travel
     * \returns Component of velocity in the direction of the heading
     */
    static constexpr wpi::units::meters_per_second_t CalculateVelocityAtHeading(ChassisVelocities const &velocity, Rotation2d const &heading)
    {
        // vel = <vx, vy> ⋅ <cos(heading), sin(heading)>
        // vel = vx * cos(heading) + vy * sin(heading)
        return velocity.vx * heading.Cos() + velocity.vy * heading.Sin();
    }
 
    /**
     * \brief Sets the current and goal states of the linear path.
     *
     * \param current The current state
     * \param goal The desired pose when the path is complete
     */
    constexpr void SetState(State const &current, Pose2d const &goal)
    {
        initialPose = current.pose;
 
        {
            // pull out the translation from our initial pose to the target
            auto const translation = goal.Translation() - initialPose.Translation();
            // pull out distance and heading to the target
            auto const distance = translation.Norm();
            if (distance > 1e-6_m) {
                heading = translation.Angle();
            } else {
                heading = {};
            }
 
            linearGoal = wpi::math::TrapezoidProfile<wpi::units::meters>::State{distance, 0_mps};
        }
 
        {
            // start at current velocity in the direction of travel
            auto const vel = CalculateVelocityAtHeading(current.velocity, heading);
            linearStart = wpi::math::TrapezoidProfile<wpi::units::meters>::State{0_m, vel};
        }
 
        angularStart = wpi::math::TrapezoidProfile<wpi::units::radians>::State{
            current.pose.Rotation().Radians(),
            current.velocity.omega
        };
        // wrap the angular goal so we take the shortest path
        angularGoal = wpi::math::TrapezoidProfile<wpi::units::radians>::State{
            current.pose.Rotation().Radians() +
                (goal.Rotation() - current.pose.Rotation()).Radians(),
            0_rad_per_s
        };
    }
 
    /**
     * \brief Calculates the pose and velocity of the path at a time t where
     * the current state is at t = 0.
     *
     * \param t How long to advance from the current state to the desired state
     * \returns The pose and velocity of the profile at time t
     */
    constexpr State Calculate(wpi::units::second_t t)
    {
        // calculate our new distance and velocity in the desired direction of travel
        auto const linearState = linearProfile.Calculate(t, linearStart, linearGoal);
        // calculate our new heading and rotational rate
        auto const angularState = angularProfile.Calculate(t, angularStart, angularGoal);
 
        // x is m_state * cos(heading), y is m_state * sin(heading)
        Pose2d pose{
            initialPose.X() + linearState.position * heading.Cos(),
            initialPose.Y() + linearState.position * heading.Sin(),
            {angularState.position}
        };
        ChassisVelocities velocity{
            linearState.velocity * heading.Cos(),
            linearState.velocity * heading.Sin(),
            angularState.velocity
        };
        return {std::move(pose), std::move(velocity)};
    }
 
public:
    /**
     * \brief Constructs a linear path.
     *
     * \param linear The constraints on the profile linear motion
     * \param angular The constraints on the profile angular motion
     */
    constexpr LinearPath(
        wpi::math::TrapezoidProfile<wpi::units::meters>::Constraints linear,
        wpi::math::TrapezoidProfile<wpi::units::radians>::Constraints angular
    ) :
        linearProfile{std::move(linear)},
        angularProfile{std::move(angular)}
    {}
 
    /**
     * \brief Calculates the pose and velocity of the path at a time t where
     * the current state is at t = 0.
     *
     * \param t How long to advance from the current state to the desired state
     * \param current The current state
     * \param goal The desired pose when the path is complete
     * \returns The pose and velocity of the profile at time t
     */
    constexpr State Calculate(wpi::units::second_t t, State const &current, Pose2d const &goal)
    {
        SetState(current, goal);
        return Calculate(t);
    }
 
    /**
     * \brief Returns the total time the profile takes to reach the goal.
     *
     * \returns The total time the profile takes to reach the goal
     */
    constexpr wpi::units::second_t TotalTime() const
    {
        return wpi::units::math::max(
            linearProfile.TotalTime(),
            angularProfile.TotalTime()
        );
    }
 
    /**
     * \brief Returns true if the profile has reached the goal.
     *
     * The profile has reached the goal if the time since the profile
     * started has exceeded the profile's total time.
     *
     * \param t The time since the beginning of the profile
     * \returns true if the profile has reached the goal
     */
    constexpr bool IsFinished(wpi::units::second_t t) const
    {
        return t >= TotalTime();
    }
};
 
}
}
}