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