def solve(nelem, h, deg, alpha, beta, gamma, k=None, b=None, c=None, f=None):
#
# Cast polynomial degree to enums
deg = POLYNOMIAL(deg)
#
# Check inputs make sense
assert (nelem > 0)
assert (nelem == len(h))
assert (deg in [POLYNOMIAL.LINEAR, POLYNOMIAL.QUADRATIC])
assert (len(alpha) == 2)
assert (len(beta) == 2)
assert (len(gamma) == 2)
#
# Make sure functions are callable on ndarrays
#
# NOTE: functions passed as None will be treated
# as a constant equal to zero.
x_test = linspace(0, 1, 3)
callable_functions = [k, b, c, f]
for fun in callable_functions:
if fun is not None:
try:
fun(x_test)
except:
try:
fun = lambda x: float(fun)
except:
raise (Exception(
"One of the functions k, b, c, f is neither callable on ndarrays nor a number. Aborting!"
))
else:
fun = lambda x: 0.
#
# Assemble the system
K = construct_stiffness()
F = construct_load()
#
# Solve the system
u = np_solve(K, F)
#
# Solve auxiliary equations
if False:
print("Hello")
#
return u
def calculate_motion(self, speeds):
"""
Invert the motion to speed calculation to obtain the actual linear and angular velocity of the chassis given
a vector of wheel speeds. See http://docs.scipy.org/doc/numpy-1.10.1/reference/generated/numpy.linalg.solve.html
:param speeds:
An array of wheel speeds, expressed as floats with units of radians per second, positive being towards
the wheel vector.
:return:
A :class:`triangula.chassis.Motion` object containing the calculated translation and rotation in the robot's
coordinate space.
"""
motion_array = np_solve(self._matrix_coefficients, np_array(speeds))
return Motion(Vector2(x=float(motion_array[0]),
y=float(motion_array[1])),
rotation=float(motion_array[2]))
def calculate_motion(self, speeds):
"""
Invert the motion to speed calculation to obtain the actual linear and angular velocity of the chassis given
a vector of wheel speeds. See http://docs.scipy.org/doc/numpy-1.10.1/reference/generated/numpy.linalg.solve.html
:param speeds:
An array of wheel speeds, expressed as floats with units of radians per second, positive being towards
the wheel vector.
:return:
A :class:`triangula.chassis.Motion` object containing the calculated translation and rotation in the robot's
coordinate space.
"""
motion_array = np_solve(self._matrix_coefficients, np_array(speeds))
return Motion(Vector2(x=float(motion_array[0]),
y=float(motion_array[1])),
rotation=float(motion_array[2]))
def solve(self, A, rhs, eps, tau=None, u0=None, verb=False):
'''
Solve linear system A u = rhs with initial guess u0 and tolerance eps
by appropriate method:
MODE_NP: np.linalg.lstsq
MODE_TT: tt.amen.amen_solve
MODE_SP: scipy.sparse.linalg.spsolve
If u0 is None, then it is set to rhs value.
If tau is not given, then tau is set to real accuracy of solver output.
The obtained solution is rounded to accuracy tau.
'''
u = rhs.copy(copy_x=False)
u.name = 'Lin. syst. solution'
if A.mode == MODE_NP:
self.solver = 'lstsq'
u.x = np_solve(A.x, rhs.x)[0]
elif A.mode == MODE_TT:
if u0 is None:
u0 = rhs.copy()
self.solver = 'amen'
self.start_capture()
u.x = tt_solve(A.x,
rhs.x,
u0.x,
eps,
nswp=self.nswp,
kickrank=self.kickrank,
local_prec=self.local_prec,
local_iters=self.local_iters,
local_restart=self.local_restart,
trunc_norm=self.trunc_norm,
max_full_size=self.max_full_size)
self.stop_capture()
if tau is None:
tau = self.max_res
u = u.round(tau)
self.present(verb)
elif A.mode == MODE_SP:
self.solver = 'spsolve'
u.x = sp_solve(A.x, rhs.x)
else:
self.solver = None
raise ValueError('Incorect mode of the input.')
return u
def equilibrium_pts(self):
#Finds equlibrium points as [M,N,Z]
from numpy import array as np_array
from numpy.linalg import solve as np_solve
g = self.k2 * (self.r2 - self.d2) / self.r2
A = np_array([[self.r1/self.k1,self.a1,0], [-self.a2,0,self.b],\
[0,self.b,self.r2/self.k2]])
c = np_array([self.r1, self.d1, self.r2 - self.d2])
self.E0,self.E1,self.E2,self.E3 = [3*[0],[self.k1,0,0],[0,0,g],\
[self.k1,0,g]]
self.E4 = [0,(self.b*self.r2*g-self.r2*self.d1)/(self.k2*self.b**2),\
self.d1/self.b]
self.E5 = np_solve(A, c).tolist()
b = vectorize(b)
c = lambda x: 0.
c = vectorize(c)
f = lambda x: -6 - 4 * x
f = vectorize(f)
K, K0, KN = construct_stiffness(elements, polynomial_order, alpha, beta, gamma,
k, b, c)
savetxt("stiffness_quad.csv", K, delimiter=" & ", fmt="%.2f")
F = construct_load(elements, polynomial_order, alpha, beta, gamma, K0, KN, f,
k)
savetxt("load_quad.csv", F, delimiter=" \\\\ ", fmt="%.2f")
x = linspace(0, 1, 100)
t = lambda x: (1 + x)**2
t = vectorize(t)
y = t(x)
# print("COND: ", cond(K))
xx = linspace(0, 1, nelems * polynomial_order + 1)
uu = np_solve(K, F).flatten()
u = set_essential_boundary_conditions(alpha, gamma, uu)
plt.cla()
plt.plot(x, y, 'k-', label='True Sol.')
plt.plot(xx, u, 'ro', label='Numerical Sol.')
plt.legend()
plt.show()