Trajectory Validation

The trajectory_validation package serves three purposes:

  1. Simulate the safety checks of the Programmable Logic Controller (PLC) on robots, which is useful for testing planners and controllers of the navigation stack.

  2. Avoid emergency stops on hardware, increasing productivity.

  3. Validate the primary planner and engage the fallback planner to avert evitable collisions.

The trajectory_validation package currently offers five kinds of checks, each executed by a component described here. While all checks can be used to validate trajectories, most of them can also be used to check the current state of the robot. This will be explained further in the next section.


SpeedCheck ensures the speed values of each degree of freedom (DOF) are compliant with each other. In particular, for a differential wheeled robot, depending on the speed of one wheel, the other wheel’s speed must be inside an interval defined by the valid_speed_regions parameter, which has the format shown below. Should the vehicle enter a speed region that is not deemed valid, the cross check fails.


"SpeedCheck": { "valid_speed_regions": [ [[wheel_1_min_0, wheel_1_max_0], [wheel_2_min_0, wheel_2_max_0]], [[wheel_1_min_1, wheel_1_max_1], [wheel_2_min_1, wheel_2_max_1]], [...] ] }


RangeCheck ensures that the robot does not attempt to exceed the desired kinematic limits. For example, to ensure linear speed is within a desired range, a RangeCheck can be used with the configuration below. You can use as many RangeCheck components as needed to check various aspects of the robot state.


"RangeCheck": { "type": "linear_speed", "min": -0.5, "max": 1.5 }


Upon receiving a trajectory, CollisionCheck samples states in time from the trajectory using the time_step_trajectory parameter as illustrated in the figure below. The smaller this parameter, the larger the number of samples from the input trajectory. Then, for each trajectory sample, the robot is simulated breaking with full deceleration, parameterized with deceleration_limit. For a differential wheeled robot, this parameter limits the deceleration of each wheel. The resulting brake trajectories are illustrated with red color in the figure below.


Then, for each braking trajectory, another sampling is carried out with time_step_brake. The figure below shows the sampling for the special case of zero angular speed.


For a trajectory to pass the collision check, every sample state obtained with this method needs to maintain at least distance_threshold meters distance from static and dynamic objects or obstacles in the environment.


"CollisionCheck": { "deceleration_limit": 12.5, "distance_threshold": 0.1, "obstacle_names": [ "map/isaac.navigation.DistanceMap", "map/restricted_area", "local_map" ] }


To be applicable, a trajectory needs to be close to the current state of the robot. For example, a candidate trajectory with a constant linear speed of -1.0 m/s is not applicable when the current linear speed is zero. The JSON block below shows an example configuration of the ApplicabilityCheck, which demands that the closest trajectory state to the current state of robot in time be within 0.1 seconds. It also expects the position in x to not differ more than 0.2 m for these two states.


"ApplicabilityCheck": { "threshold_time": 0.1, "threshold_pos_x": 0.2, "threshold_pos_y": 0.2, "threshold_heading": 0.2, "threshold_linear_speed": 1.0, "threshold_angular_speed": 1.0, "threshold_linear_acceleration": 1.0, "threshold_angular_acceleration": 1.0 }


To be feasible, the position, speed, and acceleration of a trajectory should be consistent. For example, a trajectory that demands constant non-zero acceleration and speed does not make sense. To check feasibility, FeasibilityCheck integrates the acceleration to check speeds, and it integrates speed to check positions and angles. Limits on the differences can be configured as shown below.


"FeasibilityCheck": { "limit_pos_x": 0.01, "limit_pos_y": 0.01, "limit_heading": 0.01, "limit_linear_speed": 1.0, "limit_angular_speed": 1.0 }

This section describes how the check components listed above can be used to check the current state or the candidate trajectory and how take necessary action.


As the figure below illustrates, the StateValidation codelet receives the current state information from the base and the desired command from the navigation stack. If the current state fails the checks that are plugged in to this codelet using the method described below, the robot is commanded to stop. Otherwise, the desired command is forwarded to the base. Thus, the StateValidation codelet can be used to replicate the PLC on robot hardware in simulation and can also be used to interfere before the PLC initiates on the real robot. Instead of restoring the status of the PLC, which can be cumbersome, you can easily program the StateValidation codelet to restart as desired; therefore, StateValidation provides a great boost in productivity.


To enable a check, place the check component on the same node as the StateValidation codelet. For example, the StateValidation codelet in the JSON block below will find that its node has a SpeedCheck component and will check current speed values it receives from the base.


{ "name": "state_validation", "components": [ { "name": "MessageLedger", "type": "isaac::alice::MessageLedger" }, { "name": "StateValidation", "type": "isaac::trajectory_validation::StateValidation" }, { "name": "SpeedCheck", "type": "isaac::trajectory_validation::SpeedCheck" } ] },


Just like the StateValidation codelet, the TrajectoryValidation codelet can also use the check components plugged in to the same node to execute validation checks, as shown by the JSON block below. However, the use cases of the two codelets are different. Instead of receiving the current state from the base and acting as an intermediate between the navigation stack and the base, TrajectoryValidation receives candidate trajectories from a planner and publishes whether the trajectory is valid as a message, which is later used by TrajectorySelection, as described in the next section.


{ "name": "trajectory_validation", "components": [ { "name": "MessageLedger", "type": "isaac::alice::MessageLedger" }, { "name": "TrajectoryValidation", "type": "isaac::trajectory_validation::TrajectoryValidation" }, { "name": "SpeedCheck", "type": "isaac::trajectory_validation::SpeedCheck" } ] },


As illustrated by the figure below, multiple TrajectoryValidation codelets can be used to evaluate different planners for redundancy. TrajectorySelection then has a simple logic: If the primary trajectory is invalid, try the alternative trajectory. If the alternative trajectory is also invalid, use an emergency command for maximum deceleration.



To exemplify the functionality of the trajectory_validation package, these components and codelets have been added to the packages/cart_delivery/apps:navigate application, which showcases how they are configured and how they can be integrated into other applications.

© Copyright 2018-2020, NVIDIA Corporation. Last updated on Feb 1, 2023.