DifferentialDriveVoltageConstraint seems to fail testing when using different voltage limits

[jsimpso81]

When testing the JAVA version of the differential drive voltage constraint, the test fails if different voltage are used. I don't know if this is a true failure (although it seems like one) or just an anomaly because I changed the max voltage value without changing the motor feedforward values.

= = = = = = = = = = = = = = = = = = = = = = = = = = = = voltage constraint = 12.0 Test - PASSED leftMaxExceeded = false leftMinExceeded = false
rightMaxExceeded = false
rightMinExceeded = false
leftMax = 9.956575185217648
leftMin = -11.995226276701908 rightMax = 9.95657560277206
rightMin = -11.995226276701908 = = = = = = = = = = = = = = = = = = = = = = = = = = = = voltage constraint = 11.0 Test - PASSED leftMaxExceeded = false
leftMinExceeded = false
rightMaxExceeded = false
rightMinExceeded = false
leftMax = 9.927802746621627
leftMin = -10.998140822044537 rightMax = 9.92781165132986
rightMin = -10.998140822044537 = = = = = = = = = = = = = = = = = = = = = = = = = = = = voltage constraint = 10.0 Test - PASSED leftMaxExceeded = false leftMinExceeded = false rightMaxExceeded = false rightMinExceeded = false leftMax = 9.236943640992521 leftMin = -9.99845187491805 rightMax = 9.355316198492204 rightMin = -9.998451874918054 = = = = = = = = = = = = = = = = = = = = = = = = = = = = voltage constraint = 9.0 Test - FAILED leftMaxExceeded = true leftMinExceeded = false rightMaxExceeded = true rightMinExceeded = false leftMax = 9.179414074794652 leftMin = -8.99519873539483 rightMax = 9.358212156864694 rightMin = -8.995198735394835 = = = = = = = = = = = = = = = = = = = = = = = = = = = = voltage constraint = 8.0 Test - FAILED leftMaxExceeded = true leftMinExceeded = false rightMaxExceeded = true rightMinExceeded = false leftMax = 8.574977407968161 leftMin = -7.999034759380962 rightMax = 9.354715680090512 rightMin = -7.999034759380966 = = = = = = = = = = = = = = = = = = = = = = = = = = = = voltage constraint = 7.0 Test - FAILED leftMaxExceeded = true leftMinExceeded = false rightMaxExceeded = true rightMinExceeded = false leftMax = 8.1712315228454 leftMin = -6.9995891270447075 rightMax = 9.37143570562599 rightMin = -6.999589127044711

To Reproduce Steps to reproduce the behavior: I used the latest production version of WPILIB

Here is the test program that I was using. It is a modified version of the latest test program from github.

package frc.robot.base;

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;    
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.util.Units;

import java.util.ArrayList;

public class TestDiffVoltCont {

public static void main(String[] args) {

    testDifferentialDriveVoltageConstraint(  12.0  );    
    testDifferentialDriveVoltageConstraint(  11.0  );    
    testDifferentialDriveVoltageConstraint(  10.0  );    
    testDifferentialDriveVoltageConstraint(  9.0  );    
    testDifferentialDriveVoltageConstraint(  8.0  );    
    testDifferentialDriveVoltageConstraint(  7.0  );    
}

// --------test diff drive voltage constraint
public static void testDifferentialDriveVoltageConstraint( double voltageValue ) {

    // Pick an unreasonably large kA to ensure the constraint has to do some work
    SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 1, 3);

    DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(0.5);

    double maxVoltage = voltageValue;

