Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix DifferentialDriveVoltageConstraint for tight turns w/ zero velocity (#2341)

Browse Source
This commit is contained in:
Oblarg
2020-02-15 15:36:16 -05:00
committed by GitHub
parent a226ad8509
commit 65eab93527
4 changed files with 130 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -47,30 +47,51 @@ DifferentialDriveVoltageConstraint::MinMaxAcceleration(
// Robot chassis turning on radius = 1/|curvature|. Outer wheel has radius
// increased by half of the trackwidth T. Inner wheel has radius decreased
// by half of the trackwidth. Achassis / radius = Aouter / (radius + T/2), so
// Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2)
// Inner wheel is similar.
// Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 +
// |curvature|T/2). Inner wheel is similar.
// sgn(speed) term added to correctly account for which wheel is on
// outside of turn:
// If moving forward, max acceleration constraint corresponds to wheel on
// outside of turn
// If moving backward, max acceleration constraint corresponds to wheel on
// inside of turn
auto maxChassisAcceleration =
maxWheelAcceleration /
(1 + m_kinematics.trackWidth * units::math::abs(curvature) *
wpi::sgn(speed) / (2_rad));
auto minChassisAcceleration =
minWheelAcceleration /
(1 - m_kinematics.trackWidth * units::math::abs(curvature) *
wpi::sgn(speed) / (2_rad));
// outside of turn If moving backward, max acceleration constraint corresponds
// to wheel on inside of turn
// When velocity is zero, then wheel velocities are uniformly zero (robot
// cannot be turning on its center) - we have to treat this as a special case,
// as it breaks the signum function. Both max and min acceleration are
// *reduced in magnitude* in this case.
units::meters_per_second_squared_t maxChassisAcceleration;
units::meters_per_second_squared_t minChassisAcceleration;
if (speed == 0_mps) {
maxChassisAcceleration =
maxWheelAcceleration /
(1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
minChassisAcceleration =
minWheelAcceleration /
(1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
} else {
maxChassisAcceleration =
maxWheelAcceleration /
(1 + m_kinematics.trackWidth * units::math::abs(curvature) *
wpi::sgn(speed) / (2_rad));
minChassisAcceleration =
minWheelAcceleration /
(1 - m_kinematics.trackWidth * units::math::abs(curvature) *
wpi::sgn(speed) / (2_rad));
}
// When turning about a point inside of the wheelbase (i.e. radius less than
// half the trackwidth), the inner wheel's direction changes, but the
// magnitude remains the same. The formula above changes sign for the inner
// wheel when this happens. We can accurately account for this by simply
// negating the inner wheel.
// Negate acceleration corresponding to wheel on inside of turn
// if center of turn is inside of wheelbase
if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) {
if (speed > 0_mps) {
minChassisAcceleration = -minChassisAcceleration;
} else {
} else if (speed < 0_mps) {
maxChassisAcceleration = -maxChassisAcceleration;
}
}

27
wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,7 +11,9 @@
#include <units/units.h>
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
@@ -57,3 +59,26 @@ TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
-maxVoltage - 0.05_V);
}
}
TEST(DifferentialDriveVoltageConstraintTest, HighCurvature) {
SimpleMotorFeedforward<units::meter> feedforward{1_V, 1_V / 1_mps,
3_V / 1_mps_sq};
// Large trackwidth - need to test with radius of curvature less than half of
// trackwidth
const DifferentialDriveKinematics kinematics{3_m};
const auto maxVoltage = 10_V;
auto config = TrajectoryConfig(12_fps, 12_fps_sq);
config.AddConstraint(
DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
Pose2d{1_m, 0_m, Rotation2d{90_deg}}, std::vector<Translation2d>{},
Pose2d{0_m, 1_m, Rotation2d{180_deg}}, config));
config.SetReversed(true);
EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
Pose2d{0_m, 1_m, Rotation2d{180_deg}}, std::vector<Translation2d>{},
Pose2d{1_m, 0_m, Rotation2d{90_deg}}, config));
}

43
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -82,20 +82,41 @@ public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint
// If moving forward, max acceleration constraint corresponds to wheel on outside of turn
// If moving backward, max acceleration constraint corresponds to wheel on inside of turn
double maxChassisAcceleration =
maxWheelAcceleration
/ (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond) / 2);
double minChassisAcceleration =
minWheelAcceleration
/ (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond) / 2);
// When velocity is zero, then wheel velocities are uniformly zero (robot cannot be
// turning on its center) - we have to treat this as a special case, as it breaks
// the signum function. Both max and min acceleration are *reduced in magnitude*
// in this case.
double maxChassisAcceleration;
double minChassisAcceleration;
if (velocityMetersPerSecond == 0) {
maxChassisAcceleration =
maxWheelAcceleration
/ (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
minChassisAcceleration =
minWheelAcceleration
/ (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
} else {
maxChassisAcceleration =
maxWheelAcceleration
/ (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond) / 2);
minChassisAcceleration =
minWheelAcceleration
/ (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond) / 2);
}
// When turning about a point inside of the wheelbase (i.e. radius less than half
// the trackwidth), the inner wheel's direction changes, but the magnitude remains
// the same. The formula above changes sign for the inner wheel when this happens.
// We can accurately account for this by simply negating the inner wheel.
// Negate acceleration of wheel on inside of turn if center of turn is inside of wheelbase
if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) {
if (velocityMetersPerSecond > 0) {
minChassisAcceleration = -minChassisAcceleration;
} else {
} else if (velocityMetersPerSecond < 0) {
maxChassisAcceleration = -maxChassisAcceleration;
}
}

35
wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,16 +7,21 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
import java.util.Collections;
import org.junit.jupiter.api.Test;
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.Translation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertTrue;
class DifferentialDriveVoltageConstraintTest {
@@ -67,4 +72,32 @@ class DifferentialDriveVoltageConstraintTest {
);
}
}
@Test
void testEndpointHighCurvature() {
var feedforward = new SimpleMotorFeedforward(1, 1, 3);
// Large trackwidth - need to test with radius of curvature less than half of trackwidth
var kinematics = new DifferentialDriveKinematics(3);
double maxVoltage = 10;
var constraint = new DifferentialDriveVoltageConstraint(feedforward,
kinematics,
maxVoltage);
var config = new TrajectoryConfig(12, 12).addConstraint(constraint);
// Radius of curvature should be ~1 meter.
assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
new ArrayList<Translation2d>(),
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
config));
assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
new ArrayList<Translation2d>(),
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
config.setReversed(true)));
}
}