001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.geometry;
006
007import edu.wpi.first.math.geometry.proto.Twist2dProto;
008import edu.wpi.first.math.geometry.struct.Twist2dStruct;
009import edu.wpi.first.util.protobuf.ProtobufSerializable;
010import edu.wpi.first.util.struct.StructSerializable;
011import java.util.Objects;
012
013/**
014 * A change in distance along a 2D arc since the last pose update. We can use ideas from
015 * differential calculus to create new Pose2d objects from a Twist2d and vice versa.
016 *
017 * <p>A Twist can be used to represent a difference between two poses.
018 */
019public class Twist2d implements ProtobufSerializable, StructSerializable {
020  /** Linear "dx" component. */
021  public double dx;
022
023  /** Linear "dy" component. */
024  public double dy;
025
026  /** Angular "dtheta" component (radians). */
027  public double dtheta;
028
029  /** Default constructor. */
030  public Twist2d() {}
031
032  /**
033   * Constructs a Twist2d with the given values.
034   *
035   * @param dx Change in x direction relative to robot.
036   * @param dy Change in y direction relative to robot.
037   * @param dtheta Change in angle relative to robot.
038   */
039  public Twist2d(double dx, double dy, double dtheta) {
040    this.dx = dx;
041    this.dy = dy;
042    this.dtheta = dtheta;
043  }
044
045  /**
046   * Obtain a new Transform2d from a (constant curvature) velocity.
047   *
048   * <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
049   * Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for a
050   * derivation.
051   *
052   * <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
053   * update. When the user runs exp() on the twist, the user will receive the pose delta.
054   *
055   * <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
056   * pose forward in time.
057   *
058   * @return The pose delta of the robot.
059   */
060  public Transform2d exp() {
061    double sinTheta = Math.sin(dtheta);
062    double cosTheta = Math.cos(dtheta);
063
064    double s;
065    double c;
066    if (Math.abs(dtheta) < 1E-9) {
067      s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
068      c = 0.5 * dtheta;
069    } else {
070      s = sinTheta / dtheta;
071      c = (1 - cosTheta) / dtheta;
072    }
073
074    return new Transform2d(
075        new Translation2d(dx * s - dy * c, dx * c + dy * s), new Rotation2d(cosTheta, sinTheta));
076  }
077
078  @Override
079  public String toString() {
080    return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
081  }
082
083  /**
084   * Checks equality between this Twist2d and another object.
085   *
086   * @param obj The other object.
087   * @return Whether the two objects are equal or not.
088   */
089  @Override
090  public boolean equals(Object obj) {
091    return obj instanceof Twist2d other
092        && Math.abs(other.dx - dx) < 1E-9
093        && Math.abs(other.dy - dy) < 1E-9
094        && Math.abs(other.dtheta - dtheta) < 1E-9;
095  }
096
097  @Override
098  public int hashCode() {
099    return Objects.hash(dx, dy, dtheta);
100  }
101
102  /** Twist2d protobuf for serialization. */
103  public static final Twist2dProto proto = new Twist2dProto();
104
105  /** Twist2d struct for serialization. */
106  public static final Twist2dStruct struct = new Twist2dStruct();
107}