How to use the casadi.transpose 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 casadi / casadi / test / python / typemaps.py View on Github external
def test_0(self):
    self.message("Typemap array -> DM")
    arrays = [array([[1,2],[3,4],[5,6]],dtype=double),array([[3.2,4.6,9.9]])]
    for i in range(len(arrays)):
      m = arrays[i]
      zt=c.transpose(c.transpose(m))
      self.assertTrue(isinstance(zt,DM),"DM expected")
      self.checkarray(m,zt,"DM(numpy.ndarray)")
      self.checkarray(m,zt.full(),"DM(numpy.ndarray).full()")
      if scipy_available:
        self.checkarray(m,zt.sparse(),"DM(numpy.ndarray).sparse()")
github casadi / casadi / test / python / typemaps.py View on Github external
def test_0a(self):
    self.message("Typemap array -> IM")
    arrays = [array([[1,2,3],[4,5,6]],dtype=int32),array([[1,2,3],[4,5,6]]),array([[1,2,3],[4,5,6]],dtype=int)]
    for i in range(len(arrays)):
      m = arrays[i]
      zt=c.transpose(c.transpose(m))
      self.assertTrue(isinstance(zt,IM),"IM expected")
      self.checkarray(m,zt,"IM(numpy.ndarray)")
      self.checkarray(m,zt.full(),"IM(numpy.ndarray).full()")
      #if scipy_available:
github uuvsimulator / uuv_simulator / uuv_control / uuv_trajectory_control / src / uuv_control_interfaces / sym_vehicle.py View on Github external
self.DMatrix = - casadi.diag(self._linear_damping)        
            self.DMatrix -= casadi.diag(self._linear_damping_forward_speed)
            self.DMatrix -= casadi.diag(self._quad_damping * self.nu)      

            # Build the restoring forces vectors wrt the BODY frame
            Rx = np.array([[1, 0, 0],
                        [0, casadi.cos(self.eta[3]), -1 * casadi.sin(self.eta[3])],
                        [0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])
            Ry = np.array([[casadi.cos(self.eta[4]), 0, casadi.sin(self.eta[4])],
                        [0, 1, 0],
                        [-1 * casadi.sin(self.eta[4]), 0, casadi.cos(self.eta[4])]])
            Rz = np.array([[casadi.cos(self.eta[5]), -1 * casadi.sin(self.eta[5]), 0],
                        [casadi.sin(self.eta[5]), casadi.cos(self.eta[5]), 0],
                        [0, 0, 1]])

            R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx)))

            if inertial_frame_id == 'world_ned':
                Fg = casadi.SX([0, 0, -self.mass * self.gravity])
                Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density])
            else:
                Fg = casadi.SX([0, 0, self.mass * self.gravity])
                Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density])

            self.gVec = casadi.SX.zeros(6)

            self.gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb)  
            self.gVec[3:6] = -1 * casadi.mtimes(
                R_n_to_b, casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb))
            
            # Build Jacobian
            T = 1 / casadi.cos(self.eta[4]) * np.array(
github helgeanl / GP-MPC / gp_mpc / gp_functions.py View on Github external
t  = ca.repmat(ca.exp(hyper[a, :Nx]), N, 1)
        v1 = v / t
        log_k[:, a] = 2 * hyper[a, Nx] - ca.sum2(v1 * v1) * 0.5

    # covariance with noisy input
    for a in range(Ny):
        ii = v / ca.repmat(ca.exp(2 * hyper[a, :Nx]), N, 1)
        for b in range(a + 1):
            R = ca.mtimes(inputcov, ca.diag(ca.exp(-2 * hyper[a, :Nx])
                + ca.exp(-2 * hyper[b, :Nx]))) + ca.MX.eye(Nx)
            t = 1.0 / ca.sqrt(determinant(R))
            ij = v / ca.repmat(ca.exp(2 * hyper[b, :Nx]), 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, Nx])
    covariance = covariance - ca.mtimes(mean, ca.transpose(mean))

    return [mean, covariance]
