How to use the casadi.sqrt function in casadi
To help you get started, we’ve selected a few casadi examples, based on popular ways it is used in public projects.
Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
adbuerger / casiopeia / concept_tests / sd_check_quarter_vehicle_noise.py View on Github 
pe_test.compute_covariance_matrix()
# Generate report
print("\np_mean = " + str(ca.DM(p_mean)))
print("phat_last_exp = " + str(ca.DM(pe_test.estimated_parameters)))
print("\np_sd = " + str(ca.DM(p_std)))
print("sd_from_covmat = " + str(ca.diag(ca.sqrt(pe_test.covariance_matrix))))
print("beta = " + str(pe_test.beta))
print("\ndelta_abs_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix)))))
print("delta_rel_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix))) / ca.DM(p_std)))
fname = os.path.basename(__file__)[:-3] + ".rst"
report = open(fname, "w")
report.write( \
'''Concept test: covariance matrix computation
===========================================
Simulate system. Then: add gaussian noise N~(0, sigma^2), estimate,
store estimated parameter, repeat.
.. code-block:: python
y_randn = sim_true.simulation_results + sigma * \n
(np.random.randn(*sim_true.estimated_parameters.shape))p_mean = pl.mean(p_test)
p_std = pl.std(p_test, ddof=0)
pe_test.compute_covariance_matrix()
pe_test.print_estimation_results()
# Generate report
print("\np_mean = " + str(ca.DM(p_mean)))
print("phat_last_exp = " + str(ca.DM(pe_test.estimated_parameters)))
print("\np_sd = " + str(ca.DM(p_std)))
print("sd_from_covmat = " + str(ca.diag(ca.sqrt(pe_test.covariance_matrix))))
print("beta = " + str(pe_test.beta))
print("\ndelta_abs_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix)))))
print("delta_rel_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix))) / ca.DM(p_std)))
fname = os.path.basename(__file__)[:-3] + ".rst"
report = open(fname, "w")
report.write( \
'''Concept test: covariance matrix computation
===========================================
Simulate system. Then: add gaussian noise N~(0, sigma^2), estimate,p_std = []
for j, e in enumerate(p_true):
p_mean.append(pl.mean([k[j] for k in p_test]))
p_std.append(pl.std([k[j] for k in p_test], ddof = 0))
pe_test.compute_covariance_matrix()
# Generate report
print("\np_mean = " + str(ca.DM(p_mean)))
print("phat_last_exp = " + str(ca.DM(pe_test.estimated_parameters)))
print("\np_sd = " + str(ca.DM(p_std)))
print("sd_from_covmat = " + str(ca.diag(ca.sqrt(pe_test.covariance_matrix))))
print("beta = " + str(pe_test.beta))
print("\ndelta_abs_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix)))))
print("delta_rel_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix))) / ca.DM(p_std)))
fname = os.path.basename(__file__)[:-3] + ".rst"
report = open(fname, "w")
report.write( \
'''Concept test: covariance matrix computation
===========================================
Simulate system. Then: add gaussian noise N~(0, sigma^2), estimate,def cost_saturation_lf(x, x_ref, covar_x, P):
""" Terminal Cost function: Expected Value of Saturating Cost
"""
Nx = ca.MX.size1(P)
# Create symbols
P_s = ca.SX.sym('P', Nx, Nx)
x_s = ca.SX.sym('x', Nx)
covar_x_s = ca.SX.sym('covar_z', Nx, Nx)
Z_x = ca.SX.eye(Nx) #+ 2 * covar_x_s @ P_s
cost_x = ca.Function('cost_x', [x_s, P_s, covar_x_s],
[1 - ca.exp(-(x_s.T @ ca.solve(Z_x.T, P_s.T).T @ x_s))
/ ca.sqrt(ca.det(Z_x))])
return cost_x(x - x_ref, P, covar_x)def evaluate_coordinate_state_cost(self,coordinates_state):
""" check how far coordinates are from center, if inside radius punish """
return Obstacle.trim_and_square(self._radius - cd.sqrt(cd.sum1((self._center_coordinates-coordinates_state)**2)))try:
import casadi
casadi_intrinsic = {
'log': casadi.log,
'log10': casadi.log10,
'sin': casadi.sin,
'cos': casadi.cos,
'tan': casadi.tan,
'cosh': casadi.cosh,
'sinh': casadi.sinh,
'tanh': casadi.tanh,
'asin': casadi.asin,
'acos': casadi.acos,
'atan': casadi.atan,
'exp': casadi.exp,
'sqrt': casadi.sqrt,
'asinh': casadi.asinh,
'acosh': casadi.acosh,
'atanh': casadi.atanh,
'ceil': casadi.ceil,
'floor': casadi.floor}
except ImportError:
casadi_available = False
def _check_getitemexpression(expr, i):
"""
Accepts an equality expression and an index value. Checks the
GetItemExpression at expr.arg(i) to see if it is a
:py:class:`DerivativeVar`. If so, return the
GetItemExpression for the :py:class:`DerivativeVar` and
the RHS. If not, return None.z_k = contact.p + alpha_k * contact.X * contact.t + \
beta_k * contact.Y * contact.b
self.nlp.add_equality_constraint(
u_k, lambda_k * (p_k - z_k) + gravity)
self.nlp.extend_cost(
self.weights['alpha'] * alpha_k * alpha_k * dT_k)
self.nlp.extend_cost(
self.weights['beta'] * beta_k * beta_k * dT_k)
# self.nlp.extend_cost(
# self.weights['lambda'] * lambda_k ** 2 * dT_k)
self.nlp.extend_cost(
self.weights['u'] * casadi.dot(u_k, u_k) * dT_k)
# Full precision (no Taylor expansion)
omega_k = casadi.sqrt(lambda_k)
p_next = p_k \
+ v_k / omega_k * casadi.sinh(omega_k * dT_k) \
+ u_k / lambda_k * (cosh(omega_k * dT_k) - 1.)
v_next = v_k * cosh(omega_k * dT_k) \
+ u_k / omega_k * sinh(omega_k * dT_k)
T_total = T_total + dT_k
if 2 * k < nb_steps:
T_swing = T_swing + dT_k
self.add_friction_constraint(contact, p_k, z_k)
self.add_friction_constraint(contact, p_next, z_k)
# slower:
# add_linear_friction_constraints(contact, p_k, z_k)
# add_linear_friction_constraints(contact, p_next, z_k)
p_k = self.nlp.new_variable(A4 = 28.27
gamma1 = 0.4
gamma2 = 0.4
# Differential states
xd = ca.SX.sym("xd", ndstate) # vector of states [h1,h2,h3,h4]
# Initial conditions
xDi = x0
ode = ca.vertcat(
-a1 / A1 * ca.sqrt(2 * g * xd[0]) + a3 / A1
* ca.sqrt(2 * g * xd[2]) + gamma1 / A1 * u[0],
-a2 / A2 * ca.sqrt(2 * g * xd[1]) + a4 / A2
* ca.sqrt(2 * g * xd[3]) + gamma2 / A2 * u[1],
-a3 / A3 * ca.sqrt(2 * g * xd[2]) + (1 - gamma2) / A3 * u[1],
-a4 / A4 * ca.sqrt(2 * g * xd[3]) + (1 - gamma1) / A4 * u[0])
dae = {'x': xd, 'ode': ode}
# Create a DAE system solver
opts = {}
opts['abstol'] = 1e-10 # abs. tolerance
opts['reltol'] = 1e-10 # rel. tolerance
opts['t0'] = t0
opts['tf'] = tf
Sim = ca.integrator('Sim', 'idas', dae, opts)
res = Sim(x0=xDi)
x_current = pylab.array(res['xf'])
return x_currentA1 = 50.27
A2 = 50.27
A3 = 28.27
A4 = 28.27
gamma1 = 0.4
gamma2 = 0.4
# Differential states
xd = ca.SX.sym("xd", ndstate) # vector of states [h1,h2,h3,h4]
# Initial conditions
xDi = x0
ode = ca.vertcat(
-a1 / A1 * ca.sqrt(2 * g * xd[0]) + a3 / A1 * ca.sqrt(2 * g * xd[2]) + gamma1 / A1 * u[0],
-a2 / A2 * ca.sqrt(2 * g * xd[1]) + a4 / A2 * ca.sqrt(2 * g * xd[3]) + gamma2 / A2 * u[1],
-a3 / A3 * ca.sqrt(2 * g * xd[2]) + (1 - gamma2) / A3 * u[1],
-a4 / A4 * ca.sqrt(2 * g * xd[3]) + (1 - gamma1) / A4 * u[0])
dae = {'x': xd, 'ode': ode}
# Create a DAE system solver
opts = {}
opts['abstol'] = 1e-10 # abs. tolerance
opts['reltol'] = 1e-10 # rel. tolerance
opts['t0'] = t0
opts['tf'] = tf
Sim = ca.integrator('Sim', 'idas', dae, opts)
res = Sim(x0=xDi)
x_current = pylab.array(res['xf'])
return x_currentT = ca.mtimes(v, iR)
c = ca.exp(2 * hyper[a, D]) / ca.sqrt(determinant(R)) * ca.exp(ca.sum2(hyper[a, :D]))
q2 = c * ca.exp(-ca.sum2(T * v) * 0.5)
qb = q2 * beta[:, a]
mean[a] = ca.sum1(qb)
t = ca.repmat(ca.exp(hyper[a, :D]), n, 1)
v1 = v / t
log_k[:, a] = 2 * hyper[a, D] - ca.sum2(v1 * v1) * 0.5
# covariance with noisy input
for a in range(E):
ii = v / ca.repmat(ca.exp(2 * hyper[a, :D]), n, 1)
for b in range(a + 1):
R = ca.mtimes(inputcov, ca.diag(ca.exp(-2 * hyper[a, :D]) + ca.exp(-2 * hyper[b, :D]))) + ca.MX.eye(D)
t = 1.0 / ca.sqrt(determinant(R))
ij = v / ca.repmat(ca.exp(2 * hyper[b, :D]), n, 1)
Q = ca.exp(ca.repmat(log_k[:, a], 1, n) + ca.repmat(ca.transpose(log_k[:, b]), n, 1) + maha(ii, -ij, ca.solve(R, inputcov * 0.5), n))
A = ca.mtimes(beta[:, a], ca.transpose(beta[:, b]))
if b == a:
A = A - invK[a]
A = A * Q
covariance[a, b] = t * ca.sum2(ca.sum1(A))
covariance[b, a] = covariance[a, b]
covariance[a, a] = covariance[a, a] + ca.exp(2 * hyper[a, D])
covariance = covariance - ca.mtimes(mean, ca.transpose(mean))
return [mean, covariance]casadi
CasADi -- framework for algorithmic differentiation and numeric optimization
