g:1,
I:1,
J:1,
}
r = a.subs(subs_dict)
state = {
q4: pi/6,
u3: 1,
u4: 0,
u5: 2,
}
r = r.subs(state)
return r
N = ReferenceFrame('N')
A = N.rotate("A", 3, q3)
B = A.rotate("B", 1, q4)
C = B.rotate("C", 2, q5)
#u3 = q3.diff(t)
u3 = Function("u3")(t)
u4 = Function("u4")(t)
u5 = Function("u5")(t)
#u3p = Function("u3p")(t)
#u4p = Function("u4p")(t)
#u5p = Function("u5p")(t)
P_NC_CO = r2*A[3] - r1*B[3]
#print "P_NC_CO> = ", P_NC_CO
P_NO_CO = q1*N[1] + q2*N[2] + P_NC_CO
def generate_kf_module():
# Lean, Pitch, Steer
l, p, s = symbols('l p s')
# Frame geometric parameters
rr, rrt, rf, rft, lr, lf, ls = symbols('rr rrt rf rft lr lf ls')
A = ReferenceFrame('A')
B = A.rotate('B', 1, l)
D = B.rotate('D', 2, p)
E = D.rotate('E', 3, s)
# Vector in the plane of the front wheel, pointed towards the ground
g = Vector(A[3] - dot(E[2], A[3]) * E[2]).normalized
# Holonomic constraint
f0 = dot(
A[3], -rrt * A[3] - rr * B[3] + lr * D[1] + ls * D[3] + lf * E[1] +
rf * g + rft * A[3])
f1 = f0.diff(p)
# Vector valued function
F = Matrix([f0, f1])
X = Matrix([l, p])
J = F.jacobian(X)
# Generate string representations of each function
f0_s = str(f0).replace('sin', 'np.sin').replace('cos', 'np.cos')
f1_s = str(f1).replace('sin', 'np.sin').replace('cos', 'np.cos')
J00_s = str(J[0, 0]).replace('sin', 'np.sin').replace('cos', 'np.cos')
J01_s = str(J[0, 1]).replace('sin', 'np.sin').replace('cos', 'np.cos')
J10_s = str(J[1, 0]).replace('sin', 'np.sin').replace('cos', 'np.cos')
J11_s = str(J[1, 1]).replace('sin', 'np.sin').replace('cos', 'np.cos')
# Decide on what type of float to use here
dtype = "float64"
fh = open('kinematic_feasibility.pyx', 'w')
f_s = "from __future__ import division\n"
f_s += "import numpy as np\n"
f_s += "cimport numpy as np\n"
f_s += "DTYPE = np." + dtype + "\n"
f_s += "ctypedef np." + dtype + "_t DTYPE_t\n"
f_s += "cimport cython\n"
f_s += "@cython.boundscheck(False)\n"
f_s += "def f(np.ndarray[DTYPE_t, ndim=1] x, np.ndarray[DTYPE_t, ndim=1] params):\n"
f_s += ''' """Computes the holonomic constraint and its partial derivative
with respect to pitch.
x: Numpy array of lean and steer, length 2
params: Numpy array of parameters, length 8
in the following order:
rr: Rear wheel radius.
rrt: Rear wheel tire radius.
rf: Front wheel radius.
rft: Front wheel tire radius.
lr: Rear wheel center perpendicular distance from steer axis.
lf: Front wheel center perpendicular distance from steer axis.
ls: Steer axis offset.
s: Steer angle. (treated as a parameter)
Returns a numpy array of the value of the holonomic constraint
in the first entry, and the partial derivative of the holonomic
constraint with respect to pitch in the second entry. The
zeros of this function occur on the boundary of the
kinematically feasibly region in the lean/steer plane.
"""'''
f_s += " # Generated " + time.asctime() + "\n"
f_s += " cdef np.float64_t l = x[0]\n"
f_s += " cdef np.float64_t p = x[1]\n"
f_s += " cdef np.float64_t rr = params[0]\n"
f_s += " cdef np.float64_t rrt = params[1]\n"
f_s += " cdef np.float64_t rf = params[2]\n"
f_s += " cdef np.float64_t rft = params[3]\n"
f_s += " cdef np.float64_t lr = params[4]\n"
f_s += " cdef np.float64_t lf = params[5]\n"
f_s += " cdef np.float64_t ls = params[6]\n"
f_s += " cdef np.float64_t s = params[7]\n"
f_s += " cdef np.ndarray[DTYPE_t, ndim=1] F = np.zeros([2], dtype=DTYPE)\n"
f_s += " F[0] = " + f0_s + "\n"
f_s += " F[1] = " + f1_s + "\n"
f_s += " return F\n\n\n"
f_s += "@cython.boundscheck(False)\n"
f_s += "def df(np.ndarray[DTYPE_t, ndim=1] x, np.ndarray[DTYPE_t, ndim=1] params):\n"
f_s += ''' """Evaluates holonomic constraint and its partial derivative
with respect to pitch, with the steer angle treated as parameter.
x: Numpy array of lean and steer, length 2
params: Numpy array of parameters, length 8
in the following order:
rr: Rear wheel radius.
rrt: Rear wheel tire radius.
rf: Front wheel radius.
rft: Front wheel tire radius.
lr: Rear wheel center perpendicular distance from steer axis.
lf: Front wheel center perpendicular distance from steer axis.
ls: Steer axis offset.
s: Steer angle
"""'''
f_s += " # Generated " + time.asctime() + "\n"
f_s += " cdef np.float64_t l = x[0]\n"
f_s += " cdef np.float64_t p = x[1]\n"
f_s += " cdef np.float64_t rr = params[0]\n"
f_s += " cdef np.float64_t rrt = params[1]\n"
f_s += " cdef np.float64_t rf = params[2]\n"
f_s += " cdef np.float64_t rft = params[3]\n"
f_s += " cdef np.float64_t lr = params[4]\n"
f_s += " cdef np.float64_t lf = params[5]\n"
f_s += " cdef np.float64_t ls = params[6]\n"
f_s += " cdef np.float64_t s = params[7]\n"
f_s += " cdef np.ndarray[DTYPE_t, ndim=2] dF = np.zeros([2,2], dtype=DTYPE)\n"
f_s += " dF[0, 0] = " + J00_s + "\n"
f_s += " dF[0, 1] = " + J01_s + "\n"
f_s += " dF[1, 0] = " + J10_s + "\n"
f_s += " dF[1, 1] = " + J11_s + "\n"
f_s += " return dF\n"
f_s += "@cython.boundscheck(False)\n"
f_s += "def convert_geometric(np.ndarray[DTYPE_t, ndim=1] x):\n"
f_s += " cdef np.ndarray[DTYPE_t, ndim=1] lengths = np.zeros([3], dtype=DTYPE)\n"
f_s += " cdef np.float64_t w = x[0]\n"
f_s += " cdef np.float64_t c = x[1]\n"
f_s += " cdef np.float64_t lmbda = x[2]\n"
f_s += " cdef np.float64_t rr = x[3]\n"
f_s += " cdef np.float64_t rrt = x[4]\n"
f_s += " cdef np.float64_t rf = x[5]\n"
f_s += " cdef np.float64_t rft = x[6]\n"
f_s += " lengths[0] = (w+c)*np.cos(lmbda)-(rr+rrt)*np.sin(lmbda)\n"
f_s += " lengths[1] = (rf+rft)*np.sin(lmbda)-c*np.cos(lmbda)\n"
f_s += " lengths[2] = w*np.sin(lmbda) + (rr+rrt-rf-rft)*np.cos(lmbda)\n"
f_s += " return lengths\n"
fh.write(f_s)
fh.close()
cythonit('kinematic_feasibility')
compileit('kinematic_feasibility.c', 'kinematic_feasibility.so')
def generate_kf_module():
# Lean, Pitch, Steer
l, p, s = symbols("l p s")
# Frame geometric parameters
rr, rrt, rf, rft, lr, lf, ls = symbols("rr rrt rf rft lr lf ls")
A = ReferenceFrame("A")
B = A.rotate("B", 1, l)
D = B.rotate("D", 2, p)
E = D.rotate("E", 3, s)
# Vector in the plane of the front wheel, pointed towards the ground
g = Vector(A[3] - dot(E[2], A[3]) * E[2]).normalized
# Holonomic constraint
f0 = dot(A[3], -rrt * A[3] - rr * B[3] + lr * D[1] + ls * D[3] + lf * E[1] + rf * g + rft * A[3])
f1 = f0.diff(p)
# Vector valued function
F = Matrix([f0, f1])
X = Matrix([l, p])
J = F.jacobian(X)
# Generate string representations of each function
f0_s = str(f0).replace("sin", "np.sin").replace("cos", "np.cos")
f1_s = str(f1).replace("sin", "np.sin").replace("cos", "np.cos")
J00_s = str(J[0, 0]).replace("sin", "np.sin").replace("cos", "np.cos")
J01_s = str(J[0, 1]).replace("sin", "np.sin").replace("cos", "np.cos")
J10_s = str(J[1, 0]).replace("sin", "np.sin").replace("cos", "np.cos")
J11_s = str(J[1, 1]).replace("sin", "np.sin").replace("cos", "np.cos")
# Decide on what type of float to use here
dtype = "float64"
fh = open("kinematic_feasibility.pyx", "w")
f_s = "from __future__ import division\n"
f_s += "import numpy as np\n"
f_s += "cimport numpy as np\n"
f_s += "DTYPE = np." + dtype + "\n"
f_s += "ctypedef np." + dtype + "_t DTYPE_t\n"
f_s += "cimport cython\n"
f_s += "@cython.boundscheck(False)\n"
f_s += "def f(np.ndarray[DTYPE_t, ndim=1] x, np.ndarray[DTYPE_t, ndim=1] params):\n"
f_s += ''' """Computes the holonomic constraint and its partial derivative
with respect to pitch.
x: Numpy array of lean and steer, length 2
params: Numpy array of parameters, length 8
in the following order:
rr: Rear wheel radius.
rrt: Rear wheel tire radius.
rf: Front wheel radius.
rft: Front wheel tire radius.
lr: Rear wheel center perpendicular distance from steer axis.
lf: Front wheel center perpendicular distance from steer axis.
ls: Steer axis offset.
s: Steer angle. (treated as a parameter)
Returns a numpy array of the value of the holonomic constraint
in the first entry, and the partial derivative of the holonomic
constraint with respect to pitch in the second entry. The
zeros of this function occur on the boundary of the
kinematically feasibly region in the lean/steer plane.
"""'''
f_s += " # Generated " + time.asctime() + "\n"
f_s += " cdef np.float64_t l = x[0]\n"
f_s += " cdef np.float64_t p = x[1]\n"
f_s += " cdef np.float64_t rr = params[0]\n"
f_s += " cdef np.float64_t rrt = params[1]\n"
f_s += " cdef np.float64_t rf = params[2]\n"
f_s += " cdef np.float64_t rft = params[3]\n"
f_s += " cdef np.float64_t lr = params[4]\n"
f_s += " cdef np.float64_t lf = params[5]\n"
f_s += " cdef np.float64_t ls = params[6]\n"
f_s += " cdef np.float64_t s = params[7]\n"
f_s += " cdef np.ndarray[DTYPE_t, ndim=1] F = np.zeros([2], dtype=DTYPE)\n"
f_s += " F[0] = " + f0_s + "\n"
f_s += " F[1] = " + f1_s + "\n"
f_s += " return F\n\n\n"
f_s += "@cython.boundscheck(False)\n"
f_s += "def df(np.ndarray[DTYPE_t, ndim=1] x, np.ndarray[DTYPE_t, ndim=1] params):\n"
f_s += ''' """Evaluates holonomic constraint and its partial derivative
with respect to pitch, with the steer angle treated as parameter.
x: Numpy array of lean and steer, length 2
params: Numpy array of parameters, length 8
in the following order:
rr: Rear wheel radius.
rrt: Rear wheel tire radius.
rf: Front wheel radius.
rft: Front wheel tire radius.
lr: Rear wheel center perpendicular distance from steer axis.
lf: Front wheel center perpendicular distance from steer axis.
ls: Steer axis offset.
s: Steer angle
"""'''
f_s += " # Generated " + time.asctime() + "\n"
f_s += " cdef np.float64_t l = x[0]\n"
f_s += " cdef np.float64_t p = x[1]\n"
f_s += " cdef np.float64_t rr = params[0]\n"
f_s += " cdef np.float64_t rrt = params[1]\n"
f_s += " cdef np.float64_t rf = params[2]\n"
f_s += " cdef np.float64_t rft = params[3]\n"
f_s += " cdef np.float64_t lr = params[4]\n"
f_s += " cdef np.float64_t lf = params[5]\n"
f_s += " cdef np.float64_t ls = params[6]\n"
f_s += " cdef np.float64_t s = params[7]\n"
f_s += " cdef np.ndarray[DTYPE_t, ndim=2] dF = np.zeros([2,2], dtype=DTYPE)\n"
f_s += " dF[0, 0] = " + J00_s + "\n"
f_s += " dF[0, 1] = " + J01_s + "\n"
f_s += " dF[1, 0] = " + J10_s + "\n"
f_s += " dF[1, 1] = " + J11_s + "\n"
f_s += " return dF\n"
f_s += "@cython.boundscheck(False)\n"
f_s += "def convert_geometric(np.ndarray[DTYPE_t, ndim=1] x):\n"
f_s += " cdef np.ndarray[DTYPE_t, ndim=1] lengths = np.zeros([3], dtype=DTYPE)\n"
f_s += " cdef np.float64_t w = x[0]\n"
f_s += " cdef np.float64_t c = x[1]\n"
f_s += " cdef np.float64_t lmbda = x[2]\n"
f_s += " cdef np.float64_t rr = x[3]\n"
f_s += " cdef np.float64_t rrt = x[4]\n"
f_s += " cdef np.float64_t rf = x[5]\n"
f_s += " cdef np.float64_t rft = x[6]\n"
f_s += " lengths[0] = (w+c)*np.cos(lmbda)-(rr+rrt)*np.sin(lmbda)\n"
f_s += " lengths[1] = (rf+rft)*np.sin(lmbda)-c*np.cos(lmbda)\n"
f_s += " lengths[2] = w*np.sin(lmbda) + (rr+rrt-rf-rft)*np.cos(lmbda)\n"
f_s += " return lengths\n"
fh.write(f_s)
fh.close()
cythonit("kinematic_feasibility")
compileit("kinematic_feasibility.c", "kinematic_feasibility.so")