29 units::meters_per_second_t vx = 0_mps;
34 units::meters_per_second_t vy = 0_mps;
39 units::radians_per_second_t omega = 0_rad_per_s;
49 return Twist2d{vx * dt, vy * dt, omega * dt};
76 units::meters_per_second_t vy,
77 units::radians_per_second_t omega,
81 Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
85 auto twist =
Pose2d{}.
Log(desiredDeltaPose);
88 return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
114 return Discretize(continuousSpeeds.
vx, continuousSpeeds.
vy,
115 continuousSpeeds.
omega, dt);
135 units::meters_per_second_t vx, units::meters_per_second_t vy,
136 units::radians_per_second_t omega,
const Rotation2d& robotAngle) {
139 Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
141 return {units::meters_per_second_t{rotated.X().value()},
142 units::meters_per_second_t{rotated.Y().value()}, omega};
161 return FromFieldRelativeSpeeds(fieldRelativeSpeeds.
vx,
162 fieldRelativeSpeeds.
vy,
163 fieldRelativeSpeeds.
omega, robotAngle);
183 units::meters_per_second_t vx, units::meters_per_second_t vy,
184 units::radians_per_second_t omega,
const Rotation2d& robotAngle) {
187 Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
189 return {units::meters_per_second_t{rotated.X().value()},
190 units::meters_per_second_t{rotated.Y().value()}, omega};
209 return FromRobotRelativeSpeeds(robotRelativeSpeeds.
vx,
210 robotRelativeSpeeds.
vy,
211 robotRelativeSpeeds.
omega, robotAngle);
225 return {vx + other.vx, vy + other.vy, omega + other.omega};
240 return *
this + -other;
276 return operator*(1.0 /
scalar);
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
constexpr Twist2d Log(const Pose2d &end) const
Returns a Twist2d that maps this pose to the end pose.
Definition Pose2d.h:346
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition Rotation2d.h:31
Represents a translation in 2D space.
Definition Translation2d.h:29
constexpr Translation2d RotateBy(const Rotation2d &other) const
Applies a rotation to the translation in 2D space.
Definition Translation2d.h:135
Represents the speed of a robot chassis.
Definition ChassisSpeeds.h:25
units::meters_per_second_t vx
Velocity along the x-axis.
Definition ChassisSpeeds.h:29
units::meters_per_second_t vy
Velocity along the y-axis.
Definition ChassisSpeeds.h:34
static constexpr ChassisSpeeds Discretize(const ChassisSpeeds &continuousSpeeds, units::second_t dt)
Discretizes a continuous-time chassis speed.
Definition ChassisSpeeds.h:112
units::radians_per_second_t omega
Represents the angular velocity of the robot frame.
Definition ChassisSpeeds.h:39
static constexpr ChassisSpeeds FromRobotRelativeSpeeds(const ChassisSpeeds &robotRelativeSpeeds, const Rotation2d &robotAngle)
Converts a user provided robot-relative ChassisSpeeds object into a field-relative ChassisSpeeds obje...
Definition ChassisSpeeds.h:207
constexpr Twist2d ToTwist2d(units::second_t dt) const
Creates a Twist2d from ChassisSpeeds.
Definition ChassisSpeeds.h:48
constexpr ChassisSpeeds operator+(const ChassisSpeeds &other) const
Adds two ChassisSpeeds and returns the sum.
Definition ChassisSpeeds.h:224
static constexpr ChassisSpeeds Discretize(units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, units::second_t dt)
Discretizes a continuous-time chassis speed.
Definition ChassisSpeeds.h:75
constexpr ChassisSpeeds operator-(const ChassisSpeeds &other) const
Subtracts the other ChassisSpeeds from the current ChassisSpeeds and returns the difference.
Definition ChassisSpeeds.h:239
static constexpr ChassisSpeeds FromFieldRelativeSpeeds(units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, const Rotation2d &robotAngle)
Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds object.
Definition ChassisSpeeds.h:134
static constexpr ChassisSpeeds FromRobotRelativeSpeeds(units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, const Rotation2d &robotAngle)
Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds object.
Definition ChassisSpeeds.h:182
constexpr ChassisSpeeds operator*(double scalar) const
Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
Definition ChassisSpeeds.h:261
constexpr ChassisSpeeds operator/(double scalar) const
Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
Definition ChassisSpeeds.h:275
constexpr ChassisSpeeds operator-() const
Returns the inverse of the current ChassisSpeeds.
Definition ChassisSpeeds.h:249
constexpr bool operator==(const ChassisSpeeds &other) const =default
Compares the ChassisSpeeds with another ChassisSpeed.
static constexpr ChassisSpeeds FromFieldRelativeSpeeds(const ChassisSpeeds &fieldRelativeSpeeds, const Rotation2d &robotAngle)
Converts a user provided field-relative ChassisSpeeds object into a robot-relative ChassisSpeeds obje...
Definition ChassisSpeeds.h:159
A change in distance along a 2D arc since the last pose update.
Definition Twist2d.h:22
Type representing an arbitrary unit.
Definition base.h:888