Examples with Pose2dWithCurvature com.team254.lib.geometry.Pose2dWithCurvature used on opensource projects

Example 1 with Pose2dWithCurvature

use of com.team254.lib.geometry.Pose2dWithCurvature in project K-9 by TheGreenMachine.

the class TrajectoryTest method verifyMirroredTrajectories.

public void verifyMirroredTrajectories(final Trajectory.Mirrored mirrored, boolean shouldBeReversed) {
    assertEquals(mirrored.left.length(), mirrored.right.length());
    TrajectoryIterator<TimedState<Pose2dWithCurvature>> left_iterator = new TrajectoryIterator<>(new TimedView<>(mirrored.left));
    TrajectoryIterator<TimedState<Pose2dWithCurvature>> right_iterator = new TrajectoryIterator<>(new TimedView<>(mirrored.right));
    final double dt = 0.05;
    TimedState<Pose2dWithCurvature> prev_left = null;
    TimedState<Pose2dWithCurvature> prev_right = null;
    while (!left_iterator.isDone() && !right_iterator.isDone()) {
        TimedState<Pose2dWithCurvature> left_state = left_iterator.getState();
        TimedState<Pose2dWithCurvature> right_state = right_iterator.getState();
        assertEquals(left_state.t(), right_state.t(), kTestEpsilon);
        assertEquals(left_state.velocity(), right_state.velocity(), kTestEpsilon);
        assertEquals(left_state.acceleration(), right_state.acceleration(), kTestEpsilon);
        assertTrue((shouldBeReversed ? -1.0 : 1.0) * left_state.velocity() >= -kTestEpsilon);
        assertTrue((shouldBeReversed ? -1.0 : 1.0) * right_state.velocity() >= -kTestEpsilon);
        if (prev_left != null && prev_right != null) {
            // Check there are no angle discontinuities.
            // rad
            final double kMaxReasonableChangeInAngle = 0.3;
            Twist2d left_change = Pose2d.log(prev_left.state().getPose().inverse().transformBy(left_state.state().getPose()));
            Twist2d right_change = Pose2d.log(prev_right.state().getPose().inverse().transformBy(right_state.state().getPose()));
            assertTrue(Math.abs(left_change.dtheta) < kMaxReasonableChangeInAngle);
            assertTrue(Math.abs(right_change.dtheta) < kMaxReasonableChangeInAngle);
            if (!Util.epsilonEquals(left_change.dtheta, 0.0) || !Util.epsilonEquals(right_change.dtheta, 0.0)) {
                // Could be a curvature sign change between prev and now, so just check that either matches our
                // expected sign.
                final boolean left_curvature_positive = left_state.state().getCurvature() > kTestEpsilon || prev_left.state().getCurvature() > kTestEpsilon;
                final boolean left_curvature_negative = left_state.state().getCurvature() < -kTestEpsilon || prev_left.state().getCurvature() < -kTestEpsilon;
                final boolean right_curvature_positive = right_state.state().getCurvature() > kTestEpsilon || prev_right.state().getCurvature() > kTestEpsilon;
                final boolean right_curvature_negative = right_state.state().getCurvature() < -kTestEpsilon || prev_right.state().getCurvature() < -kTestEpsilon;
                final double actual_left_curvature = left_change.dtheta / left_change.dx;
                final double actual_right_curvature = right_change.dtheta / right_change.dx;
                if (actual_left_curvature < -kTestEpsilon) {
                    assertTrue(left_curvature_negative);
                } else if (actual_left_curvature > kTestEpsilon) {
                    assertTrue(left_curvature_positive);
                }
                if (actual_right_curvature < -kTestEpsilon) {
                    assertTrue(right_curvature_negative);
                } else if (actual_right_curvature > kTestEpsilon) {
                    assertTrue(right_curvature_positive);
                }
            }
        }
        assertEquals(left_state.state().getTranslation().x(), right_state.state().getTranslation().x(), Util.kEpsilon);
        assertEquals(left_state.state().getTranslation().y(), -right_state.state().getTranslation().y(), Util.kEpsilon);
        assertEquals(left_state.state().getRotation(), right_state.state().getRotation().inverse());
        assertEquals(left_state.state().getCurvature(), -right_state.state().getCurvature(), kTestEpsilon);
        left_iterator.advance(dt);
        right_iterator.advance(dt);
        prev_left = left_state;
        prev_right = right_state;
    }
    assertTrue(left_iterator.isDone() && right_iterator.isDone());
}

Example 2 with Pose2dWithCurvature

use of com.team254.lib.geometry.Pose2dWithCurvature in project K-9 by TheGreenMachine.

the class TrajectoryUtil method trajectoryFromSplineWaypoints.

public static Trajectory<Pose2dWithCurvature> trajectoryFromSplineWaypoints(final List<Pose2d> waypoints, double maxDx, double maxDy, double maxDTheta) {
    List<QuinticHermiteSpline> splines = new ArrayList<>(waypoints.size() - 1);
    for (int i = 1; i < waypoints.size(); ++i) {
        splines.add(new QuinticHermiteSpline(waypoints.get(i - 1), waypoints.get(i)));
    }
    QuinticHermiteSpline.optimizeSpline(splines);
    return trajectoryFromSplines(splines, maxDx, maxDy, maxDTheta);
}

Example 3 with Pose2dWithCurvature

use of com.team254.lib.geometry.Pose2dWithCurvature in project K-9 by TheGreenMachine.

the class TrajectoryTest method verifyTrajectory.

private void verifyTrajectory(Trajectory<TimedState<Pose2dWithCurvature>> trajectory, boolean shouldBeReversed) {
    var iterator = new TrajectoryIterator<>(new TimedView<>(trajectory));
    final double dt = 0.05;
    TimedState<Pose2dWithCurvature> prev_left = null;
    while (!iterator.isDone()) {
        TimedState<Pose2dWithCurvature> left_state = iterator.getState();
        assertTrue((shouldBeReversed ? -1.0 : 1.0) * left_state.velocity() >= -kTestEpsilon);
        if (prev_left != null) {
            // Check there are no angle discontinuities.
            // rad
            final double kMaxReasonableChangeInAngle = 0.3;
            Twist2d left_change = Pose2d.log(prev_left.state().getPose().inverse().transformBy(left_state.state().getPose()));
            assertTrue(Math.abs(left_change.dtheta) < kMaxReasonableChangeInAngle);
            if (!Util.epsilonEquals(left_change.dtheta, 0.0)) {
                // Could be a curvature sign change between prev and now, so just check that either matches our
                // expected sign.
                final boolean left_curvature_positive = left_state.state().getCurvature() > kTestEpsilon || prev_left.state().getCurvature() > kTestEpsilon;
                final boolean left_curvature_negative = left_state.state().getCurvature() < -kTestEpsilon || prev_left.state().getCurvature() < -kTestEpsilon;
                final double actual_left_curvature = left_change.dtheta / left_change.dx;
                if (actual_left_curvature < -kTestEpsilon) {
                    assertTrue(left_curvature_negative);
                } else if (actual_left_curvature > kTestEpsilon) {
                    assertTrue(left_curvature_positive);
                }
            }
        }
        iterator.advance(dt);
        prev_left = left_state;
    }
}