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.Twist3dProto;
008import edu.wpi.first.math.geometry.struct.Twist3dStruct;
009import edu.wpi.first.math.jni.Twist3dJNI;
010import edu.wpi.first.util.protobuf.ProtobufSerializable;
011import edu.wpi.first.util.struct.StructSerializable;
012import java.util.Objects;
013
014/**
015 * A change in distance along a 3D arc since the last pose update. We can use ideas from
016 * differential calculus to create new Pose3d objects from a Twist3d and vice versa.
017 *
018 * <p>A Twist can be used to represent a difference between two poses.
019 */
020public class Twist3d implements ProtobufSerializable, StructSerializable {
021  /** Linear "dx" component. */
022  public double dx;
023
024  /** Linear "dy" component. */
025  public double dy;
026
027  /** Linear "dz" component. */
028  public double dz;
029
030  /** Rotation vector x component (radians). */
031  public double rx;
032
033  /** Rotation vector y component (radians). */
034  public double ry;
035
036  /** Rotation vector z component (radians). */
037  public double rz;
038
039  /** Default constructor. */
040  public Twist3d() {}
041
042  /**
043   * Constructs a Twist3d with the given values.
044   *
045   * @param dx Change in x direction relative to robot.
046   * @param dy Change in y direction relative to robot.
047   * @param dz Change in z direction relative to robot.
048   * @param rx Rotation vector x component.
049   * @param ry Rotation vector y component.
050   * @param rz Rotation vector z component.
051   */
052  public Twist3d(double dx, double dy, double dz, double rx, double ry, double rz) {
053    this.dx = dx;
054    this.dy = dy;
055    this.dz = dz;
056    this.rx = rx;
057    this.ry = ry;
058    this.rz = rz;
059  }
060
061  /**
062   * Obtain a new Transform3d from a (constant curvature) velocity.
063   *
064   * <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
065   * Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for a
066   * derivation.
067   *
068   * <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
069   * update. When the user runs exp() on the twist, the user will receive the pose delta.
070   *
071   * <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
072   * pose forward in time.
073   *
074   * @return The pose delta of the robot.
075   */
076  public Transform3d exp() {
077    double[] resultArray = Twist3dJNI.exp(dx, dy, dz, rx, ry, rz);
078    return new Transform3d(
079        resultArray[0],
080        resultArray[1],
081        resultArray[2],
082        new Rotation3d(
083            new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6])));
084  }
085
086  @Override
087  public String toString() {
088    return String.format(
089        "Twist3d(dX: %.2f, dY: %.2f, dZ: %.2f, rX: %.2f, rY: %.2f, rZ: %.2f)",
090        dx, dy, dz, rx, ry, rz);
091  }
092
093  /**
094   * Checks equality between this Twist3d and another object.
095   *
096   * @param obj The other object.
097   * @return Whether the two objects are equal or not.
098   */
099  @Override
100  public boolean equals(Object obj) {
101    return obj instanceof Twist3d other
102        && Math.abs(other.dx - dx) < 1E-9
103        && Math.abs(other.dy - dy) < 1E-9
104        && Math.abs(other.dz - dz) < 1E-9
105        && Math.abs(other.rx - rx) < 1E-9
106        && Math.abs(other.ry - ry) < 1E-9
107        && Math.abs(other.rz - rz) < 1E-9;
108  }
109
110  @Override
111  public int hashCode() {
112    return Objects.hash(dx, dy, dz, rx, ry, rz);
113  }
114
115  /** Twist3d protobuf for serialization. */
116  public static final Twist3dProto proto = new Twist3dProto();
117
118  /** Twist3d struct for serialization. */
119  public static final Twist3dStruct struct = new Twist3dStruct();
120}