Skip to content

C++ PPHolonomicDriveController ignores rotationTargetOverride #686

@bovlb

Description

@bovlb

From code inspection, the C++ implementation does not honour the rotation target override.

frc::Rotation2d targetRotation = referenceState.targetHolonomicRotation;
if (rotationTargetOverride) {
targetRotation = rotationTargetOverride().value_or(targetRotation);
}
units::radians_per_second_t rotationFeedback {
m_rotationController.Calculate(currentPose.Rotation().Radians(),
referenceState.targetHolonomicRotation.Radians(),
{ maxAngVel,
referenceState.constraints.getMaxAngularAcceleration() }) };

Compare with he Java implementation:

Rotation2d targetRotation = targetState.targetHolonomicRotation;
if (rotationTargetOverride != null) {
targetRotation = rotationTargetOverride.get().orElse(targetRotation);
}
double rotationFeedback =
rotationController.calculate(
currentPose.getRotation().getRadians(),
new TrapezoidProfile.State(targetRotation.getRadians(), 0),
rotationConstraints);

I suggest that this should should instead say:

        units::radians_per_second_t rotationFeedback {
                        m_rotationController.Calculate(currentPose.Rotation().Radians(),
                                        targetRotation.Radians(),
                                        { maxAngVel,
                                                        referenceState.constraints.getMaxAngularAcceleration() }) };
        units::radians_per_second_t rotationFF =

Metadata

Metadata

Assignees

No one assigned

    Labels

    PathPlannerLibChanges to PathPlannerLibbugSomething isn't working

    Projects

    No projects

    Milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions