A dynamic planning controller receives a maneuver for a robot and a current state of the robot and transforms the maneuver and the current state of the robot into a nonlinear optimization problem. The nonlinear optimization problem is configured to optimize an unknown force and an unknown position vector. At a first time instance, the controller linearizes the nonlinear optimization problem into a first linear optimization problem and determines a first solution to the first linear optimization problem using quadratic programming. At a second time instance, the controller linearizes the nonlinear optimization problem into a second linear optimization problem based on the first solution at the first time instance and determines a second solution to the second linear optimization problem based on the first solution using the quadratic programming. The controller also generates a joint command to control motion of the robot during the maneuver based on the second solution.