OTG5 Straight Motion Planner

The OTG5 planner for straight motion allows for linear movement while explicitly preventing curves. This is useful in situations where even slight deviations from the general straight movement direction leads to unexpected results. Currently, the OTG5 planner available in Isaac SDK (under //packages/otg5) supports differential wheel base vehicles. It accepts a global goal pose and transforms it into linear trajectories separated into four motion phases:

1. Stop the Robot, wait until it is stationary: The codelet rotate_translate_rotate_state_machine/RotateTranslateRotateStateMachine uses its thresholds_stationary parameter (Vector2d) to decide (according to the current odometry state) whether the robot is stationary. The first value is the maximum linear speed in m/s. The second value is the maximum angular speed in rad/s.
2. Rotate towards the goal location: The robot rotates towards the goal location (in the direction with the shortest turning angle) until its direction deviates by no more than the first value of the thresholds_direction_angle state machine parameter.
3. Linearly move towards the goal location: The robot linearly moves towards the goal location until its distance deviates by no more than the first value of the threshold_longitudinal_distance state machine parameter.
4. Rotate towards goal angle at goal location: The robot rotates towards the goal angle (in the direction with the shortest turning angle) until its direction deviates by no more than the first value of the thresholds_direction_angle state machine parameter.

The second values of thresholds_direction_angle and thresholds_longitudinal_angle are used to determine whether the delta between the current robot pose and the goal pose has changed enough to justify restarting the motion phase state machine.

The current implementation of the OTG5 planner allows a robot to perform the motion phases described above either in a forward direction (default) or a backward direction (by setting the drive_backwards parameter of the state machine to true).

To ensure the OTG5 codelets to work properly, the maximum and minimum velocity, as well as the maximum acceleration and jerk need to be configured. Under dual_otg5/DualOtg5 configure the parameters described in OTG5’s Component API entry.

A flatsim demo application showcasing the integration of OTG5 into an application can be found at //packages/flatsim/apps/demo_7.json. To run it, execute the following command in the bazel workspace:

            

            
bazel run //packages/flatsim/apps:flatsim -- --demo demo_7
        

Enable the parameter commander.robot_remote/isaac.navigation.RobotRemoteControl/disable_deadman_switch, which allows the OTG5 planner to control the simulated robot. The vehicle can also be controlled via the virtual gamepad available in Isaac Sight.