leg_pose(:,i) =  pos + rot_mat * body_struct(:, i) - footpoint_struct(:, i);