jgillis

def sympy2casadi(sympy_expr,sympy_var,casadi_var):
  import casadi
  assert casadi_var.is_vector()
  if casadi_var.shape[1]>1:
    casadi_var = casadi_var.T
  casadi_var = casadi.vertsplit(casadi_var)
  from sympy.utilities.lambdify import lambdify

  mapping = {'ImmutableDenseMatrix': casadi.blockcat,
             'MutableDenseMatrix': casadi.blockcat,

jgillis

/* This file was automatically generated by CasADi 3.6.5+.
 *  It consists of: 
 *   1) content generated by CasADi runtime: not copyrighted
 *   2) template code copied from CasADi source: permissively licensed (MIT-0)
 *   3) user code: owned by the user
 *
 */
#ifdef __cplusplus
extern "C" {
#endif

jgillis

from casadi import *

# Generate 3D data to fit
x = np.linspace(0,2,10)
y = np.linspace(2,4,10)
z = np.linspace(3,5,10)

[X,Y,Z] = np.meshgrid(x,y,z)

xyz_flat = np.vstack((X.ravel(),Y.ravel(),Z.ravel())).T

jgillis

function [x, lam_a, lam_x, f, stats] = qp_casadi(H,g,A,lba,uba,lb,ub,x0,lam_a0,lam_x0,solver,solver_options)
    % QP_CASADI: solves a QP:
    %   
    %   min   1/2*x'Hx + g'x
    %    x      subject to  lba <= Ax <= uba
    %                        lb <= x  <= ub
    %
    %   You may use [] to omit arguments.
    %
    %   Other inputs

jgillis

from casadi import *

opti = Opti()

x = opti.variable(10)
y = opti.variable(10)

opti.subject_to(x>=0)
opti.subject_to(x+y==0)
opti.subject_to(y>=0)

jgillis

m = 10 # Limite memory size

x_latest = MX.sym("x_latest",nx)
g_latest = MX.sym("g_latest",nx)

x_prev = MX.sym("x_prev",nx)
g_prev = MX.sym("g_prev",nx)

s_latest = x_latest-x_prev
y_latest = g_latest-g_prev

jgillis

import casadi as cs
import numpy as np

xp = [1, 2, 3]
fp = [3, 2, 0]

x = [2.5,1.1]

print(np.interp(x=x,xp=xp,fp=fp))

jgillis

function [casadi_expr] = mupad2casadi(mupad_expr,mupad_var,casadi_var)
  s = size(mupad_expr);
  assert(numel(s)<=2, 'Tensors not supported. Make a vector or matrix from the mupad expression first.');
  mupad_expr = mupad_expr(:);
  
  matlabFunction(symfun(mupad_expr,mupad_var),'File','temp_fun.m');
  clear temp_fun
  args = vertsplit(casadi_var);
  
  casadi_expr = temp_fun(args{:});

jgillis

import numpy as NP
import casadi as C
def qpsolve(H,g,lbx,ubx,A=NP.zeros((0,0)),lba=NP.zeros(0),uba=NP.zeros(0)):
    # Convert to CasADi types
    H = C.DMatrix(H)
    g = C.DMatrix(g)
    lbx = C.DMatrix(lbx)
    ubx = C.DMatrix(ubx)
    A = C.DMatrix(A)
    A = A.reshape((A.size1(),H.size1())) # Make sure matching dimensions

jgillis

XYZ_ref = [ simOut.EndEffector_AbsPos{1}.Values.Data,simOut.EndEffector_AbsPos{2}.Values.Data,simOut.EndEffector_AbsPos{3}.Values.Data ]';

Nsim = size(Gantry_out_D.Data,1);
XYZ = zeros(3,Nsim);
for k=1:Nsim
XYZ(:,k) = ForwardKinematicsFun(Gantry_out_D.Data(k,2),...
                     Axis1_Out_D.Data(k,2)/180*pi,...
                     Axis2_Out_D.Data(k,2)/180*pi,...
                     Axis3_Out_D.Data(k,2)/180*pi,...
                     Axis4_Out_D.Data(k,2)/180*pi,...