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