Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[py][cmdv2] Add python commands-v2 to build system (#8380)

Browse Source 
This hooks up the recently landed robotpy commands v2 to the build
system.

The swerve controller command was deleted to match #8119
This commit is contained in:
PJ Reiniger
2025-12-12 23:05:31 -05:00
committed by GitHub
parent cca035787c
commit 13abb3d332
4 changed files with 20 additions and 256 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
commandsv2/BUILD.bazel
View File

@@ -2,11 +2,12 @@ load("@allwpilib_pip_deps//:requirements.bzl", "requirement")
load("@aspect_bazel_lib//lib:write_source_files.bzl", "write_source_files")
load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_test")
load("@rules_java//java:defs.bzl", "java_binary")
load("@rules_python//python:defs.bzl", "py_binary")
load("@rules_python//python:defs.bzl", "py_binary", "py_library")
load("//commandsv2:generate.bzl", "generate_wpilib_new_commands")
load("//shared/bazel/rules:cc_rules.bzl", "third_party_cc_lib_helper", "wpilib_cc_library", "wpilib_cc_shared_library", "wpilib_cc_static_library")
load("//shared/bazel/rules:java_rules.bzl", "wpilib_java_junit5_test", "wpilib_java_library")
load("//shared/bazel/rules:packaging.bzl", "package_default_cc_project")
load("//shared/bazel/rules/robotpy:pytest_util.bzl", "robotpy_py_test")
filegroup(
name = "doxygen-files",
@@ -183,3 +184,21 @@ package_default_cc_project(
maven_artifact_name = "commandsv2-cpp",
maven_group_id = "org.wpilib.commandsv2",
)
py_library(
name = "commandsv2-py",
srcs = glob(["src/main/python/**/*.py"]),
imports = ["src/main/python"],
deps = [
"//wpilibc:robotpy-wpilib",
],
)
robotpy_py_test(
"python_tests",
srcs = glob(["src/test/python/**/*.py"]),
deps = [
":commandsv2-py",
requirement("pytest"),
],
)

2
commandsv2/src/main/python/commands2/__init__.py
View File

@@ -27,7 +27,6 @@ from .selectcommand import SelectCommand
from .sequentialcommandgroup import SequentialCommandGroup
from .startendcommand import StartEndCommand
from .subsystem import Subsystem
from .swervecontrollercommand import SwerveControllerCommand
from .timedcommandrobot import TimedCommandRobot
from .trapezoidprofilecommand import TrapezoidProfileCommand
from .trapezoidprofilesubsystem import TrapezoidProfileSubsystem
@@ -65,7 +64,6 @@ __all__ = [
"SequentialCommandGroup",
"StartEndCommand",
"Subsystem",
"SwerveControllerCommand",
"TimedCommandRobot",
"TrapezoidProfileCommand",
"TrapezoidProfileSubsystem",

111
commandsv2/src/main/python/commands2/swervecontrollercommand.py
View File

@@ -1,111 +0,0 @@
# validated: 2024-01-20 DS 192a28af4731 SwerveControllerCommand.java
# 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.
from __future__ import annotations
from typing import Callable, Optional, Union, Tuple, Sequence
from wpimath.controller import (
HolonomicDriveController,
)
from wpimath.geometry import Pose2d, Rotation2d
from wpimath.kinematics import (
SwerveDrive2Kinematics,
SwerveDrive3Kinematics,
SwerveDrive4Kinematics,
SwerveDrive6Kinematics,
SwerveModuleState,
)
from wpimath.trajectory import Trajectory
from wpilib import Timer
from .command import Command
from .subsystem import Subsystem
class SwerveControllerCommand(Command):
"""
A command that uses two PID controllers (:class:`wpimath.controller.PIDController`)
and a HolonomicDriveController (:class:`wpimath.controller.HolonomicDriveController`)
to follow a trajectory (:class:`wpimath.trajectory.Trajectory`) with a swerve drive.
This command outputs the raw desired Swerve Module States (:class:`wpimath.kinematics.SwerveModuleState`) in an
array. The desired wheel and module rotation velocities should be taken from those and used in
velocity PIDs.
The robot angle controller does not follow the angle given by the trajectory but rather goes
to the angle given in the final state of the trajectory.
"""
def __init__(
self,
trajectory: Trajectory,
pose: Callable[[], Pose2d],
kinematics: Union[
SwerveDrive2Kinematics,
SwerveDrive3Kinematics,
SwerveDrive4Kinematics,
SwerveDrive6Kinematics,
],
controller: HolonomicDriveController,
outputModuleStates: Callable[[Sequence[SwerveModuleState]], None],
requirements: Tuple[Subsystem],
desiredRotation: Optional[Callable[[], Rotation2d]] = None,
) -> None:
"""
Constructs a new SwerveControllerCommand that when executed will follow the
provided trajectory. This command will not return output voltages but
rather raw module states from the position controllers which need to be put
into a velocity PID.
Note: The controllers will *not* set the outputVolts to zero upon
completion of the path- this is left to the user, since it is not
appropriate for paths with nonstationary endstates.
:param trajectory: The trajectory to follow.
:param pose: A function that supplies the robot pose - use one of the odometry classes to
provide this.
:param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6
SwerveKinematics.
:param controller: The HolonomicDriveController for the drivetrain.
If you have x, y, and theta controllers, pass them into
HolonomicPIDController.
:param outputModuleStates: The raw output module states from the position controllers.
:param requirements: The subsystems to require.
:param desiredRotation: (optional) The angle that the drivetrain should be
facing. This is sampled at each time step. If not specified, that rotation of
the final pose in the trajectory is used.
"""
super().__init__()
self._trajectory = trajectory
self._pose = pose
self._kinematics = kinematics
self._outputModuleStates = outputModuleStates
self._controller = controller
if desiredRotation is None:
self._desiredRotation = trajectory.states()[-1].pose.rotation
else:
self._desiredRotation = desiredRotation
self._timer = Timer()
self.addRequirements(*requirements)
def initialize(self):
self._timer.restart()
def execute(self):
curTime = self._timer.get()
desiredState = self._trajectory.sample(curTime)
targetChassisSpeeds = self._controller.calculate(
self._pose(), desiredState, self._desiredRotation()
)
targetModuleStates = self._kinematics.toSwerveModuleStates(targetChassisSpeeds)
self._outputModuleStates(targetModuleStates)
def end(self, interrupted):
self._timer.stop()
def isFinished(self):
return self._timer.hasElapsed(self._trajectory.totalTime())

142
commandsv2/src/test/python/test_swervecontrollercommand.py
View File

@@ -1,142 +0,0 @@
# 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.
from typing import TYPE_CHECKING, List, Tuple
import math
from wpilib import Timer
from wpimath.geometry import Pose2d, Rotation2d, Translation2d
from wpimath.kinematics import (
SwerveModuleState,
SwerveModulePosition,
SwerveDrive4Kinematics,
SwerveDrive4Odometry,
)
from wpimath.controller import (
ProfiledPIDControllerRadians,
PIDController,
HolonomicDriveController,
)
from wpimath.trajectory import (
TrapezoidProfileRadians,
Trajectory,
TrajectoryConfig,
TrajectoryGenerator,
)
from util import * # type: ignore
if TYPE_CHECKING:
from .util import *
import pytest
import commands2
def test_swervecontrollercommand():
timer = Timer()
angle = Rotation2d(0)
swerve_module_states = (
SwerveModuleState(0, Rotation2d(0)),
SwerveModuleState(0, Rotation2d(0)),
SwerveModuleState(0, Rotation2d(0)),
SwerveModuleState(0, Rotation2d(0)),
)
swerve_module_positions = (
SwerveModulePosition(0, Rotation2d(0)),
SwerveModulePosition(0, Rotation2d(0)),
SwerveModulePosition(0, Rotation2d(0)),
SwerveModulePosition(0, Rotation2d(0)),
)
rot_controller = ProfiledPIDControllerRadians(
1,
0,
0,
TrapezoidProfileRadians.Constraints(3 * math.pi, math.pi),
)
x_tolerance = 1 / 12.0
y_tolerance = 1 / 12.0
angular_tolerance = 1 / 12.0
wheel_base = 0.5
track_width = 0.5
kinematics = SwerveDrive4Kinematics(
Translation2d(wheel_base / 2, track_width / 2),
Translation2d(wheel_base / 2, -track_width / 2),
Translation2d(-wheel_base / 2, track_width / 2),
Translation2d(-wheel_base / 2, -track_width / 2),
)
odometry = SwerveDrive4Odometry(
kinematics,
Rotation2d(0),
swerve_module_positions,
Pose2d(0, 0, Rotation2d(0)),
)
def set_module_states(states):
nonlocal swerve_module_states
swerve_module_states = states
def get_robot_pose() -> Pose2d:
odometry.update(angle, swerve_module_positions)
return odometry.getPose()
with ManualSimTime() as sim:
subsystem = commands2.Subsystem()
waypoints: List[Pose2d] = []
waypoints.append(Pose2d(0, 0, Rotation2d(0)))
waypoints.append(Pose2d(1, 5, Rotation2d(3)))
config = TrajectoryConfig(8.8, 0.1)
trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config)
end_state = trajectory.sample(trajectory.totalTime())
command = commands2.SwerveControllerCommand(
trajectory=trajectory,
pose=get_robot_pose,
kinematics=kinematics,
controller=HolonomicDriveController(
PIDController(0.6, 0, 0),
PIDController(0.6, 0, 0),
rot_controller,
),
outputModuleStates=set_module_states,
requirements=(subsystem,),
)
timer.restart()
command.initialize()
while not command.isFinished():
command.execute()
angle = trajectory.sample(timer.get()).pose.rotation()
for i in range(0, len(swerve_module_positions)):
swerve_module_positions[i].distance += (
swerve_module_states[i].speed * 0.005
)
swerve_module_positions[i].angle = swerve_module_states[i].angle
sim.step(0.005)
timer.stop()
command.end(True)
assert end_state.pose.X() == pytest.approx(get_robot_pose().X(), x_tolerance)
assert end_state.pose.Y() == pytest.approx(get_robot_pose().Y(), y_tolerance)
assert end_state.pose.rotation().radians() == pytest.approx(
get_robot_pose().rotation().radians(),
angular_tolerance,
)