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 static edu.wpi.first.units.Units.Meters; 008 009import com.fasterxml.jackson.annotation.JsonAutoDetect; 010import com.fasterxml.jackson.annotation.JsonCreator; 011import com.fasterxml.jackson.annotation.JsonIgnoreProperties; 012import com.fasterxml.jackson.annotation.JsonProperty; 013import edu.wpi.first.math.geometry.proto.Pose2dProto; 014import edu.wpi.first.math.geometry.struct.Pose2dStruct; 015import edu.wpi.first.math.interpolation.Interpolatable; 016import edu.wpi.first.units.Distance; 017import edu.wpi.first.units.Measure; 018import edu.wpi.first.util.protobuf.ProtobufSerializable; 019import edu.wpi.first.util.struct.StructSerializable; 020import java.util.Collections; 021import java.util.Comparator; 022import java.util.List; 023import java.util.Objects; 024 025/** Represents a 2D pose containing translational and rotational elements. */ 026@JsonIgnoreProperties(ignoreUnknown = true) 027@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) 028public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, StructSerializable { 029 /** 030 * A preallocated Pose2d representing the origin. 031 * 032 * <p>This exists to avoid allocations for common poses. 033 */ 034 public static final Pose2d kZero = new Pose2d(); 035 036 private final Translation2d m_translation; 037 private final Rotation2d m_rotation; 038 039 /** Constructs a pose at the origin facing toward the positive X axis. */ 040 public Pose2d() { 041 m_translation = Translation2d.kZero; 042 m_rotation = Rotation2d.kZero; 043 } 044 045 /** 046 * Constructs a pose with the specified translation and rotation. 047 * 048 * @param translation The translational component of the pose. 049 * @param rotation The rotational component of the pose. 050 */ 051 @JsonCreator 052 public Pose2d( 053 @JsonProperty(required = true, value = "translation") Translation2d translation, 054 @JsonProperty(required = true, value = "rotation") Rotation2d rotation) { 055 m_translation = translation; 056 m_rotation = rotation; 057 } 058 059 /** 060 * Constructs a pose with x and y translations instead of a separate Translation2d. 061 * 062 * @param x The x component of the translational component of the pose. 063 * @param y The y component of the translational component of the pose. 064 * @param rotation The rotational component of the pose. 065 */ 066 public Pose2d(double x, double y, Rotation2d rotation) { 067 m_translation = new Translation2d(x, y); 068 m_rotation = rotation; 069 } 070 071 /** 072 * Constructs a pose with x and y translations instead of a separate Translation2d. The X and Y 073 * translations will be converted to and tracked as meters. 074 * 075 * @param x The x component of the translational component of the pose. 076 * @param y The y component of the translational component of the pose. 077 * @param rotation The rotational component of the pose. 078 */ 079 public Pose2d(Measure<Distance> x, Measure<Distance> y, Rotation2d rotation) { 080 this(x.in(Meters), y.in(Meters), rotation); 081 } 082 083 /** 084 * Transforms the pose by the given transformation and returns the new transformed pose. 085 * 086 * <pre> 087 * [x_new] [cos, -sin, 0][transform.x] 088 * [y_new] += [sin, cos, 0][transform.y] 089 * [t_new] [ 0, 0, 1][transform.t] 090 * </pre> 091 * 092 * @param other The transform to transform the pose by. 093 * @return The transformed pose. 094 */ 095 public Pose2d plus(Transform2d other) { 096 return transformBy(other); 097 } 098 099 /** 100 * Returns the Transform2d that maps the one pose to another. 101 * 102 * @param other The initial pose of the transformation. 103 * @return The transform that maps the other pose to the current pose. 104 */ 105 public Transform2d minus(Pose2d other) { 106 final var pose = this.relativeTo(other); 107 return new Transform2d(pose.getTranslation(), pose.getRotation()); 108 } 109 110 /** 111 * Returns the translation component of the transformation. 112 * 113 * @return The translational component of the pose. 114 */ 115 @JsonProperty 116 public Translation2d getTranslation() { 117 return m_translation; 118 } 119 120 /** 121 * Returns the X component of the pose's translation. 122 * 123 * @return The x component of the pose's translation. 124 */ 125 public double getX() { 126 return m_translation.getX(); 127 } 128 129 /** 130 * Returns the Y component of the pose's translation. 131 * 132 * @return The y component of the pose's translation. 133 */ 134 public double getY() { 135 return m_translation.getY(); 136 } 137 138 /** 139 * Returns the rotational component of the transformation. 140 * 141 * @return The rotational component of the pose. 142 */ 143 @JsonProperty 144 public Rotation2d getRotation() { 145 return m_rotation; 146 } 147 148 /** 149 * Multiplies the current pose by a scalar. 150 * 151 * @param scalar The scalar. 152 * @return The new scaled Pose2d. 153 */ 154 public Pose2d times(double scalar) { 155 return new Pose2d(m_translation.times(scalar), m_rotation.times(scalar)); 156 } 157 158 /** 159 * Divides the current pose by a scalar. 160 * 161 * @param scalar The scalar. 162 * @return The new scaled Pose2d. 163 */ 164 public Pose2d div(double scalar) { 165 return times(1.0 / scalar); 166 } 167 168 /** 169 * Rotates the pose around the origin and returns the new pose. 170 * 171 * @param other The rotation to transform the pose by. 172 * @return The transformed pose. 173 */ 174 public Pose2d rotateBy(Rotation2d other) { 175 return new Pose2d(m_translation.rotateBy(other), m_rotation.rotateBy(other)); 176 } 177 178 /** 179 * Transforms the pose by the given transformation and returns the new pose. See + operator for 180 * the matrix multiplication performed. 181 * 182 * @param other The transform to transform the pose by. 183 * @return The transformed pose. 184 */ 185 public Pose2d transformBy(Transform2d other) { 186 return new Pose2d( 187 m_translation.plus(other.getTranslation().rotateBy(m_rotation)), 188 other.getRotation().plus(m_rotation)); 189 } 190 191 /** 192 * Returns the current pose relative to the given pose. 193 * 194 * <p>This function can often be used for trajectory tracking or pose stabilization algorithms to 195 * get the error between the reference and the current pose. 196 * 197 * @param other The pose that is the origin of the new coordinate frame that the current pose will 198 * be converted into. 199 * @return The current pose relative to the new origin pose. 200 */ 201 public Pose2d relativeTo(Pose2d other) { 202 var transform = new Transform2d(other, this); 203 return new Pose2d(transform.getTranslation(), transform.getRotation()); 204 } 205 206 /** 207 * Obtain a new Pose2d from a (constant curvature) velocity. 208 * 209 * <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls 210 * Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for a 211 * derivation. 212 * 213 * <p>The twist is a change in pose in the robot's coordinate frame since the previous pose 214 * update. When the user runs exp() on the previous known field-relative pose with the argument 215 * being the twist, the user will receive the new field-relative pose. 216 * 217 * <p>"Exp" represents the pose exponential, which is solving a differential equation moving the 218 * pose forward in time. 219 * 220 * @param twist The change in pose in the robot's coordinate frame since the previous pose update. 221 * For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5 222 * degrees since the previous pose update, the twist would be Twist2d(0.01, 0.0, 223 * Units.degreesToRadians(0.5)). 224 * @return The new pose of the robot. 225 */ 226 public Pose2d exp(Twist2d twist) { 227 double dx = twist.dx; 228 double dy = twist.dy; 229 double dtheta = twist.dtheta; 230 231 double sinTheta = Math.sin(dtheta); 232 double cosTheta = Math.cos(dtheta); 233 234 double s; 235 double c; 236 if (Math.abs(dtheta) < 1E-9) { 237 s = 1.0 - 1.0 / 6.0 * dtheta * dtheta; 238 c = 0.5 * dtheta; 239 } else { 240 s = sinTheta / dtheta; 241 c = (1 - cosTheta) / dtheta; 242 } 243 var transform = 244 new Transform2d( 245 new Translation2d(dx * s - dy * c, dx * c + dy * s), 246 new Rotation2d(cosTheta, sinTheta)); 247 248 return this.plus(transform); 249 } 250 251 /** 252 * Returns a Twist2d that maps this pose to the end pose. If c is the output of {@code a.Log(b)}, 253 * then {@code a.Exp(c)} would yield b. 254 * 255 * @param end The end pose for the transformation. 256 * @return The twist that maps this to end. 257 */ 258 public Twist2d log(Pose2d end) { 259 final var transform = end.relativeTo(this); 260 final var dtheta = transform.getRotation().getRadians(); 261 final var halfDtheta = dtheta / 2.0; 262 263 final var cosMinusOne = transform.getRotation().getCos() - 1; 264 265 double halfThetaByTanOfHalfDtheta; 266 if (Math.abs(cosMinusOne) < 1E-9) { 267 halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; 268 } else { 269 halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne; 270 } 271 272 Translation2d translationPart = 273 transform 274 .getTranslation() 275 .rotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)) 276 .times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta)); 277 278 return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta); 279 } 280 281 /** 282 * Returns the nearest Pose2d from a list of poses. If two or more poses in the list have the same 283 * distance from this pose, return the one with the closest rotation component. 284 * 285 * @param poses The list of poses to find the nearest. 286 * @return The nearest Pose2d from the list. 287 */ 288 public Pose2d nearest(List<Pose2d> poses) { 289 return Collections.min( 290 poses, 291 Comparator.comparing( 292 (Pose2d other) -> this.getTranslation().getDistance(other.getTranslation())) 293 .thenComparing( 294 (Pose2d other) -> 295 Math.abs(this.getRotation().minus(other.getRotation()).getRadians()))); 296 } 297 298 @Override 299 public String toString() { 300 return String.format("Pose2d(%s, %s)", m_translation, m_rotation); 301 } 302 303 /** 304 * Checks equality between this Pose2d and another object. 305 * 306 * @param obj The other object. 307 * @return Whether the two objects are equal or not. 308 */ 309 @Override 310 public boolean equals(Object obj) { 311 if (obj instanceof Pose2d) { 312 return ((Pose2d) obj).m_translation.equals(m_translation) 313 && ((Pose2d) obj).m_rotation.equals(m_rotation); 314 } 315 return false; 316 } 317 318 @Override 319 public int hashCode() { 320 return Objects.hash(m_translation, m_rotation); 321 } 322 323 @Override 324 public Pose2d interpolate(Pose2d endValue, double t) { 325 if (t < 0) { 326 return this; 327 } else if (t >= 1) { 328 return endValue; 329 } else { 330 var twist = this.log(endValue); 331 var scaledTwist = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t); 332 return this.exp(scaledTwist); 333 } 334 } 335 336 /** Pose2d protobuf for serialization. */ 337 public static final Pose2dProto proto = new Pose2dProto(); 338 339 /** Pose2d struct for serialization. */ 340 public static final Pose2dStruct struct = new Pose2dStruct(); 341}