Beispiel #1
0
 public Pose2dWithCurvature(Translation2d translation, Rotation2d rotation, double curvature,
                            double dcurvature_ds)
 {
     pose_          = new Pose2d(translation, rotation);
     curvature_     = curvature;
     dcurvature_ds_ = dcurvature_ds;
 }
Beispiel #2
0
        /**
         * Return true if this pose is (nearly) colinear with the another.
         */
        public bool isColinear(Pose2d other)
        {
            if (!getRotation().isParallel(other.getRotation()))
            {
                return(false);
            }
            Twist2d twist = log(inverse().transformBy(other));

            return(epsilonEquals(twist.dy, 0.0) && epsilonEquals(twist.dtheta, 0.0));
        }
Beispiel #3
0
        /**
         * Do twist interpolation of this pose assuming constant curvature.
         */
        public Pose2d interpolate(Pose2d other, double x)
        {
            if (x <= 0)
            {
                return(new Pose2d(this));
            }
            else if (x >= 1)
            {
                return(new Pose2d(other));
            }
            Twist2d twist = Pose2d.log(inverse().transformBy(other));

            return(transformBy(Pose2d.exp(twist.scaled(x))));
        }
Beispiel #4
0
        private static Translation2d intersectionInternal(Pose2d a, Pose2d b)
        {
            Rotation2d    a_r = a.getRotation();
            Rotation2d    b_r = b.getRotation();
            Translation2d a_t = a.getTranslation();
            Translation2d b_t = b.getTranslation();

            double tan_b = b_r.tan();
            double t     = ((a_t.x() - b_t.x()) * tan_b + b_t.y() - a_t.y()) / (a_r.sin() - a_r.cos() * tan_b);

            if (Double.IsNaN(t))
            {
                return(new Translation2d(Double.PositiveInfinity, Double.PositiveInfinity));
            }
            return(a_t.translateBy(a_r.toTranslation().scale(t)));
        }
Beispiel #5
0
        /**
         * Logical inverse of the above.
         */
        public static Twist2d log(Pose2d transform)
        {
            double dtheta        = transform.getRotation().getRadians();
            double half_dtheta   = 0.5 * dtheta;
            double cos_minus_one = transform.getRotation().cos() - 1.0;
            double halftheta_by_tan_of_halfdtheta;

            if (Math.Abs(cos_minus_one) < kEps)
            {
                halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
            }
            else
            {
                halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().sin()) / cos_minus_one;
            }
            Translation2d translation_part = transform.getTranslation()
                                             .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta, false));

            return(new Twist2d(translation_part.x(), translation_part.y(), dtheta));
        }
Beispiel #6
0
 public Pose2dWithCurvature transformBy(Pose2d transform)
 {
     return(new Pose2dWithCurvature(getPose().transformBy(transform), getCurvature(), getDCurvatureDs()));
 }
Beispiel #7
0
 public Pose2dWithCurvature(Pose2d pose, double curvature, double dcurvature_ds)
 {
     pose_          = pose;
     curvature_     = curvature;
     dcurvature_ds_ = dcurvature_ds;
 }
Beispiel #8
0
 public Pose2dWithCurvature(Pose2d pose, double curvature)
 {
     pose_          = pose;
     curvature_     = curvature;
     dcurvature_ds_ = 0.0;
 }
Beispiel #9
0
 public Pose2dWithCurvature()
 {
     pose_          = new Pose2d();
     curvature_     = 0.0;
     dcurvature_ds_ = 0.0;
 }
Beispiel #10
0
 public Pose2d(Pose2d other)
 {
     translation_ = new Translation2d(other.translation_);
     rotation_    = new Rotation2d(other.rotation_);
 }
Beispiel #11
0
 public double distance(Pose2d other)
 {
     return(Pose2d.log(inverse().transformBy(other)).norm());
 }
Beispiel #12
0
 /**
  * Transforming this RigidTransform2d means first translating by
  * other.translation and then rotating by other.rotation
  *
  * @param other The other transform.
  * @return This transform * other
  */
 public Pose2d transformBy(Pose2d other)
 {
     return(new Pose2d(translation_.translateBy(other.translation_.rotateBy(rotation_)),
                       rotation_.rotateBy(other.rotation_)));
 }