commands.auto.pathplan_to_pose
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 PathConstraints 12from wpimath.geometry import Pose2d 13from wpimath.units import degreesToRadians 14 15 16def pathplanToPose( 17 target_pose: Callable[[], Pose2d | 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 target position 22 on the field. PathPlanner uses the navgrid.json file in the deploy folder to determine 23 field obstacles - paths generated here will avoid those obstacles. 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_pose: the goal position and orientation on the field in always-blue coordiantes 32 path_constraints: the parameters for the trapezoidal motion profile. The first two 33 elements define max translational velocity (mps) and acceleration (mps^2), 34 the second two elements define max rotational velocity (dps) and acceleration (dps^2). 35 36 Returns: 37 If a target pose is available, the command to follow a path to that pose is returned. 38 Otherwise an empty command is returned. 39 """ 40 target_pose = target_pose() 41 if target_pose: 42 path_constraints = PathConstraints( 43 *path_constraints[0:2], degreesToRadians(path_constraints[2]), degreesToRadians(path_constraints[3]) 44 ) 45 46 pathplan_to_pose = AutoBuilder.pathfindToPose( 47 target_pose, 48 path_constraints, 49 goal_end_vel=0.0 50 ) 51 52 return pathplan_to_pose 53 54 return none()
17def pathplanToPose( 18 target_pose: Callable[[], Pose2d | None], 19 path_constraints: Tuple[float] = OperatorRobotConfig.teleop_pathplan_constraints, 20) -> Command: 21 """ 22 Use PathPlanner to navigate from the robot's current position to a given target position 23 on the field. PathPlanner uses the navgrid.json file in the deploy folder to determine 24 field obstacles - paths generated here will avoid those obstacles. 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_pose: the goal position and orientation on the field in always-blue coordiantes 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 If a target pose is available, the command to follow a path to that pose is returned. 39 Otherwise an empty command is returned. 40 """ 41 target_pose = target_pose() 42 if target_pose: 43 path_constraints = PathConstraints( 44 *path_constraints[0:2], degreesToRadians(path_constraints[2]), degreesToRadians(path_constraints[3]) 45 ) 46 47 pathplan_to_pose = AutoBuilder.pathfindToPose( 48 target_pose, 49 path_constraints, 50 goal_end_vel=0.0 51 ) 52 53 return pathplan_to_pose 54 55 return none()
Use PathPlanner to navigate from the robot's current position to a given target position on the field. PathPlanner uses the navgrid.json file in the deploy folder to determine field obstacles - paths generated here will avoid those obstacles.
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_pose: the goal position and orientation on the field in always-blue coordiantes 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: If a target pose is available, the command to follow a path to that pose is returned. Otherwise an empty command is returned.