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.

github adbuerger / casiopeia / concept_tests / sd_check_quarter_vehicle_noise.py View on Github external
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))
github adbuerger / casiopeia / concept_tests / sd_check_pendulum_linear.py View on Github external
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,
github adbuerger / casiopeia / concept_tests / sd_check_quarter_vehicle.py View on Github external
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,
github helgeanl / GP-MPC / gp_casadi / mpc.py View on Github external
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)
github kul-forbes / nmpc-codegen / old_code / src_python / nmpccodegen / controller / obstacles / circular.py View on Github external
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)))
github Pyomo / pyomo / pyomo / dae / simulator.py View on Github external
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.
github stephane-caron / fip-walkgen / wpg / com_control / cop_nmpc.py View on Github external
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(
github helgeanl / GP-MPC / simulation / four_tank.py View on Github external
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_current
github helgeanl / GP-MPC / simulation / four_tank.py View on Github external
A1 = 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_current
github helgeanl / GP-MPC / GP / noisyGP.py View on Github external
T  = 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]