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

SpeedCheck ensures the linear and angular speed values are compliant with each other. The constraints are defined by the valid_speed_regions parameter, which has the format shown below. Should the vehicle leave this polygon, the speed check fails.

            

            
"SpeedCheck": {
  "valid_speed_region": [
    [linear_speed_0, angular_speed_0], [linear_speed_1, angular_speed_1],
    [linear_speed_2, angular_speed_2], [linear_speed_3, angular_speed_3],
    [...]
  ]
}
        

RangeCheck

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
}
        

CollisionCheck

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"
  ]
}
        

ApplicabilityCheck

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
}
        

FeasibilityCheck

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.

StateValidation

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"
    }
  ]
},
        

TrajectoryValidation

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"
    }
  ]
},
        

TrajectorySelection

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 the emergency plan, which may be maximum deceleration.

The trajectory_validation package is used in the differential base navigation subgraph (packages/navigation/apps/differential_base_control.subgraph.json). The TrajectoryValidation and TrajectorySelection codelets prevent trajectories that violate any of the five checks from being executed. StateValidation prevents the robot from moving for at least 2 seconds if the collision or speed checks fail. The duration before restarting a failed StateValidation is determined by the wait_duration parameter of the state_validation_behavior node. The checks can be configured as shown in packages/cart_delivery/apps/navigate.app.json.

For various reasons, the robot may find itself in a pose that is already too close to an obstacle. In this case, the robot will be stuck since StateValidation will prevent it from moving and TrajectoryValidation will deem any trajectory candidate invalid. To get the robot out of such a situation, there are two options:

1. Control the robot manually to get it out of the situation.
2. Temporarily disable CollisionCheck for both state and trajectory validations using the disable_check parameter.

To disable all the checks attached to StateValidation and/or TrajectoryValidation, use the disable_checks parameter.