moved controller into add_force() loop instead of cal_fwd_dyn() loop