commands.auto.pathplan_to_path
1# Native imports 2from typing import Callable, Tuple 3 4# Internal imports 5from config import OperatorRobotConfig 6 7# Third-party imports 8from commands2 import Command 9from commands2.cmd import none 10from pathplannerlib.auto import AutoBuilder 11from pathplannerlib.path import PathPlannerPath, PathConstraints 12from wpimath.units import degreesToRadians 13 14 15def pathplanToPath( 16 target_path: Callable[[], PathPlannerPath | None], 17 path_constraints: Tuple[float] = OperatorRobotConfig.teleop_pathplan_constraints 18) -> Command: 19 """ 20 Use PathPlanner to navigate from the robot's current position to a given predefined spath. 21 PathPlanner uses the navgrid.json file in the deploy folder to determine field obstacles - 22 paths generated here will avoid those obstacles. PathPlanner recommends using this approach 23 for fine alignment at the end of the complete path. 24 25 Both translational and rotational motion are constrained using a trapezoidal profile, 26 where slopes represent max acceleration and the plateau represents max velocity. Other 27 configurations (like PID constants) are contained within AutoBuilder and were defined 28 when the drivetrain was instantiated. 29 30 Args: 31 target_path: the pre-defined path to execute immediately upon completion of the generated 32 traversal path 33 path_constraints: the parameters for the trapezoidal motion profile. The first two 34 elements define max translational velocity (mps) and acceleration (mps^2), 35 the second two elements define max rotational velocity (dps) and acceleration (dps^2). 36 37 Returns: 38 pathplan_to_path: the command to follow a path that blends into the pre-defined path 39 """ 40 target_path = target_path() 41 if target_path: 42 path_constraints = PathConstraints( 43 *path_constraints[0:2], degreesToRadians(path_constraints[2]), degreesToRadians(path_constraints[3]) 44 ) 45 46 pathplan_to_path = AutoBuilder.pathfindThenFollowPath(target_path, path_constraints) 47 48 return pathplan_to_path 49 50 return none()
16def pathplanToPath( 17 target_path: Callable[[], PathPlannerPath | None], 18 path_constraints: Tuple[float] = OperatorRobotConfig.teleop_pathplan_constraints 19) -> Command: 20 """ 21 Use PathPlanner to navigate from the robot's current position to a given predefined spath. 22 PathPlanner uses the navgrid.json file in the deploy folder to determine field obstacles - 23 paths generated here will avoid those obstacles. PathPlanner recommends using this approach 24 for fine alignment at the end of the complete path. 25 26 Both translational and rotational motion are constrained using a trapezoidal profile, 27 where slopes represent max acceleration and the plateau represents max velocity. Other 28 configurations (like PID constants) are contained within AutoBuilder and were defined 29 when the drivetrain was instantiated. 30 31 Args: 32 target_path: the pre-defined path to execute immediately upon completion of the generated 33 traversal path 34 path_constraints: the parameters for the trapezoidal motion profile. The first two 35 elements define max translational velocity (mps) and acceleration (mps^2), 36 the second two elements define max rotational velocity (dps) and acceleration (dps^2). 37 38 Returns: 39 pathplan_to_path: the command to follow a path that blends into the pre-defined path 40 """ 41 target_path = target_path() 42 if target_path: 43 path_constraints = PathConstraints( 44 *path_constraints[0:2], degreesToRadians(path_constraints[2]), degreesToRadians(path_constraints[3]) 45 ) 46 47 pathplan_to_path = AutoBuilder.pathfindThenFollowPath(target_path, path_constraints) 48 49 return pathplan_to_path 50 51 return none()
Use PathPlanner to navigate from the robot's current position to a given predefined spath. PathPlanner uses the navgrid.json file in the deploy folder to determine field obstacles - paths generated here will avoid those obstacles. PathPlanner recommends using this approach for fine alignment at the end of the complete path.
Both translational and rotational motion are constrained using a trapezoidal profile, where slopes represent max acceleration and the plateau represents max velocity. Other configurations (like PID constants) are contained within AutoBuilder and were defined when the drivetrain was instantiated.
Args: target_path: the pre-defined path to execute immediately upon completion of the generated traversal path path_constraints: the parameters for the trapezoidal motion profile. The first two elements define max translational velocity (mps) and acceleration (mps^2), the second two elements define max rotational velocity (dps) and acceleration (dps^2).
Returns: pathplan_to_path: the command to follow a path that blends into the pre-defined path