├── rayen └── __init__.py ├── examples ├── __init__.py ├── scripts │ ├── matlab │ │ ├── corridor_dim2.mat │ │ ├── corridor_dim3.mat │ │ ├── utils │ │ │ ├── randInInterval.m │ │ │ ├── containsSymCasadi.m │ │ │ ├── getTCasadi.m │ │ │ ├── keepOnlyVerticesConvexHull.m │ │ │ ├── plot2dConvHullAndVertices.m │ │ │ ├── isPlusInfCasadi.m │ │ │ ├── isMinusInfCasadi.m │ │ │ ├── cprnd │ │ │ │ ├── chebycenter.m │ │ │ │ └── license.txt │ │ │ ├── uniformSampleInUnitSphere.m │ │ │ ├── getAb_Box2D.m │ │ │ ├── plotConvexHullOfPoints.m │ │ │ ├── convertMX2Matlab.m │ │ │ ├── maxdistcolor │ │ │ │ ├── test_OSAUCS.m │ │ │ │ ├── test_OKLab.m │ │ │ │ ├── license.txt │ │ │ │ ├── CIELab_to_DIN99.m │ │ │ │ ├── CIELab_to_DIN99o.m │ │ │ │ ├── test_DIN99x.m │ │ │ │ ├── testfun_mdc.m │ │ │ │ ├── sRGB_to_OKLab.m │ │ │ │ ├── maxdistcolor_demo.m │ │ │ │ ├── sRGB_to_CIELab.m │ │ │ │ ├── sRGB_to_OSAUCS.m │ │ │ │ └── test_CIELab.m │ │ │ ├── uniformSampleInUnitBall.m │ │ │ ├── getAb_Box3D.m │ │ │ ├── getCoeffPolyCasadi.m │ │ │ ├── getPandqandrOfQuadraticExpressionCasadi.m │ │ │ ├── plot3dConvHullAndVertices.m │ │ │ ├── breakyaxis │ │ │ │ └── license.txt │ │ │ ├── getAllPqrQuadraticConstraints.m │ │ │ ├── polytopes │ │ │ │ ├── qlcon2vert.m │ │ │ │ ├── license.txt │ │ │ │ ├── addBounds.m │ │ │ │ ├── intersectionHull.m │ │ │ │ ├── unionHull.m │ │ │ │ ├── separateBounds.m │ │ │ │ └── vert2lcon.m │ │ │ ├── getAbLinearConstraints.m │ │ │ ├── samplePoints │ │ │ │ ├── license.txt │ │ │ │ └── samplePoints.m │ │ │ └── getIneqAndEqConstraintsFromOptiCasadi.m │ │ ├── doSetup.m │ │ ├── plot_eigenvalues.m │ │ ├── plotStuff.m │ │ ├── testing_accel_and_vel_costs.m │ │ ├── getABVerticesgivenP1P2.m │ │ ├── handling_degenerate_cases.m │ │ ├── getCorridorAndParamsSpline.m │ │ ├── test_idea_equality_and_inequality.m │ │ └── my_test.m │ ├── merge_all_results.py │ ├── plot_trajectories_RAYEN.m │ ├── plot_time_results.m │ ├── run.sh │ ├── time_analysis.py │ ├── train.yaml │ └── plot_losses_vs_time.m ├── requirements_examples.txt ├── fixpath.py ├── limit_torque.py ├── first_figure.py ├── other │ ├── test_bug_logpcg.py │ ├── plot_examples_with_mayavi.py │ ├── FindGurobi.cmake │ └── testing_sdp.py ├── early_stopping.py ├── cost_computer.py ├── examples_sets.py ├── create_dataset.py ├── test_layer.py └── other_utils.py ├── imgs ├── diamond.svg ├── rayen.png ├── green-tick.png ├── rayen_rays.png ├── red_cross.svg └── rayen_equations.png ├── .gitattributes ├── .gitmodules ├── pyproject.toml ├── .github └── workflows │ └── python-publish.yml ├── LICENSE └── .gitignore /rayen/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /examples/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /imgs/diamond.svg: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:d2a9187a8462f5d35f13c1cdebb10e9e5e3f839405069a9affd703c38cc87a64 3 | size 1599 4 | -------------------------------------------------------------------------------- /imgs/rayen.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:aeb55ff4fbefbe752c58105474311489c33059b963f0763ff320ed6bb514c829 3 | size 410882 4 | -------------------------------------------------------------------------------- /imgs/green-tick.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:4fda225eb758218c3ea6703b3b657be621416e5653fe4aab826a9536d7a1df69 3 | size 14221 4 | -------------------------------------------------------------------------------- /imgs/rayen_rays.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:4d775cb04880ed4d983f4cfe1377c89967e9582e73728614ce72979e6825a871 3 | size 383940 4 | -------------------------------------------------------------------------------- /imgs/red_cross.svg: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:2f1c5b915ec97e5d2f3241df09372988b175449b182283fac35ef06bfe78e59f 3 | size 2219 4 | -------------------------------------------------------------------------------- /imgs/rayen_equations.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:e352f479be16edd53d6fb2d12f49de8e4217c891eddecb1ad3dab1f8c587244d 3 | size 120125 4 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | *.mat filter=lfs diff=lfs merge=lfs -text 2 | *.m linguist-detectable=false 3 | *.png filter=lfs diff=lfs merge=lfs -text 4 | *.svg filter=lfs diff=lfs merge=lfs -text 5 | -------------------------------------------------------------------------------- /examples/scripts/matlab/corridor_dim2.mat: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:c22c49ed266a49e36cc78fb5e79b6109a435f545001a58fb6b54b7daebc47757 3 | size 824741 4 | -------------------------------------------------------------------------------- /examples/scripts/matlab/corridor_dim3.mat: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:13d52a0440d9ea2404b03c6260adbdf14f6e295cb392bd95e7974f99b5bb3ceb 3 | size 2682729 4 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "minvo"] 2 | path = examples/scripts/matlab/submodules/minvo 3 | url = https://github.com/mit-acl/minvo 4 | [submodule "export_fig"] 5 | path = examples/scripts/matlab/submodules/export_fig 6 | url = https://github.com/altmany/export_fig 7 | -------------------------------------------------------------------------------- /examples/requirements_examples.txt: -------------------------------------------------------------------------------- 1 | cvxpy>=1.3.1 2 | cvxpylayers>=0.1.6 3 | joblib>=1.2.0 4 | matplotlib>=3.6.3 5 | numpy>=1.24.2 6 | pandas>=1.5.3 7 | pycddlib>=2.1.6 8 | scipy>=1.10.1 9 | torch>=1.13.1+cu116 10 | tqdm>=4.65.0 11 | waitGPU>=0.0.3 12 | tensorboard>=2.12.0 13 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/randInInterval.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function result=randInInterval(a,b,s) 7 | result=[a + (b-a).*rand(s(1),s(2))]; 8 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/containsSymCasadi.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function result=containsSymCasadi(expression) 7 | result=(numel(symvar(expression))>0); 8 | end -------------------------------------------------------------------------------- /examples/fixpath.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------------------------- 2 | # Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | # See LICENSE file for the license information 4 | # -------------------------------------------------------------------------- 5 | 6 | #https://github.com/tartley/colorama/blob/master/demos/fixpath.py 7 | import sys 8 | from os.path import normpath, dirname, join 9 | sys.path.insert(0, normpath(join(dirname(__file__), '..'))) -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/getTCasadi.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function T=getTCasadi(t,deg) 7 | %For whatever reason, ([t.^[obj.p:-1:1] 1])' does not work properly with 8 | %other functions 9 | T=[]; 10 | for i=deg:-1:0 11 | T=[T;t^i]; 12 | end 13 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/keepOnlyVerticesConvexHull.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function V_filtered=keepOnlyVerticesConvexHull(V) 7 | 8 | [k1,av1] = convhull(V'); 9 | k_all_unique=unique(k1); 10 | V_filtered=V(:,k_all_unique); %V contains the vertices 11 | 12 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/plot2dConvHullAndVertices.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function plot2dConvHullAndVertices(V1, color_vertices, color_sides) 7 | [k,av] = convhull(V1'); 8 | scatter(V1(1,:),V1(2,:),color_vertices,'filled') 9 | hold on 10 | plot(V1(1,k),V1(2,k),color_sides,'LineWidth',2) 11 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/isPlusInfCasadi.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function result=isPlusInfCasadi(expression) 7 | if(containsSymCasadi(expression)) %if it has symbolic variables 8 | result=false; 9 | return; 10 | end 11 | result=(convertMX2Matlab(expression)==inf); 12 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/isMinusInfCasadi.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function result=isMinusInfCasadi(expression) 7 | if(containsSymCasadi(expression)) %if it has symbolic variables 8 | result=false; 9 | return; 10 | end 11 | result=(convertMX2Matlab(expression)==-inf); 12 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/cprnd/chebycenter.m: -------------------------------------------------------------------------------- 1 | function [c,r] = chebycenter(A,b) 2 | %CHEBYCENTER Compute Chebyshev center of polytope Ax <= b. 3 | % The Chebyshev center of a polytope is the center of the largest 4 | % hypersphere enclosed by the polytope. 5 | % Requires optimization toolbox. 6 | 7 | [n,p] = size(A); 8 | an = sqrt(sum(A.^2,2)); 9 | A1 = zeros(n,p+1); 10 | A1(:,1:p) = A; 11 | A1(:,p+1) = an; 12 | f = zeros(p+1,1); 13 | f(p+1) = -1; 14 | 15 | options = optimset; 16 | options = optimset(options,'Display', 'off'); 17 | c = linprog(f,A1,b,[],[],[],[],[],options); 18 | r = c(p+1); 19 | c = c(1:p); 20 | end 21 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/uniformSampleInUnitSphere.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function result=uniformSampleInUnitSphere(dim,num_points) 7 | %Method 19 of http://extremelearning.com.au/how-to-generate-uniformly-random-points-on-n-spheres-and-n-balls/ 8 | 9 | u = normrnd(0,1,dim,num_points); % each column is an array of dim normally distributed random variables 10 | u_normalized=u./vecnorm(u); 11 | result= u_normalized; 12 | 13 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/getAb_Box2D.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function result=getAb_Box2D(center,side) 7 | 8 | A1=[1 0; 9 | 0 1; 10 | -1 0; 11 | 0 -1]; 12 | 13 | of_x=center(1); 14 | of_y=center(2); 15 | 16 | 17 | b1=[side(1)/2.0+of_x; 18 | side(2)/2.0+of_y; 19 | side(1)/2.0-of_x; 20 | side(2)/2.0-of_y]; 21 | 22 | result.A=A1; 23 | result.b=b1; 24 | 25 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/plotConvexHullOfPoints.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function plotConvexHullOfPoints(V,color) 7 | 8 | x=V(1,:); 9 | y=V(2,:); 10 | z=V(3,:); 11 | 12 | [k1,av1] = convhull(x,y,z); 13 | 14 | trisurf(k1,x,y,z,'FaceColor',color) 15 | k_all=k1(:); 16 | k_all_unique=unique(k1); 17 | 18 | for i=1:length(k_all_unique) 19 | plotSphere([x(k_all_unique(i)) y(k_all_unique(i)) z(k_all_unique(i))], 0.03, color) 20 | end 21 | 22 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/convertMX2Matlab.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | %Note that MX can only have numbers 7 | %See also https://groups.google.com/g/casadi-users/c/8df2GfqrAS4/m/BTNsnkhiAwAJ 8 | function result=convertMX2Matlab(A) 9 | f = casadi.Function('f', {}, {A},... 10 | {},{'A'}); 11 | % f=f.expand(); %not strictly necessary 12 | s=f(); %now the field s.A has type DM 13 | result=full(s.A); %Convert to matlab 14 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/maxdistcolor/test_OSAUCS.m: -------------------------------------------------------------------------------- 1 | function test_OSAUCS() 2 | % Test OSA-UCS conversions against reference values. 3 | % 4 | % (c) 2017-2023 Stephen Cobeldick 5 | % 6 | % See also TESTFUN_MDC SRGB_TO_OSAUCS OSAUCS_to_SRGB 7 | 8 | fprintf('Running %s...\n',mfilename) 9 | % 10 | ist = nan(0:3); 11 | fnh = @sRGB_to_OSAUCS; 12 | % 13 | % Reference: 14 | chk = testfun_mdc(fnh,{'L','j','g'},5e-4); 15 | chk(ist,false,false, 100*[0.20654008,0.12197225,0.05136952], fnh, [-3.0049979,+2.99713697,-9.66784231]) % XYZ->OSAUCS 16 | chk() 17 | % 18 | end 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%test_OSAUCS -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/uniformSampleInUnitBall.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function result=uniformSampleInUnitBall(dim,num_points) 7 | %Method 20 of http://extremelearning.com.au/how-to-generate-uniformly-random-points-on-n-spheres-and-n-balls/ 8 | 9 | u = normrnd(0,1,dim,num_points); % each column is an array of dim normally distributed random variables 10 | u_normalized=u./vecnorm(u); 11 | r = rand(1,num_points).^(1.0/dim); %each column is the radius of each of the points 12 | result= r.*u_normalized; 13 | 14 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/doSetup.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function doSetup() 7 | 8 | set(0,'DefaultFigureWindowStyle','normal') %'normal' 'docked' 9 | set(0,'defaulttextInterpreter','latex'); set(groot, 'defaultAxesTickLabelInterpreter','latex'); set(groot, 'defaultLegendInterpreter','latex'); 10 | set(0,'defaultfigurecolor',[1 1 1]) 11 | 12 | import casadi.* 13 | addpath(genpath('./submodules/minvo/')) 14 | addpath(genpath('./submodules/export_fig/')) 15 | addpath(genpath('./utils')) 16 | 17 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/getAb_Box3D.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function result=getAb_Box3D(center,side) 7 | 8 | A1=[1 0 0; 9 | 0 1 0; 10 | 0 0 1; 11 | -1 0 0; 12 | 0 -1 0; 13 | 0 0 -1]; 14 | 15 | of_x=center(1); 16 | of_y=center(2); 17 | of_z=center(3); 18 | 19 | b1=[side(1)/2.0+of_x; 20 | side(2)/2.0+of_y; 21 | side(3)/2.0+of_z; 22 | side(1)/2.0-of_x; 23 | side(2)/2.0-of_y; 24 | side(3)/2.0-of_z]; 25 | 26 | result.A=A1; 27 | result.b=b1; 28 | 29 | end -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools", "setuptools-scm"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | 6 | [project] 7 | name = "rayen" 8 | version = "0.0.5" 9 | authors = [ 10 | { name="Jesus Tordesillas", email="jtordesillas@ethz.ch" }, 11 | ] 12 | description = "Imposition of Hard Convex Constraints on Neural Networks" 13 | readme = "README.md" 14 | requires-python = ">=3.7" 15 | classifiers = [ 16 | "Programming Language :: Python :: 3", 17 | "License :: OSI Approved :: BSD License", 18 | "Operating System :: OS Independent", 19 | ] 20 | dependencies = [ 21 | "colorama>=0.4.6", 22 | "cvxpy>=1.3.1", 23 | "cvxpylayers>=0.1.6", 24 | "numpy>=1.24.2", 25 | "pycddlib>=2.1.6", 26 | "scipy>=1.10.1", 27 | "torch>=1.13.1", 28 | "tqdm>=4.65.0", 29 | ] 30 | license = {file = "LICENSE"} 31 | 32 | [project.urls] 33 | "Homepage" = "https://github.com/leggedrobotics/rayen" 34 | "Bug Tracker" = "https://github.com/leggedrobotics/rayen/issues" 35 | 36 | [tool.setuptools] 37 | packages = ["rayen"] -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/getCoeffPolyCasadi.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | %% 7 | % clc; 8 | % import casadi.* 9 | % t=MX.sym('t',1,1); 10 | % a= 6.0 %MX.sym('a',1,1); 11 | % b= 7.0 %MX.sym('b',1,1); 12 | % c= 2.0 %MX.sym('c',1,1); 13 | % d= 1.5 %MX.sym('d',1,1); 14 | % poly=a*t^3 + b*t^2 + c*t +d; 15 | % 16 | % coeff=getCoeffPolyCasadi(poly,t, 3) 17 | % coeff(1) 18 | % coeff(2) 19 | % coeff(3) 20 | % coeff(4) 21 | function result=getCoeffPolyCasadi(poly, var, deg) 22 | import casadi.* 23 | coeff=[]; 24 | for i=0:(deg) %for each of the coefficients 25 | deriv=poly; 26 | for k=0:(i-1) 27 | deriv=jacobian(deriv,var); 28 | end 29 | 30 | coeff=[coeff casadi.substitute(deriv,var,0.0)/factorial(i)]; 31 | end 32 | result=flip(coeff); %[a b c ...] --> a^degree + b^(degree-1) + .... 33 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/maxdistcolor/test_OKLab.m: -------------------------------------------------------------------------------- 1 | function test_OKLab() 2 | % Test OKLab conversions against reference values. 3 | % 4 | % (c) 2017-2023 Stephen Cobeldick 5 | % 6 | % See also TESTFUN_MDC SRGB_TO_OKLAB OKLAB_TO_SRGB 7 | 8 | fprintf('Running %s...\n',mfilename) 9 | % 10 | ist = nan(0:3); 11 | fnh = @sRGB_to_OKLab; 12 | % 13 | % Reference: 14 | chk = testfun_mdc(fnh,{'L','a','b'},5e-4); 15 | chk(ist,[1,0,0], fnh, [+0.450,+1.236,-0.019]) % XYZ->OKLab 16 | chk(ist,[0,1,0], fnh, [+0.922,-0.671,+0.263]) % XYZ->OKLab 17 | chk(ist,[0,0,1], fnh, [+0.153,-1.415,-0.449]) % XYZ->OKLab 18 | chk() 19 | % 20 | % Reference: by hand 21 | chk = testfun_mdc(fnh,{'L','a','b'},5e-15); 22 | chk([1,0,0], fnh, [0.627925900661856, +0.224887603832926, +0.125804933240474]) % sRGB->OKLab 23 | chk([0,1,0], fnh, [0.866451874607104, -0.233921435420743, +0.179421768053411]) % sRGB->OKLab 24 | chk([0,0,1], fnh, [0.452032954406456, -0.032351637521107, -0.311620544208813]) % sRGB->OKLab 25 | % 26 | end 27 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%test_OSAUCS -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/getPandqandrOfQuadraticExpressionCasadi.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | %Quadratic expression is (1/2)*x'*P*x + q'*x + r 7 | function [P, q, r] = getPandqandrOfQuadraticExpressionCasadi(my_expression, x) 8 | 9 | import casadi.* 10 | 11 | assert(size(x,2)==1) 12 | 13 | my_gradient=gradient(my_expression,x); 14 | my_hessian=hessian(my_expression, x); 15 | 16 | P=convertMX2Matlab(my_hessian); 17 | q=convertMX2Matlab(casadi.substitute(my_gradient,x, zeros(size(x)))); 18 | r=convertMX2Matlab(casadi.substitute(my_expression,x, zeros(size(x)))); 19 | 20 | 21 | %Quick check 22 | x_random_values=10*rand(size(x)); 23 | expression_tmp=0.5*x_random_values'*P*x_random_values + q'*x_random_values + r; 24 | should_be_zero=convertMX2Matlab(expression_tmp-casadi.substitute(my_expression,x,x_random_values)); 25 | assert(abs(should_be_zero)<1e-5) 26 | end -------------------------------------------------------------------------------- /examples/limit_torque.py: -------------------------------------------------------------------------------- 1 | import fixpath 2 | import torch 3 | import numpy as np 4 | from rayen import constraints, constraint_module 5 | 6 | # y =[tau_1,...,tau_n]' 7 | 8 | n = 7; #Number of joints 9 | total_tau_max = 5.0; 10 | joint_tau_max = 1.0; 11 | 12 | #Linear constraint: |tau_j| <= joint_tau_max 13 | A1 = np.concatenate((np.eye(n), -np.eye(n)), axis=0); 14 | b1 = joint_tau_max * np.ones((2*n,1)) 15 | A2 = None; 16 | b2 = None; 17 | lc=constraints.LinearConstraint(A1, b1, A2, b2) 18 | 19 | #Quadratic constraint: tau_1^2 + ... + tau_n^2 <= total_tau_max^2 20 | P = 2*np.eye(n) 21 | q = np.zeros((n,1)) 22 | r = -np.power([[total_tau_max]], 2) 23 | qcs = [constraints.ConvexQuadraticConstraint(P, q, r)] 24 | 25 | cs = constraints.ConvexConstraints(lc=lc, qcs=qcs, socs=[], lmic=None) 26 | 27 | model = torch.nn.Sequential(torch.nn.Flatten(), torch.nn.Linear(3, 64), 28 | torch.nn.ReLU(), torch.nn.Linear(64, 64), 29 | constraint_module.ConstraintModule(cs, input_dim=64, create_map=True)) 30 | 31 | x_batched = torch.Tensor(500, 3, 1).uniform_(-1.0, 1.0) 32 | y_batched = model(x_batched) 33 | 34 | #Each element of y_batched is guaranteed to satisfy the torque constraints 35 | 36 | # loss = ... # y_batched can be used here 37 | # loss.backward() # Backpropagate -------------------------------------------------------------------------------- /examples/scripts/matlab/plot_eigenvalues.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | close all; clc;clear; 7 | doSetup(); 8 | 9 | % n=3; 10 | S=[5.1 1.1 3.1; 11 | 1.1 4.5 6.8; 12 | 3.1 6.8 8.9]; 13 | 14 | % tmp=rand(n,n); 15 | % H=tmp'*tmp + 0.01*eye(n); 16 | 17 | H= [0.8 0.6 0.9; 18 | 0.6 1.2 1.1 19 | 0.9 1.1 1.4]; 20 | 21 | all_changes=eig(-inv(H)*S); 22 | 23 | tmp=(max(all_changes)-min(all_changes))/9.0 24 | 25 | all_delta=linspace(min(all_changes)-tmp,max(all_changes)+tmp,200); 26 | all_eigenvalues=[]; 27 | for delta=all_delta 28 | [V,D] = eig(delta*H+S); 29 | all_eigenvalues=[all_eigenvalues sort(diag(D))]; 30 | end 31 | 32 | 33 | figure; hold on; 34 | xline(all_changes(1)) 35 | xline(all_changes(2)) 36 | xline(all_changes(3)) 37 | yline(0.0,'--') 38 | 39 | plot(all_delta,all_eigenvalues','LineWidth',2) 40 | legend('','','','','(eig$(\delta H +S)$)$_{\left[0\right]}$','(eig$(\delta H +S)$)$_{\left[1\right]}$','(eig$(\delta H +S)$)$_{\left[2\right]}$','Location','northwest') 41 | 42 | xlabel('$\delta$'); 43 | ylim([-4,7]) 44 | xlim([-26.32, 5.85]) 45 | 46 | set(gcf,'Position',[536 754 552 262]) 47 | export_fig eigenvalues.png -m2.5 -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/plot3dConvHullAndVertices.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | function plot3dConvHullAndVertices(V,color, radius_sphere) 7 | 8 | color_vertex=[.98 .45 .02]; 9 | radius=radius_sphere; 10 | s={}; 11 | for i=1:size(V,2) 12 | s{end+1}=plotSphere(V(:,i),radius, color_vertex); hold on; 13 | alpha(s{i},1.0) 14 | end 15 | % s2=plotSphere(v2,radius, color_vertex); 16 | % s3=plotSphere(v3,radius, color_vertex); 17 | % s4=plotSphere(v4,radius, color_vertex); 18 | % 19 | % alpha(s1,1.0) 20 | % alpha(s2,1.0) 21 | % alpha(s3,1.0) 22 | % alpha(s4,1.0) 23 | 24 | % shading faceted 25 | % hold on 26 | axis equal 27 | tmp=gca; 28 | if (size(findobj(tmp.Children,'Type','Light'))<1) %If still no light in the subplot 29 | camlight %create light 30 | end 31 | lighting phong 32 | 33 | vx=V(1,:); 34 | vy=V(2,:); 35 | vz=V(3,:); 36 | 37 | [k1,volume] = convhull(vx,vy,vz,'Simplify',true); 38 | 39 | s2=trisurf(k1,vx,vy,vz,'LineWidth',1,'FaceColor',color); 40 | 41 | alpha(s2,0.1) 42 | xlabel('x') 43 | ylabel('y') 44 | zlabel('z') 45 | 46 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/breakyaxis/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, MikeCF 2 | Copyright (c) 2013, Peter 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are 7 | met: 8 | 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the distribution 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/getAllPqrQuadraticConstraints.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | %opti_tmp is an optimization problem from Casadi 7 | %Feasible set is 0.5*x'P_i x + q'x + r <=0 8 | function [all_P, all_q, all_r]=getAllPqrQuadraticConstraints(opti_tmp) 9 | 10 | [inequality_expressions, equality_expressions]=getIneqAndEqConstraintsFromOptiCasadi(opti_tmp); 11 | 12 | 13 | if(numel(equality_expressions)>0) 14 | error("Not implemented yet") 15 | end 16 | 17 | variables=opti_tmp.x; 18 | 19 | all_P={}; 20 | all_q={}; 21 | all_r={}; 22 | 23 | %%%%%% INEQUALITY CONSTRAINTS 24 | for i=1:size(inequality_expressions,1) 25 | i 26 | [P,q,r]=getPandqandrOfQuadraticExpressionCasadi(inequality_expressions(i), variables); 27 | 28 | try 29 | P=convertMX2Matlab(P); 30 | q=convertMX2Matlab(q); 31 | r=convertMX2Matlab(r); 32 | catch %Constraints are not linear 33 | error('Are you sure you only have quadratic constraints?'); 34 | end 35 | 36 | all_P{end+1}=P; 37 | all_q{end+1}=q; 38 | all_r{end+1}=r; 39 | 40 | 41 | end 42 | %%%%%%%%%%%%%%%%%%%%%%%%% 43 | 44 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/polytopes/qlcon2vert.m: -------------------------------------------------------------------------------- 1 | function [varargout]=qlcon2vert(x0,varargin) 2 | %A quicker version of lcon2vert that will skip some intensive steps when a 3 | %point x0 in the relative interior of the polyhedron is known a priori. 4 | % 5 | % [V,nr,nre]=qlcon2vert(x0, A,b,Aeq,beq,TOL) 6 | % 7 | %Here x0 is the known relative interior point. The rest of the input/output 8 | %arguments are as in LCON2VERT. 9 | % 10 | %In the interest of speed, QLCON2VERT will perform minimal error checking, 11 | %shifting the burden to the user of verifying that the input data has 12 | %appropriate properties. In particular, it will not try to verify that x0 13 | %is in fact in the relative interior. By default also, qlcon2vert will skip the 14 | %boundedness test, and presume the user knows the polyhedron to be bounded already. 15 | %However, you can re-activate the boundedness test by calling with the syntax, 16 | % 17 | % [V,nr,nre]=qlcon2vert(x0, A,b,Aeq,beq,TOL,1) 18 | 19 | 20 | if nargin<=3 21 | 22 | varargin(3:6)={[],[],[],0}; 23 | 24 | end 25 | 26 | x0=x0(:); 27 | 28 | [A,b,Aeq,beq]=deal(varargin{1:4}); 29 | 30 | b=b-A*x0; 31 | if ~isempty(beq) 32 | beq(:)=0; 33 | end 34 | 35 | varargin{2}=b; 36 | varargin{4}=beq; 37 | 38 | [varargout{1:max(nargout,1)}] = lcon2vert(varargin{:}); 39 | 40 | V=varargout{1}; 41 | 42 | V=bsxfun(@plus,x0.',V); %undo coordinate translation 43 | 44 | varargout{1}=V; 45 | -------------------------------------------------------------------------------- /.github/workflows/python-publish.yml: -------------------------------------------------------------------------------- 1 | #See https://packaging.python.org/en/latest/guides/publishing-package-distribution-releases-using-github-actions-ci-cd-workflows/ 2 | name: Publish Python 🐍 distributions 📦 to PyPI 3 | 4 | #See https://stackoverflow.com/questions/61891328/trigger-github-action-only-on-new-tags 5 | on: 6 | push: 7 | # Pattern matched against refs/tags 8 | tags: 9 | - '**' # Push events to every tag 10 | 11 | jobs: 12 | build-n-publish: 13 | name: Build and publish Python 🐍 distributions 📦 to PyPI 14 | runs-on: ubuntu-latest 15 | steps: 16 | - uses: actions/checkout@v3 17 | - name: Set up Python 18 | uses: actions/setup-python@v4 19 | with: 20 | python-version: "3.x" 21 | - name: Install pypa/build 22 | run: >- 23 | python3 -m 24 | pip install 25 | build 26 | --user 27 | - name: Build a binary wheel and a source tarball 28 | run: >- 29 | python3 -m 30 | build 31 | --sdist 32 | --wheel 33 | --outdir dist/ 34 | . 35 | # - name: Publish distribution 📦 to Test PyPI 36 | # uses: pypa/gh-action-pypi-publish@release/v1 37 | # with: 38 | # password: ${{ secrets.TEST_PYPI_API_TOKEN }} 39 | # repository-url: https://test.pypi.org/legacy/ 40 | - name: Publish distribution 📦 to PyPI 41 | uses: pypa/gh-action-pypi-publish@release/v1 42 | with: 43 | password: ${{ secrets.PYPI_API_TOKEN }} 44 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/getAbLinearConstraints.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | %opti_tmp is an optimization problem from Casadi 7 | %Feasible set is A1x<=b1 \cap A2x=b2 8 | function [A1,b1, A2, b2]=getAbLinearConstraints(opti_tmp) 9 | 10 | [inequality_expressions, equality_expressions]=getIneqAndEqConstraintsFromOptiCasadi(opti_tmp); 11 | 12 | variables=opti_tmp.x; 13 | 14 | %%%%%%%%%% INEQUALITY CONSTRAINTS 15 | b1=-casadi.substitute(inequality_expressions, variables, zeros(size(variables))); %Note the - sign 16 | A1=jacobian(inequality_expressions, variables); 17 | try 18 | A1=convertMX2Matlab(A1); 19 | b1=convertMX2Matlab(b1); 20 | catch %Constraints are not linear 21 | error('Are you sure you only have linear constraints?'); 22 | end 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | 25 | 26 | %%%%%%%%%% EQUALITY CONSTRAINTS 27 | b2=-casadi.substitute(equality_expressions, variables, zeros(size(variables))); %Note the - sign 28 | A2=jacobian(equality_expressions, variables); 29 | try 30 | A2=convertMX2Matlab(A2); 31 | b2=convertMX2Matlab(b2); 32 | catch %Constraints are not linear 33 | error('Are you sure you only have linear constraints?'); 34 | end 35 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 36 | 37 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/polytopes/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2017, Matt Jacobson 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution 13 | 14 | * Neither the name of nor the names of its 15 | contributors may be used to endorse or promote products derived from this 16 | software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/maxdistcolor/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2023, Stephen Cobeldick 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution 13 | 14 | * Neither the name of nor the names of its 15 | contributors may be used to endorse or promote products derived from this 16 | software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/samplePoints/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Simone Parisi 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution 13 | * Neither the name of Politecnico di Milano nor the names of its 14 | contributors may be used to endorse or promote products derived from this 15 | software without specific prior written permission. 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, Robotic Systems Lab - Legged Robotics at ETH Zürich 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/cprnd/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2011, Tim Benham 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the distribution 13 | * Neither the name of the University of Queensland nor the names 14 | of its contributors may be used to endorse or promote products derived 15 | from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /examples/scripts/matlab/plotStuff.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | clear; 7 | close all; 8 | clc; 9 | 10 | load('./../example_0.mat') 11 | Aeq=double(Aeq); 12 | beq=double(beq); 13 | Aineq=double(Aineq); 14 | bineq=double(bineq); 15 | 16 | result=result'; 17 | 18 | syms x y z 19 | tol_for_aliasing=0.0001; 20 | figure; hold on; 21 | 22 | V=lcon2vert(Aineq,bineq,Aeq,beq) 23 | if(size(V,2)==3) 24 | V=[V 1.00001*V(:,end)] %Hack to prevent convhull from failing 25 | end 26 | plot3dConvHullAndVertices(V, 'g', 0.02) 27 | 28 | 29 | for i=1:size(all_P,1) 30 | P=squeeze(all_P(i,:,:)); 31 | q=squeeze(all_q(i,:,:))'; 32 | r=squeeze(all_r(i,:,:)); 33 | tmp=[x y z]'; 34 | f=0.5*tmp'*P*tmp + q'*tmp + r + 1e-100*sum(tmp); %The second part is just a hack to make fimplicit3 work correctly when f depends only on two variables (imagine a cylinder aligned with the x axis) 35 | fimplicit3( f ,'EdgeColor','none','FaceAlpha',.5 ,'MeshDensity',50 ) 36 | end 37 | 38 | zlim([0,2]) 39 | 40 | scatter3(result(1,:),result(2,:),result(3,:)) 41 | 42 | 43 | %% 44 | % close all; figure; 45 | % Q=eye(3) 46 | % tmp=[x y z]'; 47 | % c=rand(3,1); d=rand() 48 | % f=0.5*tmp'*Q*x + c'*tmp +d + 1e-100*sum(tmp); %The second part is just a hack to make fimplicit3 work correctly when f depends only on two variables (imagine a cylinder aligned with the x axis) 49 | % fimplicit3( f ,'EdgeColor','none','FaceAlpha',.5 ,'MeshDensity',50 ) 50 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/getIneqAndEqConstraintsFromOptiCasadi.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | %returns the constraints as 7 | % inequality_expressions<=0 8 | % equality_expressions=0 9 | function [inequality_expressions, equality_expressions]=getIneqAndEqConstraintsFromOptiCasadi(opti_tmp) 10 | 11 | g=opti_tmp.g(); 12 | lower=opti_tmp.lbg(); 13 | upper=opti_tmp.ubg(); 14 | 15 | inequality_expressions=[]; 16 | equality_expressions=[]; 17 | 18 | %constraints are lower<=g<=upper 19 | 20 | all_g=[]; 21 | all_upper=[]; 22 | 23 | for i=1:size(g,1) 24 | 25 | u_i=upper(i); 26 | l_i=lower(i); 27 | g_i=g(i); 28 | 29 | if(abs(convertMX2Matlab(u_i-l_i))<1e-7) %Equality constraint 30 | equality_expressions=[equality_expressions; g_i-u_i]; % l_i<=g_i<=u_i (with l_i==u_i) <--> g_i-u_i = 0 31 | continue 32 | end 33 | 34 | if(isPlusInfCasadi(u_i)==false) 35 | inequality_expressions=[inequality_expressions; g_i-u_i]; % g_i<=u_i <--> g_i-u_i <=0 36 | end 37 | if(isMinusInfCasadi(l_i)==false) 38 | inequality_expressions=[inequality_expressions; l_i-g_i]; % l_i<=g_i <--> l_i-g_i <=0 39 | end 40 | end 41 | 42 | % The constraints are now 43 | % inequality_expressions<=0 44 | % equality_expressions==0 45 | 46 | 47 | end -------------------------------------------------------------------------------- /examples/scripts/merge_all_results.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------------------------- 2 | # Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | # See LICENSE file for the license information 4 | # -------------------------------------------------------------------------- 5 | 6 | import glob, os 7 | import pandas as pd 8 | 9 | os.chdir("./results") 10 | all_pkl=[] 11 | for file in glob.glob("dataset*.pkl"): 12 | all_pkl.append(file) 13 | all_pkl = sorted(all_pkl) 14 | all_dataframes=[] 15 | for file in all_pkl: 16 | all_dataframes.append(pd.read_pickle(file)) 17 | 18 | 19 | df = pd.concat(all_dataframes) 20 | 21 | 22 | df=df.drop_duplicates(subset=['method']) 23 | # print(df) 24 | 25 | 26 | results2d = df[df['method'].str.startswith('dataset2d')] 27 | results3d = df[df['method'].str.startswith('dataset3d')] 28 | 29 | results2d=results2d.set_index('method') 30 | results3d=results3d.set_index('method') 31 | 32 | print(results2d) 33 | print(results3d) 34 | 35 | 36 | results2d["[In dist] loss"]/=(results2d.loc["dataset2d_Optimization"].at["[In dist] loss"]) #Normalize the cost 37 | results2d["[Out dist] loss"]/=(results2d.loc["dataset2d_Optimization"].at["[Out dist] loss"]) #Normalize the cost 38 | 39 | results3d["[In dist] loss"]/=(results3d.loc["dataset3d_Optimization"].at["[In dist] loss"]) #Normalize the cost 40 | results3d["[Out dist] loss"]/=(results3d.loc["dataset3d_Optimization"].at["[Out dist] loss"]) #Normalize the cost 41 | 42 | 43 | 44 | df = pd.concat([results2d, results3d]) 45 | 46 | df=df.rename(columns={"[In dist] loss": "[In dist] n.loss", "[Out dist] loss": "[Out dist] n.loss"}) 47 | 48 | 49 | print(df) 50 | 51 | # results2d['[In dist] loss']=results2d['[In dist] loss']/results2d[ results2d['method']=='dataset2d_Optimization' ] 52 | 53 | df.to_csv('./merged.csv') 54 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/maxdistcolor/CIELab_to_DIN99.m: -------------------------------------------------------------------------------- 1 | function Lab99 = CIELab_to_DIN99(Lab) 2 | % Convert a matrix of CIELAB L* a* b* values to DIN99 L99 a99 b99 values (DIN 6176). 3 | % 4 | % (c) 2018-2023 Stephen Cobeldick 5 | % 6 | %%% Syntax: 7 | % Lab99 = CIELab_to_DIN99(Lab) 8 | % 9 | % 10 | % 11 | %% Inputs and Outputs 12 | % 13 | %%% Input Argument: 14 | % Lab = Numeric Array, size Nx3 or RxCx3, where the last dimension 15 | % encodes CIELAB values [L*,a*,b*] in the range 0<=L*<=100. 16 | % 17 | %%% Output Argument: 18 | % Lab99 = Numeric Array, same size as , where the last dimension 19 | % encodes the DIN99 values [L99,a99,b99]. 20 | % 21 | % See also SRGB_TO_CIELAB CIELAB_TO_DIN99O 22 | % SRGB_TO_CAM02UCS SRGB_TO_OKLAB SRGB_TO_OSAUCS 23 | % MAXDISTCOLOR MAXDISTCOLOR_VIEW MAXDISTCOLOR_DEMO 24 | 25 | %% Input Wrangling %% 26 | % 27 | isz = size(Lab); 28 | assert(isnumeric(Lab),... 29 | 'SC:CIELab_to_DIN99:Lab:NotNumeric',... 30 | '1st input must be numeric.') 31 | assert(isreal(Lab),... 32 | 'SC:CIELab_to_DIN99:Lab:ComplexValue',... 33 | '1st input cannot be complex.') 34 | assert(isz(end)==3,... 35 | 'SC:CIELab_to_DIN99:Lab:InvalidSize',... 36 | '1st input last dimension must have size 3 (e.g. Nx3 or RxCx3).') 37 | Lab = reshape(Lab,[],3); 38 | assert(all(Lab(:,1)>=0&Lab(:,1)<=100),... 39 | 'SC:CIELab_to_DIN99:Lab:OutOfRange',... 40 | '1st input L values must be within the range 0<=L<=100') 41 | % 42 | if ~isfloat(Lab) 43 | Lab = double(Lab); 44 | end 45 | % 46 | %% Lab2DIN99 %% 47 | % 48 | L99 = 105.51 * log(1 + 0.0158*Lab(:,1)); 49 | e = (Lab(:,2).*cosd(16)+Lab(:,3).*sind(16)); 50 | f = 0.7*(Lab(:,3).*cosd(16)-Lab(:,2).*sind(16)); 51 | G = sqrt(e.^2 + f.^2); 52 | C99 = log(1 + 0.045*G)./(0.045); 53 | h99 = atan2(f,e); 54 | a99 = C99 .* cos(h99); 55 | b99 = C99 .* sin(h99); 56 | % 57 | Lab99 = reshape([L99,a99,b99],isz); 58 | % 59 | end 60 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%CIELab_to_DIN99 -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/maxdistcolor/CIELab_to_DIN99o.m: -------------------------------------------------------------------------------- 1 | function Lab99o = CIELab_to_DIN99o(Lab) 2 | % Convert a matrix of CIELAB L* a* b* values to DIN99o L99o a99o b99o values (DIN 6176). 3 | % 4 | % (c) 2018-2023 Stephen Cobeldick 5 | % 6 | %%% Syntax: 7 | % Lab99o = CIELab_to_DIN99o(Lab) 8 | % 9 | % 10 | % 11 | %% Inputs and Outputs 12 | % 13 | %%% Input Argument: 14 | % Lab = Numeric Array, size Nx3 or RxCx3, where the last dimension 15 | % encodes CIELAB values [L*,a*,b*] in the range 0<=L*<=100. 16 | % 17 | %%% Output Argument: 18 | % Lab99o = Numeric Array, same size as , where the last dimension 19 | % encodes the DIN99o values [L99o,a99o,b99o]. 20 | % 21 | % See also SRGB_TO_CIELAB CIELAB_TO_DIN99 22 | % SRGB_TO_CAM02UCS SRGB_TO_OKLAB SRGB_TO_OSAUCS 23 | % MAXDISTCOLOR MAXDISTCOLOR_VIEW MAXDISTCOLOR_DEMO 24 | 25 | %% Input Wrangling %% 26 | % 27 | isz = size(Lab); 28 | assert(isnumeric(Lab),... 29 | 'SC:CIELab_to_DIN99o:Lab:NotNumeric',... 30 | '1st input must be numeric.') 31 | assert(isreal(Lab),... 32 | 'SC:CIELab_to_DIN99o:Lab:ComplexValue',... 33 | '1st input cannot be complex.') 34 | assert(isz(end)==3,... 35 | 'SC:CIELab_to_DIN99o:Lab:InvalidSize',... 36 | '1st input last dimension must have size 3 (e.g. Nx3 or RxCx3).') 37 | Lab = reshape(Lab,[],3); 38 | assert(all(Lab(:,1)>=0&Lab(:,1)<=100),... 39 | 'SC:CIELab_to_DIN99o:Lab:OutOfRange',... 40 | '1st input L values must be within the range 0<=L<=100') 41 | % 42 | if ~isfloat(Lab) 43 | Lab = double(Lab); 44 | end 45 | % 46 | kCH = 1; 47 | kE = 1; 48 | % 49 | %% Lab2DIN99o %% 50 | % 51 | L99o = 303.67/kE * log(1 + 0.0039*Lab(:,1)); 52 | eo = (Lab(:,2).*cosd(26)+Lab(:,3).*sind(26)); 53 | fo = 0.83*(Lab(:,3).*cosd(26)-Lab(:,2).*sind(26)); 54 | G = sqrt(eo.^2 + fo.^2); 55 | C99o = log(1 + 0.075*G)./(0.0435*kCH*kE); 56 | h99o = atan2(fo,eo) + 26*pi/180; 57 | a99o = C99o .* cos(h99o); 58 | b99o = C99o .* sin(h99o); 59 | % 60 | Lab99o = reshape([L99o,a99o,b99o],isz); 61 | % 62 | end 63 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%CIELab_to_DIN99o -------------------------------------------------------------------------------- /examples/scripts/matlab/testing_accel_and_vel_costs.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | addpath(genpath('./../submodules/minvo/')) 7 | addpath(genpath('./../submodules/export_fig/')) 8 | addpath(genpath('./utils')) 9 | 10 | clear; clc; 11 | t0=2.1; 12 | tf=3.9; 13 | import casadi.* 14 | 15 | opti = casadi.Opti();%'conic' I think you need to use 'conic' for gurobi 16 | deg_pos=3; 17 | dim_pos=1; 18 | num_seg =1; 19 | sp=MyClampedUniformSpline(t0,tf,deg_pos, dim_pos, num_seg, opti); 20 | cpoints=rand(dim_pos,sp.num_cpoints); 21 | cpoints_cell_array=mat2cell(cpoints,dim_pos,ones(1,sp.num_cpoints)); 22 | 23 | sp.setCPoints(cpoints_cell_array) 24 | 25 | vel_cost=sp.getVelCost(); 26 | convertMX2Matlab(vel_cost) 27 | sp.getApproxVelCost(300) 28 | 29 | accel_cost=sp.getAccelCost(); 30 | convertMX2Matlab(accel_cost) 31 | sp.getApproxAccelCost(600) 32 | %% 33 | % opti = casadi.Opti();%'conic' I think you need to use 'conic' for gurobi 34 | % p=opti.variable(1,4); 35 | % p_value=[4 3 6 9]; 36 | % 37 | % t=MX.sym('t') 38 | % pt=p*[t^3;t^2;t;1] 39 | % 40 | % deg_spline=3; 41 | % 42 | % vt=gradient(pt,t); 43 | % at=gradient(vt,t); 44 | % 45 | % vt_squared=vt*vt; %[at^5 + bt^4 +ct^3 +dt^2 +...] 46 | % 47 | % v_squared=getCoeffPolyCasadi(vt_squared, t, 2*deg_spline); %[a b c d ...] 48 | % 49 | % 50 | % tfinal=1.0; 51 | % t0=0.0 52 | % 53 | % tmp=[numel(v_squared):-1:1]; 54 | % result=sum(v_squared.*((tfinal.^tmp)./tmp)) - sum(v_squared.*((t0.^tmp)./tmp)) 55 | % result=convertMX2Matlab(substitute(result,p,p_value)) 56 | % 57 | % v=polyder(p_value); 58 | % v_squared=conv(v,v); 59 | % diff(polyval(polyint(v_squared),[t0 tfinal])) 60 | 61 | %% 62 | p=sym('p',[1,4]); 63 | t=sym('t'); 64 | pt=p*getT(3,t); 65 | vt=diff(pt,t) 66 | at=diff(vt,t) 67 | jt=diff(at,t) 68 | 69 | vel_cost=int(vt*vt,t,0,1) 70 | accel_cost=int(at*at,t,0,1) 71 | 72 | total_cost=vel_cost+accel_cost; 73 | 74 | H = double(hessian(total_cost)/2); 75 | 76 | eig(H) 77 | 78 | % p = [p(1)]; 79 | % q = polyder(p) -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/maxdistcolor/test_DIN99x.m: -------------------------------------------------------------------------------- 1 | function test_DIN99x() 2 | % Test DIN99/DIN99o conversions against reference values. 3 | % 4 | % (c) 2017-2023 Stephen Cobeldick 5 | % 6 | % See also TESTFUN_MDC CIELAB_TO_DIN99 CIELAB_TO_DIN99O DIN99_TO_SRGB 7 | 8 | fprintf('Running %s...\n',mfilename) 9 | % 10 | fnh = @CIELab_to_DIN99; 11 | % 12 | % Reference: 13 | chk = testfun_mdc(fnh,{'L99','a99','b99'},1e-3); 14 | chk([41.52787529,52.63858304,26.92317922], fnh, [53.22821988,28.41634656,3.89839552]) % Lab->DIN99 15 | chk() 16 | % 17 | % Reference: 18 | chk = testfun_mdc(fnh,{'L99','a99','b99'},5e-3); 19 | chk([ 50,+10,+10], fnh, [61.43, +9.70, +3.76]) % Lab->DIN99 20 | chk([ 50,+50,+50], fnh, [61.43,+28.64,+11.11]) % Lab->DIN99 21 | chk([ 50,-10,+10], fnh, [61.43, -5.57, +7.03]) % Lab->DIN99 22 | chk([ 50,-50,+50], fnh, [61.43,-17.22,+21.75]) % Lab->DIN99 23 | chk([ 50,-10,-10], fnh, [61.43, -9.70, -3.76]) % Lab->DIN99 24 | chk([ 50,-50,-50], fnh, [61.43,-28.64,-11.11]) % Lab->DIN99 25 | chk([ 50,+10,-10], fnh, [61.43, +5.57, -7.03]) % Lab->DIN99 26 | chk([ 50,+50,-50], fnh, [61.43,+17.22,-21.75]) % Lab->DIN99 27 | chk([ 0, 0, 0], fnh, [ 0, 0, 0]) % Lab->DIN99 28 | chk([100, 0, 0], fnh, [ 100, 0, 0]) % Lab->DIN99 29 | chk() 30 | % 31 | fnh = @CIELab_to_DIN99o; 32 | % 33 | % Reference: 34 | chk = testfun_mdc(fnh,{'L99o','a99o','b99o'},5e-4); 35 | chk([ 50,+10,+10], fnh, [54.098,+12.215,+10.979]) % Lab->DIN99o 36 | chk([ 50,+50,+50], fnh, [54.098,+31.237,+28.076]) % Lab->DIN99o 37 | chk([ 50,-10,+10], fnh, [54.098,-11.067, +9.780]) % Lab->DIN99o 38 | chk([ 50,-50,+50], fnh, [54.098,-29.384,+25.968]) % Lab->DIN99o 39 | chk([ 50,-10,-10], fnh, [54.098,-12.215,-10.979]) % Lab->DIN99o 40 | chk([ 50,-50,-50], fnh, [54.098,-31.237,-28.076]) % Lab->DIN99o 41 | chk([ 50,+10,-10], fnh, [54.098,+11.067, -9.780]) % Lab->DIN99o 42 | chk([ 50,+50,-50], fnh, [54.098,+29.384,-25.968]) % Lab->DIN99o 43 | chk([ 0, 0, 0], fnh, [ 0, 0, 0]) % Lab->DIN99o 44 | chk([100, 0, 0], fnh, [ 100, 0, 0]) % Lab->DIN99o 45 | chk() 46 | % 47 | end 48 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%test_DIN99x -------------------------------------------------------------------------------- /examples/first_figure.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------------------------- 2 | # Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | # See LICENSE file for the license information 4 | # -------------------------------------------------------------------------- 5 | 6 | import numpy as np 7 | import torch 8 | import torch.nn as nn 9 | import math 10 | import matplotlib.pyplot as plt 11 | import mpl_toolkits.mplot3d as a3 12 | import matplotlib.colors as colors 13 | import numpy as np 14 | import scipy 15 | import os 16 | import time 17 | 18 | from examples_sets import getExample 19 | import utils_examples 20 | 21 | import fixpath #Following this example: https://github.com/tartley/colorama/blob/master/demos/demo01.py 22 | from rayen import constraints, constraint_module, utils 23 | 24 | method='RAYEN' 25 | index_example=13 26 | 27 | 28 | fig = plt.figure() 29 | fig.suptitle(method, fontsize=10) 30 | 31 | tmp=getExample(index_example) 32 | 33 | constraint=constraints.ConvexConstraints(lc=tmp.lc, qcs=tmp.qcs, socs=tmp.socs, lmic=tmp.lmic, y0=np.array([[0.5], [0.0], [0.8]])) 34 | 35 | ax = fig.add_subplot(1,1,1, projection="3d") 36 | 37 | my_layer=constraint_module.ConstraintModule(constraint, method=method, create_map=False) 38 | 39 | num_samples=500 #12000 40 | v_batched_x=torch.Tensor(num_samples, 1, 1).uniform_(-0.5, 0.6) 41 | v_batched_y=torch.Tensor(num_samples, 1, 1).uniform_(-0.55, 0.47) 42 | v_batched_z=torch.Tensor(num_samples, 1, 1).uniform_(-0.4, 0.3) 43 | v_batched = torch.cat((v_batched_x, v_batched_y, v_batched_z),1) 44 | 45 | my_layer.eval() #This changes the self.training variable of the module 46 | 47 | time_start=time.time() 48 | result=my_layer(v_batched) 49 | total_time_per_sample= (time.time()-time_start)/num_samples 50 | 51 | result=result.detach().numpy(); 52 | 53 | y0=my_layer.gety0(); 54 | 55 | 56 | ax.scatter(y0[0,0], y0[1,0], y0[2,0],color='r',s=500) 57 | ax.scatter(result[:,0,0], result[:,1,0], result[:,2,0]) 58 | 59 | 60 | my_dict=constraint.getDataAsDict(); 61 | my_dict["result"]=result 62 | my_dict["total_time_per_sample"]=total_time_per_sample 63 | my_dict["y0"]=constraint.y0 64 | my_dict["v"]=v_batched.detach().numpy() 65 | directory='./first_figure' 66 | if not os.path.exists(directory): 67 | os.makedirs(directory) 68 | scipy.io.savemat(directory+'/first_figure.mat', my_dict) 69 | 70 | utils.printInBoldBlue(f"Example {index_example}, total_time_per_sample={total_time_per_sample}") 71 | 72 | plt.show() -------------------------------------------------------------------------------- /examples/scripts/plot_trajectories_RAYEN.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | 7 | close all; clc;clear; 8 | 9 | set(0,'DefaultFigureWindowStyle','normal') %'normal' 'docked' 10 | set(0,'defaulttextInterpreter','latex'); set(groot, 'defaultAxesTickLabelInterpreter','latex'); set(groot, 'defaultLegendInterpreter','latex'); 11 | set(0,'defaultfigurecolor',[1 1 1]) 12 | 13 | import casadi.* 14 | addpath(genpath('./matlab/')) 15 | 16 | 17 | position=[675 165 1041 797]; 18 | doPlot(2); ylim([-5,17]) 19 | set(gcf,'Position',[675 165 1041/1.2 797/1.2]) 20 | export_fig trajectories2d.png -m2.5 21 | 22 | 23 | doPlot(3) 24 | view(-90,90); set(gcf,'Position',position) 25 | export_fig trajectories3d_top.png -m2.5 26 | view(-59,23); set(gcf,'Position',position) 27 | export_fig trajectories3d_side.png -m2.5 28 | 29 | function doPlot(dimension) 30 | 31 | [allA, allb, allV, p0, t0,tf,deg_pos, num_seg, num_of_seg_per_region, use_quadratic]=getCorridorAndParamsSpline(dimension); 32 | load(['./results/results_test_in_dist_dataset',num2str(dimension),'d_RAYEN_weight_soft_cost_0.0.mat']) 33 | 34 | opti = casadi.Opti('conic');%'conic' I think you need to use 'conic' for gurobi 35 | sp=MyClampedUniformSpline(t0,tf,deg_pos, dimension, num_seg, opti); 36 | 37 | all_x=squeeze(all_x); 38 | all_y=squeeze(all_y); 39 | all_y_predicted=squeeze(all_y_predicted); 40 | % figure; 41 | hold on; 42 | 43 | num_samples=100; 44 | indexes_all_samples=randperm(size(all_y_predicted,1),num_samples); %https://ch.mathworks.com/matlabcentral/answers/71181-how-to-generate-non-repeating-random-numbers-from-1-to-49#answer_81589 45 | j=0; 46 | for i=indexes_all_samples 47 | j/num_samples 48 | c_points=all_y_predicted(i,:)'; 49 | c_points=reshape(c_points,dimension,[]); 50 | sp.updateCPsWithSolution(c_points); 51 | xlabel('x (m)'); ylabel('y (m)'); 52 | if(dimension==2) 53 | sp.plotPos2D(1,'r') 54 | else 55 | sp.plotPos3D(1,'r') 56 | zlabel('z (m)'); 57 | view(48,38); 58 | % plot3(P(1,:),P(2,:),P(3,:),'--','LineWidth',2) 59 | end 60 | j=j+1; 61 | end 62 | 63 | end 64 | 65 | -------------------------------------------------------------------------------- /examples/scripts/matlab/getABVerticesgivenP1P2.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | %If radius=0, This gives a polyhedron with 6 faces around the line p1-->p2 7 | %If radius>0, some extra points are taken around the vertices of the polyhedron 8 | %Region is {x such that A1*x<=b1} 9 | function [A, b, V]=getABVerticesgivenP1P2(p1,p2, hside, radius, num_samples_per_vertex) 10 | 11 | h=norm(p1-p2); 12 | 13 | 14 | b_p1_a=[hside -hside 0]'; 15 | b_p1_b=[hside hside 0]'; 16 | b_p1_c=[-hside hside 0]'; 17 | b_p1_d=[-hside -hside 0]'; 18 | 19 | b_p2_a=[hside -hside h]'; 20 | b_p2_b=[hside hside h]'; 21 | b_p2_c=[-hside hside h]'; 22 | b_p2_d=[-hside -hside h]'; 23 | 24 | yaw=0.0; 25 | 26 | zb=(p2-p1)/norm(p2-p1); 27 | xb=cross([-sin(yaw) cos(yaw) 0]',zb); 28 | assert(norm(xb)>0) 29 | xb=xb/norm(xb); 30 | yb=cross(zb,xb); 31 | w_R_b=[xb yb zb]; 32 | 33 | w_p1_a=w_R_b*b_p1_a+p1; 34 | w_p1_b=w_R_b*b_p1_b+p1; 35 | w_p1_c=w_R_b*b_p1_c+p1; 36 | w_p1_d=w_R_b*b_p1_d+p1; 37 | 38 | w_p2_a=w_R_b*b_p2_a+p1; 39 | w_p2_b=w_R_b*b_p2_b+p1; 40 | w_p2_c=w_R_b*b_p2_c+p1; 41 | w_p2_d=w_R_b*b_p2_d+p1; 42 | 43 | b_A=[1 0 0; 44 | 0 1 0; 45 | 0 0 1; 46 | -1 0 0; 47 | 0 -1 0; 48 | 0 0 -1]; 49 | A=[]; 50 | 51 | for i=1:size(b_A,1) 52 | A=[A;(w_R_b*b_A(i,:)')']; 53 | end 54 | 55 | b=[A(1,:)*w_p1_a; 56 | A(2,:)*w_p1_b; 57 | A(3,:)*w_p2_a; 58 | A(4,:)*w_p1_c; 59 | A(5,:)*w_p1_d; 60 | A(6,:)*w_p1_d;]; 61 | 62 | [V,nr,nre]=lcon2vert(A,b,[],[]); 63 | V=V'; 64 | 65 | 66 | all_samples=[]; 67 | for i=1:size(V,2) 68 | vertex=V(:,i); 69 | all_samples_of_this_vertex=[]; 70 | while size(all_samples_of_this_vertex,2)%3$s|%2$d:'; 15 | else 16 | fmt = '%3$s|%2$d:'; 17 | end 18 | % 19 | function nestfun(varargin) 20 | % (in1,in2,..,fnh,out1,out2,..) 21 | % 22 | dbs = dbstack(); 23 | % 24 | if ~nargin % post-processing 25 | fprintf(fmt, dbs(2).file, dbs(2).line, fnt); 26 | fprintf(' %d of %d testcases failed.\n',cnt,itr); 27 | return 28 | end 29 | % 30 | idx = find(cellfun(@(f)isequal(f,fnh),varargin)); 31 | assert(nnz(idx)==1,'SC:test_fun:MissFun','Missing/duplicated function handle.') 32 | xpc = varargin(idx+1:end); 33 | opc = cell(size(xpc)); 34 | % 35 | [opc{:}] = fnh(varargin{1:idx-1}); 36 | % 37 | ido = ~cellfun(@(a)isequal(a,@i),xpc); 38 | assert(nnz(ido)==1,'SC:test_fun:OneOutput','Select exactly one output.') 39 | % 40 | opa = opc{ido}; 41 | xpa = xpc{ido}; 42 | % 43 | boo = false; 44 | % 45 | if ~isequal(class(opa),class(xpa)) 46 | boo = true; 47 | xpt = sprintf(' class %s',class(xpa)); 48 | opt = sprintf(' class %s',class(opa)); 49 | elseif ~isequal(size(opa),size(xpa)) 50 | boo = true; 51 | xpt = sprintf(',%d',size(xpa)); 52 | opt = sprintf(',%d',size(opa)); 53 | xpt = sprintf(' size [%s]',xpt(2:end)); 54 | opt = sprintf(' size [%s]',opt(2:end)); 55 | elseif any(abs(opa(:)-xpa(:))>tol) 56 | boo = true; 57 | xpu = [num2cell(xpa(:).');dnm]; 58 | opu = [num2cell(opa(:).');dnm]; 59 | xpt = sprintf(fmn,xpu{:}); 60 | opt = sprintf(fmn,opu{:}); 61 | end 62 | % 63 | if boo 64 | dmn = min(numel(opt),numel(xpt)); 65 | dmx = max(numel(opt),numel(xpt)); 66 | dtx = repmat('^',1,dmx); 67 | dtx(opt(1:dmn)==xpt(1:dmn)) = ' '; 68 | % 69 | fprintf(fmt, dbs(2).file, dbs(2).line, fnt); 70 | fprintf(' (output #%d)\n',find(ido)); 71 | fprintf('actual: %s\n', opt) 72 | fprintf('expect: %s\n', xpt) 73 | fprintf('diff: ') 74 | fprintf(2,'%s\n',dtx); % red! 75 | end 76 | % 77 | cnt = cnt+boo; 78 | itr = itr+1; 79 | end 80 | % 81 | end 82 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%testfun_mdc -------------------------------------------------------------------------------- /examples/other/test_bug_logpcg.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | import scipy 4 | 5 | # def using_power_iteration(C): 6 | 7 | # b_k = np.random.rand(C.shape[0],1) 8 | 9 | # tol = 1e-6 10 | # max_iter = 100 11 | 12 | # for i in range(max_iter): 13 | # b_k1 = C @ b_k 14 | # b_k1 = b_k1 / np.linalg.norm(b_k1) 15 | 16 | # lam=(b_k1.T @ b_k)/(b_k.T @ b_k) 17 | # print(lam) 18 | # # x = C @ x / np.linalg.norm(C @ x) 19 | # # lam = (x.T @ C @ x) / (x.T @ x) 20 | # # # print(lam) 21 | # # if np.abs(lam - lam_prev) < tol: 22 | # # break 23 | # # lam_prev = lam 24 | 25 | # return lam 26 | 27 | def eigenvalue(A, v): 28 | Av = A.dot(v) 29 | return v.dot(Av) 30 | 31 | def power_iteration(A): 32 | n, d = A.shape 33 | 34 | v = np.ones(d) / np.sqrt(d) 35 | ev = eigenvalue(A, v) 36 | 37 | while True: 38 | Av = A.dot(v) 39 | v_new = Av / np.linalg.norm(Av) 40 | 41 | ev_new = eigenvalue(A, v_new) #v_new.dot(A.dot(v_new)) # 42 | if np.abs(ev - ev_new) < 1e-12: 43 | break 44 | 45 | v = v_new 46 | ev = ev_new 47 | 48 | return ev_new #, v_new 49 | 50 | 51 | dim=3 52 | 53 | while True: 54 | print("\n\n-------") 55 | 56 | #Random generation of matrices A and B 57 | tmp = np.random.uniform(-1, 1, (dim, dim)) 58 | A=(tmp+tmp.T)/2.0 #A is symmetric by construction 59 | 60 | tmp = np.random.uniform(-1, 1, (dim, dim)) 61 | B = np.dot(tmp, tmp.transpose()) + np.eye(dim) #B is symmetric positive definite by construction 62 | 63 | print(f"A={A}\n") 64 | print(f"B={B}\n") 65 | 66 | 67 | X=np.random.rand(A.shape[0], 1) #Initial guess for the eigenvector 68 | 69 | #### USING lobpcg 70 | lambda_lobpcg, _ = torch.lobpcg(A=torch.from_numpy(A), k=1, B=torch.from_numpy(B), niter=-1, X=torch.from_numpy(X), tol=1e-12) 71 | lambda_lobpcg = lambda_lobpcg.item() 72 | print(f"lambda_lobpcg={lambda_lobpcg}") 73 | 74 | # #### USING Scipy 75 | lambda_scipy, _ =scipy.sparse.linalg.lobpcg(A=A, B=B, X=X, maxiter=10000) 76 | lambda_scipy=lambda_scipy[0] 77 | print(f"lambda_scipy={lambda_scipy}") 78 | 79 | #### USING normal eigendecomposition 80 | all_lambdas, _=np.linalg.eig(np.linalg.inv(B)@A); 81 | # print(f"all_lambdas={all_lambdas}") 82 | lambda_eig=np.max(all_lambdas) 83 | print(f"lambda_eig={lambda_eig}") 84 | 85 | #### USING Power iteration 86 | lambda_power_iter=power_iteration(np.linalg.inv(B)@A) 87 | print(f"lambda_power_iter={lambda_power_iter}") 88 | 89 | assert abs(lambda_lobpcg-lambda_eig)<1e-6 90 | assert abs(lambda_scipy-lambda_eig)<1e-6 91 | assert abs(lambda_power_iter-lambda_eig)<1e-6 92 | 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /examples/early_stopping.py: -------------------------------------------------------------------------------- 1 | #Code taken from https://github.com/Bjarten/early-stopping-pytorch/blob/master/pytorchtools.py 2 | #Some minor modifications by jtorde@mit.edu 3 | 4 | import numpy as np 5 | import torch 6 | import uuid 7 | import os 8 | 9 | class EarlyStopping: 10 | """Early stops the training if validation loss doesn't improve after a given patience.""" 11 | def __init__(self, patience=20, verbose=False, delta=0, trace_func=print): 12 | """ 13 | Args: 14 | patience (int): How long to wait after last time validation loss improved. 15 | Default: 7 16 | verbose (bool): If True, prints a message for each validation loss improvement. 17 | Default: False 18 | delta (float): Minimum change in the monitored quantity to qualify as an improvement. 19 | Default: 0 20 | path (str): Path for the checkpoint to be saved to. 21 | Default: 'checkpoint.pt' 22 | trace_func (function): trace print function. 23 | Default: print 24 | """ 25 | self.patience = patience 26 | self.verbose = verbose 27 | self.counter = 0 28 | self.best_score = None 29 | self.early_stop = False 30 | self.val_loss_min = np.Inf 31 | self.delta = delta 32 | folder="./early_stopping_checkpoints/" 33 | os.system("mkdir -p "+folder) 34 | self.path = folder+"checkpoint_"+uuid.uuid4().hex+".pt" #https://stackoverflow.com/a/62277811 35 | self.trace_func = trace_func 36 | def __call__(self, val_loss, model): 37 | 38 | score = -val_loss 39 | 40 | if self.best_score is None: 41 | self.best_score = score 42 | self.save_checkpoint(val_loss, model) 43 | elif score < self.best_score + self.delta: 44 | self.counter += 1 45 | if self.verbose: 46 | self.trace_func(f'EarlyStopping counter: {self.counter} out of {self.patience}') 47 | if self.counter >= self.patience: 48 | self.early_stop = True 49 | else: 50 | self.best_score = score 51 | self.save_checkpoint(val_loss, model) 52 | self.counter = 0 53 | 54 | def save_checkpoint(self, val_loss, model): 55 | '''Saves model when validation loss decrease.''' 56 | if self.verbose: 57 | # self.trace_func(f'Validation loss decreased ({self.val_loss_min:.6f} --> {val_loss:.6f}). Saving model ...') 58 | self.trace_func(f'Best validation loss so far: {self.val_loss_min:.6f}. Saving model ...') 59 | torch.save(model.state_dict(), self.path) 60 | self.val_loss_min = val_loss 61 | 62 | #Added by jtorde 63 | def load_best_model(self, model): 64 | model.load_state_dict(torch.load(self.path)) 65 | 66 | def __del__(self): 67 | os.system("rm "+self.path) 68 | # body of destructor 69 | 70 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/polytopes/addBounds.m: -------------------------------------------------------------------------------- 1 | function [A,b,Aeq,beq]=addBounds(A0,b0,Aeq0,beq0,lb,ub) 2 | %addBounds - given a sextuplet {A, b, Aeq, beq, lb, ub} of matrices and vectors 3 | %of the kind used by the Optimization Toolbox to express linear 4 | %constraints, the routine will convert the sextuplet to a quadruplet {A, b, 5 | %Aeq, beq}. 6 | % 7 | % [A,b,Aeq,beq]=addBounds(A,b,Aeq,beq,lb,ub) 8 | % 9 | %In other words, it will re-write the bounds given by vectors lb, ub as equalities 10 | %and inequalities C*x<=c, D*x=d and append these as new rows to the matrices 11 | %A, b, Aeq, beq. 12 | % 13 | % 14 | %EXAMPLE: 15 | % 16 | % >> [A,b,Aeq,beq]=addBounds([1 1 1],1,[],[],[0;0;1],[1;1;1]) 17 | % 18 | % A = 19 | % 20 | % 1 1 1 21 | % -1 0 0 22 | % 0 -1 0 23 | % 1 0 0 24 | % 0 1 0 25 | % 26 | % 27 | % b = 28 | % 29 | % 1 30 | % 0 31 | % 0 32 | % 1 33 | % 1 34 | % 35 | % 36 | % Aeq = 37 | % 38 | % 0 0 1 39 | % 40 | % 41 | % beq = 42 | % 43 | % 1 44 | 45 | 46 | 47 | 48 | %%%%begin parsing 49 | 50 | if nargin<5, error 'At least 5 arguments required'; end 51 | 52 | if size(A0,1)~=length(b0) 53 | error 'Incompatible inequality matrix data sizes: size(A,1) ~= length(b)' 54 | end 55 | 56 | if size(Aeq0,1)~=length(beq0) 57 | error 'Incompatible equality matrix data sizes: size(Aeq,1) ~= length(beq)' 58 | end 59 | 60 | 61 | 62 | if ~exist('lb','var'), lb=[]; end 63 | if ~exist('ub','var'), ub=[]; end 64 | 65 | if ~isempty(lb) 66 | N=length(lb); 67 | elseif ~isempty(ub); 68 | N=length(ub); 69 | else 70 | error 'Either lb or ub must be nonempty.' 71 | end 72 | 73 | if isempty(lb) 74 | lb=-inf(N,1); 75 | end 76 | 77 | if isempty(ub) 78 | ub=+inf(N,1); 79 | end 80 | 81 | if length(ub)~=length(lb) 82 | error 'Arguments lb and ub must be either [] or the same lengths' 83 | end 84 | 85 | 86 | lb=lb(:); ub=ub(:); 87 | 88 | %%%end parsing 89 | 90 | 91 | lisinf=~isfinite(lb); 92 | uisinf=~isfinite(ub); 93 | idx_eq=(ub==lb)& ~lisinf & ~uisinf; 94 | 95 | Au=[eye(N),ub]; 96 | Al=[-eye(N),-lb]; 97 | Aeq=Au(idx_eq,:); 98 | Au(idx_eq|uisinf,:)=[]; 99 | Al(idx_eq|lisinf,:)=[]; 100 | 101 | A=[Al;Au]; b=A(:,end); A(:,end)=[]; 102 | beq=Aeq(:,end); Aeq(:,end)=[]; 103 | 104 | 105 | A=[A0;A]; 106 | b=[b0;b]; 107 | Aeq=[Aeq0;Aeq]; 108 | beq=[beq0;beq]; 109 | 110 | if any(lb==inf | ub==-inf) 111 | A(end+1,end)=0; b(end+1)=-1; 112 | end 113 | 114 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/maxdistcolor/sRGB_to_OKLab.m: -------------------------------------------------------------------------------- 1 | function [Lab,XYZ] = sRGB_to_OKLab(rgb,test) 2 | % Convert a matrix of sRGB R G B values to OKLAB L a b values. 3 | % 4 | % (c) 2018-2023 Stephen Cobeldick 5 | % 6 | %%% Syntax: 7 | % Lab = sRGB_to_OKLab(rgb) 8 | % 9 | % 10 | % 11 | %% Inputs and Outputs 12 | % 13 | %%% Input Argument: 14 | % rgb = Numeric Array, size Nx3 or RxCx3, where the last dimension 15 | % encodes sRGB values [R,G,B] in the range 0<=RGB<=1. 16 | % 17 | %%% Output Argument: 18 | % Lab = Numeric Array, same size as , where the last dimension 19 | % encodes OKLAB values [L,a,b] in the range 0<=L<=1. 20 | % 21 | % See also SRGB_TO_CAM02UCS SRGB_TO_OSAUCS SRGB_TO_CIELAB 22 | % CIELAB_TO_SRGB CIELAB_TO_DIN99 CIELAB_TO_DIN99O 23 | % MAXDISTCOLOR MAXDISTCOLOR_VIEW MAXDISTCOLOR_DEMO 24 | 25 | %% Input Wrangling %% 26 | % 27 | isz = size(rgb); 28 | assert(isnumeric(rgb),... 29 | 'SC:sRGB_to_OKLab:rgb:NotNumeric',... 30 | '1st input array must be numeric.') 31 | assert(isreal(rgb),... 32 | 'SC:sRGB_to_OKLab:rgb:ComplexValue',... 33 | '1st input cannot be complex.') 34 | assert(isz(end)==3,... 35 | 'SC:sRGB_to_OKLab:rgb:InvalidSize',... 36 | '1st input last dimension must have size 3 (e.g. Nx3 or RxCx3).') 37 | rgb = reshape(rgb,[],3); 38 | assert(all(rgb(:)>=0&rgb(:)<=1),... 39 | 'SC:sRGB_to_OKLab:rgb:OutOfRange',... 40 | '1st input values must be within the range 0<=rgb<=1') 41 | % 42 | if ~isfloat(rgb) 43 | rgb = double(rgb); 44 | end 45 | % 46 | %% RGB2Lab %% 47 | % 48 | M = [... Standard sRGB to XYZ matrix: 49 | 0.4124,0.3576,0.1805;... 50 | 0.2126,0.7152,0.0722;... 51 | 0.0193,0.1192,0.9505]; 52 | % source: IEC 61966-2-1:1999 53 | % 54 | % M = [... High-precision sRGB to XYZ matrix: 55 | % 0.4124564,0.3575761,0.1804375;... 56 | % 0.2126729,0.7151522,0.0721750;... 57 | % 0.0193339,0.1191920,0.9503041]; 58 | % source: 59 | % 60 | % RGB2XYZ 61 | if nargin>1 && isequal(isz,0:3) 62 | isz = size(test); 63 | XYZ = test; 64 | else 65 | XYZ = sGammaInv(rgb) * M.'; 66 | end 67 | % 68 | % XYZ2OKLab 69 | M1 = [... XYZ to approximate cone responses: 70 | +0.8189330101, +0.3618667424, -0.1288597137;... 71 | +0.0329845436, +0.9293118715, +0.0361456387;... 72 | +0.0482003018, +0.2643662691, +0.6338517070]; 73 | M2 = [... nonlinear cone responses to Lab: 74 | +0.2104542553, +0.7936177850, -0.0040720468;... 75 | +1.9779984951, -2.4285922050, +0.4505937099;... 76 | +0.0259040371, +0.7827717662, -0.8086757660]; 77 | lms = XYZ * M1.'; 78 | lmsp = nthroot(lms,3); 79 | Lab = lmsp * M2.'; 80 | Lab = reshape(Lab,isz); 81 | % source: 82 | % 83 | end 84 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%sRGB_to_OKLab 85 | function out = sGammaInv(inp) 86 | % Inverse gamma correction: Nx3 sRGB -> Nx3 linear RGB. 87 | idx = inp > 0.04045; 88 | out = inp / 12.92; 89 | out(idx) = real(power((inp(idx) + 0.055) / 1.055, 2.4)); 90 | end 91 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%sGammaInv -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/maxdistcolor/maxdistcolor_demo.m: -------------------------------------------------------------------------------- 1 | %%% MAXDISTCOLOR Demo Script %%% 2 | % Plot the colormap in the UCS, as a distance plot, and as colorbars with colornames. 3 | % 4 | % The CAM02UCS colorspace functions must be obtained separately, for 5 | % example my implementation here: 6 | % 7 | %%% Define the colorspace function: 8 | fun = @sRGB_to_OKLab; % OKLab (an improvement over CIELab) 9 | %fun = @sRGB_to_CIELab; % CIELab (not particularly uniform colorspace) 10 | %fun = @(m)sRGB_to_CAM02UCS(m,true,'LCD'); % CAM02-LCD (recommended) 11 | %fun = @(m)sRGB_to_CAM02UCS(m,true,'UCS'); % CAM02-UCS 12 | %fun = @(m)sRGB_to_OSAUCS(m,true,true); % OSA-UCS (modified) 13 | %fun = @(m)CIELab_to_DIN99o(sRGB_to_CIELab(m)); % DIN99o 14 | %fun = @(m)CIELab_to_DIN99(sRGB_to_CIELab(m)); % DIN99 15 | % 16 | %%% Generate colormap of distinctive colors: 17 | [rgb,ucs,sts] = maxdistcolor(9,fun); 18 | %[rgb,ucs,sts] = maxdistcolor(9,fun,'exc',[]); 19 | %[rgb,ucs,sts] = maxdistcolor(9,fun,'class','single'); 20 | %[rgb,ucs,sts] = maxdistcolor(9,fun,'Cmin',0.5,'Cmax',0.6); 21 | %[rgb,ucs,sts] = maxdistcolor(9,fun,'Lmin',0.4,'Lmax',0.6); 22 | %[rgb,ucs,sts] = maxdistcolor(9,fun,'inc',[0,0,0;1,0,1],'exc',[0,1,0]); 23 | %[rgb,ucs,sts] = maxdistcolor(9,fun,'sort','longest','disp','verbose'); 24 | %[rgb,ucs,sts] = maxdistcolor(9,fun, 'bitR',8,'bitG',8,'bitB',8); % Truecolor -> slow! 25 | %[rgb,ucs,sts] = maxdistcolor(64,fun,'bitR',2,'bitG',2,'bitB',2, 'exc',[]); % entire RGB gamut. 26 | N = size(rgb,1); 27 | % 28 | %%% Plot color distance matrix: 29 | figure(); 30 | for k = 1:N 31 | dst = sqrt(sum(bsxfun(@minus,ucs,ucs(k,:)).^2,2)); 32 | scatter3(k*ones(1,N),1:N,dst, 123, rgb,... 33 | 'MarkerFaceColor',rgb(k,:), 'LineWidth',2.8, 'Marker','o') 34 | hold on 35 | end 36 | title(sprintf('Colormap Euclidean Distances in %s Colorspace',sts.colorspace)) 37 | zlabel('Euclidean Distance') 38 | ylabel('Colormap Index') 39 | xlabel('Colormap Index') 40 | set(gca,'XTick',1:N,'YTick',1:N) 41 | % 42 | %%% Plot colors in UCS: 43 | figure(); 44 | scatter3(ucs(:,3),ucs(:,2),ucs(:,1), 256, rgb, 'filled') 45 | text(ucs(:,3),ucs(:,2),ucs(:,1),cellstr(num2str((1:N).')), 'HorizontalAlignment','center') 46 | % 47 | %%% Plot outline of RGB cube: 48 | M = 23; 49 | [X,Y,Z] = ndgrid(linspace(0,1,M),0:1,0:1); 50 | mat = fun([X(:),Y(:),Z(:);Y(:),Z(:),X(:);Z(:),X(:),Y(:)]); 51 | X = reshape(mat(:,3),M,[]); 52 | Y = reshape(mat(:,2),M,[]); 53 | Z = reshape(mat(:,1),M,[]); 54 | line(X,Y,Z,'Color','k') 55 | axis('equal') 56 | title(sprintf('Colormap in %s Colorspace',sts.colorspace)) 57 | zlabel(sts.axesLabels{1}) 58 | ylabel(sts.axesLabels{2}) 59 | xlabel(sts.axesLabels{3}) 60 | % 61 | %%% Plot colorband image: 62 | figure() 63 | image(permute(rgb,[1,3,2])) 64 | title('Colormap in Colorbands') 65 | ylabel('Colormap Index') 66 | set(gca,'XTick',[], 'YTick',1:N, 'YDir','normal') 67 | %%% Add colornames (if COLORNAMES is available): 68 | try %#ok 69 | text(ones(1,N), 1:N, colornames('CSS',rgb),... 70 | 'HorizontalAlignment','center', 'BackgroundColor','white') 71 | end 72 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%maxdistcolor_demo -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/samplePoints/samplePoints.m: -------------------------------------------------------------------------------- 1 | function points = samplePoints(lo, hi, N, useSimplex, linearly) 2 | % Samples continuous points in an interval. It allows for both linear and 3 | % uniform random sampling. It is also possible to draw points only in the 4 | % simplex of the interval. 5 | % 6 | % Inputs: 7 | % - lo : lower bound (column of length D) 8 | % - hi : upper bound (column of length D) 9 | % - N : number of points to sample 10 | % - useSimplex : put 1 if you want to sample from the simplex of the bound, 11 | % 0 otherwise 12 | % - linearly : put 1 if you want the points to be linearly spaced, 0 13 | % otherwise 14 | % 15 | % Outputs: 16 | % - points : D-by-NN matrix with random points, where NN >= N 17 | % 18 | % More information about sampling from the simplex: 19 | % http://math.stackexchange.com/questions/502583/uniform-sampling-of-points-on-a-simplex 20 | % 21 | % It requires 'inhull': 22 | % http://www.mathworks.com/matlabcentral/fileexchange/10226-inhull 23 | 24 | if ~iscolumn(lo) || ~iscolumn(hi) 25 | error('Bounds must be column.') 26 | end 27 | if length(lo) ~= length(hi) 28 | error('Bounds must have the same length.') 29 | end 30 | if max(lo > hi) 31 | error('Lower bound higher than upper bound.') 32 | end 33 | 34 | dim = length(lo); 35 | 36 | if linearly 37 | linspaces = cell(dim,0); 38 | 39 | % When using the simplex, the volume of the hypercuboid defined by 40 | % [lo,hi] is reduced by a factor DIM!. This explains why we need to 41 | % increase N to guarantee the desired number of samples. 42 | % Notice that the final number of points will be higher than the 43 | % desired one. However, it is the closest integer to N that ensures a 44 | % complete linear sampling in the simplex of [lo,hi]. 45 | if useSimplex 46 | if dim == 1 47 | points = linspace(lo,hi,N); 48 | return 49 | end 50 | N = N*factorial(dim); 51 | end 52 | 53 | for i = 1 : dim 54 | linspaces{i} = linspace(lo(i),hi(i),ceil(nthroot(N,dim))); 55 | end 56 | 57 | c = cell(1,dim); 58 | [c{:}] = ndgrid(linspaces{:}); 59 | points = cell2mat( cellfun(@(v)v(:), c, 'UniformOutput', false) )'; 60 | 61 | if useSimplex 62 | pp = zeros(dim+1, dim); 63 | for i = 1 : dim 64 | p = lo'; 65 | p(i) = hi(i); 66 | pp(i,:) = p; 67 | end 68 | pp(end,:) = lo'; 69 | idx = inhull(points',pp); 70 | points = points(:,idx); 71 | end 72 | return 73 | end 74 | 75 | if (useSimplex == 0) || dim == 1 76 | points = bsxfun(@plus, lo, bsxfun(@times, ... 77 | (hi - lo), rand(dim, N))); 78 | else 79 | real_lo = lo; % shift if lower bound is not 0 80 | hi = hi - lo; 81 | lo = lo - lo; 82 | rnd = rand(dim, N); 83 | rnd = -log(rnd); 84 | points = rnd; 85 | tot = sum(rnd); 86 | tot = tot - log(rand(1, N)); 87 | points = bsxfun(@plus, lo, bsxfun(@times, ... 88 | (hi - lo), points)); 89 | points = bsxfun(@times, points, 1 ./ tot); 90 | points = bsxfun(@plus, points, real_lo); % shift back 91 | end 92 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/maxdistcolor/sRGB_to_CIELab.m: -------------------------------------------------------------------------------- 1 | function [Lab,XYZ] = sRGB_to_CIELab(rgb,test) 2 | % Convert a matrix of sRGB R G B values to CIELAB L* a* b* values. 3 | % 4 | % (c) 2018-2023 Stephen Cobeldick 5 | % 6 | %%% Syntax: 7 | % Lab = sRGB_to_CIELab(rgb) 8 | % 9 | % 10 | % 11 | % 12 | %% Inputs and Outputs 13 | % 14 | %%% Input Argument: 15 | % rgb = Numeric Array, size Nx3 or RxCx3, where the last dimension 16 | % encodes sRGB values [R,G,B] in the range 0<=RGB<=1. 17 | % 18 | %%% Output Argument: 19 | % Lab = Numeric Array, same size as , where the last dimension 20 | % encodes CIELAB values [L*,a*,b*] in the range 0<=L*<=100. 21 | % 22 | % See also CIELAB_TO_SRGB CIELAB_TO_DIN99 CIELAB_TO_DIN99O 23 | % SRGB_TO_CAM02UCS SRGB_TO_OKLAB SRGB_TO_OSAUCS 24 | % MAXDISTCOLOR MAXDISTCOLOR_VIEW MAXDISTCOLOR_DEMO 25 | 26 | %% Input Wrangling %% 27 | % 28 | isz = size(rgb); 29 | assert(isnumeric(rgb),... 30 | 'SC:sRGB_to_CIELab:rgb:NotNumeric',... 31 | '1st input array must be numeric.') 32 | assert(isreal(rgb),... 33 | 'SC:sRGB_to_CIELab:rgb:ComplexValue',... 34 | '1st input cannot be complex.') 35 | assert(isz(end)==3,... 36 | 'SC:sRGB_to_CIELab:rgb:InvalidSize',... 37 | '1st input last dimension must have size 3 (e.g. Nx3 or RxCx3).') 38 | rgb = reshape(rgb,[],3); 39 | assert(all(rgb(:)>=0&rgb(:)<=1),... 40 | 'SC:sRGB_to_CIELab:rgb:OutOfRange',... 41 | '1st input values must be within the range 0<=rgb<=1') 42 | % 43 | if ~isfloat(rgb) 44 | rgb = double(rgb); 45 | end 46 | % 47 | %% RGB2Lab %% 48 | % 49 | M = [... Standard sRGB to XYZ matrix: 50 | 0.4124,0.3576,0.1805;... 51 | 0.2126,0.7152,0.0722;... 52 | 0.0193,0.1192,0.9505]; 53 | % source: IEC 61966-2-1:1999 54 | % 55 | % M = [... High-precision sRGB to XYZ matrix: 56 | % 0.4124564,0.3575761,0.1804375;... 57 | % 0.2126729,0.7151522,0.0721750;... 58 | % 0.0193339,0.1191920,0.9503041]; 59 | % source: 60 | % 61 | wpt = [0.95047,1,1.08883]; % D65 62 | % 63 | % Approximately equivalent to this function, requires Image Toolbox: 64 | %Lab = applycform(rgb,makecform('srgb2lab','AdaptedWhitePoint',wpt)) 65 | % 66 | % RGB2XYZ 67 | if nargin>1 && isequal(isz,0:3) 68 | isz = size(test); 69 | XYZ = test; 70 | else 71 | XYZ = sGammaInv(rgb) * M.'; 72 | end 73 | % 74 | % XYZ2Lab 75 | epsilon = 216/24389; 76 | kappa = 24389/27; 77 | % source: 78 | xyzr = bsxfun(@rdivide,XYZ,wpt); 79 | idx = xyzr>epsilon; 80 | fxyz = (kappa*xyzr+16)/116; 81 | fxyz(idx) = nthroot(xyzr(idx),3); 82 | Lab = reshape([max(0,min(100,... 83 | 116*fxyz(:,2)-16)),... 84 | 500*(fxyz(:,1)-fxyz(:,2)),... 85 | 200*(fxyz(:,2)-fxyz(:,3))],isz); 86 | % source: 87 | % 88 | end 89 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%sRGB_to_CIELab 90 | function out = sGammaInv(inp) 91 | % Inverse gamma correction: Nx3 sRGB -> Nx3 linear RGB. 92 | idx = inp > 0.04045; 93 | out = inp / 12.92; 94 | out(idx) = real(power((inp(idx) + 0.055) / 1.055, 2.4)); 95 | end 96 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%sGammaInv -------------------------------------------------------------------------------- /examples/scripts/matlab/handling_degenerate_cases.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | close all; clc;clear; 7 | set(0,'DefaultFigureWindowStyle','docked')%normal or docked 8 | addpath(genpath('./../deep_panther/panther/matlab')) 9 | addpath(genpath('./../deep_panther/submodules/minvo')) 10 | addpath(genpath('./utils')) 11 | 12 | center=[0.5,0.5,0.5]; 13 | side=[1.0 1.0 1.0]; 14 | box =getAb_Box3D(center,side); 15 | A=box.A; 16 | b=box.b; 17 | [V,nr,nre]=lcon2vert(A,b,[],[]); 18 | V=V'; %my convention 19 | Aeq=[1 1 1; 20 | -1 1 1]; 21 | beq=[1;0.1]; 22 | 23 | 24 | % Aeq=[1 0 0]; 25 | % beq=0.0; 26 | 27 | plot3dConvHullAndVertices(V, 0.02) 28 | 29 | syms x y z 30 | for i=1:size(Aeq,1) 31 | fimplicit3(Aeq(i,:)*[x;y;z]-beq(i,1),[-1 1 -1 1 -1 1],'EdgeColor','none','FaceAlpha',.5) 32 | end 33 | 34 | 35 | z = sdpvar(size(A,2),1); 36 | 37 | 38 | A=[A;Aeq;-Aeq]; 39 | b=[b;beq;-beq]; 40 | 41 | %%Remove constraints that are redundant 42 | %Eq 1.5 of https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/167108/1/thesisFinal_MaySzedlak.pdf 43 | %See also https://mathoverflow.net/a/69667 44 | redundant_set=[]; 45 | all_objectives=[]; 46 | for i=size(A,1):-1:1 47 | objective=-A(i,:)*z; %Note that we are maximizing A(i,:)*z 48 | tmp=setdiff(1:size(A,1),i); %all the rows except the i-th one 49 | constraints=[A(tmp,:)*z<=b(tmp) A(i,:)*z<=(b(i)+1)]; %The +1 prevents the problem to become unbounded because of the change in the constraints 50 | optimize(constraints,objective,sdpsettings('solver','mosek','verbose',false)); 51 | obj=value(objective) 52 | all_objectives=[all_objectives obj]; 53 | 54 | if((-obj)<=b(i)) %Note that I need -obj because we are maximizing 55 | disp("deleting") 56 | i 57 | A(i,:)=[]; %Note that I need to do it here (instead of batched at the end) because if there are two constraints that are equal --> don't wanna delete both 58 | b(i,:)=[]; 59 | end 60 | end 61 | 62 | %%Now let's find the equality set 63 | % Section 5.2 of https://www.researchgate.net/publication/268373838_Polyhedral_Tools_for_Control 64 | % Eq. 1.5 of https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/167108/1/thesisFinal_MaySzedlak.pdf 65 | % See also Definition 2.16 of https://sites.math.washington.edu/~thomas/teaching/m583_s2008_web/main.pdf 66 | z = sdpvar(size(A,2),1); 67 | equality_set=[]; 68 | all_objectives=[]; 69 | 70 | for i=1:size(A,1) 71 | 72 | objective=(A(i,:)*z-b(i)); 73 | constraints=[A*z<=b]; 74 | s=optimize(constraints,objective,sdpsettings('solver','mosek')); 75 | obj=value(objective); 76 | all_objectives=[all_objectives obj] 77 | 78 | %https://yalmip.github.io/command/optimize/ 79 | if s.problem == 0 80 | % disp('Solver thinks it is feasible') 81 | elseif s.problem == 1 82 | error('Solver thinks it is infeasible') 83 | else 84 | error('Something else happened') 85 | end 86 | 87 | tol=1e-6; 88 | assert(obj<=tol,"obj is %f",obj) 89 | if(obj>-tol) %objective is zero. %Note that the objective is always negative 90 | equality_set=[equality_set i]; 91 | end 92 | end 93 | 94 | 95 | %remove the ones that are in the equality set 96 | A_eq_set=A(equality_set,:); 97 | b_eq_set=b(equality_set,:); 98 | A(equality_set,:)=[]; 99 | b(equality_set,:)=[]; 100 | 101 | NA_eq_set=null(A_eq_set); 102 | x0=pinv(A_eq_set)*b_eq_set; 103 | bbb=b-A*x0; 104 | AAA=A*NA_eq_set; 105 | 106 | 107 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | **/*.asv 2 | **/*.lyx~ 3 | 4 | # Byte-compiled / optimized / DLL files 5 | __pycache__/ 6 | *.py[cod] 7 | *$py.class 8 | 9 | # C extensions 10 | *.so 11 | 12 | # Distribution / packaging 13 | .Python 14 | build/ 15 | develop-eggs/ 16 | dist/ 17 | downloads/ 18 | eggs/ 19 | .eggs/ 20 | lib/ 21 | lib64/ 22 | parts/ 23 | sdist/ 24 | var/ 25 | wheels/ 26 | share/python-wheels/ 27 | *.egg-info/ 28 | .installed.cfg 29 | *.egg 30 | MANIFEST 31 | 32 | # PyInstaller 33 | # Usually these files are written by a python script from a template 34 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 35 | *.manifest 36 | *.spec 37 | 38 | # Installer logs 39 | pip-log.txt 40 | pip-delete-this-directory.txt 41 | 42 | # Unit test / coverage reports 43 | htmlcov/ 44 | .tox/ 45 | .nox/ 46 | .coverage 47 | .coverage.* 48 | .cache 49 | nosetests.xml 50 | coverage.xml 51 | *.cover 52 | *.py,cover 53 | .hypothesis/ 54 | .pytest_cache/ 55 | cover/ 56 | 57 | # Translations 58 | *.mo 59 | *.pot 60 | 61 | # Django stuff: 62 | *.log 63 | local_settings.py 64 | db.sqlite3 65 | db.sqlite3-journal 66 | 67 | # Flask stuff: 68 | instance/ 69 | .webassets-cache 70 | 71 | # Scrapy stuff: 72 | .scrapy 73 | 74 | # Sphinx documentation 75 | docs/_build/ 76 | 77 | # PyBuilder 78 | .pybuilder/ 79 | target/ 80 | 81 | # Jupyter Notebook 82 | .ipynb_checkpoints 83 | 84 | # IPython 85 | profile_default/ 86 | ipython_config.py 87 | 88 | # pyenv 89 | # For a library or package, you might want to ignore these files since the code is 90 | # intended to run in multiple environments; otherwise, check them in: 91 | # .python-version 92 | 93 | # pipenv 94 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 95 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 96 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 97 | # install all needed dependencies. 98 | #Pipfile.lock 99 | 100 | # poetry 101 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 102 | # This is especially recommended for binary packages to ensure reproducibility, and is more 103 | # commonly ignored for libraries. 104 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 105 | #poetry.lock 106 | 107 | # pdm 108 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 109 | #pdm.lock 110 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 111 | # in version control. 112 | # https://pdm.fming.dev/#use-with-ide 113 | .pdm.toml 114 | 115 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 116 | __pypackages__/ 117 | 118 | # Celery stuff 119 | celerybeat-schedule 120 | celerybeat.pid 121 | 122 | # SageMath parsed files 123 | *.sage.py 124 | 125 | # Environments 126 | .env 127 | .venv 128 | env/ 129 | venv/ 130 | ENV/ 131 | env.bak/ 132 | venv.bak/ 133 | .asv 134 | 135 | # Spyder project settings 136 | .spyderproject 137 | .spyproject 138 | 139 | # Rope project settings 140 | .ropeproject 141 | 142 | # mkdocs documentation 143 | /site 144 | 145 | # mypy 146 | .mypy_cache/ 147 | .dmypy.json 148 | dmypy.json 149 | 150 | # Pyre type checker 151 | .pyre/ 152 | 153 | # pytype static type analyzer 154 | .pytype/ 155 | 156 | # Cython debug symbols 157 | cython_debug/ 158 | 159 | # PyCharm 160 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 161 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 162 | # and can be added to the global gitignore or merged into this file. For a more nuclear 163 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 164 | #.idea/ 165 | -------------------------------------------------------------------------------- /examples/scripts/plot_time_results.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | close all; clear; clc; 7 | 8 | set(0,'DefaultFigureWindowStyle','normal') %'normal' 'docked' 9 | set(0,'defaulttextInterpreter','latex'); 10 | set(groot, 'defaultAxesTickLabelInterpreter','latex'); set(groot, 'defaultLegendInterpreter','latex'); 11 | set(0,'defaultfigurecolor',[1 1 1]) 12 | 13 | addpath(genpath('./matlab/')) 14 | 15 | lin=readtable('./results/times_lin.csv'); 16 | qp=readtable('./results/times_qp.csv'); 17 | soc=readtable('./results/times_soc.csv'); 18 | lmi=readtable('./results/times_lmi.csv'); 19 | 20 | default_colors = colororder(); 21 | 22 | 23 | linewidth=1.2; 24 | w=297; 25 | w_soc=466; 26 | position=[228 722 w 427]; 27 | position_soc=[228 722 w_soc 427]; 28 | tmp=[w w w w_soc] 29 | tmp/sum(tmp) %This is for latex 30 | % %Linear CONSTRAINTS 31 | figure; hold on; legend; 32 | num_color=1; 33 | for i=sort(unique(lin.r_A1)','descend') 34 | if(i==1 || i==3000)%|| r_M==200 35 | continue 36 | end 37 | indexes=(lin.r_A1==i); 38 | plot(lin.k(indexes), 1e3*lin.Time(indexes),'-o','DisplayName',['$r=',num2str(i),'$'], 'LineWidth',linewidth, 'MarkerFaceColor',default_colors(num_color,:),'MarkerEdgeColor',default_colors(num_color,:)) 39 | xlabel('$k$'); ylabel('Time (ms)') 40 | title("\textbf{Linear Constraints}") 41 | legend('Location','northwest') 42 | num_color=num_color+1; 43 | end 44 | set(gcf, 'Position', position); 45 | export_fig time_linear.png -m2.5 46 | 47 | % 48 | % %QP CONSTRAINTS 49 | figure; hold on; legend; 50 | num_color=1; 51 | for i=sort(unique(qp.eta)','descend') 52 | indexes=(qp.eta==i); 53 | plot(qp.k(indexes), 1e3*qp.Time(indexes),'-o','DisplayName',['$\eta=',num2str(i),'$'], 'LineWidth',linewidth, 'MarkerFaceColor',default_colors(num_color,:),'MarkerEdgeColor',default_colors(num_color,:)) 54 | xlabel('$k$'); ylabel('Time (ms)') 55 | title("\textbf{QP Constraints}") 56 | legend('Location','northwest') 57 | num_color=num_color+1; 58 | end 59 | set(gcf, 'Position', position); 60 | export_fig time_qp.png -m2.5 61 | 62 | %SOC CONSTRAINTS 63 | % tiledlayout(2,2); 64 | figure; hold on; 65 | for mu=sort(unique(soc.mu)','ascend') 66 | if(mu==1 || mu==200 || mu==400 )%|| r_M==200 67 | continue 68 | end 69 | nexttile; hold on; legend; 70 | num_color=1; 71 | for r_M=sort(unique(soc.r_M)','descend') 72 | if(r_M==1 )%|| r_M==200 73 | continue 74 | end 75 | indexes=(soc.r_M==r_M & soc.mu==mu); %r_{\emph{\textbf M}} 76 | plot(soc.k(indexes), 1e3*soc.Time(indexes),'-o','DisplayName',['$r=',num2str(r_M),'$'], 'LineWidth',linewidth, 'MarkerFaceColor',default_colors(num_color,:),'MarkerEdgeColor',default_colors(num_color,:)) 77 | xlabel('$k$'); ylabel('Time (ms)') 78 | title(['$\mu=',num2str(mu),'$']) 79 | legend('Location','northwest') 80 | num_color=num_color+1; 81 | end 82 | end 83 | sgtitle('\textbf{SOC Constraints}', 'Interpreter','latex') 84 | set(gcf, 'Position', position_soc); 85 | export_fig time_soc.png -m2.5 86 | 87 | % %LMI CONSTRAINTS 88 | num_color=1; 89 | figure; hold on; legend; 90 | for i=sort(unique(lmi.r_F)','descend') 91 | indexes=(lmi.r_F==i); 92 | plot(lmi.k(indexes), 1e3*lmi.Time(indexes),'-o','DisplayName',['$r=',num2str(i),'$'], 'LineWidth',linewidth, 'MarkerFaceColor',default_colors(num_color,:),'MarkerEdgeColor',default_colors(num_color,:)) 93 | xlabel('$k$'); ylabel('Time (ms)') 94 | title("\textbf{LMI Constraint}") 95 | legend('Location','northwest') 96 | num_color=num_color+1; 97 | end 98 | set(gcf, 'Position', position); 99 | export_fig time_lmi.png -m2.5 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/maxdistcolor/sRGB_to_OSAUCS.m: -------------------------------------------------------------------------------- 1 | function [Ljg,XYZ] = sRGB_to_OSAUCS(rgb,isd,isc,test) 2 | % Convert a matrix of sRGB R G B values to OSA-UCS L j g values. 3 | % 4 | % (c) 2020-2023 Stephen Cobeldick 5 | % 6 | %%% Syntax: 7 | % Ljg = sRGB_to_OSAUCS(rgb) 8 | % Ljg = sRGB_to_OSAUCS(rgb,isd) 9 | % Ljg = sRGB_to_OSAUCS(rgb,isd,isc) 10 | % 11 | % If the output is being used for calculating the Euclidean color distance 12 | % (i.e. deltaE) use isd=true, so that L is NOT divided by sqrt(2). 13 | % 14 | % The reference formula divides by zero when Y0^(1/3)==2/3 (dark colors), 15 | % this unfortunate numeric discontinuity can be avoided with isc=true. 16 | % 17 | % 18 | % 19 | % 20 | %% Inputs and Outputs 21 | % 22 | %%% Input Argument (*==default): 23 | % rgb = NumericArray, size Nx3 or RxCx3, where the last dimension 24 | % encodes sRGB values [R,G,B] in the range 0<=RGB<=1. 25 | % isd = LogicalScalar, true/false* = Euclidean distance/reference output values. 26 | % isc = LogicalScalar, true/false* = modified continuous/reference output values. 27 | % 28 | %%% Output Argument: 29 | % Ljg = Numeric Array, same size as , where the last dimension 30 | % encodes OSA-UCS values [L,j,g]. 31 | % 32 | % See also SRGB_TO_CAM02UCS SRGB_TO_OKLAB SRGB_TO_CIELAB 33 | % CIELAB_TO_SRGB CIELAB_TO_DIN99 CIELAB_TO_DIN99O 34 | % MAXDISTCOLOR MAXDISTCOLOR_VIEW MAXDISTCOLOR_DEMO 35 | 36 | %% Input Wrangling %% 37 | % 38 | isz = size(rgb); 39 | assert(isnumeric(rgb),... 40 | 'SC:sRGB_to_OSAUCS:rgb:NotNumeric',... 41 | '1st input array must be numeric.') 42 | assert(isreal(rgb),... 43 | 'SC:sRGB_to_OSAUCS:rgb:ComplexValue',... 44 | '1st input cannot be complex.') 45 | assert(isz(end)==3,... 46 | 'SC:sRGB_to_OSAUCS:rgb:InvalidSize',... 47 | '1st input last dimension must have size 3 (e.g. Nx3 or RxCx3).') 48 | rgb = reshape(rgb,[],3); 49 | assert(all(rgb(:)>=0&rgb(:)<=1),... 50 | 'SC:sRGB_to_OSAUCS:rgb:OutOfRange',... 51 | '1st input values must be within the range 0<=rgb<=1') 52 | % 53 | if ~isfloat(rgb) 54 | rgb = double(rgb); 55 | end 56 | % 57 | assert(nargin<2||ismember(isd,0:1),... 58 | 'SC:sRGB_to_OSAUCS:isd:NotScalarLogical',... 59 | 'Second input must be true/false.') 60 | ddd = 2-(nargin>1&&isd); 61 | % 62 | assert(nargin<3||ismember(isc,0:1),... 63 | 'SC:sRGB_to_OSAUCS:isc:NotScalarLogical',... 64 | 'Third input must be true/false.') 65 | ccc = 30*(nargin>2&&isc); 66 | % 67 | %% RGB2Ljg %% 68 | % 69 | M = [... Standard sRGB to XYZ matrix: 70 | 0.4124,0.3576,0.1805;... 71 | 0.2126,0.7152,0.0722;... 72 | 0.0193,0.1192,0.9505]; 73 | % source: IEC 61966-2-1:1999 74 | % 75 | % M = [... High-precision sRGB to XYZ matrix: 76 | % 0.4124564,0.3575761,0.1804375;... 77 | % 0.2126729,0.7151522,0.0721750;... 78 | % 0.0193339,0.1191920,0.9503041]; 79 | % source: 80 | % 81 | % RGB2XYZ 82 | if nargin>3 && isequal(isz,0:3) 83 | isz = size(test); 84 | XYZ = test; 85 | else 86 | XYZ = 100 * sGammaInv(rgb) * M.'; 87 | end 88 | % 89 | % XYZ2Ljg 90 | xyz = bsxfun(@rdivide,XYZ,sum(XYZ,2)); 91 | xyz(isnan(xyz)) = 0; 92 | % 93 | K = 1.8103 + (xyz(:,1:2).^2)*[4.4934;4.3034] - ... 94 | prod(xyz(:,1:2),2)*4.276 - xyz(:,1:2)*[1.3744;2.5643]; 95 | Y0 = K.*XYZ(:,2); 96 | Lp = 5.9*(nthroot(Y0,3)-2/3 + 0.042*nthroot(Y0-30,3)); 97 | L = (Lp-14.3993)./sqrt(ddd); 98 | %C = 1 + (0.042*nthroot(Y0-30,3))./(nthroot(Y0,3)-2/3); % !!!!! divide by zero !!!!! 99 | C = 1 + (0.042*nthroot(Y0-30,3))./(nthroot(max(ccc,Y0),3)-2/3); 100 | tmp = nthroot(XYZ*[0.799,0.4194,-0.1648;-0.4493,1.3265,0.0927;-0.1149,0.3394,0.717].',3); 101 | a = tmp*[-13.7;17.7;-4]; 102 | b = tmp*[1.7;8;-9.7]; 103 | Ljg = reshape([L,C.*b,C.*a],isz); 104 | % 105 | end 106 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%sRGB_to_OSAUCS 107 | function out = sGammaInv(inp) 108 | % Inverse gamma correction: Nx3 sRGB -> Nx3 linear RGB. 109 | idx = inp > 0.04045; 110 | out = inp / 12.92; 111 | out(idx) = real(power((inp(idx) + 0.055) / 1.055, 2.4)); 112 | end 113 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%sGammaInv -------------------------------------------------------------------------------- /examples/scripts/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | pkill -f tmux 4 | 5 | 6 | trap "exit" INT 7 | set -e 8 | SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) 9 | 10 | cd $SCRIPT_DIR 11 | rm -rf results 12 | mkdir results 13 | 14 | #TRAINING 15 | tmuxp load train.yaml 16 | 17 | #TESTING, one by one to get a more accurate computation time 18 | cd .. 19 | 20 | #We use the -O flag to remove the asserts, see https://docs.python.org/3/using/cmdline.html#cmdoption-O 21 | python -O main.py --method RAYEN --dimension_dataset 2 --weight_soft_cost 0 --train False 22 | python -O main.py --method RAYEN_old --dimension_dataset 2 --weight_soft_cost 0 --train False 23 | python -O main.py --method UU --dimension_dataset 2 --weight_soft_cost 0 --train False 24 | python -O main.py --method UU --dimension_dataset 2 --weight_soft_cost 10 --train False 25 | python -O main.py --method UU --dimension_dataset 2 --weight_soft_cost 100 --train False 26 | python -O main.py --method UU --dimension_dataset 2 --weight_soft_cost 1000 --train False 27 | python -O main.py --method UU --dimension_dataset 2 --weight_soft_cost 5000 --train False 28 | python -O main.py --method UP --dimension_dataset 2 --weight_soft_cost 0 --train False 29 | python -O main.py --method UP --dimension_dataset 2 --weight_soft_cost 10 --train False 30 | python -O main.py --method UP --dimension_dataset 2 --weight_soft_cost 100 --train False 31 | python -O main.py --method UP --dimension_dataset 2 --weight_soft_cost 1000 --train False 32 | python -O main.py --method UP --dimension_dataset 2 --weight_soft_cost 5000 --train False 33 | python -O main.py --method DC3 --dimension_dataset 2 --weight_soft_cost 0 --train False 34 | python -O main.py --method DC3 --dimension_dataset 2 --weight_soft_cost 10 --train False 35 | python -O main.py --method DC3 --dimension_dataset 2 --weight_soft_cost 100 --train False 36 | python -O main.py --method DC3 --dimension_dataset 2 --weight_soft_cost 1000 --train False 37 | python -O main.py --method DC3 --dimension_dataset 2 --weight_soft_cost 5000 --train False 38 | python -O main.py --method PP --dimension_dataset 2 --weight_soft_cost 0 --train False 39 | python -O main.py --method Bar --dimension_dataset 2 --weight_soft_cost 0 --train False 40 | 41 | 42 | #Note that the Bar method cannot be used with quadratic constraints 43 | python -O main.py --method RAYEN --dimension_dataset 3 --weight_soft_cost 0 --train False 44 | python -O main.py --method RAYEN_old --dimension_dataset 3 --weight_soft_cost 0 --train False 45 | python -O main.py --method UU --dimension_dataset 3 --weight_soft_cost 0 --train False 46 | python -O main.py --method UU --dimension_dataset 3 --weight_soft_cost 10 --train False 47 | python -O main.py --method UU --dimension_dataset 3 --weight_soft_cost 100 --train False 48 | python -O main.py --method UU --dimension_dataset 3 --weight_soft_cost 1000 --train False 49 | python -O main.py --method UU --dimension_dataset 3 --weight_soft_cost 5000 --train False 50 | python -O main.py --method UP --dimension_dataset 3 --weight_soft_cost 0 --train False 51 | python -O main.py --method UP --dimension_dataset 3 --weight_soft_cost 10 --train False 52 | python -O main.py --method UP --dimension_dataset 3 --weight_soft_cost 100 --train False 53 | python -O main.py --method UP --dimension_dataset 3 --weight_soft_cost 1000 --train False 54 | python -O main.py --method UP --dimension_dataset 3 --weight_soft_cost 5000 --train False 55 | python -O main.py --method DC3 --dimension_dataset 3 --weight_soft_cost 0 --train False 56 | python -O main.py --method DC3 --dimension_dataset 3 --weight_soft_cost 10 --train False 57 | python -O main.py --method DC3 --dimension_dataset 3 --weight_soft_cost 100 --train False 58 | python -O main.py --method DC3 --dimension_dataset 3 --weight_soft_cost 1000 --train False 59 | python -O main.py --method DC3 --dimension_dataset 3 --weight_soft_cost 5000 --train False 60 | python -O main.py --method PP --dimension_dataset 3 --weight_soft_cost 0 --train False 61 | 62 | 63 | cd $SCRIPT_DIR 64 | python merge_all_results.py 65 | 66 | python -O time_analysis.py -------------------------------------------------------------------------------- /examples/scripts/matlab/getCorridorAndParamsSpline.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | 7 | function [allA, allb, allV, p0, t0,tf,deg_pos, num_seg, num_of_seg_per_region, use_quadratic]=getCorridorAndParamsSpline(dimension) 8 | 9 | if(dimension==2) 10 | rng('default'); rng(6); %So that random is repeatable 11 | % P=3*[0 5.5 7.5 10.5 12.5; 12 | % 0 4 0 0 4 ]; 13 | P=3*[0 5.5 7.5 12.5; 14 | 0 4 0 4 ]; 15 | radius=4.0; 16 | num_of_seg_per_region=2; 17 | samples_per_step=5; 18 | use_quadratic=false; 19 | tf=35.0; 20 | deg_pos=2; %Note that I'm including the jerk cost. If I use deg_pos=2, then the jerk cost will always be zero 21 | else 22 | rng('default'); rng(2); %So that random is repeatable 23 | P=3*[0 1 2 3 4 3 0; 24 | 0 1 1 2 4 4 4; 25 | 0 1 1 1 4 1 0]; 26 | radius=4*1.3; 27 | num_of_seg_per_region=2; 28 | samples_per_step=3; 29 | use_quadratic=true; 30 | tf=15.0; 31 | deg_pos=3; %Note that I'm including the jerk cost. If I use deg_pos=2, then the jerk cost will always be zero 32 | p0=[0.5;0]; 33 | end 34 | 35 | t0=0.0; 36 | 37 | allA={}; 38 | allb={}; 39 | allV={}; 40 | 41 | steps=2; 42 | 43 | for i=1:(size(P,2)-1) 44 | if(dimension==3) 45 | [A, b, V]=getABVerticesgivenP1P2(P(:,i),P(:,i+1), 1.0, 1.0, 2); 46 | else 47 | [A, b, V]=getAbVerticesPolyhedronAroundP1P2(P(:,i),P(:,i+1), steps, samples_per_step, radius); 48 | end 49 | % 50 | allA{end+1}=A; 51 | allb{end+1}=b; 52 | allV{end+1}=V; 53 | end 54 | 55 | num_of_regions=size(allA,2); 56 | num_seg =num_of_seg_per_region*num_of_regions; %First region has only 1 segment 57 | 58 | % 0.8*P(:,1) + 0.2*P(:,2); 59 | % pf=mean(allV{end},2); %0.2*P(:,end-1) + 0.8*P(:,end); 60 | 61 | if(dimension==2) 62 | p0=[5.0;1]; 63 | else 64 | p0=mean(allV{1},2); 65 | end 66 | 67 | figure; 68 | hold on; 69 | alpha=0.2; 70 | 71 | for i=1:size(allA,2) 72 | plotregion(-allA{i},- allb{i}, [], [],'g',alpha) 73 | end 74 | 75 | % plotPolyhedron(P,'r') 76 | camlight 77 | lighting phong 78 | 79 | if(dimension==2) 80 | delta=4.0; 81 | xlim([min(P(1,:))-delta,max(P(1,:))+delta]); 82 | ylim([min(P(2,:))-delta,max(P(2,:))+delta]); 83 | else 84 | delta=4.0; 85 | xlim([min(P(1,:))-delta,max(P(1,:))+delta]); 86 | ylim([min(P(2,:))-delta,max(P(2,:))+delta]); 87 | zlim([min(P(3,:))-delta,max(P(3,:))+delta]); 88 | view(-71,40) 89 | xlabel('x') 90 | ylabel('y') 91 | zlabel('z') 92 | end 93 | 94 | if(dimension==2) 95 | scatter(p0(1),p0(2),'filled','b') 96 | % scatter(pf(1),pf(2),'filled','r') 97 | end 98 | 99 | if(dimension==3) 100 | zlim([min(P(3,:))-delta,max(P(3,:))+delta]); 101 | plotSphere(p0,0.2,'b') 102 | % plotSphere(pf,0.2,'r') 103 | end 104 | 105 | end 106 | 107 | %This gives the vertices of a polyhedron around the line p1-->p2 108 | function [A, b, V]=getAbVerticesPolyhedronAroundP1P2(p1,p2, steps, samples_per_step, radius) 109 | 110 | 111 | dimension=size(p1,1); 112 | 113 | % samples_per_step=5; 114 | % radius=0.5; 115 | all_points=[]; 116 | 117 | for alpha=linspace(0,1,steps) 118 | point=alpha*p1 + (1-alpha)*p2; 119 | results=point + radius*uniformSampleInUnitBall(dimension,samples_per_step); 120 | all_points=[all_points results]; 121 | end 122 | 123 | [k1,av1] = convhull(all_points'); 124 | k_all_unique=unique(k1); 125 | 126 | V=all_points(:,k_all_unique); %V contains the vertices 127 | 128 | [A,b,Aeq,beq]=vert2lcon(V'); 129 | 130 | assert(numel(Aeq)==0) 131 | assert(numel(beq)==0) 132 | 133 | % for tmp=k_all_unique' 134 | % tmp 135 | % P(:,tmp) 136 | % plotSphere(P(:,tmp), 0.03, 'r'); 137 | % end 138 | 139 | 140 | end -------------------------------------------------------------------------------- /examples/other/plot_examples_with_mayavi.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from mayavi import mlab 3 | import utils 4 | import examples_sets 5 | import random 6 | import copy 7 | from joblib import Parallel, delayed 8 | 9 | index_example=1 10 | 11 | cs=examples_sets.getExample(index_example) 12 | 13 | # @np.vectorize 14 | def get_conditions(x): 15 | conditions=[]; 16 | all_is_ineq_condition=[] 17 | 18 | if(cs.has_linear_ineq_constraints): 19 | for i in range(cs.lc.A1.shape[0]): 20 | expression=cs.lc.A1[i,:]@x-cs.lc.b1[i,0] 21 | conditions.append(expression[0]) 22 | all_is_ineq_condition.append(True) 23 | 24 | if(cs.has_linear_eq_constraints): 25 | for i in range(cs.lc.A2.shape[0]): 26 | expression=cs.lc.A2[i,:]@x-cs.lc.b2[i,0] 27 | conditions.append(expression[0]) 28 | all_is_ineq_condition.append(False) 29 | 30 | 31 | if(cs.has_quadratic_constraints): 32 | for qc in cs.qcs: 33 | expression=0.5*x.T@qc.P@x + qc.q.T@x + qc.r 34 | conditions.append(expression[0,0]) 35 | all_is_ineq_condition.append(True) 36 | 37 | 38 | if(cs.has_soc_constraints): 39 | for soc in cs.socs: 40 | expression=np.linalg.norm(soc.M@x +soc.s) -soc.c.T@x - soc.d 41 | conditions.append(expression[0,0]) 42 | all_is_ineq_condition.append(True) 43 | 44 | 45 | return conditions, all_is_ineq_condition 46 | 47 | num_points=100j 48 | dist_min=0 49 | dist_max=2 50 | all_x, all_y, all_z = np.mgrid[-dist_min:dist_max:num_points, -dist_min:dist_max:num_points, -dist_min:dist_max:num_points] 51 | 52 | num_points=int(num_points.imag) 53 | 54 | tmp, all_is_ineq_condition=get_conditions(np.zeros((3,1))) 55 | num_of_conditions=len(tmp) 56 | 57 | ### Create dummy list of numpy arrays 58 | conditions=[]; 59 | for i in range(num_of_conditions): 60 | zero_tensor=np.zeros((num_points,num_points,num_points)) 61 | conditions.append(zero_tensor) 62 | 63 | def my_function(i, j, k, conditions): 64 | print((i,j,k)) 65 | x=np.array([[all_x[i,j,k]],[all_y[i,j,k]],[all_z[i,j,k]]]); 66 | tmp,_=get_conditions(x) 67 | return tmp 68 | 69 | 70 | result=Parallel(n_jobs=40)(delayed(my_function)(i, j, k, conditions) for i in range(all_x.shape[0]) for j in range(all_x.shape[1]) for k in range(all_x.shape[2])) 71 | 72 | print(len(result)) 73 | print(len(result[0])) 74 | 75 | index=0; 76 | for i in range(all_x.shape[0]): 77 | for j in range(all_x.shape[1]): 78 | for k in range(all_x.shape[2]): 79 | 80 | for index_cond in range(num_of_conditions):#For each condition 81 | conditions[index_cond][i,j,k]=result[index][index_cond] 82 | 83 | index+=1 84 | 85 | 86 | conditions_processed=[] 87 | for i in range(len(conditions)): 88 | 89 | cond_i=copy.deepcopy(conditions[i]) 90 | for j in range(len(conditions)): 91 | if (i==j): 92 | continue 93 | cond_j=conditions[j] 94 | 95 | cond_j_is_ineq=all_is_ineq_condition[j] 96 | 97 | if(cond_j_is_ineq): 98 | cond_i[cond_j>0.0]=None #See https://stackoverflow.com/questions/40461045/mayavi-combining-two-implicit-3d-surfaces 99 | else: 100 | cond_i[cond_j>0.0]=None 101 | cond_i[cond_j<0.0]=None 102 | 103 | conditions_processed.append(cond_i) 104 | 105 | 106 | for i in range(len(conditions_processed)): 107 | # print(i) 108 | cond_i=conditions_processed[i] 109 | # print(cond_i) 110 | if(np.isnan(cond_i).all()): 111 | continue 112 | maximum=np.nanmax(cond_i) 113 | minimum=np.nanmin(cond_i) 114 | if(maximum<0 or minimum>0): 115 | print("Continue") 116 | continue; 117 | color=tuple(random.random() for _ in range(3)) 118 | tmp=mlab.contour3d(all_x,all_y,all_z,cond_i, contours = [0], color=color, opacity=1.0) 119 | # tmp.actor.property.interpolation = 'phong' #https://stackoverflow.com/a/31754643 120 | # tmp.actor.property.specular = 0.9 121 | # tmp.actor.property.specular_power = 128 122 | 123 | lensoffset=0.0 124 | xx = yy = zz = np.arange(0.0,1.5,0.1) 125 | xy = xz = yx = yz = zx = zy = np.zeros_like(xx) 126 | mlab.plot3d(yx,yy+lensoffset,yz,line_width=0.01,tube_radius=0.02) 127 | mlab.plot3d(zx,zy+lensoffset,zz,line_width=0.01,tube_radius=0.02) 128 | mlab.plot3d(xx,xy+lensoffset,xz,line_width=0.01,tube_radius=0.02) 129 | 130 | print("Showing!") 131 | mlab.show() 132 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/polytopes/intersectionHull.m: -------------------------------------------------------------------------------- 1 | function I = intersectionHull(varargin) 2 | %intersectionHull - computes a bounded convex polyhedron resulting from the 3 | %intersection of other convex polyhedra. The other polyhedra need not individually 4 | %be bounded. Only the final intersection resulting from them must be bounded. 5 | %Any number of polyhedra can be input to the intersection operation. They can 6 | %be expressed using either vertices or linear in/equalities, according to 7 | %the input scheme, 8 | % 9 | % 10 | % I = intersectionHull('vert', V1, 'lcon', A2,b2, 'lcon', A3,b3,Aeq3,,beq3,...) 11 | % 12 | %The arguments specifying different polyhedra are separated using labels 13 | %'vert' and 'lcon'. The label 'vert' signifies that an input polyhedron will 14 | %be expressed using vertices and is to be followed by any string of input arguments 15 | %accepted by vert2lcon(). The label 'lcon' signifies that an input polyhedron will 16 | %be expressed using linear constraints and is to be followed by any string of 17 | %input arguments accepted by lcon2vert(). 18 | % 19 | %The output, I, is a struct containing fields 20 | % 21 | % I.vert: A matrix whose rows are the vertices of the polyhedron formed from 22 | % the intersection. 23 | % I.lcon: The quadruplet of linear constraint data {A,b,Aeq,beq} 24 | % describing the polyhedral intersection. 25 | % 26 | %EXAMPLE 1: This example computes the intersection of a unit square and an 27 | %oblique 2D line segment, both expressed in terms of their vertices. 28 | % 29 | % V1=dec2bin(0:2^2-1,2)-'0'; %vertices of unit square 30 | % 31 | % V2=[1,1;0,-1]; %vertices of 2D line segment 32 | % 33 | % I=intersectionHull('vert',V1,'vert',V2); %compute intersection 34 | % 35 | %The intersection is another line segment with vertices 36 | % 37 | % >> I.vert 38 | % 39 | % ans = 40 | % 41 | % 0.5000 0 42 | % 1.0000 1.0000 43 | % 44 | %EXAMPLE 2: This example computes the intersection of a unit cube, expressed in 45 | %terms of its vertices, and an infinite oblique 3D line, expressed in terms of linear equalities. 46 | %Note that the line is an unbounded polyhedron. This is okay, since we know in advance 47 | %that the final polyhedron formed from the intersection is bounded. 48 | % 49 | % V=dec2bin(0:2^3-1,3)-'0'; %vertices of unit cube 50 | % 51 | % Aeq=[1 -1 0; 0 1 -1]; beq=[0;0]; %oblique line in 3D 52 | % 53 | % I=intersectionHull('vert',V,'lcon',[],[],Aeq,beq); %compute intersection 54 | % 55 | %Once again, the intersection is a line segment. Its vertices are 56 | % 57 | % >> I.vert %vertices of line segment of intersection 58 | % 59 | % ans = 60 | % 61 | % 0.0000 0.0000 0.0000 62 | % 1.0000 1.0000 1.0000 63 | 64 | 65 | %%%%begin parsing 66 | 67 | if isnumeric(varargin{1}) 68 | TOL=varargin{1}; 69 | varargin(1)=[]; 70 | else 71 | TOL=[]; 72 | end 73 | 74 | N=length(varargin); 75 | idxType = [find(cellfun(@ischar,varargin)),N+1]; 76 | 77 | L=length(idxType)-1; 78 | S(L).type=[]; 79 | S(L).args={}; 80 | S(L).A=[]; 81 | S(L).b=[]; 82 | S(L).Aeq=[]; 83 | S(L).beq=[]; 84 | 85 | for i=1:L 86 | 87 | j=idxType(i); 88 | k=idxType(i+1); 89 | S(i).type=varargin{j}; 90 | S(i).args=varargin(j+1:k-1); 91 | 92 | if isempty(S(i).args) 93 | error 'Syntax error - arguments missing' 94 | end 95 | 96 | lcon=cell(1,4); 97 | 98 | switch S(i).type 99 | 100 | case 'vert' 101 | 102 | [lcon{1:4}] = vert2lcon(S(i).args{:}); 103 | 104 | case 'lcon' 105 | 106 | lcon(1:k-j-1) = S(i).args; 107 | 108 | case 'qlcon' %deliberately undocumented - no point in using this 109 | 110 | lcon(1:k-j-1) = S(i).args(2:end); 111 | 112 | otherwise 113 | 114 | error(['Unrecognized representation label of polyhedron ' num2str(i)]); 115 | 116 | end 117 | 118 | 119 | [S(i).A, S(i).b, S(i).Aeq, S(i).beq] = deal(lcon{:}); 120 | 121 | end 122 | 123 | 124 | %%%%end parsing 125 | 126 | 127 | A=vertcat(S.A); 128 | b=vertcat(S.b); 129 | Aeq=vertcat(S.Aeq); 130 | beq=vertcat(S.beq); 131 | 132 | [V,nr,nre]=lcon2vert(A,b,Aeq,beq,TOL); 133 | 134 | I.vert=V; 135 | I.lcon={A(nr,:),b(nr,:), Aeq(nre,:),beq(nre,:)}; 136 | 137 | 138 | -------------------------------------------------------------------------------- /examples/other/FindGurobi.cmake: -------------------------------------------------------------------------------- 1 | # TAKEN FROM https://gitlab.inf.unibe.ch/CGG-public/cmake-library/-/blob/master/finders/FindGurobi.cmake 2 | # Once done this will define 3 | # Gurobi_FOUND - System has Gurobi 4 | # Targets: 5 | # Gurobi::GurobiC - only the C interface 6 | # Gurobi::GurobiCXX - C and C++ interface 7 | 8 | find_path(GUROBI_HOME 9 | NAMES include/gurobi_c++.h 10 | PATHS 11 | $ENV{GUROBI_HOME} 12 | "/opt/gurobi/linux64/" 13 | NO_DEFAULT_PATH # avoid finding /usr 14 | ) 15 | 16 | find_path(GUROBI_INCLUDE_DIR 17 | NAMES gurobi_c++.h 18 | HINTS 19 | "${GUROBI_HOME}/include" 20 | ) 21 | mark_as_advanced(GUROBI_INCLUDE_DIR) 22 | 23 | set(GUROBI_BIN_DIR "${GUROBI_HOME}/bin") 24 | set(GUROBI_LIB_DIR "${GUROBI_HOME}/lib") 25 | 26 | if (WIN32) 27 | file(GLOB GUROBI_LIBRARY_LIST 28 | RELATIVE ${GUROBI_BIN_DIR} 29 | ${GUROBI_BIN_DIR}/gurobi*.dll 30 | ) 31 | else() 32 | file(GLOB GUROBI_LIBRARY_LIST 33 | RELATIVE ${GUROBI_LIB_DIR} 34 | ${GUROBI_LIB_DIR}/libgurobi*.so 35 | ) 36 | endif() 37 | 38 | # Ignore libgurobiXY_light.so, libgurobi.so (without version): 39 | string(REGEX MATCHALL 40 | "gurobi([0-9]+)\\..*" 41 | GUROBI_LIBRARY_LIST 42 | "${GUROBI_LIBRARY_LIST}" 43 | ) 44 | 45 | string(REGEX REPLACE 46 | ".*gurobi([0-9]+)\\..*" 47 | "\\1" 48 | GUROBI_LIBRARY_VERSIONS 49 | "${GUROBI_LIBRARY_LIST}") 50 | list(LENGTH GUROBI_LIBRARY_VERSIONS GUROBI_NUMVER) 51 | 52 | #message("GUROBI LIB VERSIONS: ${GUROBI_LIBRARY_VERSIONS}") 53 | 54 | if (GUROBI_NUMVER EQUAL 0) 55 | message(STATUS "Found no Gurobi library version, GUROBI_HOME = ${GUROBI_HOME}.") 56 | elseif (GUROBI_NUMVER EQUAL 1) 57 | list(GET GUROBI_LIBRARY_VERSIONS 0 GUROBI_LIBRARY_VERSION) 58 | else() 59 | # none or more than one versioned library -let's try without suffix, 60 | # maybe the user added a symlink to the desired library 61 | message(STATUS "Found more than one Gurobi library version (${GUROBI_LIBRARY_VERSIONS}), trying without suffix. Set GUROBI_LIBRARY if you want to pick a certain one.") 62 | set(GUROBI_LIBRARY_VERSION "") 63 | endif() 64 | 65 | if (WIN32) 66 | find_library(GUROBI_LIBRARY 67 | NAMES "gurobi${GUROBI_LIBRARY_VERSION}" 68 | PATHS 69 | ${GUROBI_BIN_DIR} 70 | ) 71 | find_library(GUROBI_IMPLIB 72 | NAMES "gurobi${GUROBI_LIBRARY_VERSION}" 73 | PATHS 74 | ${GUROBI_LIB_DIR} 75 | ) 76 | mark_as_advanced(GUROBI_IMPLIB) 77 | else () 78 | find_library(GUROBI_LIBRARY 79 | NAMES "gurobi${GUROBI_LIBRARY_VERSION}" 80 | PATHS 81 | ${GUROBI_LIB_DIR} 82 | ) 83 | endif() 84 | mark_as_advanced(GUROBI_LIBRARY) 85 | 86 | if(GUROBI_LIBRARY AND NOT TARGET Gurobi::GurobiC) 87 | add_library(Gurobi::GurobiC SHARED IMPORTED) 88 | target_include_directories(Gurobi::GurobiC INTERFACE ${GUROBI_INCLUDE_DIR}) 89 | set_target_properties(Gurobi::GurobiC PROPERTIES IMPORTED_LOCATION ${GUROBI_LIBRARY}) 90 | if (GUROBI_IMPLIB) 91 | set_target_properties(Gurobi::GurobiC PROPERTIES IMPORTED_IMPLIB ${GUROBI_IMPLIB}) 92 | endif() 93 | endif() 94 | 95 | # Gurobi ships with some compiled versions of its C++ library for specific 96 | # compilers, however it also comes with the source code. We will compile 97 | # the source code outselves -- this is much safer, as it guarantees the same 98 | # compiler version and flags. 99 | # (Note: doing this is motivated by actual sometimes-subtle ABI compatibility bugs) 100 | 101 | find_path(GUROBI_SRC_DIR NAMES "Model.h" PATHS "${GUROBI_HOME}/src/cpp/") 102 | mark_as_advanced(GUROBI_SRC_DIR) 103 | 104 | file(GLOB GUROBI_CXX_SRC CONFIGURE_DEPENDS ${GUROBI_SRC_DIR}/*.cpp) 105 | if(TARGET Gurobi::GurobiC AND GUROBI_CXX_SRC AND NOT TARGET Gurobi::GurobiCXX) 106 | add_library(GurobiCXX STATIC EXCLUDE_FROM_ALL ${GUROBI_CXX_SRC}) 107 | add_library(Gurobi::GurobiCXX ALIAS GurobiCXX) 108 | 109 | if(MSVC) 110 | target_compile_definitions(GurobiCXX PRIVATE "WIN64") 111 | endif() 112 | 113 | target_include_directories(GurobiCXX PUBLIC ${GUROBI_INCLUDE_DIR}) 114 | target_link_libraries(GurobiCXX PUBLIC Gurobi::GurobiC) 115 | # We need to be able to link this into a shared library: 116 | set_target_properties(GurobiCXX PROPERTIES POSITION_INDEPENDENT_CODE ON) 117 | 118 | endif() 119 | 120 | # legacy support: 121 | set(GUROBI_INCLUDE_DIRS "${GUROBI_INCLUDE_DIR}") 122 | set(GUROBI_LIBRARIES Gurobi::GurobiC Gurobi::GurobiCXX) 123 | 124 | include(FindPackageHandleStandardArgs) 125 | find_package_handle_standard_args(Gurobi DEFAULT_MSG GUROBI_LIBRARY GUROBI_INCLUDE_DIR GUROBI_SRC_DIR) 126 | -------------------------------------------------------------------------------- /examples/scripts/matlab/test_idea_equality_and_inequality.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | close all; clc;clear; 7 | doSetup(); 8 | set(0,'DefaultFigureWindowStyle','normal') %'normal' 'docked' 9 | 10 | %Example of a plane and a plyhedron 11 | radius=1.0; 12 | % V=radius*uniformSampleInUnitBall(3,10); 13 | 14 | V=[ 0.3345 0.1760 -0.4391 0.3967 0.0207 -0.7195 0.3139 -0.4449 -0.3855 -0.2624 15 | 0.4675 0.7414 -0.0131 -0.2303 0.6983 0.0610 -0.0889 -0.1703 0.7373 0.7028 16 | 0.1872 -0.5452 -0.5910 -0.0152 0.2959 -0.0226 -0.6567 -0.2332 -0.4466 0.2622]; 17 | 18 | V=keepOnlyVerticesConvexHull(V); 19 | [A,b,Aeq,beq]=vert2lcon(V'); 20 | assert(numel(Aeq)==0); assert(numel(beq)==0) 21 | 22 | 23 | 24 | 25 | % center=[0.5,0.5,0.5]; 26 | % side=[1.0 1.0 1.0]; 27 | % box =getAb_Box3D(center,side) 28 | % A=box.A; 29 | % b=box.b; 30 | % [V,nr,nre]=lcon2vert(A,b,[],[]) 31 | % V=V'; %my convention 32 | 33 | Aeq=[0.5 0.5 1]; 34 | beq=0.2; 35 | 36 | plot3dConvHullAndVertices(V, 'g', 0.02) 37 | 38 | syms x y z 39 | tol_for_aliasing=0.0001; 40 | plane_color=[0.98, 0.33, 0.06]; 41 | fimplicit3(Aeq*[x;y;z]-beq+tol_for_aliasing,[-0.5 0.4 -0.2 0.8 -1 1],'FaceColor',plane_color,'EdgeColor','none','FaceAlpha',0.3) 42 | 43 | NAeq=null(Aeq); 44 | 45 | %See slide 8-6 here: https://see.stanford.edu/materials/lsoeldsee263/08-min-norm.pdf 46 | y1=pinv(Aeq)*beq 47 | % plotSphere(x0,0.05, 'r'); 48 | 49 | 50 | %%%% Plot Ellipsoid (x-c)'*E*(x-c)<=1 51 | % E=[3 0 0; 52 | % 0 3 0; 53 | % 0 0 5]; 54 | % c=[-0.5;0.8;0]; 55 | E=[5 0 0; 56 | 0 5 0; 57 | 0 0 7]; 58 | c=[-0.45;0.5;0]; 59 | syms x y z 60 | tmp=[x y z]'; 61 | P=2*E; 62 | q=(-2*E*c) 63 | r=c'*E*c-1 64 | 65 | f=0.5*tmp'*P*tmp + q'*tmp + r ; 66 | 67 | ellipsoid_color=[0.94, 0.85, 1]; 68 | fimplicit3( f ,'EdgeColor','none','FaceAlpha',0.2 ,'MeshDensity',40 ,'LineWidth',0.0001,'FaceColor',ellipsoid_color) 69 | %%%% 70 | 71 | 72 | 73 | Ap=A*NAeq; 74 | bp=b-A*y1; 75 | [Vp,nr,nre]=lcon2vert(Ap,bp,[],[]); 76 | Vp=Vp'; %my convention 77 | figure;hold on; 78 | 79 | % IF YOU WANNA VISUALIZE THE CONSTRAINTS: 80 | % for i=1:size(Ap,1) 81 | % fimplicit(Ap(i,:)*[x;y]-bp(i),[-1 1 -1 1]) 82 | % end 83 | 84 | plot2dConvHullAndVertices(Vp,'r','r'); 85 | 86 | %%%%%%%%%%%% 87 | 88 | tmp=NAeq*[x y]' + y1; 89 | f=0.5*tmp'*P*tmp + q'*tmp + r 90 | fimplicit(f,'Color','b','LineWidth',2.0) 91 | 92 | %%%%%%%%%%%%%%%%%%%%%% 93 | 94 | Vp_lifted=[]; 95 | for i=1:size(Vp,2) 96 | Vp_lifted=[Vp_lifted NAeq*Vp(:,i)+y1]; 97 | end 98 | axis off 99 | figure(2) 100 | export_fig intersection.png -m2.5 101 | 102 | figure(1) 103 | shp = alphaShape(Vp_lifted(1,:)',Vp_lifted(2,:)',Vp_lifted(3,:)'); 104 | % plot(shp,'EdgeColor','none','FaceColor','r') 105 | axis off; axis equal; 106 | view([63.6 20.59]) 107 | set(gcf,'Position',[394 288 1603 981]) 108 | 109 | 110 | export_fig polyhedron3d.png -m2.5 111 | 112 | % plotAxesArrowsT(length, w_T_b) 113 | 114 | % myaa 115 | % print('PeaksSurface','-dpng','-r400') 116 | % handle=gcf; 117 | % name_figure='test.pdf' 118 | % set(handle,'Units','inches'); 119 | % screenposition = get(handle,'Position'); 120 | % set(gcf,'PaperPosition',[0 0 screenposition(3:4)],'PaperSize',[screenposition(3:4)]); 121 | % print (name_figure, '-dpdf', '-painters') 122 | % exportgraphics(handle,name_figure,'BackgroundColor','none','ContentType','vector') 123 | % set(gcf,'renderer','Painters') 124 | % system(['pdfcrop ',name_figure,'.pdf ',name_figure,'.pdf']); 125 | 126 | 127 | % myaa 128 | % exportAsPdf(gca,'polyhedron_3d') 129 | 130 | % lighting phong 131 | % myaa %removes aliasing 132 | 133 | % export_fig('polyhedron_3d','-pdf') 134 | % 135 | % exportAsPdf(gcf,'polyhedron_3d') 136 | 137 | % tmp=gca; 138 | % if (size(findobj(tmp.Children,'Type','Light'))<1) %If still no light in the subplot 139 | % camlight %create light 140 | % end 141 | % lighting phong 142 | 143 | % myaa 144 | % fill3(Vp_lifted(1,:),Vp_lifted(2,:),Vp_lifted(3,:),'r') 145 | % plot3(Vp_lifted(1,:), Vp_lifted(2,:), Vp_lifted(3,:)) 146 | 147 | %Note that the whole space is covered, but randomly sampling means that 148 | %with a high probability, the sample will only fall in the middle of the 149 | %polyhedron 150 | 151 | % %Let's take a bunch of random 2d points in the triangle: 152 | % all_q=[]; 153 | % for i=1:1000 154 | % lambda=rand(size(VVV,2),1); lambda=lambda/sum(lambda); %lambda has the barycentric coordinates 155 | % all_q=[all_q VVV*lambda]; 156 | % end 157 | % 158 | % scatter(all_q(1,:), all_q(2,:)); 159 | 160 | %Let's lift them up to the original 3d space 161 | % all_q_lifted=[]; 162 | % for i=1:size(all_q,2) 163 | % q=all_q(:,i); 164 | % q_lifted=NAeq*q+x0; 165 | % all_q_lifted=[all_q_lifted q_lifted]; 166 | % end 167 | % 168 | % figure(1) 169 | % scatter3(all_q_lifted(1,:), all_q_lifted(2,:), all_q_lifted(3,:)); 170 | 171 | 172 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/maxdistcolor/test_CIELab.m: -------------------------------------------------------------------------------- 1 | function test_CIELab() 2 | % Test CIELab conversions against sample values. 3 | % 4 | % (c) 2017-2023 Stephen Cobeldick 5 | % 6 | % See also TESTFUN_MDC SRGB_TO_CIELAB CIELAB_TO_SRGB 7 | 8 | fprintf('Running %s...\n',mfilename) 9 | % 10 | ist = nan(0:3); 11 | fnh = @sRGB_to_CIELab; 12 | % 13 | % Reference: 14 | chk = testfun_mdc(fnh,{'X','Y','Z'},5e-16); 15 | chk([0,0,0], fnh, @i, [0,0,0]) % sRGB->XYZ 16 | chk([1,1,1], fnh, @i, [0.9505,1,1.089]) % sRGB->XYZ 17 | chk([0.1,0.2,0.3], fnh, @i, [0.0291913093288767, 0.0310952343831350, 0.0737531562712688]) % sRGB->XYZ 18 | chk([0.4,0.5,0.6], fnh, @i, [0.1888337010968200, 0.2043291062158660, 0.3308567751593710]) % sRGB->XYZ 19 | chk([0.7,0.8,0.9], fnh, @i, [0.5428069959016100, 0.5839508165288460, 0.8290577762222190]) % sRGB->XYZ 20 | chk([0.9,0.8,0.7], fnh, @i, [0.6215193929671810, 0.6316059288531500, 0.5129862620029020]) % sRGB->XYZ 21 | chk([0.6,0.5,0.4], fnh, @i, [0.2318925351756980, 0.2303983615184730, 0.1579529964002000]) % sRGB->XYZ 22 | chk([0.3,0.2,0.1], fnh, @i, [0.0438511299462668, 0.0399707790777515, 0.0148862957326157]) % sRGB->XYZ 23 | chk([0.04,0.02,0.01], fnh, @i, [0.00197004643962848, 0.00182120743034056, 0.000979953560371517]) % sRGB->XYZ 24 | chk([0.03,0.02,0.01], fnh, @i, [0.00165085139318885, 0.00165665634674923, 0.000965015479876161]) % sRGB->XYZ 25 | chk([0.02,0.02,0.01], fnh, @i, [0.00133165634674923, 0.00149210526315789, 0.000950077399380805]) % sRGB->XYZ 26 | chk([0.01,0.02,0.01], fnh, @i, [0.00101246130030960, 0.00132755417956656, 0.000935139318885449]) % sRGB->XYZ 27 | chk() 28 | % 29 | % Reference: !!! not accurate sRGB->CIELab !!! 30 | chk = testfun_mdc(fnh,{'L*','a*','b*'},1e-2); % sRGB->CIELab 31 | chk([0.2,0.4,0.6], fnh, [42.0099857768665, -0.147373964354935, -32.8445986139017]) % sRGB->CIELab 32 | chk([0.1,0.2,0.3], fnh, [20.4772929305063, -0.649398365518222, -18.63125696672030]) % sRGB->CIELab 33 | chk([0.9,0.8,0.7], fnh, [83.5268033115757, +4.986300524367880, +15.98132781035020]) % sRGB->CIELab 34 | chk([0.04,0.02,0.01], fnh, [1.64501872414861, +0.978923677309326, +1.43484259764831]) % sRGB->CIELab 35 | chk() 36 | % 37 | % Reference: 38 | chk = testfun_mdc(fnh,{'L*','a*','b*'},5e-5); 39 | chk(ist, [0,0,0], fnh, [0,0,0]) % XYZ->CIELab 40 | chk(ist, [0.1,0.2,0.3], fnh, [51.8372, -56.3591, -13.1812]) % XYZ->CIELab 41 | chk(ist, [0.4,0.5,0.6], fnh, [76.0693, -22.1559, -5.2284]) % XYZ->CIELab 42 | chk(ist, [0.7,0.8,0.9], fnh, [91.6849, -12.6255, -2.0335]) % XYZ->CIELab 43 | chk(ist, [0.9,0.9,0.9], fnh, [95.9968, +8.2439, +5.4008]) % XYZ->CIELab 44 | chk(ist, [0.9,0.8,0.7], fnh, [91.6849, +26.8297, +13.0496]) % XYZ->CIELab 45 | chk(ist, [0.6,0.5,0.4], fnh, [76.0693, +32.0677, +15.5004]) % XYZ->CIELab 46 | chk(ist, [0.3,0.2,0.1], fnh, [51.8372, +48.0307, +26.7254]) % XYZ->CIELab 47 | chk(ist, [0.006,0.005,0.004], fnh, [4.5165, +5.1109, +2.0656]) % XYZ->CIELab 48 | chk(ist, [0.003,0.002,0.001], fnh, [1.8066, +4.5022, +1.6845]) % XYZ->CIELab 49 | chk(ist, [0.001,0.002,0.003], fnh, [1.8066, -3.6906, -1.1762]) % XYZ->CIELab 50 | chk(ist, [0.004,0.005,0.006], fnh, [4.5165, -3.0819, -0.7951]) % XYZ->CIELab 51 | chk() 52 | % 53 | % Reference: colorspacious.cspace_convert() 54 | chk = testfun_mdc(fnh,{'L*','a*','b*'},5e-13); 55 | chk([0,0,0], fnh, [0,0,0]) % sRGB->CIELab 56 | % Note that COLORSPACIOUS does not use the sRGB->XYZ matrix defined in 57 | % IEC 61966, but instead uses the inverse of the XYZ->sRGB matrix, which 58 | % differs slightly after the fourth decimal digits. If we modify 59 | % COLORSPACIOUS to use the standard matrix, these are the results: 60 | chk([0.1,0.2,0.3], fnh, [20.47729293050626, -0.6477509840630280, -18.635499506210450]) % sRGB->CIELab 61 | chk([0.4,0.5,0.6], fnh, [52.32317935858785, -2.7423177310761380, -16.660446906196704]) % sRGB->CIELab 62 | chk([0.7,0.8,0.9], fnh, [80.95794732626959, -3.0900960941968036, -15.460893809249443]) % sRGB->CIELab 63 | chk([0.9,0.8,0.7], fnh, [83.52680331157573, +4.9908664399307410, +15.973229378445343]) % sRGB->CIELab 64 | chk([0.6,0.5,0.4], fnh, [55.11334675196463, +5.9069971684236890, +17.521347800346620]) % sRGB->CIELab 65 | chk([0.3,0.2,0.1], fnh, [23.66177930857765, +8.3731659005595310, +20.559154151582720]) % sRGB->CIELab 66 | chk([0.04,0.02,0.01], fnh, [1.6450899266139203, +0.97922076074241640, +1.4346858921472416]) % sRGB->CIELab 67 | chk([0.03,0.02,0.01], fnh, [1.4964515422543272, +0.31234842724181533, +1.1997794911020432]) % sRGB->CIELab 68 | chk([0.02,0.02,0.01], fnh, [1.3478131578947340, -0.35452390625879960, +0.9648730900568503]) % sRGB->CIELab 69 | chk([0.01,0.02,0.01], fnh, [1.1991747735351446, -1.02139623975940050, +0.7299666890116518]) % sRGB->CIELab 70 | chk() 71 | % Unmodified COLORSPACIOUS results (quite different!): 72 | chk = testfun_mdc(fnh,{'L*','a*','b*'},5e-3); 73 | chk([0.1,0.2,0.3], fnh, [20.476868826608616, -0.6479456121413207, -18.635946031759400]) % sRGB->CIELab 74 | chk([0.4,0.5,0.6], fnh, [52.322284675856210, -2.7417959374207435, -16.661347434860897]) % sRGB->CIELab 75 | chk([0.7,0.8,0.9], fnh, [80.956635208316530, -3.0889674319084515, -15.462182842749028]) % sRGB->CIELab 76 | chk([0.9,0.8,0.7], fnh, [83.525303672498030, +4.9934265610782070, +15.971981007339942]) % sRGB->CIELab 77 | chk([0.6,0.5,0.4], fnh, [55.112248327000200, +5.9090692144755375, +17.520501311385940]) % sRGB->CIELab 78 | chk([0.3,0.2,0.1], fnh, [23.661122238740510, +8.3747095232966360, +20.558842534531685]) % sRGB->CIELab 79 | chk() 80 | % 81 | end 82 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%test_CIELab -------------------------------------------------------------------------------- /examples/cost_computer.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------------------------- 2 | # Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | # See LICENSE file for the license information 4 | # -------------------------------------------------------------------------- 5 | 6 | import torch 7 | import torch.nn as nn 8 | import numpy as np 9 | import scipy 10 | import cvxpy as cp 11 | import math 12 | from cvxpylayers.torch import CvxpyLayer 13 | import random 14 | import copy 15 | 16 | import fixpath #Following this example: https://github.com/tartley/colorama/blob/master/demos/demo01.py 17 | from rayen import utils 18 | 19 | 20 | 21 | class CostComputer(nn.Module): #Using nn.Module to be able to use register_buffer (and hence to be able to have the to() method) 22 | def __init__(self, cs): 23 | super().__init__() 24 | 25 | if(cs.has_quadratic_constraints): 26 | all_P, all_q, all_r = utils.getAllPqrFromQcs(cs.qcs) 27 | self.register_buffer("all_P", torch.Tensor(np.array(all_P))) 28 | self.register_buffer("all_q", torch.Tensor(np.array(all_q))) 29 | self.register_buffer("all_r", torch.Tensor(np.array(all_r))) 30 | 31 | if(cs.has_soc_constraints): 32 | all_M, all_s, all_c, all_d = utils.getAllMscdFromSocs(cs.socs) 33 | self.register_buffer("all_M", torch.Tensor(np.array(all_M))) 34 | self.register_buffer("all_s", torch.Tensor(np.array(all_s))) 35 | self.register_buffer("all_c", torch.Tensor(np.array(all_c))) 36 | self.register_buffer("all_d", torch.Tensor(np.array(all_d))) 37 | 38 | #See https://discuss.pytorch.org/t/model-cuda-does-not-convert-all-variables-to-cuda/114733/9 39 | # and https://discuss.pytorch.org/t/keeping-constant-value-in-module-on-correct-device/10129 40 | self.register_buffer("A_p", torch.Tensor(cs.A_p)) 41 | self.register_buffer("b_p", torch.Tensor(cs.b_p)) 42 | self.register_buffer("yp", torch.Tensor(cs.yp)) 43 | self.register_buffer("NA_E", torch.Tensor(cs.NA_E)) 44 | self.register_buffer("z0", torch.Tensor(cs.z0)) 45 | 46 | self.has_linear_ineq_constraints=cs.has_linear_ineq_constraints 47 | self.has_linear_eq_constraints=cs.has_linear_eq_constraints 48 | self.has_quadratic_constraints=cs.has_quadratic_constraints 49 | self.has_soc_constraints=cs.has_soc_constraints 50 | self.has_lmi_constraints=cs.has_lmi_constraints 51 | 52 | if self.has_linear_ineq_constraints: 53 | self.register_buffer("A1", torch.Tensor(cs.lc.A1)) 54 | self.register_buffer("b1", torch.Tensor(cs.lc.b1)) 55 | 56 | if self.has_linear_eq_constraints: 57 | self.register_buffer("A2", torch.Tensor(cs.lc.A2)) 58 | self.register_buffer("b2", torch.Tensor(cs.lc.b2)) 59 | 60 | def getyFromz(self, z): 61 | y=self.NA_E@z + self.yp 62 | return y 63 | 64 | #This function below assumes that y is in the plane spanned by the columns of NA_E!! 65 | # def getzFromy(self, y): 66 | # z=self.NA_E.T@(y - self.yp) 67 | # return z 68 | 69 | def getSumSoftCostAllSamples(self, y): 70 | ################## STACK THE VALUES OF ALL THE INEQUALITIES 71 | all_inequalities=torch.empty((y.shape[0],0,1), device=y.device) 72 | ##### Ap*z<=bp 73 | # z=self.getzFromy(y); #I cannot use this function, since this function assumes that y lies in \mathcal{Y}_L 74 | #I could use it for DC3, but not for the "UU method" 75 | # all_inequalities=torch.cat((all_inequalities, self.A_p@z-self.b_p), dim=1); 76 | ##### A1*y<=b1 77 | if self.has_linear_ineq_constraints: 78 | all_inequalities=torch.cat((all_inequalities, self.A1@y-self.b1), dim=1); 79 | 80 | ##### g(y)<=0 81 | if(self.has_quadratic_constraints): 82 | for i in range(self.all_P.shape[0]): 83 | P=self.all_P[i,:,:] 84 | q=self.all_q[i,:,:] 85 | r=self.all_r[i,:,:] 86 | all_inequalities=torch.cat((all_inequalities, utils.quadExpression(y=y,P=P,q=q,r=r)), dim=1) 87 | 88 | if(self.has_soc_constraints): 89 | for i in range(self.all_M.shape[0]): 90 | M=self.all_M[i,:,:] 91 | s=self.all_s[i,:,:] 92 | c=self.all_c[i,:,:] 93 | d=self.all_d[i,:,:] 94 | 95 | lhs=torch.linalg.vector_norm(M@y + s, dim=1, keepdim=True) - c.T@y - d 96 | 97 | all_inequalities=torch.cat((all_inequalities, lhs), dim=1) 98 | 99 | if(self.has_lmi_constraints): 100 | raise NotImplementedError 101 | 102 | soft_cost = torch.sum(torch.square(torch.nn.functional.relu(all_inequalities))) 103 | 104 | if self.has_linear_eq_constraints: 105 | soft_cost_equalities = torch.sum(torch.square(self.A2@y-self.b2)); #This is zero for the DC3 method, and nonzero for the UU method 106 | soft_cost += soft_cost_equalities 107 | 108 | ######################################################################## 109 | 110 | return soft_cost 111 | 112 | 113 | def getSumObjCostAllSamples(self, y, Pobj, qobj, robj): 114 | tmp=utils.quadExpression(y=y,P=Pobj,q=qobj,r=robj) 115 | assert tmp.shape==(y.shape[0], 1, 1) 116 | 117 | return torch.sum(tmp) 118 | 119 | def getSumSupervisedCostAllSamples(self, y, y_predicted): 120 | return torch.sum(torch.square(y-y_predicted)) 121 | 122 | def getSumLossAllSamples(self, params, y, y_predicted, Pobj, qobj, robj, isTesting=False): 123 | 124 | loss=0.0; 125 | 126 | if(params['use_supervised']): 127 | loss += self.getSumSupervisedCostAllSamples(y, y_predicted) 128 | else: 129 | loss += self.getSumObjCostAllSamples(y_predicted, Pobj, qobj, robj) 130 | 131 | if (isTesting==False and params['weight_soft_cost']>0): #I need the term params['weight_soft_cost']>0 because the soft cost is not implemented for LMI constraints yet 132 | loss += params['weight_soft_cost']*self.getSumSoftCostAllSamples(y_predicted) 133 | 134 | # loss=params['use_supervised']*self.getSumSupervisedCostAllSamples(y, y_predicted) + \ 135 | # (1-isTesting)*params['weight_soft_cost']*self.getSumSoftCostAllSamples(y_predicted) + \ 136 | # (1-params['use_supervised'])*self.getSumObjCostAllSamples(y_predicted, Pobj, qobj, robj) 137 | 138 | return loss 139 | -------------------------------------------------------------------------------- /examples/examples_sets.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------------------------- 2 | # Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | # See LICENSE file for the license information 4 | # -------------------------------------------------------------------------- 5 | 6 | import numpy as np 7 | # import sys, os 8 | # sys.path.insert(1, os.path.join(sys.path[0], '..')) #See first comment of this answer: https://stackoverflow.com/a/11158224 9 | # import constraints 10 | import fixpath 11 | from rayen import constraints 12 | 13 | 14 | def getCube(): 15 | A1=np.array([ [1.0, 0, 0], 16 | [0, 1.0, 0], 17 | [0, 0, 1.0], 18 | [-1.0, 0, 0], 19 | [0, -1.0, 0], 20 | [0, 0, -1.0]]); 21 | 22 | b1=np.array([[1.0], 23 | [1.0], 24 | [1.0], 25 | [0], 26 | [0], 27 | [0]]) 28 | 29 | return A1, b1 30 | 31 | #Ellipsoid is defined as {x | (x-c)'E(x-c)<=1} 32 | #Where E is a positive semidefinite matrix 33 | def getEllipsoidConstraint(E, c): 34 | #Convert to (1/2)x'P_ix + q_i'x +r_i <=0 35 | P=2*E; 36 | q=(-2*E@c) 37 | r=c.T@E@c-1 38 | return constraints.ConvexQuadraticConstraint(P, q, r) 39 | 40 | #Sphere of radius r centered around c 41 | def getSphereConstraint(r, c): 42 | return getEllipsoidConstraint((1/(r*r))*np.eye(c.shape[0]),c) 43 | 44 | def getParaboloid3DConstraint(): 45 | P=np.array([[1.0, 0.0, 0.0], 46 | [0.0, 1.0, 0.0], 47 | [0.0, 0.0, 0.0]]) 48 | q=np.array([[0.0],[0.0],[-1.0]]) 49 | r=np.array([[0.0]]) 50 | 51 | return constraints.ConvexQuadraticConstraint(P,q,r) 52 | 53 | def getSOC3DConstraint(): 54 | M=np.array([[1.0, 0.0, 0.0], 55 | [0.0, 1.0, 0.0], 56 | [0.0, 0.0, 0.0]]) 57 | s=np.array([[0.0],[0.0],[0.0]]) 58 | c=np.array([[0.0],[0.0],[1.0]]) 59 | d=np.array([[0.0]]) 60 | 61 | return constraints.SOCConstraint(M, s, c, d) 62 | 63 | def getPSDCone3DConstraint(): 64 | #[x y;y z] >> 0 65 | F0=np.array([[1.0, 0.0], 66 | [0.0, 0.0]]) 67 | 68 | F1=np.array([[0.0, 1.0], 69 | [1.0, 0.0]]) 70 | 71 | F2=np.array([[0.0, 0.0], 72 | [0.0, 1.0]]) 73 | 74 | F3=np.array([[0.0, 0.0], 75 | [0.0, 0.0]]) 76 | 77 | return constraints.LMIConstraint([F0, F1, F2, F3]) 78 | 79 | def getNoneLinearConstraints(): 80 | return None, None, None, None 81 | 82 | def getNoneQuadraticConstraints(): 83 | return [], [], [] 84 | 85 | def getExample(example): 86 | 87 | # A1, b1, A2, b2 = getNoneLinearConstraints() 88 | # all_P, all_q, all_r = getNoneQuadraticConstraints() 89 | lc=None 90 | qcs=[] 91 | socs=[] 92 | lmic=None 93 | 94 | if example==0: #A 2D polygon embeded in 3D 95 | A1, b1=getCube() 96 | A2=np.array([[1.0, 1.0, 1.0]]); 97 | b2=np.array([[1.0]]); 98 | lc=constraints.LinearConstraint(A1, b1, A2, b2) 99 | 100 | elif example==1: #A polygon embeded in 3D with an sphere 101 | 102 | A1, b1=getCube() 103 | A2=np.array([[1.0, 1.0, 1.0]]); 104 | b2=np.array([[1.0]]); 105 | lc=constraints.LinearConstraint(A1, b1, A2, b2) 106 | qcs.append(getSphereConstraint(0.8,np.zeros((3,1)))) 107 | 108 | 109 | elif example==2: #Just a sphere 110 | 111 | qcs.append(getSphereConstraint(2.0,np.zeros((3,1)))) 112 | 113 | elif example==3: #Just a paraboloid 114 | 115 | qcs.append(getParaboloid3DConstraint()) 116 | 117 | #A 2d polyhedron 118 | elif (example==4 119 | #A 2d polyhedron with a cirle 120 | or example==5): 121 | A1=np.array([[-1,0], 122 | [0, -1.0], 123 | [0, 1.0], 124 | [0.6, 0.9701]]); 125 | 126 | b1=np.array([[0], 127 | [0], 128 | [1], 129 | [1.2127]]) 130 | 131 | lc=constraints.LinearConstraint(A1, b1, None, None) 132 | 133 | if(example==5): 134 | qcs.append(getSphereConstraint(1.25,np.zeros((2,1)))) 135 | 136 | elif example==6: #The intersection between a cube and two planes 137 | A1, b1=getCube() 138 | A2=np.array([[1.0, 1.0, 1.0], 139 | [-1.0, 1.0, 1.0] ]); 140 | b2=np.array([[1.0],[0.1]]); 141 | lc=constraints.LinearConstraint(A1, b1, A2, b2) 142 | 143 | elif example==7: #Just a plane 144 | A2=np.array([[1.0, 1.0, 1.0]]); 145 | b2=np.array([[1.0]]); 146 | lc=constraints.LinearConstraint(None, None, A2, b2) 147 | 148 | 149 | elif example==8: #Unbounded 2d polyhedron. It has two vertices and two rays 150 | 151 | A1=np.array([[0.0,-1.0], [2.0,-4.0], [-2.0,1.0]]); 152 | b1=np.array([[-2.0], [1.0], [-5.0]]); 153 | lc=constraints.LinearConstraint(A1, b1, None, None) 154 | 155 | elif example==9: #A paraboloid and a plane 156 | qcs.append(getParaboloid3DConstraint()) 157 | 158 | A2=np.array([[1.0, 1.0, 3.0]]); 159 | b2=np.array([[1.0]]); 160 | lc=constraints.LinearConstraint(None, None, A2, b2) 161 | 162 | elif example==10: #A paraboloid and a shpere 163 | qcs.append(getParaboloid3DConstraint()) 164 | qcs.append(getSphereConstraint(2.0,np.zeros((3,1)))) 165 | 166 | elif example==11: #A second-order cone 167 | socs.append(getSOC3DConstraint()) 168 | 169 | elif example==12: #The PSD cone in 3D 170 | lmic = getPSDCone3DConstraint() 171 | 172 | elif example==13: #Many of them 173 | A1=np.array([[-1.0,-1.0,-1.0]]) 174 | b1=np.array([[-1.0]]) 175 | lc=constraints.LinearConstraint(A1, b1, None, None) 176 | E_ellipsoid=np.array([[0.1,0,0], 177 | [0.0,1.0,0.0], 178 | [0.0,0.0,1.0]]) 179 | qcs.append(getEllipsoidConstraint(E_ellipsoid, np.zeros((3,1)))) 180 | socs.append(getSOC3DConstraint()) 181 | lmic = getPSDCone3DConstraint() 182 | 183 | elif example==14: #Many of them 184 | A1=np.array([[-1.0,-1.0,-1.0], 185 | [-1.0,2.0,2.0]]) 186 | b1=np.array([[-1.0],[1.0]]) 187 | lc=constraints.LinearConstraint(A1, b1, None, None) 188 | E_ellipsoid=np.array([[0.6,0,0], 189 | [0.0,1.0,0.0], 190 | [0.0,0.0,1.0]]) 191 | qcs.append(getEllipsoidConstraint(E_ellipsoid, np.zeros((3,1)))) 192 | # qcs.append(getParaboloid3DConstraint()) 193 | # socs.append(getSOC3DConstraint()) 194 | # lmic = getPSDCone3DConstraint() 195 | 196 | else: 197 | raise Exception("Not implemented yet") 198 | 199 | 200 | return constraints.ConvexConstraints(lc=lc, qcs=qcs, socs=socs, lmic=lmic, print_debug_info=False) -------------------------------------------------------------------------------- /examples/create_dataset.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------------------------- 2 | # Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | # See LICENSE file for the license information 4 | # -------------------------------------------------------------------------- 5 | 6 | # Import libraries 7 | import torch 8 | from torch.utils.data import Dataset, DataLoader 9 | from examples_sets import getExample 10 | import numpy as np 11 | import cvxpy as cp 12 | import matplotlib.pyplot as plt 13 | import scipy.io 14 | import time 15 | 16 | import fixpath #Following this example: https://github.com/tartley/colorama/blob/master/demos/demo01.py 17 | from rayen import constraints, utils 18 | 19 | # create custom dataset class 20 | class CustomDataset(Dataset): 21 | def __init__(self, all_x, all_y, all_Pobj, all_qobj, all_robj, all_times_s, all_costs): 22 | self.all_x = [torch.Tensor(item) for item in all_x] 23 | self.all_y = [torch.Tensor(item) for item in all_y] 24 | self.all_Pobj = [torch.Tensor(item) for item in all_Pobj] 25 | self.all_qobj = [torch.Tensor(item) for item in all_qobj] 26 | self.all_robj = [torch.Tensor(item) for item in all_robj] 27 | self.all_times_s = [torch.Tensor(item) for item in all_times_s] 28 | self.all_costs = [torch.Tensor(item) for item in all_costs] 29 | 30 | 31 | def __len__(self): 32 | return len(self.all_y) 33 | 34 | def __getitem__(self, idx): 35 | return self.all_x[idx], self.all_y[idx], self.all_Pobj[idx], self.all_qobj[idx], self.all_robj[idx], self.all_times_s[idx], self.all_costs[idx] 36 | 37 | def getNumelX(self): 38 | return self.all_x[0].shape[0] #Using the first element 39 | 40 | def getNumelY(self): 41 | return self.all_y[0].shape[0] #Using the first element 42 | 43 | # def plot(self, ax): 44 | # dim=self.all_x[0].shape[0] 45 | 46 | # # print(f"x[0]={x[0]}") 47 | 48 | # all_x_np=np.concatenate(self.all_x, axis=1 ) 49 | # all_y_np=np.concatenate(self.all_y, axis=1 ) 50 | 51 | # print(f"dim={dim}") 52 | # print(f"dim={dim}") 53 | 54 | 55 | # if(dim==3): 56 | # ax.scatter3D(all_x_np[0,:], all_x_np[1,:], all_x_np[2,:],color='red') 57 | # ax.scatter3D(all_y_np[0,:], all_y_np[1,:], all_y_np[2,:],color='blue') 58 | 59 | # print(f"all_x_np={all_x_np}") 60 | 61 | # if(dim==2): 62 | # ax.scatter(all_x_np[0,:], all_x_np[1,:],color='red') 63 | # ax.scatter(all_y_np[0,:], all_y_np[1,:],color='blue') 64 | 65 | def createProjectionDataset(num_samples, cs, bbox_half_side): 66 | 67 | all_x=[]; 68 | all_y=[]; 69 | all_Pobj=[]; 70 | all_qobj=[]; 71 | all_robj=[]; 72 | all_times_s=[]; 73 | all_costs=[]; 74 | 75 | for i in range(num_samples): 76 | x=np.random.uniform(low=-bbox_half_side, high=bbox_half_side, size=(cs.k,1)) 77 | all_x.append(x) 78 | start=time.time(); 79 | y, cost = cs.project(x) 80 | all_times_s.append(time.time()-start) 81 | all_costs.append(cost) 82 | all_y.append(y) 83 | 84 | assert x.shape[1]==1 85 | 86 | # ||x-y||^2 = y'*y -2x'*y + x'*x 87 | # Match with 0.5*y'*P_obj*y + q_obj'*y + r_obj 88 | Pobj=2*np.eye(x.shape[0]) 89 | qobj=-2*x 90 | robj=x.T@x 91 | 92 | assert qobj.shape[1]==1 93 | assert robj.shape[1]==1, f"robj.shape={robj.shape}" 94 | 95 | all_Pobj.append(Pobj) 96 | all_qobj.append(qobj) 97 | all_robj.append(robj) 98 | 99 | my_dataset = CustomDataset(all_x, all_y, all_Pobj, all_qobj, all_robj, all_times_s, all_costs) 100 | 101 | 102 | ###plotting stuff 103 | # fig = plt.figure() 104 | # if(dim_ambient_space==3): 105 | # ax = fig.add_subplot(111, projection="3d") 106 | # else: 107 | # ax = fig.add_subplot(111) 108 | # my_dataset.plot(ax); 109 | # plt.show() 110 | ####### 111 | # exit() 112 | 113 | return my_dataset 114 | 115 | def getCorridorConstraints(dimension): 116 | mat = scipy.io.loadmat('./scripts/matlab/corridor_dim'+str(dimension)+'.mat') 117 | 118 | A1=mat['A1']; 119 | b1=mat['b1']; 120 | 121 | A2=mat['A2']; 122 | b2=mat['b2']; 123 | 124 | if(len(mat["all_P"])>0): 125 | all_P=list(mat["all_P"][0]) 126 | all_q=list(mat["all_q"][0]) 127 | all_r=list(mat["all_r"][0]) 128 | else: 129 | all_P=[] 130 | all_q=[] 131 | all_r=[] 132 | 133 | assert A1.ndim==2 134 | assert b1.ndim==2 135 | 136 | assert A2.ndim==2 137 | assert b2.ndim==2 138 | 139 | lc=constraints.LinearConstraint(A1=A1, b1=b1, A2=A2, b2=b2); 140 | 141 | qcs=[]; 142 | for i in range(len(all_P)): 143 | qc=constraints.ConvexQuadraticConstraint(P=all_P[i], q=all_q[i], r=all_r[i]); 144 | qcs.append(qc) 145 | 146 | cs=constraints.ConvexConstraints(lc=lc, qcs=qcs, socs=[], lmic=None) 147 | 148 | return cs 149 | 150 | def getCorridorDatasets(dimension): 151 | 152 | mat = scipy.io.loadmat('./scripts/matlab/corridor_dim'+str(dimension)+'.mat') 153 | 154 | all_x=list(mat["all_x"][0]) 155 | all_y=list(mat["all_y"][0]) 156 | all_Pobj=list(mat["all_Pobj"][0]) 157 | all_qobj=list(mat["all_qobj"][0]) 158 | all_robj=list(mat["all_robj"][0]) 159 | all_times_s=list(mat["all_times_s"][0]) 160 | all_costs=list(mat["all_costs"][0]) 161 | 162 | all_x_out_dist=list(mat["all_x_out_dist"][0]) 163 | all_y_out_dist=list(mat["all_y_out_dist"][0]) 164 | all_Pobj_out_dist=list(mat["all_Pobj_out_dist"][0]) 165 | all_qobj_out_dist=list(mat["all_qobj_out_dist"][0]) 166 | all_robj_out_dist=list(mat["all_robj_out_dist"][0]) 167 | all_times_s_out_dist=list(mat["all_times_s_out_dist"][0]) 168 | all_costs_out_dist=list(mat["all_costs_out_dist"][0]) 169 | 170 | # polyhedron=mat["polyhedron"] 171 | 172 | # A1=polyhedron['A1'][0,0]; 173 | # b1=polyhedron['b1'][0,0]; 174 | 175 | # polyhedron=mat["polyhedron"] 176 | 177 | # A1=polyhedron['A1'][0,0]; 178 | # b1=polyhedron['b1'][0,0]; 179 | 180 | 181 | 182 | assert all_y[0].shape[1]==1 183 | assert all_x[0].shape[1]==1 184 | assert all_x_out_dist[0].shape[1]==1 185 | assert all_y_out_dist[0].shape[1]==1 186 | 187 | 188 | my_dataset = CustomDataset(all_x, all_y, all_Pobj, all_qobj, all_robj, all_times_s, all_costs) 189 | my_dataset_out_dist = CustomDataset(all_x_out_dist, all_y_out_dist, all_Pobj_out_dist, all_qobj_out_dist, all_robj_out_dist, all_times_s_out_dist, all_costs_out_dist) 190 | 191 | 192 | return my_dataset, my_dataset_out_dist 193 | 194 | 195 | 196 | 197 | -------------------------------------------------------------------------------- /examples/test_layer.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------------------------- 2 | # Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | # See LICENSE file for the license information 4 | # -------------------------------------------------------------------------- 5 | 6 | import numpy as np 7 | import torch 8 | import torch.nn as nn 9 | import math 10 | import matplotlib.pyplot as plt 11 | import mpl_toolkits.mplot3d as a3 12 | import matplotlib.colors as colors 13 | import numpy as np 14 | import scipy 15 | import os 16 | import time 17 | 18 | from examples_sets import getExample 19 | import utils_examples 20 | 21 | import fixpath #Following this example: https://github.com/tartley/colorama/blob/master/demos/demo01.py 22 | from rayen import constraints, constraint_module, utils 23 | 24 | methods=['RAYEN_old', 'RAYEN', 'Bar', 'UU', 'PP', 'UP', 'DC3'] 25 | index_examples_to_run=list(range(15)) 26 | ############### 27 | 28 | num_of_examples=len(index_examples_to_run) 29 | rows=math.ceil(math.sqrt(num_of_examples)) 30 | cols=rows 31 | 32 | for method in methods: 33 | utils.printInBoldRed(f"==================== METHOD: {method} ==========================") 34 | fig = plt.figure() 35 | fig.suptitle(method, fontsize=10) 36 | for i in range(num_of_examples): 37 | index_example=index_examples_to_run[i] 38 | utils.printInBoldGreen(f"==================== Example: {index_example} ") 39 | 40 | 41 | constraint=getExample(index_example) 42 | 43 | if(method=='Bar' and (constraint.has_quadratic_constraints or constraint.has_soc_constraints or constraint.has_lmi_constraints)): 44 | continue 45 | 46 | if(method=='DC3' and (constraint.has_soc_constraints or constraint.has_lmi_constraints)): 47 | continue 48 | 49 | # fig = plt.figure() 50 | if(constraint.k==3): 51 | ax = fig.add_subplot(rows,cols,i+1, projection="3d") 52 | if(constraint.has_linear_ineq_constraints): 53 | utils_examples.plot3DPolytopeHRepresentation(constraint.lc.A1,constraint.lc.b1,[-1, 2, -1, 2, -1, 2], ax) 54 | 55 | 56 | else: 57 | ax = fig.add_subplot(rows,cols,i+1) 58 | 59 | if(method=='DC3'): 60 | args_DC3={} 61 | args_DC3['lr'] = 1e-4 62 | args_DC3['eps_converge'] = 1e-4 63 | args_DC3['momentum'] = 0.5 64 | args_DC3['max_steps_training'] = 10 65 | args_DC3['max_steps_testing'] = 50000 #float("inf") 66 | else: 67 | args_DC3 = None 68 | 69 | 70 | my_layer=constraint_module.ConstraintModule(constraint, method=method, create_map=False, args_DC3=args_DC3) 71 | 72 | numel_output_mapper=my_layer.getDimAfterMap() 73 | 74 | num_samples=500 #12000 75 | x_batched=torch.Tensor(num_samples, numel_output_mapper, 1).uniform_(-5.0, 5.0) 76 | 77 | # mapper=nn.Sequential(nn.Linear(x_batched.shape[1], numel_output_mapper)) 78 | # mapper=nn.Sequential() #do nothing. 79 | # my_layer.setMapper(mapper) 80 | 81 | my_layer.eval() #This changes the self.training variable of the module 82 | 83 | time_start=time.time() 84 | result=my_layer(x_batched) 85 | total_time_per_sample= (time.time()-time_start)/num_samples 86 | 87 | result=result.detach().numpy(); 88 | 89 | y0=my_layer.gety0(); 90 | 91 | if(constraint.k==3): 92 | ax.scatter(y0[0,0], y0[1,0], y0[2,0],color='r',s=500) 93 | ax.scatter(result[:,0,0], result[:,1,0], result[:,2,0]) 94 | 95 | 96 | if(constraint.k==2): 97 | ax.scatter(result[:,0,0], result[:,1,0]) 98 | utils_examples.plot2DPolyhedron(constraint.lc.A1,constraint.lc.b1,ax) 99 | 100 | ax.scatter(y0[0,0], y0[1,0]) 101 | # utils_examples.plot2DEllipsoidB(my_layer.B.numpy(),my_layer.z0.numpy(),ax) 102 | ax.set_aspect('equal') 103 | 104 | ax.set_xlim(-0.5,8) 105 | ax.set_ylim(-0.5,8) 106 | 107 | 108 | if method=='RAYEN': 109 | my_dict=constraint.getDataAsDict(); 110 | my_dict["result"]=result 111 | my_dict["total_time_per_sample"]=total_time_per_sample 112 | directory='./examples_mat' 113 | if not os.path.exists(directory): 114 | os.makedirs(directory) 115 | scipy.io.savemat(directory+'/example_'+str(index_example)+'.mat', my_dict) 116 | 117 | utils.printInBoldBlue(f"Example {index_example}, total_time_per_sample={total_time_per_sample}") 118 | 119 | 120 | plt.show() 121 | 122 | 123 | 124 | 125 | 126 | ######OLD 127 | ##This samples different angles 128 | # all_angles = np.arange(0,2*math.pi, 0.01) 129 | # x_batched=torch.empty(len(all_angles), numel_output_mapper, 1) 130 | 131 | # for i in range(x_batched.shape[0]): #for each element of the batch 132 | # theta=all_angles[i] 133 | # if(my_layer.dim==2): 134 | # tmp=torch.Tensor(np.array([[math.cos(theta)],[math.sin(theta)],[3000]])); #Assumming my_layer.dim==2 here 135 | # else: 136 | # raise("Not implemented yet") 137 | # tmp=torch.unsqueeze(tmp, dim=0) 138 | # print(f"tmp.shape={tmp.shape}") 139 | # x_batched[i,:,:]=tmp 140 | 141 | 142 | # E=1.7*np.eye(constraint.k) 143 | # ellipsoid=utils.Ellipsoid(E=E, c=np.zeros((constraint.k,1))) 144 | # cqc_list=[ellipsoid.convertToQuadraticConstraint()] 145 | 146 | # print(f"P={cqc_list[0].P}") 147 | # print(f"q={cqc_list[0].q}") 148 | # print(f"r={cqc_list[0].r}") 149 | 150 | 151 | # if(method=='RAYEN'): 152 | # num_directions=500; #for each direction you have several samples 153 | # x_batched=torch.empty(0, numel_output_mapper, 1) 154 | # for i in range(num_directions): #for each direction 155 | # direction=utils.uniformSampleInUnitSphere(my_layer.dim) 156 | # for scalar in list(np.linspace(-8.0, 8.0, num=100)): 157 | # scalar_np=np.array([[scalar]]) 158 | # direction_and_scalar=np.concatenate((direction,scalar_np), axis=0); 159 | # tmp=torch.Tensor(direction_and_scalar) 160 | # tmp=torch.unsqueeze(tmp, dim=0) 161 | # # print(f"direction_and_scalar={direction_and_scalar}") 162 | # x_batched=torch.cat((x_batched, tmp), axis=0) 163 | 164 | # if(method=='Bar' or method=='PP' or method=='UP'): 165 | # x_batched=torch.empty(0, numel_output_mapper, 1) 166 | 167 | # for i in range(1000): 168 | # # sample_lambda = utils.runif_in_simplex(my_layer.num_vertices); 169 | # # sample_mu = np.random.uniform(0.0,2.5,my_layer.num_rays); 170 | # # sample=np.concatenate((sample_lambda, sample_mu)); 171 | # sample=np.random.uniform(-5,5,numel_output_mapper); 172 | # sample=torch.Tensor(np.expand_dims(sample, axis=1)) 173 | # sample=torch.unsqueeze(sample, dim=0) 174 | # x_batched=torch.cat((x_batched, sample), axis=0) 175 | -------------------------------------------------------------------------------- /examples/other/testing_sdp.py: -------------------------------------------------------------------------------- 1 | # Import packages. 2 | import cvxpy as cp 3 | import numpy as np 4 | from numpy import linalg as LA 5 | import utils 6 | import torch 7 | import scipy 8 | 9 | 10 | A=torch.Tensor([ 11 | 12 | [[2.0, -12.0], 13 | [1.0, -5.0]], 14 | 15 | [[-7.0, 0.0], 16 | [0.0, 5.0]], 17 | 18 | [[-2.0, 0.0], 19 | [0.0, 6.0]] 20 | 21 | ]) 22 | 23 | guess_v = torch.nn.functional.normalize(torch.rand(A.shape[1],1), dim=0) 24 | 25 | lamb = utils.findLargestEigenvalue(A, guess_v) 26 | 27 | L, V = torch.linalg.eig(A) 28 | print(L) 29 | 30 | print(f"Found lambda={lamb}") 31 | 32 | exit() 33 | 34 | dim=3 35 | 36 | for trial in range(200): 37 | 38 | # Generate a random SDP. 39 | tmp = np.random.rand(dim, dim) 40 | # tmp = np.random.randint(0, 20, size=(dim, dim)) 41 | H = np.dot(tmp, tmp.transpose()) + np.eye(dim) #H is psd by construction 42 | 43 | tmp = np.random.rand(dim, dim) 44 | # tmp = np.random.randint(0, 20, size=(dim, dim)) 45 | S=(tmp+tmp.T)/2.0 #M is symmetric by construction 46 | 47 | # tmp = np.random.rand(dim, dim) 48 | # M = np.dot(tmp, tmp.transpose()) #H is psd by construction 49 | 50 | Hinv=np.linalg.inv(H) 51 | 52 | print("\n\n-------") 53 | print("-------") 54 | 55 | print(f"H={H}\n") 56 | print(f"S={S}\n") 57 | 58 | 59 | 60 | # kappa_opt = cp.Variable() 61 | # constraints = [(kappa_opt*H+S) >> 0, kappa_opt>=0] 62 | 63 | # prob = cp.Problem(cp.Minimize(kappa_opt), constraints) 64 | # prob.solve(solver=cp.SCS, verbose=True, eps=1e-7) 65 | 66 | # if(prob.status=='unbounded'): 67 | # utils.printInBoldRed("Unbounded!!!!") #When kappa is the decision variable, there is no way the result is unbounded 68 | # else: 69 | # kappa_opt=kappa_opt.value 70 | # utils.printInBoldBlue(f"kappa_opt={kappa_opt}") 71 | 72 | 73 | X=np.random.rand(S.shape[0], 1) 74 | 75 | #### USING lobpcg 76 | kappa_lobpcg, _ = torch.lobpcg(A=torch.from_numpy(-S), k=1, B=torch.from_numpy(H), niter=-1, X=torch.from_numpy(X)) 77 | # kappa_lobpcg = torch.relu(kappa_lobpcg).item() 78 | kappa_lobpcg = kappa_lobpcg.item() 79 | utils.printInBoldBlue(f"kappa_lobpcg={kappa_lobpcg}") 80 | 81 | # #### USING Scipy 82 | # kappa_scipy, _ =scipy.sparse.linalg.lobpcg(A=-S, B=H, X=X, maxiter=-1) 83 | # kappa_scipy=kappa_scipy[0] 84 | # utils.printInBoldBlue(f"kappa_scipy={kappa_scipy}") 85 | 86 | #### USING normal eigendecomposition 87 | all_kappas, _ = np.linalg.eig(-Hinv@S); #Be careful because Hinv@M is NOT symmetric (Hinv and M is) 88 | print(f"all_kappas={all_kappas}") 89 | # kappa_eig=np.maximum(np.max(all_kappas),0.0) 90 | kappa_eig=np.max(all_kappas) 91 | utils.printInBoldBlue(f"kappa_eig={kappa_eig}") 92 | 93 | tol=1e-6 94 | assert abs(kappa_lobpcg-kappa_eig)> beta*M] 140 | 141 | # prob = cp.Problem(cp.Minimize(beta), constraints) 142 | # prob.solve() 143 | # print(f"beta={beta.value}") 144 | 145 | # Hinv=np.linalg.inv(H) 146 | # constraints = [(np.eye(2)+lam*Hinv@M) >> 0] 147 | # prob = cp.Problem(cp.Maximize(lam), constraints) 148 | # prob.solve() 149 | # print(f"lam={lam.value}") 150 | 151 | # L = np.linalg.cholesky(H) 152 | # # print(L@L.T-H) #good 153 | 154 | # Y=np.linalg.inv(L)@M@(np.linalg.inv(L).T) 155 | 156 | # constraints = [(Y+lam*np.eye(2)) >> 0] 157 | 158 | # prob = cp.Problem(cp.Maximize(lam), constraints) 159 | # prob.solve() 160 | # print(f"lam={lam.value}") 161 | 162 | 163 | 164 | 165 | 166 | #Inspired by https://github.com/rfeinman/Torch-ARPACK/blob/master/arpack/power_iteration.py 167 | # def power_iteration(A, tol=1e-20, max_iter=10000, eps=1e-12, check_freq=2): 168 | 169 | # v = torch.nn.functional.normalize(torch.rand(A.shape[1],1), dim=0) 170 | 171 | # n_iter = 0 172 | # converging = torch.ones(A.shape[0], dtype=torch.bool)# [True True True ...] 173 | # print(converging) 174 | # while n_iter < max_iter: 175 | # n_iter += 1 176 | # if(n_iter==1): 177 | # u = torch.nn.functional.normalize(A@v, dim=1) 178 | # v = u 179 | # continue 180 | 181 | # else: 182 | 183 | # print(f"converging before={converging}") 184 | 185 | # u[converging,:,:] = torch.nn.functional.normalize(A[converging,:,:]@v[converging,:,:], dim=1) 186 | 187 | # distance=torch.abs(1 - torch.abs( torch.transpose(v,1,2)@u )) 188 | # print(f"distance={distance}") 189 | 190 | # v[converging,:,:] = u[converging,:,:] 191 | 192 | # converging = (distance>tol).flatten() 193 | 194 | # print(f"converging after={converging}") 195 | 196 | 197 | # if (torch.all(converging == False)): #All of them converged 198 | # break 199 | 200 | # else: 201 | # warnings.warn('power iteration did not converge') 202 | 203 | 204 | 205 | # lamb = torch.transpose(v,1,2)@A@v #torch.dot(v, torch.mv(A, v)) 206 | 207 | # return lamb -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/polytopes/unionHull.m: -------------------------------------------------------------------------------- 1 | function U = unionHull(varargin) 2 | %unionHull - computes the convex hull of the union of other bounded convex polyhedra. 3 | %Any number of polyhedra can be input to the union operation. They can 4 | %be expressed using either vertices or linear in/equalities, according to 5 | %the input scheme, 6 | % 7 | % 8 | % I = intersectionHull('vert', V1, 'lcon', A2,b2,...,'qlcon', x3,A3,b3,...) 9 | % 10 | %The arguments specifying different polyhedra are separated using labels 11 | %'vert', 'lcon', or 'qlcon'. The label 'vert' signifies that an input polyhedron will 12 | %be expressed using vertices and is to be followed by any string of input arguments 13 | %accepted by vert2lcon(). The label 'lcon' signifies that an input polyhedron will 14 | %be expressed using linear constraints and is to be followed by any string of 15 | %input arguments accepted by lcon2vert(). The label 'qlcon' signifies that an input 16 | %polyhedron will be expressed using linear constraints and a known interior point and 17 | %is to be followed by input arguments accepted by qlcon2vert(). 18 | % 19 | %The output, U, is a struct containing fields 20 | % 21 | % U.vert: A matrix whose rows are the vertices of the output polyhedron. 22 | % U.lcon: The quadruplet of linear constraint data {A,b,Aeq,beq} 23 | % describing the output polyhedron. 24 | % 25 | %EXAMPLE 1: The unit simplex in 3D is formed as the convex hull of the union 26 | %of a cube and a triangle, both expressed in terms of their vertices. 27 | % 28 | % V1=(dec2bin(0:2^3-1,3)-'0')/3.0001; %vertices of 3D cube 29 | % V2=eye(3); %vertices of triangle 30 | % 31 | % U=unionHull('vert',V1,'vert',V2); %compute the union 32 | % 33 | %One can verify that the vertices of the result are those of the unit 34 | %simplex, 35 | % 36 | % >> U.vert 37 | % 38 | % ans = 39 | % 40 | % 0 0 0 41 | % 0 0 1 42 | % 0 1 0 43 | % 1 0 0 44 | % 45 | %We also demonstrate the use of separateBounds() to convert the result to 46 | %Optimization Toolbox representation. The tolerance parameter 0.99 is used 47 | %to deal with residual floating point noise. 48 | % 49 | % >> [A,b,Aeq,beq,lb,ub]=separateBounds(U.lcon{:},.99) 50 | % 51 | % A = 52 | % 53 | % 0.5774 0.5774 0.5774 54 | % 55 | % 56 | % b = 57 | % 58 | % 0.5774 59 | % 60 | % 61 | % Aeq = 62 | % 63 | % [] 64 | % 65 | % 66 | % beq = 67 | % 68 | % Empty matrix: 0-by-1 69 | % 70 | % 71 | % lb = 72 | % 73 | % 0 74 | % 0 75 | % 0 76 | % 77 | % 78 | % ub = 79 | % 80 | % Inf 81 | % Inf 82 | % Inf 83 | % 84 | % 85 | % 86 | %EXAMPLE 2: The unit cube is formed as the union of a simplex, a square, 87 | %and a single 3D point. The simplex and square are both represented using 88 | %equalites and inequalities. For convenience, we use the addBounds() utility 89 | %to compose the in/equality matrices. The representation of the square illustrates 90 | %the use of the 'qlcon' label, when a known interior point is available. 91 | % 92 | % 93 | % [A1,b1,Aeq1,beq1] = addBounds([1 1 1],1,[],[],[0;0;0]); %unit simplex 94 | % 95 | % [A2,b2,Aeq2,beq2] = addBounds([],[],[],[],[1;0;0],[1;1;1]); %a square face of the unit cube 96 | % 97 | % x2 = [1,0.5,0.5];%a point in the relative interior of the square 98 | % 99 | % V3 = [0,1,1]; %one vertex of the unit cube 100 | % 101 | % 102 | % U=unionHull('lcon',A1,b1,Aeq1,beq1,... %compute the union 103 | % 'qlcon',x2,A2,b2,Aeq2,beq2,... 104 | % 'vert',V3); 105 | % 106 | %One can verify that the result contains the 8 vertices of the unit cube, 107 | % 108 | % >> U.vert 109 | % 110 | % ans = 111 | % 112 | % 0 -0.0000 -0.0000 113 | % 0.0000 -0.0000 1.0000 114 | % -0.0000 1.0000 -0.0000 115 | % 0 1.0000 1.0000 116 | % 1.0000 0.0000 0.0000 117 | % 1.0000 0.0000 1.0000 118 | % 1.0000 1.0000 0.0000 119 | % 1.0000 1.0000 1.0000 120 | % 121 | %As before, we can use separateBounds() to convert to Optimization Toolbox 122 | %form, 123 | % 124 | % >> [A,b,Aeq,beq,lb,ub]=separateBounds(U.lcon{:},.99) 125 | % 126 | %which results in A=b=Aeq=beq=[] and 127 | % 128 | % lb = 129 | % 130 | % 0 131 | % 0 132 | % 0 133 | % 134 | % 135 | % ub = 136 | % 137 | % 1.0000 138 | % 1.0000 139 | % 1.0000 140 | 141 | 142 | %%%%begin parsing 143 | 144 | if isnumeric(varargin{1}) 145 | TOLcell=varargin(1); 146 | varargin(1)=[]; 147 | else 148 | TOLcell={}; 149 | end 150 | 151 | N=length(varargin); 152 | idxType = [find(cellfun(@ischar,varargin)),N+1]; 153 | 154 | L=length(idxType)-1; 155 | S(L).type=[]; 156 | S(L).args={}; 157 | 158 | 159 | for i=1:L 160 | 161 | j=idxType(i); 162 | k=idxType(i+1); 163 | S(i).type=varargin{j}; 164 | S(i).args=varargin(j+1:k-1); 165 | 166 | if isempty(S(i).args) 167 | error 'Syntax error - arguments missing' 168 | end 169 | 170 | switch S(i).type 171 | 172 | case 'lcon' 173 | 174 | S(i).V = lcon2vert(S(i).args{:}); 175 | 176 | case 'qlcon' 177 | 178 | S(i).V = qlcon2vert(S(i).args{:}); 179 | 180 | case 'vert' 181 | 182 | S(i).V = S(i).args{1}; 183 | 184 | otherwise 185 | 186 | error(['Unrecognized representation label of polyhedron ' num2str(i)]); 187 | 188 | 189 | end 190 | 191 | 192 | end 193 | 194 | 195 | 196 | %%%%end parsing 197 | 198 | 199 | V=vertcat(S.V); 200 | 201 | x0=mean(V,1); 202 | 203 | [U.lcon{1:4}]=vert2lcon(V,TOLcell{:}); 204 | 205 | U.vert=qlcon2vert(x0,U.lcon{:},TOLcell{:}); 206 | 207 | 208 | 209 | -------------------------------------------------------------------------------- /examples/other_utils.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------------------------- 2 | # Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | # See LICENSE file for the license information 4 | # -------------------------------------------------------------------------- 5 | 6 | #It operates on numpy stuff 7 | #polytope defined as Ax<=b 8 | def largestBallInPolytope(A,b, max_radius=None): 9 | 10 | if len(b.shape) == 1: 11 | b = np.expand_dims(b, axis=1) #Make b a column vector 12 | 13 | n=A.shape[1]; 14 | r = cp.Variable()#A scalar 15 | x0 = cp.Variable((n,1)) 16 | constraints=[] 17 | 18 | #https://programtalk.com/vs2/python/2718/cvxpy/examples/chebyshev.py/ 19 | #See also https://dkenefake.github.io/blog/ChebBall for when there are equality constraints 20 | for i in range(A.shape[0]): 21 | a_i=A[i,:].T 22 | constraints.append(r*cp.norm(a_i)+a_i.T@x0<=b[i,0]) 23 | 24 | if(max_radius is not None): 25 | constraints.append(r<=max_radius) 26 | 27 | objective = cp.Minimize(-r) #This is just a linear program 28 | prob = cp.Problem(objective, constraints) 29 | print("Calling solve...") 30 | result = prob.solve(verbose=False); 31 | print("Solved!") 32 | if(prob.status != 'optimal'): 33 | raise Exception("Value is not optimal") 34 | 35 | B=r*np.eye(n) 36 | 37 | printInBoldGreen(f"Found ball of radius r={r.value}") 38 | 39 | return B.value, x0.value 40 | 41 | 42 | # E representation --> {x s.t. (x-x0)'E(x-x0) <= 1}. Here, E is a psd matrix 43 | # B representation --> {x s.t. x=B*p_bar + x0, ||p_bar||<=1} \equiv {x s.t. ||inv(B)(x-x0)||<=1} \equiv {x s.t. (x-x0)'*inv(B)'*inv(B)*(x-x0)<=1}. 44 | # B is \in R^nxn (although Boyd's book says we can assume B is psd (and therefore also symmetric) without loss of generality, see section 8.4.1 45 | # More info about the B representation: https://ieeexplore.ieee.org/abstract/document/7839930 46 | 47 | #It returns the ellipsoid in B representation 48 | #It operates on numpy stuff 49 | def largestEllipsoidBInPolytope(A,b): 50 | 51 | if len(b.shape) == 1: 52 | b = np.expand_dims(b, axis=1) #Make b a column vector 53 | 54 | n=A.shape[1]; 55 | B = cp.Variable((n,n), symmetric=True) 56 | x0 = cp.Variable((n,1)) 57 | constraints=[] 58 | 59 | #Eq. 8.15 of https://web.stanford.edu/~boyd/cvxbook/bv_cvxbook.pdf 60 | #Also http://web.cvxr.com/cvx/examples/cvxbook/Ch08_geometric_probs/html/max_vol_ellip_in_polyhedra.html 61 | for i in range(A.shape[0]): 62 | a_i=A[i,:].T 63 | constraints.append(cp.norm(B@a_i)+a_i.T@x0<=b[i,0]) 64 | 65 | objective = cp.Minimize(-cp.atoms.log_det(B)) 66 | prob = cp.Problem(objective, constraints) 67 | print("Calling solve...") 68 | result = prob.solve(verbose=True); 69 | print("Solved!") 70 | 71 | return B.value, x0.value 72 | 73 | 74 | 75 | def makeColumnVector(a): 76 | return a[:,None] 77 | 78 | #Operates on torch stuff 79 | def squared_norm_of_each_row(D): 80 | # print(f"D**2.shape={(D**2).shape}") 81 | result=torch.sum(D**2, dim=2, keepdim=True) 82 | # print(f"result.shape={result.shape}") 83 | 84 | return result #@torch.ones((D.shape[1],1),device=D.device) 85 | 86 | #Operates on torch stuff 87 | def scaleEllipsoidB(B,A,b,x0): 88 | 89 | # print("\n\n") 90 | # # #====================First way========================== 91 | # # ======================================================== 92 | # minimum_so_far=torch.Tensor([float("inf")]) 93 | # for i in range(torch.numel(b)): 94 | # # print(A[i,:]) 95 | # a_i=makeColumnVector(A[i,:]) 96 | 97 | # tmp=(b[i,0]-a_i.mT@x0)**2/(a_i.mT@B@B.T@a_i); 98 | # # print(f"numerator={(b[i,0]-a_i.mT@x0)**2}, denominator={a_i.mT@B@B.T@a_i}, result={tmp}") 99 | # # print(f"tmp is {tmp}") 100 | # # print(f"tmp[0,0] is {tmp[0]}") 101 | # minimum_so_far=torch.minimum(minimum_so_far, tmp[0,0]) 102 | 103 | # # print(f"-------> minimum so far={minimum_so_far}") 104 | 105 | # result = B*torch.sqrt(minimum_so_far); 106 | # print(f"First way: \n {result}") 107 | 108 | # # #===================Second way========================== 109 | # # ======================================================== 110 | # c=squared_norm_of_each_row(A@B) 111 | # e=torch.min(((b-A@x0)**2)/c) 112 | # result=B*torch.sqrt(e) 113 | # print(f"Second way: \n {result}") 114 | 115 | # #===================Third way========================== 116 | # ======================================================== 117 | 118 | sqrt_c=torch.sqrt(squared_norm_of_each_row(A@B)) #This could be computed in the constructor (and not in each forward call) 119 | 120 | # print(f"sqrt_c={sqrt_c}") 121 | 122 | 123 | Ax0=A@x0; 124 | b_minus_Ax0=torch.sub(torch.unsqueeze(b,dim=0),Ax0) 125 | abs_b_minus_Ax0=torch.abs(b_minus_Ax0) #Note that if x0 is inside the ellipsoid, then I don't need the abs(), since Ax0<=b --> b-Ax0>=0 126 | abs_b_minus_Ax0_divided_by_sqrt_c=torch.div(abs_b_minus_Ax0,sqrt_c) 127 | tmp=torch.min(abs_b_minus_Ax0_divided_by_sqrt_c,dim=1,keepdim=True) 128 | sqrt_e=tmp.values 129 | result=B*sqrt_e 130 | # print(f"Third way: \n {result}") 131 | 132 | return result; 133 | 134 | 135 | # sqrt 136 | 137 | # print(f"sqrt_e={sqrt_e}") 138 | 139 | 140 | # print(f"sqrt_e.shape={sqrt_e.shape}") 141 | # print(f"B.shape={B.shape}") 142 | 143 | 144 | # print("-------------\n") 145 | # print(f"x0.shape={x0.shape}") 146 | 147 | # print(f"B.shape={B.shape}") 148 | # print(f"A.shape={A.shape}") 149 | # print(f"A@B.shape={(A@B).shape}") 150 | # print(f"b.shape={b.shape}") 151 | 152 | 153 | # print("==============================") 154 | # print(f"A={A}") 155 | # print(f"B={B}") 156 | # print(f"b={b}") 157 | # print(f"x0={x0}\n") 158 | 159 | 160 | def plot2DEllipsoidB(B,x0,ax): 161 | x1, x2 = sympy.symbols('x1 x2') 162 | x=np.array([[x1],[x2]]) 163 | try: 164 | B_inv=np.linalg.inv(B); 165 | except np.linalg.LinAlgError as err: 166 | print(str(err)) 167 | return 168 | tmp=(x-x0).T@B_inv.T@B_inv@(x-x0)-1 #This is [[scalar]] 169 | expression=tmp[0,0]; 170 | f=sympy.lambdify([x1,x2], expression) 171 | 172 | eigenvalues=np.linalg.eigvals(B) 173 | tmp_eig=np.amax(eigenvalues); 174 | xx = np.linspace(x0[0,0]-tmp_eig, x0[0,0]+tmp_eig, 600) 175 | yy = np.linspace(x0[1,0]-tmp_eig, x0[1,0]+tmp_eig, 600) 176 | xxx, yyy = np.meshgrid(xx, yy) 177 | result=f(xxx, yyy) 178 | ax.contour(xxx, yyy, result, levels=[0]) 179 | 180 | #OTHER OPTION, but you run into this visualization issue: https://github.com/sympy/sympy/issues/20056 181 | # tmp=sympy.plot_implicit(expression,show=True,points=300, adaptive=False, depth = 2) 182 | # move_sympyplot_to_axes(tmp, ax) 183 | # pts = tmp.get_points() 184 | # plt.plot(pts[0], pts[1]) -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/polytopes/separateBounds.m: -------------------------------------------------------------------------------- 1 | function [A,b,Aeq,beq,lb,ub,report] = separateBounds( A,b,Aeq,beq,tol ) 2 | %separateBounds - given a quadruplet {A, b, Aeq, beq} of linear constraint 3 | %matrices and vectors corresponding to the region A*x<=b, Aeq*x=beq, the routine 4 | %will convert to a sextuplet {A, b, Aeq, beq, lb, ub} of the kind used 5 | %in the Optimization Toolbox. 6 | % 7 | % [A,b,Aeq,beq,lb,ub,report] = separateBounds( A,b,Aeq,beq ) 8 | % 9 | %In other words, the code will look through the rows of A and Aeq for 10 | %constraints corresponding to simple upper and lower bounds. It will then 11 | %separate these constraints from the rest, expressing them instead using 12 | %vectors lb, ub. The "report" output argument is a structure containing 13 | %some relevnt stats, 14 | % 15 | % report.infeasibleBounds: indices, i, of any lb(i)>ub(i) 16 | % report.inequalitiesRemoved: indices of rows removed from A,b 17 | % report.equalitiesRemoved: indices of rows removed from Aeq,beq 18 | % 19 | %EXAMPLE: 20 | % 21 | % A = [1 1 1 22 | % -1 0 0 23 | % 0 -1 0 24 | % 1 0 0 25 | % 0 1 0] ; 26 | % 27 | % b=[1 0 0 1 1].'; 28 | % 29 | % Aeq=[0 0 1]; 30 | % beq=1; 31 | % 32 | % >>[A,b,Aeq,beq,lb,ub] = separateBounds( A,b,Aeq,beq ) 33 | % 34 | % A = 35 | % 36 | % 1 1 1 37 | % 38 | % 39 | % b = 40 | % 41 | % 1 42 | % 43 | % 44 | % Aeq = 45 | % 46 | % Empty matrix: 0-by-3 47 | % 48 | % 49 | % beq = 50 | % 51 | % Empty matrix: 1-by-0 52 | % 53 | % 54 | % lb = 55 | % 56 | % 0 57 | % 0 58 | % 1 59 | % 60 | % 61 | % ub = 62 | % 63 | % 1 64 | % 1 65 | % 1 66 | % 67 | %By default, the code will consider a constraint A(i,:)<=b(i) to correspond 68 | %to a pure bound if a row A(i,:) or similarly Aeq(i,:) contains no more 69 | %than one non-zero element. In certain situations, however, the matrices A 70 | %and Aeq are the output of floating point calculations and such rows will 71 | %contain non-zero elements representing numerical noise. In such instances, 72 | %one can call the code with a tolerance parameter 73 | % 74 | % [A,b,Aeq,beq,lb,ub,report] = separateBounds( A,b,Aeq,beq,tol ) 75 | % 76 | %where 0<=tol<=1. The criterion for deciding whether a constraint is a pure 77 | %bound is then, 78 | % 79 | % max(abs(A(:,i)))>=tol*sum(abs(A(i,:))) 80 | % 81 | %The default beavior corresponds to tol=1. 82 | 83 | 84 | %%%%begin parsing 85 | 86 | if ~exist('Aeq','var'), Aeq=[]; end 87 | if ~exist('beq','var'), beq=[]; end 88 | if ~exist('tol','var')||isempty(tol), 89 | tol=1; 90 | elseif tol<0 || tol>1 91 | error 'Tolerance parameter must satisfy 0<=tol<=1'; 92 | end 93 | 94 | Na=nan; Nb=nan; 95 | if xor(isempty(A),isempty(b)) 96 | error 'If A is empty then b must also be empty and vice versa' 97 | elseif ~isempty(A) 98 | Na=size(A,2); 99 | elseif isempty(A) 100 | A=[];b=[]; 101 | end 102 | 103 | if xor(isempty(Aeq),isempty(beq)) 104 | error 'If Aeq is empty then beq must also be empty and vice versa' 105 | elseif ~isempty(Aeq) 106 | Nb=size(Aeq,2); 107 | elseif isempty(Aeq) 108 | Aeq=[]; beq=[]; 109 | end 110 | 111 | if ~isnan(Na) && ~isnan(Nb) 112 | 113 | if Na~=Nb 114 | error 'If both A and Aeq are both non-empty, they must have same number of columns' 115 | end 116 | 117 | 118 | elseif isnan(Na) && isnan(Nb) 119 | 120 | lb=[]; ub=[]; 121 | return 122 | 123 | 124 | end 125 | 126 | N=max(Na,Nb); 127 | 128 | 129 | b=b(:); beq=beq(:); %henceforth we are sure b-data are column vectors 130 | 131 | 132 | if size(A,1)~=length(b) 133 | error 'Incompatible inequality matrix data sizes: size(A,1) ~= length(b)' 134 | end 135 | 136 | if size(Aeq,1)~=length(beq) 137 | error 'Incompatible equality matrix data sizes: size(Aeq,1) ~= length(beq)' 138 | end 139 | 140 | 141 | %%%%end parsing 142 | 143 | 144 | 145 | [ls1,lv1,us1,uv1,A1,b1,rows1] = extract(A,b,tol); 146 | [ls2,lv2,us2,uv2,A2,b2,rows2] = extract(Aeq,beq,tol); 147 | [ls3,lv3,us3,uv3] = deal(us2,uv2,ls2,lv2); 148 | 149 | A=A1; 150 | b=b1; 151 | Aeq=A2; 152 | beq=b2; 153 | lb=accumarray([ls1;ls2;ls3] , [lv1;lv2;lv3] , [N,1], @max , -inf); 154 | ub=accumarray([us1;us2;us3] , [uv1;uv2;uv3] , [N,1], @min , +inf); 155 | 156 | report.infeasibleBounds=find(ubUB(i) ) were detected.\n\n'),... 163 | sprintf( 'To disable this warning, call with 7 output argument syntax:\n\n'),... 164 | sprintf( ' [A,b,Aeq,beq,lb,ub,report] = separateBounds(...)')]; 165 | 166 | warning('separateBounds:infeasible', warnstr); 167 | 168 | end 169 | 170 | 171 | function [lsubs,lvals,usubs,uvals,A,b,rows] = extract(A,b,tol) 172 | 173 | lsubs=[]; usubs=[]; 174 | lvals=[]; uvals=[]; 175 | rows=[]; 176 | 177 | if isempty(A), return; end 178 | 179 | b=b(:); 180 | 181 | absA=abs(A); 182 | 183 | [maxA,idx]=max(absA,[],2); 184 | sumA=sum(absA,2); 185 | 186 | rows =find( maxA>=tol*sumA & sumA>0 ); 187 | 188 | 189 | subs=idx(rows); 190 | 191 | ind=sub2ind(size(A),rows, subs); 192 | num=b(rows); 193 | den=A(ind); 194 | 195 | vals=num./den; 196 | 197 | lidx=(den<0); 198 | uidx=(den>0); 199 | 200 | lsubs=subs(lidx); 201 | lvals=vals(lidx); 202 | 203 | 204 | usubs=subs(uidx); 205 | uvals=vals(uidx); 206 | 207 | 208 | zrows=(sumA==0); 209 | rowsInfeas=find(zrows & b<0); 210 | rowsFeas=find(zrows & b>=0); 211 | 212 | if ~isempty(rowsInfeas) %row of all zeros 213 | 214 | N=size(A,2); 215 | lsubs=(1:N).'; 216 | lvals=inf(N,1); 217 | usubs=lsubs; 218 | uvals=-lvals; 219 | rows=[rows;rowsInfeas]; 220 | end 221 | 222 | if ~isempty(rowsFeas) %row of all zeros 223 | rows=[rows;rowsFeas]; 224 | end 225 | 226 | A(rows,:)=[]; 227 | b(rows)=[]; 228 | 229 | 230 | 231 | -------------------------------------------------------------------------------- /examples/scripts/matlab/utils/polytopes/vert2lcon.m: -------------------------------------------------------------------------------- 1 | function [A,b,Aeq,beq]=vert2lcon(V,tol) 2 | %An extension of Michael Kleder's vert2con function, used for finding the 3 | %linear constraints defining a polyhedron in R^n given its vertices. This 4 | %wrapper extends the capabilities of vert2con to also handle cases where the 5 | %polyhedron is not solid in R^n, i.e., where the polyhedron is defined by 6 | %both equality and inequality constraints. 7 | % 8 | %SYNTAX: 9 | % 10 | % [A,b,Aeq,beq]=vert2lcon(V,TOL) 11 | % 12 | %The rows of the N x n matrix V are a series of N vertices of a polyhedron 13 | %in R^n. TOL is a rank-estimation tolerance (Default = 1e-10). 14 | % 15 | %Any point x inside the polyhedron will/must satisfy 16 | % 17 | % A*x <= b 18 | % Aeq*x = beq 19 | % 20 | %up to machine precision issues. 21 | % 22 | % 23 | %EXAMPLE: 24 | % 25 | %Consider V=eye(3) corresponding to the 3D region defined 26 | %by x+y+z=1, x>=0, y>=0, z>=0. 27 | % 28 | % 29 | % >>[A,b,Aeq,beq]=vert2lcon(eye(3)) 30 | % 31 | % 32 | % A = 33 | % 34 | % 0.4082 -0.8165 0.4082 35 | % 0.4082 0.4082 -0.8165 36 | % -0.8165 0.4082 0.4082 37 | % 38 | % 39 | % b = 40 | % 41 | % 0.4082 42 | % 0.4082 43 | % 0.4082 44 | % 45 | % 46 | % Aeq = 47 | % 48 | % 0.5774 0.5774 0.5774 49 | % 50 | % 51 | % beq = 52 | % 53 | % 0.5774 54 | 55 | 56 | %%initial stuff 57 | 58 | if nargin<2, tol=1e-10; end 59 | 60 | [M,N]=size(V); 61 | 62 | 63 | if M==1 64 | A=[];b=[]; 65 | Aeq=eye(N); beq=V(:); 66 | return 67 | end 68 | 69 | 70 | 71 | 72 | 73 | p=V(1,:).'; 74 | X=bsxfun(@minus,V.',p); 75 | 76 | 77 | %In the following, we need Q to be full column rank 78 | %and we prefer E compact. 79 | 80 | if M>N %X is wide 81 | 82 | [Q, R, E] = qr(X,0); %economy-QR ensures that E is compact. 83 | %Q automatically full column rank since X wide 84 | 85 | else%X is tall, hence non-solid polytope 86 | 87 | [Q, R, P]=qr(X); %non-economy-QR so that Q is full-column rank. 88 | 89 | [~,E]=max(P); %No way to get E compact. This is the alternative. 90 | clear P 91 | end 92 | 93 | 94 | diagr = abs(diag(R)); 95 | 96 | 97 | if nnz(diagr) 98 | 99 | %Rank estimation 100 | r = find(diagr >= tol*diagr(1), 1, 'last'); %rank estimation 101 | 102 | 103 | iE=1:length(E); 104 | iE(E)=iE; 105 | 106 | 107 | 108 | Rsub=R(1:r,iE).'; 109 | 110 | if r>1 111 | 112 | [A,b]=vert2con(Rsub,tol); 113 | 114 | elseif r==1 115 | 116 | A=[1;-1]; 117 | b=[max(Rsub);-min(Rsub)]; 118 | 119 | end 120 | 121 | A=A*Q(:,1:r).'; 122 | b=bsxfun(@plus,b,A*p); 123 | 124 | if r0; 219 | A(idx,:)=bsxfun(@rdivide,A(idx,:),normsA(idx)); 220 | b(idx)=b(idx)./normsA(idx); 221 | 222 | 223 | function [r,idx,Xsub]=lindep(X,tol) 224 | %Extract a linearly independent set of columns of a given matrix X 225 | % 226 | % [r,idx,Xsub]=lindep(X) 227 | % 228 | %in: 229 | % 230 | % X: The given input matrix 231 | % tol: A rank estimation tolerance. Default=1e-10 232 | % 233 | %out: 234 | % 235 | % r: rank estimate 236 | % idx: Indices (into X) of linearly independent columns 237 | % Xsub: Extracted linearly independent columns of X 238 | 239 | if ~nnz(X) %X has no non-zeros and hence no independent columns 240 | 241 | Xsub=[]; idx=[]; 242 | return 243 | end 244 | 245 | if nargin<2, tol=1e-10; end 246 | 247 | 248 | 249 | [Q, R, E] = qr(X,0); 250 | 251 | diagr = abs(diag(R)); 252 | 253 | 254 | %Rank estimation 255 | r = find(diagr >= tol*diagr(1), 1, 'last'); %rank estimation 256 | 257 | if nargout>1 258 | idx=sort(E(1:r)); 259 | idx=idx(:); 260 | end 261 | 262 | 263 | if nargout>2 264 | Xsub=X(:,idx); 265 | end -------------------------------------------------------------------------------- /examples/scripts/matlab/my_test.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | close all; clc;clear; 7 | addpath(genpath('./deep_panther/panther/matlab')) 8 | addpath(genpath('./deep_panther/submodules/minvo')) 9 | 10 | doSetup(); 11 | import casadi.* 12 | 13 | const_p={}; const_y={}; 14 | opti = casadi.Opti(); 15 | 16 | t0_n=0.0; 17 | tf_n=1.0; 18 | 19 | basis="MINVO";%MINVO B_SPLINE 20 | deg_pos=3; 21 | num_seg =2; %right now it needs to be equal to the number of obstacles 22 | dim_pos=2; 23 | 24 | sp=MyClampedUniformSpline(t0_n,tf_n,deg_pos, dim_pos, num_seg, opti); %spline position. 25 | 26 | % center1=[0,0,0]; 27 | % center2=[0.5,0.0,0.0]; 28 | % center3=[1.5,-0.8,0.0]; 29 | % center4=[2.8,0.5,0.0]; 30 | % C=[center1', center2', center3', center4']; 31 | % 32 | % side1=[2.0, 2.0, 2.0]; 33 | % side2=[2.0, 2.0, 2.0]; 34 | % side3=[1.1, 1.8, 1.8]; 35 | % side4=[1.8, 1.8, 1.8]; 36 | 37 | 38 | % V1_raw=rand(3,4); 39 | % 40 | % [k1,av1] =convhull(V1_raw(1,:),V1_raw(2,:),V1_raw(3,:)); 41 | % k=unique(k1(:)); 42 | % V1=V1_raw(:,k); 43 | % V2=V1; 44 | % 45 | % [A1,b1,Aeq1,beq1]=vert2lcon(V1',1e-10); 46 | % [A2,b2,Aeq2,beq2]=vert2lcon(V2',1e-10); 47 | % 48 | % polyhedra{1}.A=A1; 49 | % polyhedra{1}.b=b1; 50 | % 51 | % polyhedra{2}.A=A2; 52 | % polyhedra{2}.b=b2; 53 | 54 | % polyhedra{1}=getAb_Box(center1, side1); 55 | % polyhedra{2}=getAb_Box(center2, side2); 56 | %polyhedra{3}=getAb_Box(center3, side3); 57 | %polyhedra{4}=getAb_Box(center4, side4); 58 | 59 | % polyhedra{1}=getAb_Box2D([0.5,0.5],[1.0,1.0]); 60 | % polyhedra{2}=getAb_Box2D([1.0,0.5],[1.0,1.0]); 61 | 62 | %First polytope 63 | V1=[0 1 1 0; 64 | 0 0 1 1]; 65 | 66 | %Second polytope 67 | V2=V1+[0.6;0.8]; 68 | 69 | [A1,b1,Aeq1,beq1]=vert2lcon(V1',1e-10); 70 | [A2,b2,Aeq2,beq2]=vert2lcon(V2',1e-10); 71 | 72 | polyhedra{1}.A=A1; polyhedra{1}.b=b1; 73 | polyhedra{2}.A=A2; polyhedra{2}.b=b2; 74 | 75 | const_p_obs_avoid={}; 76 | 77 | for j=1:(sp.num_seg) 78 | %Get the control points of the interval 79 | Q=sp.getCPs_XX_Pos_ofInterval(basis, j); 80 | 81 | for kk=1:size(Q,2) %for all the control points of the interval 82 | const_p_obs_avoid{end+1}= polyhedra{j}.A*Q{kk} - polyhedra{j}.b <= 0; 83 | end 84 | end 85 | % 86 | opti.subject_to([const_p_obs_avoid]); 87 | [A,b]=getAbLinearConstraints(opti); 88 | Aeq=[]; beq=[]; TOL=[]; %default tolerance, it is 1e-10 89 | 90 | [V,nr,nre]=lcon2vert(A,b,Aeq,beq,TOL); 91 | V=V'; 92 | 93 | figure; hold on; 94 | 95 | %%OPTION 1: Check that there exist barycentric coordinates for a fixed spline (which is known to 96 | %%satisfy the constraints) 97 | bs_cps=[0.1 0.4 0.8 0.9 1.4; 98 | 0.1 0.6 1.35 0.5 1.5]; 99 | num_bary_coor=size(V,2); 100 | f=zeros(num_bary_coor,1); 101 | % A=ones(1,num_bary_coor); b=1; %Ax<=1 102 | A=[]; b=[]; %Ax<=1 103 | Aeq=[V; ones(1,num_bary_coor)]; beq=[bs_cps(:) ; 1]; %Vx=variables and sum(x)=1 104 | lb=zeros(num_bary_coor,1); 105 | ub=ones(num_bary_coor,1); 106 | bar_coord = linprog(f,A,b,Aeq,beq, lb, ub); 107 | assert(all(bar_coord>=0)); 108 | assert(abs(sum(bar_coord)-1)<1e-7); 109 | bs_cps=V*bar_coord; 110 | 111 | %NOTE: THE "PROBLEM" with options 2 and 3 is that we are sampling from a very high-dimensional 112 | % simplex. This means that, even if we sample uniformly, most of the samples will fall close to the 113 | % center of the simplex, because there is a lot of volume there. 114 | 115 | %OPTION 2: 116 | % bar_coord=sampleBarCoord(1,size(V,2)); 117 | % bs_cps=V*bar_coord; 118 | 119 | %OPTION 3: (See paper "Geometrically Constrained Traj Opt for Multicopters) 120 | % v0=V(:,1); 121 | % Vhat=V(:,2:end)-repmat(v0,1,size(V,2)-1); 122 | % bar_coord=samplePointsSimplex(1,size(Vhat,2)); 123 | % bs_cps=v0 + Vhat*bar_coord; 124 | 125 | %NOTE that the paper "Geometrically Constrained Traj Opt for Multicopters" 126 | %does not guarantee safety along the whole trajectory (only in the 127 | %intermediate waypoints). They only impose A_i q_j <= b_i, where (A_i, b_i) is the 128 | %polytope and q_j is a waypoint. This helps A LOT reducing the 129 | %computational complexity, since there is no dependency between the 130 | %different waypoints. In other words, the system that has all the 131 | %constraints can be written as A*q<=b, where A is block-diagonal matrix, and q:=[q1;q2;q3;...]. 132 | %First polytope: 133 | % A1q1<=b1 134 | % A1q2<=b1 135 | % A1q3<=b1 136 | 137 | %Second polytope: 138 | % A2q3<=b2 139 | % A2q4<=b2 140 | % A2q5<=b2 141 | 142 | %Hence, you need to simply obtain the vertexes of (A1,b1), (A2,b2), and 143 | %(A1,b1)intersected_with(A2,b2) 144 | 145 | bs_cps=reshape(bs_cps,sp.dim,[]); 146 | 147 | bs_cps=num2cell(bs_cps,1); 148 | sp.setCPoints(bs_cps); 149 | 150 | sp.plotPos2D() 151 | 152 | for j=1:(sp.num_seg) 153 | %Get the control points of the interval 154 | Q=cell2mat(sp.getCPs_XX_Pos_ofInterval(basis, j)); 155 | scatter(Q(1,:),Q(2,:)) 156 | end 157 | 158 | % end 159 | 160 | 161 | plot2dConvHullAndVertices(V1); 162 | plot2dConvHullAndVertices(V2); 163 | 164 | %% 165 | 166 | % close all 167 | % tmp=samplePointsSimplex(10000,3); %Sample points from a simplex 168 | % scatter3(tmp(1,:),tmp(2,:),tmp(3,:),'filled'); axis equal; 169 | 170 | %Example in 2D: x>=0, y>=0, x+y=1 171 | function result=sampleBarCoordVersion2(num_points, dim) 172 | %https://stackoverflow.com/a/67202070 173 | samples=[]; 174 | 175 | for i=1:num_points 176 | sample=exprnd(1e9,dim,1); 177 | samples=[samples sample]; 178 | end 179 | result=samples./sum(samples); %divide by the sum of each column 180 | end 181 | 182 | %Example in 2D: x>=0, y>=0, x+y=1 183 | function result=sampleBarCoord(num_points, dim) 184 | %https://cs.stackexchange.com/a/3229 185 | samples=[]; 186 | 187 | for i=1:num_points 188 | tmp=[0; rand(dim-1,1); 1]; 189 | sample=diff(sort(tmp)); 190 | samples=[samples sample]; 191 | end 192 | result=samples; 193 | end 194 | 195 | %Example in 2D: x>=0, y>=0, x+y<=1 196 | function result=samplePointsSimplex(num_points, dim) 197 | % samples=[]; 198 | % 199 | % for i=1:num_points 200 | % sample=samplePoints(zeros(dim,1),ones(dim,1),num_points,1,0)'; 201 | % samples=[samples sample]; 202 | % end 203 | % result=samples; 204 | result=samplePoints(zeros(dim,1),ones(dim,1),num_points,1,0); 205 | end 206 | 207 | %% 208 | % hold on; 209 | % P=getA_MV(2,[-1,1]); 210 | % sym t 211 | % Pt=P*getT(2,t); 212 | % fplot3(Pt(1),Pt(2),Pt(3),[-1,1],'r'); axis equal; 213 | 214 | % hold on; 215 | % trisurf(k1,V1_raw(1,:),V1_raw(2,:),V1_raw(3,:),'FaceColor','cyan','FaceAlpha',0.5) 216 | % scatter3(V1(1,:),V1(2,:),V1(3,:),'filled') 217 | % axis equal 218 | 219 | % [V1,nr,nre]=lcon2vert(polyhedra{1}.A,polyhedra{1}.b,Aeq,beq,TOL); 220 | % [V2,nr,nre]=lcon2vert(polyhedra{2}.A,polyhedra{2}.b,Aeq,beq,TOL); 221 | 222 | % for i=1:2000 223 | 224 | % bar_coord=sampleBarCoord(1,size(V,2)); 225 | % bar_coord=(1:numel(bar_coord))'; bar_coord=bar_coord/sum(bar_coord); 226 | 227 | -------------------------------------------------------------------------------- /examples/scripts/time_analysis.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------------------------- 2 | # Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | # See LICENSE file for the license information 4 | # -------------------------------------------------------------------------- 5 | 6 | import numpy as np 7 | import torch 8 | import time 9 | from joblib import Parallel, delayed 10 | import pandas as pd 11 | import sys 12 | import os 13 | 14 | 15 | sys.path.insert(1, os.path.join(sys.path[0], '..')) #See first comment of this answer: https://stackoverflow.com/a/11158224 16 | 17 | from examples_sets import getExample 18 | import fixpath #Following this example: https://github.com/tartley/colorama/blob/master/demos/demo01.py 19 | from rayen import constraints, constraint_module, utils 20 | 21 | path="./results/" 22 | if not os.path.exists(path): 23 | os.makedirs(path) 24 | 25 | torch.set_default_dtype(torch.float64) 26 | 27 | def getTime_sMethod(cs, num_samples): 28 | print("Calling constructor for the layer...") 29 | my_layer=constraint_module.ConstraintModule(cs, method="RAYEN", create_map=False) 30 | print("Called") 31 | 32 | 33 | 34 | numel_output_mapper=my_layer.getDimAfterMap() 35 | x_batched=torch.Tensor(num_samples, numel_output_mapper, 1).uniform_(-1.0, 1.0) 36 | x_batched.to(torch.device('cuda:0')) 37 | print("Calling method...") 38 | 39 | cuda_timer=utils.CudaTimer() 40 | cuda_timer.start() 41 | result=my_layer(x_batched) 42 | total_s_per_sample= cuda_timer.endAndGetTimeSeconds()/num_samples 43 | print("Called") 44 | 45 | print(f"total_s_per_sample={total_s_per_sample} s") 46 | return total_s_per_sample 47 | 48 | # all_k = [1, 10, 100, 1000, 10000] 49 | 50 | ############################################ WARM UP THE GPU (for more accurate computation time) 51 | my_layer=constraint_module.ConstraintModule(getExample(12), method="RAYEN", create_map=False) 52 | x_dummy=torch.Tensor(300, my_layer.getDimAfterMap(), 1).uniform_(-5.0, 5.0) 53 | _ = my_layer(x_dummy) 54 | 55 | ####################################################### 56 | 57 | num_samples=2000 58 | 59 | # ########################################### Linear CONSTRAINTS 60 | times_lin = [] 61 | 62 | all_k = [1, 10, 100, 1000, 2000, 3000, 4000, 5000, 10000] 63 | all_r_A1 = [1, 10, 100, 500, 1000, 2000, 3000] 64 | 65 | for r_A1 in all_r_A1: 66 | for k in all_k: 67 | utils.printInBoldRed(f"r_A1 = {r_A1}, k={k}") 68 | A1=np.random.uniform(low=-1.0, high=1.0, size=(r_A1,k)) 69 | b1=np.random.uniform(low= 0.1, high=1.0, size=(r_A1,1)) #In this way, y=0 is always a feasible solution 70 | 71 | lc=constraints.LinearConstraint(A1=A1, b1=b1, A2=None, b2=None); 72 | cs=constraints.ConvexConstraints(lc=lc, qcs=[], socs=[], lmic=None, y0=np.zeros((k,1)), do_preprocessing_linear=False) 73 | total_s=getTime_sMethod(cs, num_samples) 74 | times_lin.append({'k': k,'r_A1': r_A1,'Time': total_s}) 75 | 76 | times_lin=pd.DataFrame(times_lin) 77 | # utils.savepickle(times_lin, path+"times_lin.pkl") 78 | times_lin.to_csv(path+"times_lin.csv",index=False) 79 | 80 | 81 | # ########################################### QP CONSTRAINTS 82 | times_qp = [] 83 | all_eta = [1, 10, 50, 100, 500, 1000] #np.power(10, np.array([0,1,2,3])) 84 | all_k = [1, 10, 100, 200, 300, 400, 500, 600, 700, 800, 900, 1000] 85 | for eta in all_eta: 86 | for k in all_k: 87 | utils.printInBoldRed(f"QP constraints, eta = {eta}, k={k}") 88 | 89 | qcs=[] 90 | 91 | def createRandomQPConstraint(i): 92 | tmp=np.random.uniform(low=-1.0, high=1.0, size=(k,k)) 93 | # tmp=(torch.empty((k,k), device="cuda:0").uniform_(-1.0, 1.0)).cpu().numpy() # 94 | 95 | P=tmp@tmp.T #To make sure it's a (symmetric) PSD matrix 96 | q=np.random.uniform(low=-1.0, high=1.0, size=(k,1)) 97 | r=np.random.uniform(low=-1.0, high=0.0, size=(1,1)) 98 | qc=constraints.ConvexQuadraticConstraint(P=P, q=q, r=r, do_checks_P=False); 99 | return qc 100 | 101 | print("Creating random constraints") 102 | qcs = Parallel(n_jobs=15)(delayed(createRandomQPConstraint)(i) for i in range(eta)) 103 | print("Created") 104 | 105 | assert len(qcs)==eta 106 | 107 | cs=constraints.ConvexConstraints(lc=None, qcs=qcs, socs=[], lmic=None, y0=np.zeros((k,1))) 108 | total_s=getTime_sMethod(cs, num_samples) 109 | times_qp.append({'k': k,'eta': eta,'Time': total_s}) 110 | 111 | times_qp=pd.DataFrame(times_qp) 112 | # utils.savepickle(times_qp, path+"times_qp.pkl") 113 | times_qp.to_csv(path+"times_qp.csv",index=False) 114 | 115 | 116 | ########################################### SOC CONSTRAINTS 117 | times_soc = [] 118 | all_r_M = [10, 100, 200, 300] 119 | all_mu = [10, 100, 300, 500] 120 | all_k = [10, 100, 500, 1000] 121 | 122 | for r_M in all_r_M: 123 | for mu in all_mu: 124 | for k in all_k: 125 | utils.printInBoldRed(f"r_M = {r_M}, mu={mu}, k={k}") 126 | 127 | def createRandomSOCConstraint(i): 128 | M = np.random.uniform(low=-1.0, high=1.0, size=(r_M,k)) 129 | s = np.random.uniform(low=-1.0, high=1.0, size=(r_M,1)) 130 | c = np.random.uniform(low=-1.0, high=1.0, size=(k,1)) 131 | d = np.linalg.norm(s)+np.array([[0.5]]) #This ensures that 0 will always be a point in the interior of the feasible set 132 | return M, s, c, d 133 | 134 | print("Creating random constraints") 135 | all_Mscd = Parallel(n_jobs=15)(delayed(createRandomSOCConstraint)(i) for i in range(k)) 136 | print("Created") 137 | 138 | socs=[] 139 | for Mscd in all_Mscd: 140 | socs.append(constraints.SOCConstraint(Mscd[0], Mscd[1], Mscd[2], Mscd[3])) 141 | 142 | print("Creating cs") 143 | cs=constraints.ConvexConstraints(lc=None, qcs=[], socs=socs, lmic=None, y0=np.zeros((k,1))) 144 | print("Created") 145 | 146 | total_s=getTime_sMethod(cs, num_samples) 147 | 148 | times_soc.append({'k': k,'r_M': r_M,'mu': mu,'Time': total_s}) 149 | 150 | times_soc=pd.DataFrame(times_soc) 151 | # utils.savepickle(times_soc, path+"times_soc.pkl") 152 | times_soc.to_csv(path+"times_soc.csv",index=False) 153 | 154 | print(times_soc) 155 | 156 | 157 | ########################################### LMI CONSTRAINTS 158 | times_lmi = [] 159 | all_r_F = [10, 100, 200, 300] 160 | all_k = [100, 500, 1000, 2000, 5000, 7000, 10000] 161 | 162 | for r_F in all_r_F: 163 | for k in all_k: 164 | utils.printInBoldRed(f"r_F = {r_F}, k={k}") 165 | 166 | def createRandomSymmetricMatrix(i): 167 | tmp=np.random.uniform(low=-1.0, high=1.0, size=(r_F,r_F)) 168 | return (tmp + tmp.T)/2 #To ensure that it is symmetric 169 | 170 | print("Creating random constraints") 171 | all_F = Parallel(n_jobs=15)(delayed(createRandomSymmetricMatrix)(i) for i in range(k)) 172 | 173 | tmp = np.random.uniform(-1, 1, (r_F, r_F)) 174 | F_k = np.dot(tmp, tmp.transpose()) + 0.5*np.eye(r_F) #F_k is symmetric positive definite by construction 175 | all_F.append(F_k) 176 | print("Created") 177 | 178 | print("Creating lmic") 179 | lmic=constraints.LMIConstraint(all_F) 180 | print("Created lmic") 181 | 182 | print("Creating cs") 183 | cs=constraints.ConvexConstraints(lc=None, qcs=[], socs=[], lmic=lmic, y0=np.zeros((k,1))) 184 | print("Created") 185 | 186 | total_s=getTime_sMethod(cs, num_samples) 187 | 188 | times_lmi.append({'k': k,'r_F': r_F,'Time': total_s}) 189 | 190 | times_lmi=pd.DataFrame(times_lmi) 191 | # utils.savepickle(times_lmi, path+"times_lmi.pkl") 192 | times_lmi.to_csv(path+"times_lmi.csv",index=False) 193 | 194 | print(times_lmi) -------------------------------------------------------------------------------- /examples/scripts/train.yaml: -------------------------------------------------------------------------------- 1 | session_name: session_dataset2d 2 | windows: 3 | - window_name: dev window 4 | layout: tiled 5 | shell_command_before: 6 | - cd .. # run as a first command in all panes 7 | panes: 8 | # - shell_command: # pane no. 1 9 | # - cd /var/log # run multiple commands in this pane 10 | # - ls -al | grep \.log 11 | - python main.py --method RAYEN --dimension_dataset 2 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 12 | - python main.py --method RAYEN_old --dimension_dataset 2 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 13 | 14 | - python main.py --method UU --dimension_dataset 2 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 15 | - python main.py --method UU --dimension_dataset 2 --weight_soft_cost 10 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 16 | - python main.py --method UU --dimension_dataset 2 --weight_soft_cost 100 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 17 | - python main.py --method UU --dimension_dataset 2 --weight_soft_cost 1000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 18 | - python main.py --method UU --dimension_dataset 2 --weight_soft_cost 5000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 19 | 20 | - python main.py --method UP --dimension_dataset 2 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 21 | - python main.py --method UP --dimension_dataset 2 --weight_soft_cost 10 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 22 | - python main.py --method UP --dimension_dataset 2 --weight_soft_cost 100 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 23 | - python main.py --method UP --dimension_dataset 2 --weight_soft_cost 1000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 24 | - python main.py --method UP --dimension_dataset 2 --weight_soft_cost 5000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 25 | 26 | - python main.py --method DC3 --dimension_dataset 2 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 27 | - python main.py --method DC3 --dimension_dataset 2 --weight_soft_cost 10 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 28 | - python main.py --method DC3 --dimension_dataset 2 --weight_soft_cost 100 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 29 | - python main.py --method DC3 --dimension_dataset 2 --weight_soft_cost 1000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 30 | - python main.py --method DC3 --dimension_dataset 2 --weight_soft_cost 5000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 31 | 32 | - python main.py --method PP --dimension_dataset 2 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 33 | 34 | - python main.py --method Bar --dimension_dataset 2 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 35 | 36 | 37 | ######################################### 3d 38 | - python main.py --method RAYEN --dimension_dataset 3 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 39 | - python main.py --method RAYEN_old --dimension_dataset 3 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 40 | 41 | - python main.py --method UU --dimension_dataset 3 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 42 | - python main.py --method UU --dimension_dataset 3 --weight_soft_cost 10 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 43 | - python main.py --method UU --dimension_dataset 3 --weight_soft_cost 100 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 44 | - python main.py --method UU --dimension_dataset 3 --weight_soft_cost 1000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 45 | - python main.py --method UU --dimension_dataset 3 --weight_soft_cost 5000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 46 | 47 | - python main.py --method UP --dimension_dataset 3 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 48 | - python main.py --method UP --dimension_dataset 3 --weight_soft_cost 10 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 49 | - python main.py --method UP --dimension_dataset 3 --weight_soft_cost 100 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 50 | - python main.py --method UP --dimension_dataset 3 --weight_soft_cost 1000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 51 | - python main.py --method UP --dimension_dataset 3 --weight_soft_cost 5000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 52 | 53 | - python main.py --method DC3 --dimension_dataset 3 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 54 | - python main.py --method DC3 --dimension_dataset 3 --weight_soft_cost 10 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 55 | - python main.py --method DC3 --dimension_dataset 3 --weight_soft_cost 100 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 56 | - python main.py --method DC3 --dimension_dataset 3 --weight_soft_cost 1000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 57 | - python main.py --method DC3 --dimension_dataset 3 --weight_soft_cost 5000 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi 58 | 59 | - python main.py --method PP --dimension_dataset 3 --weight_soft_cost 0 --test False && if [[ $(tmux list-panes | wc -l) -eq 1 ]]; then tmux kill-session; else exit; fi -------------------------------------------------------------------------------- /examples/scripts/plot_losses_vs_time.m: -------------------------------------------------------------------------------- 1 | % -------------------------------------------------------------------------- 2 | % Jesus Tordesillas Torres, Robotic Systems Lab, ETH Zürich 3 | % See LICENSE file for the license information 4 | % -------------------------------------------------------------------------- 5 | 6 | close all; clear; clc; 7 | 8 | set(0,'DefaultFigureWindowStyle','normal') %'normal' 'docked' 9 | set(0,'defaulttextInterpreter','latex'); 10 | set(groot, 'defaultAxesTickLabelInterpreter','latex'); set(groot, 'defaultLegendInterpreter','latex'); 11 | set(0,'defaultfigurecolor',[1 1 1]) 12 | 13 | addpath(genpath('./matlab/submodules/minvo/')) 14 | addpath(genpath('./matlab/submodules/export_fig/')) 15 | addpath(genpath('./matlab/utils')) 16 | 17 | my_table=readtable('./results/merged.csv'); 18 | 19 | %Remove RAYEN_old method 20 | hasMatch = ~cellfun('isempty', regexp(my_table.method, 'RAYEN_old', 'once')) ; 21 | my_table(hasMatch, :)=[]; 22 | 23 | %Change the name Optimization with Gurobi 24 | hasMatch = find(~cellfun('isempty', regexp(my_table.method, 'dataset2d_Optimization', 'once')),1) ; 25 | my_table(hasMatch, 1).method={'dataset2d_Gurobi'}; 26 | 27 | hasMatch = find(~cellfun('isempty', regexp(my_table.method, 'dataset3d_Optimization', 'once')),1) ; 28 | my_table(hasMatch, 1).method={'dataset3d_Gurobi'}; 29 | 30 | 31 | 32 | hasMatch = ~cellfun('isempty', regexp(my_table.method, 'dataset2d', 'once')) ; 33 | results2d=my_table(hasMatch, :); 34 | results2d = moveToEndOfTable(results2d, 'dataset2d_DC3_weight_soft_cost_5000.0'); 35 | results2d = moveToEndOfTable(results2d, 'dataset2d_RAYEN_weight_soft_cost_0.0'); 36 | 37 | 38 | hasMatch = ~cellfun('isempty', regexp(my_table.method, 'dataset3d', 'once')) ; 39 | results3d=my_table(hasMatch, :); 40 | results3d = moveToEndOfTable(results3d, 'dataset3d_RAYEN_weight_soft_cost_0.0'); %move RAYEN to the last position 41 | 42 | 43 | %%%%%%%%%%%%%%%%%%%%%%%% FIGURES WITHOUT LINE BREAK 44 | position=[1000 1117 560 305]; 45 | 46 | %%% DATASET 2D 47 | figure; 48 | tcl = tiledlayout(1,2); 49 | nexttile(tcl); hold on; 50 | [circle_plots, names ]=plotTimevsCost(results2d,"InDist", "\textbf{Inside dist.}"); 51 | hL = legend(circle_plots,names{:},'Location','eastoutside' ); 52 | hL.Layout.Tile = 'East'; 53 | 54 | nexttile(tcl); hold on; 55 | plotTimevsCost(results2d,"OutDist", "\textbf{Outside dist.}") 56 | title(tcl,'\textbf{Optimization 1}','interpreter','latex') 57 | set(gcf,'Position',position) 58 | export_fig time_loss_opt1_no_break.png -m2.5 59 | 60 | %%% DATASET 3D 61 | figure; 62 | tcl = tiledlayout(1,2); 63 | nexttile(tcl); hold on; 64 | [circle_plots, names ]=plotTimevsCost(results3d,"InDist", "\textbf{Inside dist.}"); 65 | hL = legend(circle_plots,names{:},'Location','eastoutside' ); 66 | hL.Layout.Tile = 'East'; 67 | nexttile(tcl); hold on; 68 | plotTimevsCost(results3d,"OutDist", "\textbf{Outside dist.}") 69 | title(tcl,'\textbf{Optimization 2}','interpreter','latex') 70 | 71 | set(gcf,'Position',position) 72 | export_fig time_loss_opt2_no_break.png -m2.5 73 | 74 | %%%%%%%%%%%%%%%%%%%%%%%% FIGURES WITH LINE BREAK 75 | %%% DATASET 2D 76 | figure; subplot(1,2,1); hold on; 77 | plotTimevsCost(results2d,"InDist", "\textbf{Inside dist.}"); 78 | min_y=0.95; max_y=11.85;ylim([min_y,max_y]);yticks(1.0:0.05:max_y); 79 | breakyaxis([1.16,11.8]) 80 | 81 | subplot(1,2,2); hold on; 82 | plotTimevsCost(results2d,"OutDist", "\textbf{Outside dist.}") 83 | min_y=0.95; max_y=4.7;ylim([min_y,max_y]);yticks(1.0:0.1:max_y); 84 | breakyaxis([1.45,4.56]) 85 | 86 | set(gcf,'Position',position) 87 | export_fig time_loss_opt1_break.png -m2.5 88 | 89 | %%% DATASET 3D 90 | figure; subplot(1,2,1); hold on; 91 | plotTimevsCost(results3d,"InDist", "\textbf{Inside dist.}"); 92 | min_y=0.95; max_y=8.0;ylim([min_y,max_y]);yticks(1.0:0.15:max_y); 93 | breakyaxis([2.5,7.8]) 94 | 95 | subplot(1,2,2); hold on; 96 | plotTimevsCost(results3d,"OutDist", "\textbf{Outside dist.}") 97 | min_y=0.95; max_y=10.4;ylim([min_y,max_y]);yticks(1.0:0.15:max_y); 98 | breakyaxis([2.6,10.2]) 99 | 100 | set(gcf,'Position',position) 101 | export_fig time_loss_opt2_break.png -m2.5 102 | 103 | 104 | %% Plot Model Complexity 105 | 106 | position=[1000 1117 560 205]; 107 | 108 | figure; hold on; 109 | plotModelComplexity(results2d, position, ["UU", "UP", "PP", "DC3", "Bar", "RAYEN"]) 110 | % export_fig model_complexity_opt1.png -m2.5 111 | % ylim([0,15*10^4]) 112 | 113 | figure; hold on; 114 | plotModelComplexity(results3d, position, ["UU", "UP", "PP", "DC3", "RAYEN"]) 115 | ylim([0,15*10^4]) 116 | % export_fig model_complexity_opt2.png -m2.5 117 | 118 | 119 | function plotModelComplexity(results, position, names) 120 | 121 | indexes=[]; 122 | for n=names 123 | n 124 | tmp=find(~cellfun('isempty', regexp(results.method, n, 'once')),1) 125 | indexes=[indexes, tmp] 126 | end 127 | 128 | X = categorical(names); 129 | % X = reordercats(X,results.method'); 130 | bar(X,results.num_trainable_params(indexes)) 131 | % set(gca,'YScale','log') 132 | % set(gca, 'XTickLabel', results.method'); 133 | ylabel('Params.') 134 | set(gcf,'Position',position) 135 | 136 | 137 | end 138 | 139 | 140 | %% 141 | 142 | function my_table=moveToEndOfTable(my_table, my_string) 143 | match=~cellfun('isempty', regexp(my_table.method, my_string, 'once')); 144 | tmp=my_table(match, :); my_table(match, :)=[]; my_table=[my_table; tmp]; 145 | end 146 | 147 | function [circle_plots, names]=plotTimevsCost(results, type, my_title) 148 | % figure; 149 | circle_plots=[]; 150 | names={}; 151 | has_bar=false; 152 | for i=1:numel(results.method) 153 | if(type=="InDist") 154 | t = results.x_InDist_Time_us(i); 155 | n_loss = results.x_InDist_N_loss(i); 156 | violation = results.x_InDist_Violation(i); 157 | else 158 | t = results.x_OutDist_Time_us(i); 159 | n_loss = results.x_OutDist_N_loss(i); 160 | violation = results.x_OutDist_Violation(i); 161 | end 162 | 163 | name=results.method(i); 164 | name=name{1}; 165 | name=strrep(name,'dataset2d_',''); 166 | name=strrep(name,'dataset3d_',''); 167 | name=strrep(name,'_weight_soft_cost_',', $\omega=$ '); 168 | if(contains(name,"Bar")) 169 | has_bar=true; 170 | end 171 | if(contains(name,"RAYEN") || contains(name,"PP") || contains(name,"Bar")) 172 | name = extractBefore(name,', $\omega=$ '); %These algorithms don't use omega 173 | end 174 | 175 | if(violation>1e-6) 176 | continue 177 | end 178 | 179 | tmp=scatter(t,n_loss,80.0,'o',"filled",'MarkerEdgeColor','k', 'LineWidth',1.0); 180 | circle_plots=[circle_plots tmp(1)]; 181 | names{end+1}=name; 182 | end 183 | 184 | xlabel('Time ($\mu$s)') 185 | ylabel('N. Loss') 186 | yline(1.0,'--') 187 | color_bar=[0.5,0.5,0.5]; 188 | color_rayen=[0,1,0]; 189 | if(has_bar) 190 | tmp=numel(circle_plots)-1; 191 | else 192 | tmp=numel(circle_plots); 193 | end 194 | my_colormap=maxdistcolor(tmp,@sRGB_to_OKLab, 'exc',[color_bar; 1,1,1; color_rayen], 'Lmin',0.6, 'Lmax',1.0);%, 'inc',[1,0,0; 0,1,0] 195 | if(has_bar) 196 | my_colormap=[color_bar ;my_colormap]; 197 | end 198 | my_colormap(end,:) = color_rayen; 199 | set(gca,'colororder',my_colormap); 200 | 201 | % set(gca, 'YScale', 'log') 202 | 203 | 204 | 205 | 206 | title(my_title) 207 | 208 | 209 | 210 | end 211 | 212 | % set(gca, 'XScale', 'log') 213 | % set(gca, 'YScale', 'log') --------------------------------------------------------------------------------