github helgeanl / GP-MPC / GP / prediction.py View on Github external
kss = sf2
        ks = ca.MX.zeros(n, 1)

        for i in range(n):
            ks[i] = covSEard2(X[i, :], inputmean, ell, sf2)

        invKks = ca.mtimes(iK, ks)
        mean[a] = ca.mtimes(ks.T, alpha)
        var[a] = kss - ca.mtimes(ks.T, invKks)
        d_mean[a] = ca.mtimes(ca.transpose(w[a] * v[:, a] * ks), alpha)

    for d in range(E):
        for e in range(E):
            dd_var1a = ca.mtimes(ca.transpose(v[:, d] * ks), iK)
            dd_var1b = ca.mtimes(dd_var1a, v[e] * ks)
            dd_var2 = ca.mtimes(ca.transpose(v[d] * v[e] * ks), invKks)
            dd_var[d, e] = -2 * w[d] * w[e] * (dd_var1b + dd_var2)
            if d == e:
                dd_var[d, e] = dd_var[d, e] + 2 * w[d] * (kss - var[d])

    # covariance with noisy input
    for a in range(E):
        covar1 = ca.mtimes(d_mean, d_mean.T)
        covar[0, 0] = inputcov[a]
        covariance[a, a] = var[a] + ca.trace(ca.mtimes(covar, .5 * dd_var + covar1))
        #covariance[a] = var[a] + ca.trace(ca.mtimes(inputcov, .5 * dd_var + covar1))
    return [mean, covariance]
github helgeanl / GP-MPC / GP / noisyGP.py View on Github external
def maha(a1, b1, Q1, n):
    aQ = ca.mtimes(a1, Q1)
    bQ = ca.mtimes(b1, Q1)
    K1  = ca.repmat(ca.sum2(aQ * a1), 1, n) + ca.repmat(ca.transpose(ca.sum2(bQ * b1)), n, 1) - 2 * ca.mtimes(aQ, ca.transpose(b1))
    return K1
github helgeanl / GP-MPC / gp_mpc / gp_functions.py View on Github external
ks = ca.MX.zeros(N, 1)
        for i in range(N):
            ks[i] = covSE(X[i, :], inputmean, ell, sf2)

        invKks = ca.mtimes(iK, ks)
        mean[a] = ca.mtimes(ks.T, alpha)
        var[a] = kss - ca.mtimes(ks.T, invKks)
        d_mean[a] = ca.mtimes(ca.transpose(w[a] * v[:, a] * ks), alpha)

        #BUG: This don't take into account the covariance between states
        for d in range(Ny):
            for e in range(Ny):
                dd_var1a = ca.mtimes(ca.transpose(v[:, d] * ks), iK)
                dd_var1b = ca.mtimes(dd_var1a, v[e] * ks)
                dd_var2 = ca.mtimes(ca.transpose(v[d] * v[e] * ks), invKks)
                dd_var[d, e] = -2 * w[d] * w[e] * (dd_var1b + dd_var2)
                if d == e:
                    dd_var[d, e] = dd_var[d, e] + 2 * w[d] * (kss - var[d])

        mean_mat = ca.mtimes(d_mean, d_mean.T)
        covar_temp[0, 0] = inputcovar[a, a]
        covariance[a, a] = var[a] + ca.trace(ca.mtimes(covar_temp, .5
                                         * dd_var + mean_mat))

    return [mean, covariance]
github pymoca / pymoca / src / pymoca / backends / casadi / generator.py View on Github external
if for_loop is not None:
            if isinstance(indices[0], ca.MX):
                if len(indices) > 1:
                    s = s[:, indices[1]]
                    indexed_symbol = _new_mx('{}[{},{}]'.format(tree.name, for_loop.name, indices[1]), s.size2())
                    index_function = lambda i : (i, indices[1])
                else:
                    indexed_symbol = _new_mx('{}[{}]'.format(tree.name, for_loop.name))
                    index_function = lambda i : i

                # If the indexed symbol is empty, we know we do not have to
                # map the for loop over it
                if np.prod(s.shape) != 0:
                    for_loop.register_indexed_symbol(indexed_symbol, index_function, True, tree, indices[0])
            else:
                s = ca.transpose(s[indices[0], :])
                indexed_symbol = _new_mx('{}[{},{}]'.format(tree.name, indices[0], for_loop.name), s.size2())
                index_function = lambda i: (indices[0], i)
                if np.prod(s.shape) != 0:
                    for_loop.register_indexed_symbol(indexed_symbol, index_function, False, tree, indices[1])
            return indexed_symbol
        else:
            if len(indices) == 1:
                return s[indices[0]]
            else:
                return s[indices[0], indices[1]]