def dot(v1, v2):
"""Vector dot product.
between UnitVector, Vector, and Dyad classes
Returns a scalar sympy expression in the case of the dot product between
two UnitVectors/Vectors. Returns a UnitVector/Vector in the case of
the dot product between a Dyad and a UnitVector/Vector.
In the scalar dot product, the operation commutes, i.e. dot(v1, v2) dot(v2,
v1). In the vector/dyad dot product, the operation is noncommutative,
i.e., dot(v1, v2) != dot(v2, v1)
"""
if isinstance(v1, (UnitVector, Vector)) and isinstance(v2, (UnitVector,
Vector)):
return v1.dot(v2)
elif isinstance(v1, Dyad) and isinstance(v2, (UnitVector, Vector)):
return v1.rdot(v2)
elif isinstance(v2, Dyad) and isinstance(v1, (UnitVector, Vector)):
return v2.ldot(v1)
else:
if not isinstance(v1, (UnitVector, Vector)):
v1 = Vector(v1)
if not isinstance(v2, (UnitVector, Vector)):
v2 = Vector(v2)
return v1.dot(v2)
def mass_center(O, points):
"""Calculate the mass center of a list of points relative to the point O.
The position of each point in the list, relative to the point O, must be
defined.
The points list can either be of the form:
[P1, P2, ..., Pn]
or
[(P1, m1), (P2, m2), ..., (Pn, mn)]
The second form is useful when you want to form the center of mass of the
system and not assign mass to individual points.
"""
assert isinstance(O, Point), "First argument must be a Point object"
mt = S(0)
cm = {}
for p in points:
if isinstance(p, Point):
pi = p.rel(O) # Position vector from O to P_i
mi = p.mass # Mass of point P_i
elif isinstance(p, tuple):
pi = p[0].rel(O)
mi = p[1]
# Compute the total mass of all the points/particles in the given list.
mt += mi
for k, v in pi.dict.items():
cm[k] = cm.get(k, S(0)) + mi*v
# Divide UnitVector coefficient by the total mass
for k in cm:
cm[k] /= mt
return Vector(cm)
def express(v, frame):
"""Expresses a vector in terms of UnitVectors fixed in a specified frame.
"""
v = Vector(v)
if (isinstance(v, UnitVector) or isinstance(v, Vector)) and \
(isinstance(frame, ReferenceFrame)):
return v.express(frame)
else:
raise TypeError('v must be UnitVector or Vector object and frame must \
be a ReferenceFrame object')
def cross(v1, v2):
"""Vector cross product.
Parameters
v1, v2: PyDy UnitVector or Vector objects.
Returns
A UnitVector or Vector object.
See Also
L{dot}, L{express}
"""
if (isinstance(v1, UnitVector) or isinstance(v1, Vector)) and \
(isinstance(v2, UnitVector) or isinstance(v2, Vector)):
return v1.cross(v2)
else:
if not (isinstance(v1, UnitVector) or isinstance(v1, Vector)):
v1 = Vector(v1)
if not (isinstance(v2, UnitVector) or isinstance(v2, Vector)):
v2 = Vector(v2)
return v1.cross(v2)
def matrixv_multiply(A, B):
"""For multplying a matrix of PyDy Vector/UnitVectors with matrices of
Sympy expressions.
Normal matrix_multiply doesn't work because PyDy vectors are not derived
from Basic."""
ma, na = A.shape
mb, nb = B.shape
if na != mb:
raise ShapeError()
product = Matrix(ma, nb, lambda i,j: 0)
for i in xrange(ma):
for j in xrange(nb):
s = Vector(0)
for k in range(na):
aik = A[i, k]
bkj = B[k, j]
if isinstance(aik, Vector):
assert not isinstance(bkj, (UnitVector, Vector))
p = {}
for uv, val in aik.dict.items():
p[uv] = bkj*val
elif isinstance(aik, UnitVector):
assert not isinstance(bkj, (UnitVector, Vector))
p = bkj*aik
elif isinstance(bkj, Vector):
assert not isinstance(aik, (UnitVector, Vector))
p = {}
for uv, val in bkj.dict.items():
p[uv] = aik*val
elif isinstance(bkj, UnitVector):
assert not isinstance(aik, (UnitVector, Vector))
p = aik*bkj
else:
raise NotImplementedError()
s += Vector(p)
product[i, j] = s
return product
def dt(v, frame):
"""Time derivative of a vector as viewed by an observer fixed in a frame.
"""
v = Vector(v)
if isinstance(frame, ReferenceFrame):
if isinstance(v, (UnitVector, Vector)):
res = v.dt(frame)
return res
else:
raise TypeError('First argument must be a Vector or \
UnitVector, instead a %s object was given' % str(type(v)))
else:
raise TypeError('Second argument must be a ReferenceFrame, \
instead a %s object was given' % str(type(v)))
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')