This manuscript presents an algorithm for planning the divergent component of motion trajectory. In particular, the proposed approach takes into account a variable center of mass height while considering the zero moment point as a contact feasibility criterion. The algorithm is then framed in a non-linear optimization problem. Tests have been performed in the Gazebo simulation environment using the one-meter-tall iCub humanoid robot model. Results show the effectiveness of the approach in generating feasible trajectories while walking on steps.