    var constraint = new DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage);

    boolean leftMaxExceeded = false;
    boolean rightMaxExceeded = false;
    boolean leftMinExceeded = false;
    boolean rightMinExceeded =false ;

    double leftMax = -9999.0;
    double rightMax = -9999.0;
    double leftMin = 9999.0;
    double rightMin = 9999.0;

    double maxVelocity = Units.feetToMeters(12.0);
    double maxAccel = Units.feetToMeters(12);

        // 2018 cross scale auto waypoints.

    Pose2d sideStart =
        new Pose2d(Units.feetToMeters(1.54), Units.feetToMeters(23.23), Rotation2d.fromDegrees(-180));

    Pose2d  crossScale =
        new Pose2d(Units.feetToMeters(23.7), Units.feetToMeters(6.8), Rotation2d.fromDegrees(-160));

    var waypoints = new ArrayList<Pose2d>();

    waypoints.add(sideStart);

    waypoints.add(
        sideStart.plus(
            new Transform2d(
                new Translation2d(Units.feetToMeters(-13), Units.feetToMeters(0)), 
                new Rotation2d())));

    waypoints.add(
        sideStart.plus(
            new Transform2d(
                new Translation2d(Units.feetToMeters(-19.5), Units.feetToMeters(5)),
                Rotation2d.fromDegrees(-90))));

    waypoints.add(crossScale);

    TrajectoryConfig config =
        new TrajectoryConfig(maxVelocity, maxAccel).setReversed(true).addConstraint(constraint);

    Trajectory trajectory =
        TrajectoryGenerator.generateTrajectory( waypoints, config);

    double duration = trajectory.getTotalTimeSeconds();
    double t = 0.0;
    double dt = 0.02;
    double xxtemp;

    boolean testPassed = false;

    while ( t < duration ) {

        var point = trajectory.sample(t);

        ChassisSpeeds chassisSpeeds =
            new ChassisSpeeds(
                point.velocityMetersPerSecond,
                0,
                point.velocityMetersPerSecond * point.curvatureRadPerMeter);

        var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);

        t += dt;

        // Not really a strictly-correct test as we're using the chassis accel instead of the
        // wheel accel, but much easier than doing it "properly" and a reasonable check anyway

        xxtemp = feedforward.calculate(
                      wheelSpeeds.leftMetersPerSecond,  
                    point.accelerationMetersPerSecondSq);

        leftMaxExceeded |= ( xxtemp >= (maxVoltage + 0.05) ) ;
        leftMinExceeded |= ( xxtemp <= (-maxVoltage - 0.05) ) ;

        if ( xxtemp > leftMax) { leftMax = xxtemp; }
        if ( xxtemp < leftMin) { leftMin = xxtemp; }

        xxtemp = feedforward.calculate(
                      wheelSpeeds.rightMetersPerSecond, 
                      point.accelerationMetersPerSecondSq);

        rightMaxExceeded |= ( xxtemp >= ( maxVoltage + 0.05) ) ;
        rightMaxExceeded |= ( xxtemp <= (-maxVoltage - 0.05) ) ;

        if ( xxtemp > rightMax) { rightMax = xxtemp; }
        if ( xxtemp < rightMin) { rightMin = xxtemp; }

    }   // END OF LOOP

    testPassed = !( leftMaxExceeded || leftMinExceeded || rightMaxExceeded || rightMinExceeded );

    System.out.println(" ====================================================== " );
    System.out.println(" voltage constraint  = " + maxVoltage );
    if ( testPassed ) {
        System.out.println("     Test - PASSED" );
    } else {
        System.out.println("     Test - FAILED" );
    }
    System.out.println("   ");
    System.out.println(" leftMaxExceeded = "+leftMaxExceeded);
    System.out.println(" leftMinExceeded = "+leftMinExceeded);
    System.out.println(" rightMaxExceeded = "+rightMaxExceeded);
    System.out.println(" rightMinExceeded = "+rightMinExceeded);
    System.out.println(" leftMax = "+leftMax );
    System.out.println(" leftMin = "+leftMin );
    System.out.println(" rightMax = "+rightMax );
    System.out.println(" rightMin = "+rightMin );

}

void testEndpointHighCurvature() {
    SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 1, 3);

    // Large trackwidth - need to test with radius of curvature less than half of trackwidth
    DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(3);
    double maxVoltage = 10;
    DifferentialDriveVoltageConstraint constraint = new DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage);

    TrajectoryConfig config = new TrajectoryConfig(12, 12).addConstraint(constraint);

    // Radius of curvature should be ~1 meter.
    TrajectoryGenerator.generateTrajectory(
            new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
            new ArrayList<Translation2d>(),
            new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
            config);

    TrajectoryGenerator.generateTrajectory(
            new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
            new ArrayList<Translation2d>(),
            new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
            config.setReversed(true));
}
}

Expected behavior I thought that the tests would pass.

Screenshots The test results are shown above.

Desktop (please complete the following information):

• WPILib Version: [.2021.3.1]
• OS: [e.g. Windows 10]
• Java version [uncertain] whatever came with the 2021 visual studio release
• C++ version [uncertain]

Additional context

[calcmogul]

As per the source-level comment, the test's voltage check is technically incorrect since it's using chassis acceleration instead of wheel acceleration.

[jsimpso81]

So I modified the test program to calculate the acceleration using the previous and next samples. I created a variant that widened the time by using a previous and next sample that were 2 dt away. Just for fun I added a kinematics constraint. -- Unless I messed the accel calc up, which is entirely likely -- there still seemed to be an issue with this constraint. I didn't start to look for the issue, but I'd probably start with the trajectory parameterizer. I attached the test program and a spreadsheet plotting the results.

(After thinking about this for a while, it would seem to make sense that the individual wheel accel is more aggressive that the chassis accel.)

DiffVoltConstraint_TestResults.xlsx

TestDiffVoltCont.txt

[sciencewhiz]

I'm not sure how it would be possible for chassis acceleration to exceed without at least one wheel acceleration to also exceed

[calcmogul]

It isn't possible.

[jsimpso81]

I agree. That is why when the original test failed with a different voltage. I was fairly certain it would fail if I calculated wheel accel. ( I added the extra sample mostly to see the data.)