Calculate the velocity difference cost of a given path. Args: trajectory (Trajectory): The path to evaluate. ref_vel_list (Union[float, np.ndarray]): The reference velocity list for the path. weight_config (dict): The weight configuration for the cost calculation.
(trajectory: Trajectory, ref_vel_list: Union[float, np.ndarray],
weight_config: dict)
| 37 | |
| 38 | |
| 39 | def vel_diff(trajectory: Trajectory, ref_vel_list: Union[float, np.ndarray], |
| 40 | weight_config: dict) -> float: |
| 41 | """ |
| 42 | Calculate the velocity difference cost of a given path. |
| 43 | |
| 44 | Args: |
| 45 | trajectory (Trajectory): The path to evaluate. |
| 46 | ref_vel_list (Union[float, np.ndarray]): The reference velocity list for the path. |
| 47 | weight_config (dict): The weight configuration for the cost calculation. |
| 48 | |
| 49 | Returns: |
| 50 | float: The velocity difference cost. |
| 51 | """ |
| 52 | velocities = np.array([state.vel for state in trajectory.states]) |
| 53 | cost_vel_diff = np.linalg.norm(velocities - ref_vel_list, 2)**2 |
| 54 | return weight_config["W_VEL_DIFF"] * cost_vel_diff |
| 55 | |
| 56 | |
| 57 | def time(trajectory: Trajectory, weight_config: dict) -> float: |
nothing calls this directly
no outgoing calls
no test coverage detected