Buggy code:
public class SwingState extends AbstractUnconstrainedState implements SwingStateInterface { private final boolean visualizeSwingTrajectory = true ; private final BooleanYoVariable replanTrajectory ; private final YoVariableDoubleProvider swingTimeRemaining ; private final PositionTrajectoryGenerator positionTrajectoryGenerator ; private final PositionTrajectoryGenerator pushRecoveryPositionTrajectoryGenerator ; private final VelocityConstrainedOrientationTrajectoryGenerator orientationTrajectoryGenerator ; private final CurrentConfigurationProvider initialConfigurationProvider ; private final VectorProvider initialVelocityProvider ; private final YoSE3ConfigurationProvider finalConfigurationProvider ; private final TrajectoryParametersProvider trajectoryParametersProvider = new TrajectoryParametersProvider ( new TrajectoryParameters ( ) ) ; private final DoubleProvider swingTimeProvider ; private final DoubleYoVariable swingTimeSpeedUpFactor ; private final DoubleYoVariable maxSwingTimeSpeedUpFactor ; private final DoubleYoVariable minSwingTimeForDisturbanceRecovery ; private final BooleanYoVariable isSwingSpeedUpEnabled ; private final DoubleYoVariable currentTime ; private final DoubleYoVariable currentTimeWithSwingSpeedUp ; private final VectorProvider currentAngularVelocityProvider ; private final FrameOrientation initialOrientation = new FrameOrientation ( ) ; private final FrameVector initialAngularVelocity = new FrameVector ( ) ; private final BooleanYoVariable hasInitialAngularConfigurationBeenProvided ; private final DoubleYoVariable finalSwingHeightOffset ; private final double controlDT ; private final ReferenceFrame footFrame ; public SwingState ( FootControlHelper footControlHelper , DoubleProvider swingTimeProvider , VectorProvider touchdownVelocityProvider , VectorProvider touchdownAccelerationProvider , YoSE3PIDGains gains , YoVariableRegistry registry ) { } private PositionTrajectoryGenerator setupPushRecoveryTrajectoryGenerator ( DoubleProvider swingTimeProvider , YoVariableRegistry registry , String namePrefix , ArrayList < PositionTrajectoryGenerator > pushRecoveryPositionTrajectoryGenerators , YoGraphicsListRegistry yoGraphicsListRegistry , PositionTrajectoryGenerator swingTrajectoryGenerator , PositionTrajectoryGenerator touchdownTrajectoryGenerator ) { } @ Override public void setInitialDesireds ( FrameOrientation initialOrientation , FrameVector initialAngularVelocity ) { hasInitialAngularConfigurationBeenProvided . set ( true ) ; <START_BUG> this . initialAngularVelocity . setIncludingFrame ( initialAngularVelocity ) ; <END_BUG> } @ Override protected void initializeTrajectory ( ) { } @ Override protected void computeAndPackTrajectory ( ) { } private final FramePose newFootstepPose = new FramePose ( ) ; private final FramePoint oldFootstepPosition = new FramePoint ( ) ; @ Override public void setFootstep ( Footstep footstep ) { } @ Override public void replanTrajectory ( Footstep newFootstep ) { } @ Override public void requestSwingSpeedUp ( double speedUpFactor ) { } @ Override public void doTransitionIntoAction ( ) { } @ Override public void doTransitionOutOfAction ( ) { } }

Fix:
orientationTrajectoryGenerator . setInitialConditions ( initialOrientation , initialAngularVelocity ) ;