1function [F ds rec_out] = gravity_control(t, N, dt, q, dq, t_all, x_all, cons, s_in, rec_in, Tj, Tm, Tp, dTj, dTm, dTp, M)
2
3F = zeros(9, 1);
4pm = zeros(0,1);
5vm = zeros(0,1);
6F = dTm' * (M * repmat([0; -9.81; 0], length(Tm)/3, 1));
7ds = [];
8rec_out = [];