import numpy
def f(q, q_dot, xi):
    l1, l2, l3 = q[0,0], q[1,0], q[2,0]
    l1_dot, l2_dot, l3_dot = q_dot[0,0], q_dot[1,0], q_dot[2,0]

    return 9.00239411475607e-11*l1**2 - 1.10330950130372e-10*l1*l2 - 7.07348217074767e-11*l1*l3 + 1.54854445732105e-13*l1 + 3.38058845865336e-11*l2**2 + 4.33428102083718e-11*l2*l3 - 9.49119158842984e-14*l2 + 1.38960161444059e-11*l3**2 - 6.08170432755409e-14*l3 + 6.666876091763e-17