├── .gitignore ├── Lossless Monoped ├── SLIPdynamics.m ├── animateSlip.m ├── raibertHopperCtrl.m ├── raibertHopperCtrltext.m └── runSlipDynamics.m ├── Lossy Monoped ├── +Auto │ ├── Base.m │ ├── DetailDynamics.m │ ├── Dynamics.m │ ├── Jacobian.m │ ├── desktop.ini │ ├── equationsToMatrix.m │ └── readme.txt ├── Controllers │ ├── EGB │ │ ├── EGBcontroller.m │ │ ├── findNeutralAngle.m │ │ ├── generateLookupTable.m │ │ └── liftOffSimulation.m │ └── Raibert │ │ └── raibertController.m ├── IPOPT │ ├── desktop.ini │ ├── ipopt.m │ └── ipopt_auxdata.m ├── KevinSlipHopperFlight.m ├── KevinSlipHopperStance.m ├── Nlp │ ├── @Nlp │ │ ├── Nlp.m │ │ ├── addConstraint.m │ │ ├── addObjective.m │ │ ├── addPdeConstraint.m │ │ ├── addVariable.m │ │ ├── desktop.ini │ │ ├── display.m │ │ ├── plot.m │ │ └── removeConstraint.m │ ├── Constraint.m │ ├── DirectCollocation.m │ ├── Objective.m │ ├── Phase.m │ ├── Solver │ │ ├── @Fmincon │ │ │ ├── Fmincon.m │ │ │ ├── desktop.ini │ │ │ ├── dev │ │ │ │ ├── desktop.ini │ │ │ │ └── fmincon_hessMult.m │ │ │ ├── export.m │ │ │ └── solve.m │ │ ├── @Ipopt │ │ │ ├── Ipopt.m │ │ │ ├── desktop.ini │ │ │ ├── dev │ │ │ │ ├── desktop.ini │ │ │ │ └── hessian.m │ │ │ ├── export.m │ │ │ └── solve.m │ │ ├── @Snopt │ │ │ ├── Snopt.m │ │ │ ├── desktop.ini │ │ │ ├── export.m │ │ │ └── solve.m │ │ ├── @Solver │ │ │ ├── Solver.m │ │ │ ├── desktop.ini │ │ │ ├── display.m │ │ │ └── feasibility.m │ │ ├── FunctionGenerator.m │ │ ├── MatlabFunctionGenerator.m │ │ └── desktop.ini │ ├── Variable.m │ └── desktop.ini ├── RK4Integrate.m ├── SNOPT │ ├── _snoptMatlab.pdf │ ├── desktop.ini │ ├── setpath.m │ ├── snJac.m │ ├── snfindG.m │ ├── snget.m │ ├── sngetStatus.m │ ├── sngeti.m │ ├── sngetr.m │ ├── snopt.m │ ├── snpmat.m │ ├── snprint.m │ ├── snprintfile.m │ ├── snscreen.m │ ├── snset.m │ ├── snsetStatus.m │ ├── snseti.m │ ├── snsetr.m │ ├── snsolve.m │ ├── snspec.m │ ├── snsummary.m │ └── snwrapper.m ├── Symbolic │ ├── Binary Operator Nodes │ │ ├── BinaryOperatorNode.m │ │ ├── DivideBinaryOperatorNode.m │ │ ├── MinusBinaryOperatorNode.m │ │ ├── PlusBinaryOperatorNode.m │ │ ├── PowerBinaryOperatorNode.m │ │ ├── TimesBinaryOperatorNode.m │ │ └── desktop.ini │ ├── ConstantNode.m │ ├── ExpressionNode.m │ ├── ExpressionNodeTest.m │ ├── Functional Operator Nodes │ │ ├── FunctionalOperatorNode.m │ │ └── desktop.ini │ ├── Unary Operator Nodes │ │ ├── AbsUnaryOperatorNode.m │ │ ├── CosUnaryOperatorNode.m │ │ ├── ExpUnaryOperatorNode.m │ │ ├── LogUnaryOperatorNode.m │ │ ├── MinusUnaryOperatorNode.m │ │ ├── PlusUnaryOperatorNode.m │ │ ├── SignUnaryOperatorNode.m │ │ ├── SinUnaryOperatorNode.m │ │ ├── SqrtUnaryOperatorNode.m │ │ ├── SumUnaryOperatorNode.m │ │ ├── TanUnaryOperatorNode.m │ │ ├── UnaryOperatorNode.m │ │ └── desktop.ini │ ├── VariableNode.m │ └── desktop.ini ├── Terrain.m ├── _LTK │ ├── InteractiveAnalysis.m │ └── desktop.ini ├── lookupTable.mat ├── monopedAnimation.m ├── prismaticMonopod.m └── runSlipDynamics.m └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | # Windows default autosave extension 2 | *.asv 3 | 4 | # Windows hidden file 5 | desktop.ini 6 | 7 | # OSX / *nix default autosave extension 8 | *.m~ 9 | 10 | # Compiled MEX binaries (all platforms) 11 | *.mex* 12 | 13 | #All Videos 14 | *.mp4 15 | 16 | #Imaged 17 | *.png 18 | 19 | # Simulink Code Generation 20 | slprj/ 21 | 22 | # COALESCE-generated files 23 | _build/ 24 | 25 | #Specific Autogenerated Files 26 | massMatrix*.m 27 | dynamics*.m 28 | jacobian.m 29 | -------------------------------------------------------------------------------- /Lossless Monoped/SLIPdynamics.m: -------------------------------------------------------------------------------- 1 | classdef SLIPdynamics 2 | %SLIPDYNAMICS This is a dynamic simulation of a spring loaded inverted 3 | %pendulum (SLIP) on flat ground. It is controlled via a controller 4 | %function that is called at the start of each flight phase and is 5 | %expected to alter the desired touchdown Leg Angle (phi_td) 6 | 7 | properties 8 | % Fixed Values 9 | %Gravity (m/s^2) 10 | g = 9.81; 11 | %Body Mass (kg) 12 | M = 10; 13 | %Resting leg lenth (m) 14 | L0 = 0.7; 15 | %Leg Spring Constant (N/m) 16 | K = 2000; 17 | %time step between data points (sec) 18 | %Only affects ODE45 output, not calculations 19 | dataTimeStep = 0.05 20 | 21 | %State: 0=flight, 1=Stance 22 | dynamic_state = 0; 23 | %Desired Touchdown Leg Angle 24 | phi_td = 0; 25 | 26 | %Time 27 | t = []; 28 | %Recorded State 29 | dynamic_state_arr = 0; 30 | %Body Position (m) 31 | x_body = 0; 32 | y_body = 1; 33 | %Body Velocity (m/s) 34 | dx_body = 0; 35 | dy_body = 0; 36 | %Leg Angle (foot to body from vertical) (rads) 37 | phi = 0; 38 | dphi = 0; 39 | %Leg Length 40 | L = 0.7; 41 | dL = 0; 42 | 43 | %foot x position (m) 44 | xf = 0; 45 | end %properties 46 | 47 | methods 48 | 49 | function obj = SLIPdynamics() 50 | %Potentially used in the future for Graphics init? 51 | end 52 | 53 | function o = simulate(o,controller,tspan,des_vel) 54 | %simulate Run a simulation with specified controller 55 | 56 | o.t = tspan(1); 57 | while o.t(end) < max(tspan) 58 | switch o.dynamic_state 59 | case 0 %flight 60 | dyn = @(t,y) [y(2);0;... 61 | y(4);-o.g]; 62 | 63 | options = odeset('Events',@(t,y) (flightTransitions(t,y,o))); 64 | 65 | y0 = [o.x_body(end);o.dx_body(end);o.y_body(end);o.dy_body(end)]; 66 | 67 | case 1 %Stance 68 | dyn = @(t,y) [y(2);y(1)*o.K*(o.L0-sqrt(y(1)^2 + y(3)^2))/(sqrt(y(1)^2 + y(3)^2)*o.M);... 69 | y(4);y(3)*o.K*(o.L0-sqrt(y(1)^2 + y(3)^2))/(sqrt(y(1)^2 + y(3)^2)*o.M)-o.g]; 70 | 71 | options = odeset('Events',@(t,y) (stanceTransitions(t,y,o))); 72 | y0 = [o.x_body(end) - o.xf(end);o.dx_body(end);o.y_body(end);o.dy_body(end)]; 73 | end %switch 74 | 75 | %Adjust to pick up next simulation at where the last one ended 76 | tspan = o.t(end):o.dataTimeStep:tspan(end); 77 | 78 | [t_out,y_out,~,~,~] = ode45(dyn,tspan,y0,options); 79 | 80 | o = fillSimData(o,t_out,y_out); 81 | o = switchMode(o); 82 | o = controller(o,des_vel); %update leg touchdown angle 83 | end %while 84 | 85 | end %function simulate 86 | 87 | end %Methods 88 | 89 | end %Class 90 | 91 | function [o] = switchMode(o) 92 | %This controls the state machine for the simulation. For a SLIP monoped it 93 | %is very simple. The structure is the same as would be used for more 94 | %complicated state transitions 95 | 96 | switch o.dynamic_state 97 | case 0 98 | o.dynamic_state = 1; %Flight can only enter stance 99 | case 1 100 | o.dynamic_state = 0; %Stance can only enter flight 101 | end %switch 102 | 103 | end %function switchMode 104 | 105 | function [value,isterminal,direction] = flightTransitions(t,y,o) 106 | %ODE Event function sensing foot touchdown during flight when moving 107 | %downward 108 | value = y(3) - o.L0*cos(o.phi_td) + max(y(4),0); 109 | isterminal = 1; 110 | direction = []; 111 | end %function flightTransitions 112 | 113 | function [value,isterminal,direction] = stanceTransitions(t,y,o) 114 | %ODE Event function sensing foot liftoff during stance while moving upward 115 | 116 | %On a pure elastic leg zero force on the foot is identical to when the 117 | %leg length reaches the unstretched length 118 | value = sqrt(y(1)^2 + y(3)^2) - o.L0 + min(y(4),0); 119 | isterminal = 1; 120 | direction = []; 121 | end %function stanceTransitions 122 | 123 | 124 | function o = fillSimData(o,t,y) 125 | %fillSimData This function copies the state data (y) into the object 126 | %members. What the state variables represent is different depending on the 127 | %dynamic state (stance, flight). 128 | 129 | o.dynamic_state_arr = [o.dynamic_state_arr;o.dynamic_state*ones(size(t))]; 130 | switch o.dynamic_state 131 | case 0 %Flight phase 132 | o.t = [o.t;t]; 133 | o.x_body = [o.x_body; y(:,1)]; 134 | o.y_body = [o.y_body; y(:,3)]; 135 | 136 | o.dx_body = [o.dx_body; y(:,2)]; 137 | o.dy_body = [o.dy_body; y(:,4)]; 138 | 139 | new_phi = o.phi_td*ones(size(t)); 140 | new_phi(y(:,4) > 0) = o.phi(end); 141 | o.phi = [o.phi;new_phi]; 142 | o.dphi = [o.dphi;zeros(size(t))]; 143 | o.L = [o.L;o.L0*ones(size(t))]; 144 | o.dL = [o.dL;zeros(size(t))]; 145 | 146 | o.xf = [o.xf; y(:,1) + o.L0*sin(o.phi_td)]; 147 | 148 | case 1 %Left Leg Single Stance 149 | o.t = [o.t;t]; 150 | o.x_body = [o.x_body; y(:,1) + o.xf(end)]; 151 | o.y_body = [o.y_body; y(:,3)]; 152 | 153 | o.dx_body = [o.dx_body; y(:,2)]; 154 | o.dy_body = [o.dy_body; y(:,4)]; 155 | 156 | o.phi = [o.phi;atan2(-y(:,1),y(:,3))]; 157 | o.dphi = [o.dphi;zeros(size(t))]; %Todo fix 158 | o.L = [o.L;sqrt(y(:,1).^2 + y(:,3).^2)]; 159 | o.dL = [o.dL;y(:,2).*y(:,1)./sqrt(y(:,1).^2 + y(:,3).^2) + y(:,4).*y(:,3)./sqrt(y(:,1).^2 + y(:,3).^2)]; 160 | 161 | o.xf = [o.xf; o.xf(end)*ones(size(t))]; 162 | end %switch 163 | end %Function fillSimData 164 | 165 | 166 | -------------------------------------------------------------------------------- /Lossless Monoped/animateSlip.m: -------------------------------------------------------------------------------- 1 | function [ ] = animateSlip( obj ) 2 | %UNTITLED Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | figure(7); 6 | cla 7 | ylim([-0.1,1.4]); 8 | xlim([-0.3,2.3]); 9 | grid on; 10 | axis equal; 11 | title('SLIP Monoped Animation'); 12 | %Create visual patches 13 | ground_patch = patch([-50,-50,50,50],[0,-10,-10,0],[0.8,0.8,0.8]); 14 | 15 | body_patch = patch(obj.x_body(1) + 0.1*sin(0:0.1:2*pi),obj.y_body(1) + 0.1*cos(0:0.1:2*pi),[70,216,226]./255); 16 | 17 | leg_patch = patch(obj.x_body(1) + [0.01,0.01,-0.01,-0.01]*cos(obj.phi(1)) + obj.L(1)*[0,1,1,0]*sin(obj.phi(1)),...); 18 | obj.y_body(1) - [0.01,0.01,-0.01,-0.01]*sin(obj.phi(1)) + obj.L(1)*[0,-1,-1,0]*cos(obj.phi(1)),'k'); 19 | 20 | %Loop through the data updating the graphics 21 | for i = 1:length(obj.t) 22 | body_patch.Vertices = [obj.x_body(i) + 0.1*sin(0:0.1:2*pi);obj.y_body(i) + 0.1*cos(0:0.1:2*pi)]'; 23 | 24 | leg_patch.Vertices = [obj.x_body(i) + [0.01,0.01,-0.01,-0.01]*cos(obj.phi(i)) + obj.L(i)*[0,1,1,0]*sin(obj.phi(i));...); 25 | obj.y_body(i) + [0.01,0.01,-0.01,-0.01]*sin(obj.phi(i)) + obj.L(i)*[0,-1,-1,0]*cos(obj.phi(i))]'; 26 | 27 | ylim([-0.1,1.4]); 28 | 29 | %Increment the screen by 0.5 m increments 30 | xlim([-1,1]+round(obj.x_body(i)*2)/2); 31 | drawnow; 32 | end 33 | 34 | end 35 | 36 | -------------------------------------------------------------------------------- /Lossless Monoped/raibertHopperCtrl.m: -------------------------------------------------------------------------------- 1 | function [ obj ] = raibertHopperCtrl( obj, des_vel ) 2 | %RAIBERTHOPPERCTRL Simple Raibert hopper Controler for SLIP monoped 3 | %This controller is called at lift off to adjust desired leg touchdown 4 | %angle to maintain a desired forward velocity 5 | 6 | k_ctrl = 0.05; % m/(m/s) foot disp per velocity error 7 | 8 | if obj.dynamic_state == 0 %Entering Flight Phase 9 | 10 | %Desired forward foot displacement 11 | % 0.225 (sec) is a hard coded stance duration. This converges faster 12 | % than using the measured stance duration 13 | x_f0 = 0.5*obj.dx_body(end)*0.225 + k_ctrl*(obj.dx_body(end) - des_vel); 14 | 15 | %Desired touchdown leg angle 16 | obj.phi_td = asin(x_f0/obj.L0); 17 | 18 | end %If entering stance, no need for the controller to run 19 | 20 | end 21 | 22 | -------------------------------------------------------------------------------- /Lossless Monoped/raibertHopperCtrltext.m: -------------------------------------------------------------------------------- 1 | function [ obj ] = raibertHopperCtrl( obj, des_vel ) 2 | %RAIBERTHOPPERCTRL Simple Raibert hopper Controler for SLIP monoped 3 | %This controller is called at lift off to adjust desired leg touchdown 4 | %angle to maintain a desired forward velocity 5 | 6 | k_ctrl = 0.05; % m/(m/s) foot disp per velocity error 7 | 8 | if obj.dynamic_state == 0 %%Entering Flight Phase 9 | touchdown_index = find(diff(obj.dynamic_state_arr),1,'last'); 10 | Ts = obj.t(end) - obj.t(touchdown_index(end)+1); %Length of stance 11 | 12 | %Calculate the mean velocity of the last step. If we don't have enough 13 | %data just use current velocity 14 | % dx_bar = (obj.x_body(end)-obj.x_body(touchdown_index))/Ts; 15 | 16 | %Desired forward foot displacement 17 | % 0.225 (sec) is a hard coded stance duration 18 | x_f0 = 0.5*obj.dx_body(end)*0.225 + k_ctrl*(obj.dx_body(end) - des_vel); 19 | %Desired touchdown leg angle 20 | obj.phi_td = asin(x_f0/obj.L0); 21 | 22 | end %If entering stance, no need for the controller to run 23 | 24 | end 25 | 26 | -------------------------------------------------------------------------------- /Lossless Monoped/runSlipDynamics.m: -------------------------------------------------------------------------------- 1 | % close all 2 | obj = SLIPdynamics(); 3 | obj.dataTimeStep = 0.008; %Output timestep (sec) 4 | des_vel = 0.5; %m/s 5 | 6 | obj = simulate(obj,@raibertHopperCtrl,[0,6],des_vel); 7 | 8 | figure(9) 9 | subplot(2,1,1) 10 | hold off 11 | plot(obj.t,obj.y_body); 12 | ylabel('Body Vertical Position (m)'); 13 | xlabel('time (sec)'); 14 | title(['SLIP Raibert Hopper, Desired Speed ',num2str(des_vel),' m/sec']); 15 | 16 | subplot(2,1,2) 17 | hold off 18 | plot(obj.t,obj.dx_body); 19 | hold on 20 | plot(obj.t,des_vel*ones(size(obj.t)),'--r'); 21 | legend('Simulation','Desired'); 22 | ylabel('Body Horizontal velocity (m/sec)'); 23 | xlabel('time (sec)'); 24 | 25 | animateSlip( obj ) -------------------------------------------------------------------------------- /Lossy Monoped/+Auto/Base.m: -------------------------------------------------------------------------------- 1 | classdef Base 2 | %BASE Base class for all Auto* classes 3 | % Copyright 2017 Andy Abate (abatea@oregonstate.edu) 4 | 5 | properties (Abstract, Constant, Hidden) 6 | domainRank % end subclasses set this, positive integer 7 | end 8 | 9 | methods 10 | function [props] = properties(o) 11 | props = builtin('properties',o); 12 | end 13 | function [o,props] = makeFieldsSymbolic(o) 14 | %MAKEFIELDSSYMBOLIC Takes an object and populates available fields with same-name symbols. 15 | % Returns the modified object and cell array of replaced field names. 16 | props = o.properties(); % get names of available properties 17 | for i = 1:numel(props) % loop through names 18 | p = props{i}; 19 | o.(p) = sym(p,'real'); % set field to symbol 20 | end 21 | end 22 | function [q,qdot,qddot] = symbolicStateVariables(o,var) 23 | %SYMBOLICSTATEVARIABLES creates symbolic state variables 24 | % var: character array variable name 25 | if ~exist('var','var'), var = 'q'; end 26 | n = o.domainRank; 27 | q = sym(var,[n,1],'real'); 28 | qdot = sym([var 'dot'],[n,1],'real'); 29 | qddot = sym([var 'ddot'],[n,1],'real'); 30 | end 31 | function f = classfile(o) 32 | f = which(class(o)); 33 | end 34 | function d = classfolder(o) 35 | d = fileparts(o.classfile); 36 | end 37 | function m = classmethod(o,methodname) 38 | m = fullfile(o.classfolder,strcat(methodname,'.m')); 39 | end 40 | end 41 | methods (Access = protected) 42 | function runBuildRule(o,methodnames,buildfunc) 43 | if ~iscell(methodnames), methodnames = {methodnames}; end 44 | destfiles = cellfun(@(s)o.classmethod(s),methodnames,'uniformoutput',false); 45 | sourcefiles = {o.classfile}; 46 | if needsRebuild(sourcefiles,destfiles), buildfunc(); end 47 | %assert(needsRebuild(sourcefiles,destfiles) == false, 'Build rule did not produce required files') 48 | end 49 | function writeMethod(o,f,variables,parameters,methodname) 50 | filepath = o.classmethod(methodname); 51 | if ~iscell(variables), variables = {variables}; end 52 | if ~iscell(parameters), parameters = {parameters}; end 53 | 54 | % GENERATE INITIAL FUNCTION FILE 55 | matlabFunction(f,'vars',{variables{:},parameters{:}},'file',filepath); % cell array must be row 56 | % READ FROM FILE AND MODIFY 57 | s = fileread(filepath); % read all text from file 58 | idx = strfind(s,newline); idx = idx(1); % get end of first line 59 | header = s(1:idx); % get the whole first line (function definition) 60 | body = s(idx+1:end); 61 | % add obj as first argument to header 62 | header = char(insertAfter(header,'(','obj,')); 63 | % remove parameters from function header 64 | header = erase(header,strcat(',',parameters)); 65 | % replace parameters in body 66 | body = regexprep(body,parameters,strcat('obj.',parameters)); 67 | 68 | % REPLACE FILE WITH MODIFIED CODE 69 | fid = fopen(filepath,'w'); 70 | if fid ~= -1 71 | clean = onCleanup(@() fclose(fid)); 72 | fprintf(fid,'%s',[header,body]); 73 | end 74 | end 75 | end 76 | 77 | end 78 | 79 | function b = needsRebuild(sourcefiles,destfiles) % expects cells of char strings 80 | youngestSource = 0; 81 | oldestDestination = Inf; 82 | 83 | for i = 1:numel(sourcefiles) 84 | d = dir(sourcefiles{i}); 85 | if ~isempty(d), youngestSource = max(youngestSource,d.datenum); end 86 | end 87 | for i = 1:numel(destfiles) 88 | d = dir(destfiles{i}); 89 | if ~isempty(d), oldestDestination = min(oldestDestination,d.datenum); 90 | else, oldestDestination = -1; end % destination is missing, always rebuild 91 | end 92 | 93 | % youngest source needs to be younger than oldest destination 94 | % rebuild if it is not 95 | b = ~(youngestSource < oldestDestination); 96 | 97 | b = 1; %Temporary Hack, force rebuild every time 98 | end 99 | -------------------------------------------------------------------------------- /Lossy Monoped/+Auto/DetailDynamics.m: -------------------------------------------------------------------------------- 1 | classdef DetailDynamics < Auto.Dynamics 2 | %DYNAMICSDETAILED Subclass this to generate symbolic Coriolis, stiffness, and potential terms. Inherits from AutoDynamics. 3 | % Copyright 2017 Andy Abate (abatea@oregonstate.edu) 4 | 5 | properties (Constant,Access=private) 6 | targets = {'coriolisMatrix','potentialTerms','stiffnessMatrix'} 7 | end 8 | 9 | methods 10 | function o = DetailDynamics() 11 | o.runBuildRule(o.targets,@()build(o,o.targets{:})); 12 | end 13 | end 14 | 15 | end 16 | 17 | function build(o,coriolisname,potentialname,stiffnessname) 18 | fprintf(['Creating ' cell2mat(strcat(o.targets,{'(x,xdot), ','(x) and ','(x)'})) ' for ' class(o) '...\n']); 19 | 20 | [o,params] = o.makeFieldsSymbolic(); 21 | [x,xdot] = o.symbolicStateVariables(); 22 | 23 | b = o.dynamics(x,xdot); % M(x)*xddot + b(x,xdot) = u 24 | [C,G] = Auto.equationsToMatrix(b,xdot); % C(x,xdot)*xdot + G(x) 25 | K = jacobian(G,x); 26 | 27 | % export symbolic methods 28 | o.writeMethod(C,{x,xdot},params,coriolisname); 29 | o.writeMethod(G,x,params,potentialname); 30 | o.writeMethod(K,x,params,stiffnessname); 31 | end 32 | -------------------------------------------------------------------------------- /Lossy Monoped/+Auto/Dynamics.m: -------------------------------------------------------------------------------- 1 | classdef Dynamics < Auto.Jacobian 2 | %AUTODYNAMICS Subclass this to generate symbolic equations of motion. Inherits from AutoJacobian. 3 | % Copyright 2017 Andy Abate (abatea@oregonstate.edu) 4 | 5 | % subclasses must implement these 6 | methods (Abstract) 7 | T = kineticEnergy(o,x,xdot) 8 | V = potentialEnergy(o,x) 9 | end 10 | 11 | methods 12 | function o = Dynamics() 13 | o.runBuildRule({'massMatrix','dynamics'},@()build(o,'massMatrix','dynamics')); 14 | end 15 | 16 | function [t,x,xdot,f] = simulate(o,x0,xdot0,tspan,controller) 17 | if ~exist('controller','var') 18 | controller = @(t,x,xdot) 0; 19 | end 20 | n = o.domainRank; 21 | opts = odeset('reltol',1e-8,'abstol',1e-8); 22 | dyn = @(t,y) [y(n+1:end); o.massMatrix(y(1:n))\(controller(t,y(1:n),y(n+1:end))-o.dynamics(y(1:n),y(n+1:end)))]; 23 | y0 = [x0;xdot0]; 24 | [t,y] = ode45(dyn,tspan,y0,opts); 25 | t = t'; 26 | y = y'; 27 | x = y(1:n,:); 28 | xdot = y(n+1:end,:); 29 | f = nan(size(x)); 30 | for i = 1:numel(t) 31 | f(:,i) = controller(t(i),x(:,i),xdot(:,i)); 32 | end 33 | end 34 | 35 | function [ u,qdot,qddot ] = inverseDynamics(o, dt,q ) 36 | %INVERSEDYNAMICS Estimates velocity, acceleration, and required force for the path of a system. 37 | % dt: fixed timestep 38 | % x: row of column vectors of position coordinates 39 | % Uses central differencing to approximate velocity 40 | % Uses forward/backward differencing to approximate acceleration 41 | % Forward differencing does not work as well. Central seems to work best. 42 | 43 | M = @o.massMatrix; 44 | h = @o.dynamics; 45 | 46 | % estimate velocity and acceleration from position 47 | qdot = cdiff(q)/dt; 48 | qddot = ddiff(q)/dt^2; 49 | 50 | [m,n] = size(q); 51 | 52 | % preallocate torque vector 53 | u = zeros(m,n); 54 | 55 | for i = 1:n 56 | qi = q(:,i); 57 | qdoti = qdot(:,i); 58 | qddoti = qddot(:,i); 59 | u(:,i) = M(qi)*qddoti + h(qi,qdoti); 60 | end 61 | 62 | end 63 | 64 | end 65 | 66 | end 67 | 68 | function build(o,massname,dynamicsname) 69 | fprintf(['Creating massMatrix(x) and dynamics(x,xdot) for ' class(o) '...\n']); 70 | 71 | [o,params] = o.makeFieldsSymbolic(); 72 | [x,xdot,xddot] = o.symbolicStateVariables(); 73 | 74 | T = o.kineticEnergy(x,xdot); 75 | V = o.potentialEnergy(x); 76 | L = T - V; 77 | 78 | lhs = jacobian(gradient(L,xdot),[x;xdot]) * [xdot;xddot]; % d/dt [part L wrt qdot] 79 | rhs = gradient(L,x); % part L wrt q 80 | eq = simplify(lhs-rhs,'ignoreanalyticconstraints',true); 81 | [M,h] = equationsToMatrix(eq,xddot); % M(x)*xddot = b(x,xdot) 82 | 83 | 84 | % export symbolic methods 85 | o.writeMethod(M,x,params,massname); 86 | o.writeMethod(-h,{x,xdot},params,dynamicsname); % M*xddot + b(x,xdot) = u 87 | 88 | assert(rank(M)==o.domainRank, ['Deficient mass matrix for class ' class(o)]) 89 | end 90 | 91 | -------------------------------------------------------------------------------- /Lossy Monoped/+Auto/Jacobian.m: -------------------------------------------------------------------------------- 1 | classdef Jacobian < Auto.Base 2 | %AUTOJACOBIAN Subclass this to automatically generate a symbolic Jacobian. Inherits from Auto. 3 | % Copyright 2017 Andy Abate (abatea@oregonstate.edu) 4 | 5 | % subclasses must implement this 6 | methods (Abstract) 7 | y = eval(o,x) 8 | end 9 | 10 | methods 11 | function o = Jacobian() 12 | o.runBuildRule('jacobian',@()build(o)) 13 | end 14 | end 15 | end 16 | 17 | function build(o) 18 | fprintf(['Creating jacobian(x) for ' class(o) '...\n']) 19 | [o,params] = o.makeFieldsSymbolic(); 20 | x = o.symbolicStateVariables(); 21 | 22 | % make function evaluation symbolic 23 | f = reshape(o.eval(x),[],1); 24 | % calculate symbolic jacobian 25 | J = jacobian(f,x); 26 | 27 | % export symbolic method 28 | o.writeMethod(J,x,params,'jacobian'); 29 | end 30 | 31 | -------------------------------------------------------------------------------- /Lossy Monoped/+Auto/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/+Auto/equationsToMatrix.m: -------------------------------------------------------------------------------- 1 | function [A,b] = equationsToMatrix( eq, x ) 2 | %EQUATIONSTOMATRIX equationsToMatrix for nonlinear equations 3 | % factors out the vector x from eq such that eq = Ax + b 4 | % eq does not need to be linear in x 5 | % eq must be a vector of equations, and x must be a vector of symbols 6 | % Copyright 2017 Andy Abate (abatea@oregonstate.edu) 7 | 8 | assert(isa(eq,'sym'), 'Equations must be symbolic') 9 | assert(isa(x,'sym'), 'Vector x must be symbolic') 10 | 11 | n = numel(eq); 12 | m = numel(x); 13 | 14 | A = repmat(sym(0),n,m); 15 | 16 | for i = 1:n % loop through equations 17 | [c,p] = coeffs(eq(i),x); % separate equation into coefficients and powers of x(1)...x(n) 18 | for k = 1:numel(p) % loop through found powers/coefficients 19 | for j = 1:m % loop through x(1)...x(n) 20 | if has(p(k),x(j)) 21 | % transfer term c(k)*p(k) into A, factoring out x(j) 22 | A(i,j) = A(i,j) + c(k)*p(k)/x(j); 23 | break % move on to next term c(k+1), p(k+1) 24 | end 25 | end 26 | end 27 | end 28 | 29 | b = simplify(eq - A*x,'ignoreanalyticconstraints',true); % makes sure to fully cancel terms 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /Lossy Monoped/+Auto/readme.txt: -------------------------------------------------------------------------------- 1 | Auto: automate method construction for kinematic and dynamic systems 2 | COPYRIGHT 2017 Andy Abate (abatea@oregonstate.edu) 3 | 4 | Contains facilities for deriving Jacobians and equations of motion for 5 | arbitrary systems. Requires R2016b. 6 | 7 | To use, subclass any single Auto* class. 8 | Implement abstract methods as required. 9 | 10 | Add public properties for any constants associated with the system 11 | (e.g. mass, link length). 12 | 13 | The child class must be contained in a class folder 14 | (e.g. '@ClassName/ClassName.m'). 15 | 16 | The Auto* classes will automatically build class methods for the subclass 17 | and place them in the class folder. 18 | You may have to try instantiating a class multiple times on first use. 19 | MATLAB does not see the methods right away, and proceeds to crash when 20 | the methods are called. 21 | 22 | Methods will be rebuilt automatically when the main class file changes. 23 | Touch the class file to force updates. 24 | 25 | 26 | 27 | 28 | 29 | 30 | TODO: 31 | - command to build template class (which avoids errors when generating folder class methods) -------------------------------------------------------------------------------- /Lossy Monoped/Controllers/EGB/EGBcontroller.m: -------------------------------------------------------------------------------- 1 | function [ u,ctrlParams ] = EGBcontroller(obj,q,qdot,lookupTable,desVel) 2 | 3 | 4 | loadingCompression = 0.05; %Amount of compression for Loading and Unloading phases (m) 5 | liftOffClearance = 0.05; %Distance toe must rise before the leg can be swung forward during flight (m) 6 | k_legAngle = 0.05; %m/(m/s) foot distance 7 | L_flight = 0.7; %unstretched leg length during compression and flight (m) 8 | L_extension = 0.72; %unstretched leg length during thrust (m) 9 | des_vel = desVel; %m/s 10 | phi_des = 0; %Rads 11 | 12 | %Low Level controller gains, UNTUNED 13 | kp_swing = 500; 14 | kv_swing = 10; 15 | kp_hip = 0;%100; 16 | kv_hip = 0;%10; 17 | kp_L0 = 50000; 18 | kv_L0 = 1000; 19 | 20 | %States: 1= Loading, 2= Compression, 3= Thrust, 4= Unloading, 5=Flight 21 | persistent stateMachine footTDAngle 22 | if isempty(stateMachine) || isempty(footTDAngle) 23 | stateMachine = 5; %Simulation starts by dropping robot 24 | footTDAngle = 0; 25 | disp('initialized persistents in raibert controller'); 26 | end 27 | 28 | if length(q) == 4 29 | ydot = qdot; 30 | y = q; 31 | alpha = atan2(-y(1),y(2)); 32 | alphadot = ( y(1)*ydot(2)/(y(2)^2) - ydot(1)/y(2) ) / ( 1 + (y(1)/y(2))^2 ); 33 | %Leg Length: toe is at (0,0) 34 | L = sqrt(y(1)^2 + y(2)^2); 35 | Ldot = ( y(1)*ydot(1) + y(2)*ydot(2) ) / sqrt(y(1)^2 + y(2)^2); 36 | q = [y(1:3) ;alpha ;y(4) ;L]; 37 | qdot = [ydot(1:3);alphadot;ydot(4);Ldot]; 38 | end 39 | 40 | %Check for a chance in the state 41 | switch stateMachine 42 | case 1 %Loading 43 | %Transition once the leg has compressed a small amount 44 | if q(5) - q(6) >= loadingCompression 45 | stateMachine = 2; 46 | disp('Controller End Loading'); 47 | end 48 | case 2 %Compression 49 | %Transition if the leg begins to extend 50 | if qdot(6) >= 0 51 | stateMachine = 3; 52 | disp('Controller End Compression'); 53 | end 54 | case 3 %Thrust 55 | %Transition once the leg compression is less than the threshold 56 | if q(5) - q(6) <= loadingCompression 57 | stateMachine = 4; 58 | disp('Controller End Thrust'); 59 | end 60 | case 4 %Unloading 61 | %Transition when the foot has sufficient clearance 62 | if q(2) - q(6)*cos(q(4)) >= liftOffClearance 63 | stateMachine = 5; 64 | disp(['New TD Angle ',num2str(footTDAngle)]); 65 | end 66 | case 5 %Flight 67 | %Transition if the foot touches the ground 68 | if q(2) - q(6)*cos(q(4)) <= 0 69 | stateMachine = 1; 70 | disp('Controller has Landed'); 71 | end 72 | end 73 | 74 | %Force and Torque Controllers depending on state 75 | ctrlParams(4) = 0; 76 | switch stateMachine 77 | case 1 %Loading 78 | u(1,1) = -kp_L0*(q(5) - L_flight) - kv_L0*qdot(5); 79 | u(2,1) = 0; %Zero Hip Torque 80 | 81 | ctrlParams(1) = L_flight; 82 | ctrlParams(2) = 0; 83 | ctrlParams(3) = 0; 84 | case 2 %Compression 85 | u(1,1) = -kp_L0*(q(5) - L_flight) - kv_L0*qdot(5); 86 | u(2,1) = kp_hip*(q(3) - phi_des) + kv_hip*qdot(3); 87 | 88 | ctrlParams(1) = L_flight; 89 | ctrlParams(2) = phi_des; 90 | ctrlParams(3) = 0; 91 | case 3 %Thrust 92 | u(1,1) = -kp_L0*(q(5) - L_extension) - kv_L0*qdot(5); 93 | u(2,1) = kp_hip*(q(3) - phi_des) + kv_hip*qdot(3); 94 | 95 | ctrlParams(1) = L_extension; 96 | ctrlParams(2) = phi_des; 97 | ctrlParams(3) = 0; 98 | case 4 %Unloading 99 | u(1,1) = -kp_L0*(q(5) - L_flight) - kv_L0*qdot(5); 100 | u(2,1) = 0; %Zero Hip Torque 101 | 102 | ctrlParams(1) = L_extension; 103 | ctrlParams(2) = 0; 104 | ctrlParams(3) = 0; 105 | case 5 %Flight 106 | xDot =clamp(qdot(1),min(lookupTable.xDot),max(lookupTable.xDot)); 107 | % xDot = qdot(1); 108 | yDot =clamp(qdot(2),min(lookupTable.yDot),max(lookupTable.yDot)); 109 | 110 | footTDAngle = interp2(lookupTable.dX,lookupTable.dY,lookupTable.alpha,xDot,yDot); 111 | ctrlParams(4) = footTDAngle; 112 | footTDAngle = footTDAngle + k_legAngle*(qdot(1) - des_vel); 113 | if isnan(footTDAngle) 114 | keyboard 115 | end 116 | u(1,1) = -kp_L0*(q(5) - L_flight) - kv_L0*qdot(5); 117 | u(2,1) = -kp_swing*(q(4) - footTDAngle) - kv_swing*qdot(4); %Swing leg forward 118 | 119 | ctrlParams(1) = L_flight; 120 | ctrlParams(2) = 0; 121 | ctrlParams(3) = footTDAngle; 122 | end 123 | 124 | if sum(imag(q)) + sum(imag(qdot)) ~= 0 125 | keyboard 126 | end 127 | 128 | end 129 | 130 | function var_out = clamp(var,min_in,max_in) 131 | 132 | var_out = min(max(var,min_in),max_in); 133 | end 134 | 135 | 136 | 137 | -------------------------------------------------------------------------------- /Lossy Monoped/Controllers/EGB/findNeutralAngle.m: -------------------------------------------------------------------------------- 1 | function [ alphaTD ] = findNeutralAngle(simObject, xDot, yDot) 2 | %UNTITLED Summary of this function goes here 3 | % Detailed explanation goes here 4 | persistent obj; 5 | if isempty(obj) 6 | obj = liftOffSimulation(simObject,0.7); 7 | end 8 | obj.xDot = xDot; 9 | obj.yDot = yDot; 10 | % options = optimset('Display','iter'); 11 | alphaTD = fminbnd(@(x)obj.simulate(x),-pi/2,pi/2); 12 | % keyboard 13 | end 14 | 15 | -------------------------------------------------------------------------------- /Lossy Monoped/Controllers/EGB/generateLookupTable.m: -------------------------------------------------------------------------------- 1 | xDot = -2:0.1:2; 2 | yDot = -0.3:-0.1:-3; 3 | 4 | [dX,dY] = meshgrid(xDot,yDot); 5 | 6 | for i = 1:size(dX,1) 7 | for j = 1:size(dX,2) 8 | alpha(i,j) = findNeutralAngle(0, dX(i,j), dY(i,j)); 9 | end 10 | end 11 | 12 | %% 13 | 14 | surf(dX,dY,alpha) 15 | 16 | interp2(dX,dY,alpha,xq,yq) -------------------------------------------------------------------------------- /Lossy Monoped/Controllers/EGB/liftOffSimulation.m: -------------------------------------------------------------------------------- 1 | classdef liftOffSimulation 2 | %SLIPDYNAMICS This is a dynamic simulation of a nondimensionalized 3 | % spring loaded inverted pendulum (SLIP) on flat ground. It is used to 4 | % create a lookup table for touchdown angle 5 | 6 | properties 7 | %y velocity 8 | yDot = -1; 9 | %x velocity 10 | xDot = 0; 11 | robot; 12 | L0 = 0.7; 13 | 14 | end %properties 15 | 16 | methods 17 | 18 | function obj = liftOffSimulation(inputObj,L0) 19 | %Potentially used in the future for Graphics init? 20 | obj.robot = inputObj; 21 | obj.L0 = L0; 22 | end 23 | 24 | function r2 = simulate(o,alphaTD) 25 | %simulate Run a simulation return the deviation from symetric 26 | %squared. 27 | dyn = @(t,y) stanceDynamics(o,o.robot,y); 28 | 29 | options = odeset('Events',@(t,y) (stanceTransitions(t,y,o))); 30 | y0 = [-o.L0*sin(alphaTD);o.L0*cos(alphaTD);0;o.L0;o.xDot;o.yDot;0;0]; 31 | 32 | [~,y_out,~,~,~] = ode45(dyn,[0,20],y0,options); 33 | 34 | r2 = (atan2(-y_out(end,1),y_out(end,2)) + alphaTD)^2; 35 | end 36 | end %Methods 37 | 38 | end %Class 39 | 40 | function [value,isterminal,direction] = stanceTransitions(t,y,o) 41 | %ODE Event function sensing foot liftoff during stance while moving upward 42 | 43 | %On a pure elastic leg zero force on the foot is identical to when the 44 | %leg length reaches the unstretched length 45 | value = sqrt(y(1)^2 + y(2)^2) - o.L0; 46 | isterminal = 1; 47 | direction = 1; 48 | end %function stanceTransitions 49 | 50 | function [dX] = stanceDynamics(obj,robot,X) 51 | %Low Level controller gains, UNTUNED 52 | kp_hip = 0; %100; 53 | kv_hip = 0; %10; 54 | kp_L0 = 50000; 55 | kv_L0 = 1000; 56 | 57 | q = X(1:4); 58 | qdot = X(5:8); 59 | 60 | u(1,1) = -kp_L0*(q(4) - obj.L0) - kv_L0*qdot(4); 61 | u(2,1) = kp_hip*(q(3) - 0) + kv_hip*qdot(3); 62 | 63 | dX(1:4) = qdot; 64 | 65 | M = robot.massMatrixStance(q); 66 | f_d = robot.dampingStance(q,qdot); 67 | B = robot.controlStance(q); 68 | h = robot.dynamicsStance(q,qdot); 69 | %The Second Order dynamics M*q_ddot + h = Bu + f_damping 70 | dX(5:8) = M\(B*u + f_d - h); 71 | 72 | %Fix the leg resting length 73 | % dX(4) = 0; 74 | % dX(8) = 0; 75 | 76 | dX = dX'; 77 | end 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /Lossy Monoped/Controllers/Raibert/raibertController.m: -------------------------------------------------------------------------------- 1 | function [ u ] = raibertController(obj,q,qdot) 2 | %RAIBERTCONTROLLER A faithful implementation of Raibert's monopod hopping 3 | %controller 4 | 5 | 6 | loadingCompression = 0.05; %Amount of compression for Loading and Unloading phases (m) 7 | liftOffClearance = 0.05; %Distance toe must rise before the leg can be swung forward during flight (m) 8 | k_legAngle = 0.1; %m/(m/s) foot distance 9 | L_flight = 0.7; %unstretched leg length during compression and flight (m) 10 | L_extension = 0.75; %unstretched leg length during thrust (m) 11 | des_vel = 0.2; %m/s 12 | phi_des = 0; %Rads 13 | 14 | %Low Level controller gains, UNTUNED 15 | kp_swing = 90; 16 | kv_swing = 7; 17 | kp_hip = 100; 18 | kv_hip = 10; 19 | kp_L0 = 5000; 20 | kv_L0 = 300; 21 | 22 | %States: 1= Loading, 2= Compression, 3= Thrust, 4= Unloading, 5=Flight 23 | persistent stateMachine footTDAngle 24 | if isempty(stateMachine) || isempty(footTDAngle) 25 | stateMachine = 5; %Simulation starts by dropping robot 26 | footTDAngle = 0; 27 | disp('initialized persistents in raibert controller'); 28 | end 29 | 30 | if length(q) == 4 31 | ydot = qdot; 32 | y = q; 33 | alpha = atan2(-y(1),y(2)); 34 | alphadot = ( y(1)*ydot(2)/(y(2)^2) - ydot(1)/y(2) ) / ( 1 + (y(1)/y(2))^2 ); 35 | %Leg Length: toe is at (0,0) 36 | L = sqrt(y(1)^2 + y(2)^2); 37 | Ldot = ( y(1)*ydot(1) + y(2)*ydot(2) ) / sqrt(y(1)^2 + y(2)^2); 38 | q = [y(1:3) ;alpha ;y(4) ;L]; 39 | qdot = [ydot(1:3);alphadot;ydot(4);Ldot]; 40 | end 41 | 42 | %Check for a chance in the state 43 | switch stateMachine 44 | case 1 %Loading 45 | %Transition once the leg has compressed a small amount 46 | if q(5) - q(6) >= loadingCompression 47 | stateMachine = 2; 48 | disp('Controller End Loading'); 49 | end 50 | case 2 %Compression 51 | %Transition if the leg begins to extend 52 | if qdot(6) >= 0 53 | stateMachine = 3; 54 | disp('Controller End Compression'); 55 | end 56 | case 3 %Thrust 57 | %Transition once the leg compression is less than the threshold 58 | if q(5) - q(6) <= loadingCompression 59 | stateMachine = 4; 60 | disp('Controller End Thrust'); 61 | end 62 | case 4 %Unloading 63 | %Transition when the foot has sufficient clearance 64 | if q(2) - q(6)*cos(q(4)) >= liftOffClearance 65 | stateMachine = 5; 66 | disp('Controller End Liftoff'); 67 | 68 | %Calculate the new desired leg angle 69 | x_f0 = 0.5*qdot(1)*0.12 + k_legAngle*(qdot(1) - des_vel); 70 | 71 | x_f0 = min(max(x_f0 ,-0.7*L_flight),0.7*L_flight); %Cap the forward leg displacement to keep it reasonable 72 | %Desired touchdown leg angle 73 | footTDAngle = asin(x_f0/L_flight); 74 | disp(['New TD Angle ',num2str(footTDAngle)]); 75 | end 76 | case 5 %Flight 77 | %Transition if the foot touches the ground 78 | if q(2) - q(6)*cos(q(4)) <= 0 79 | stateMachine = 1; 80 | disp('Controller has Landed'); 81 | end 82 | end 83 | 84 | %Force and Torque Controllers depending on state 85 | switch stateMachine 86 | case 1 %Loading 87 | u(1,1) = -kp_L0*(q(5) - L_flight) - kv_L0*qdot(5); 88 | u(2,1) = 0; %Zero Hip Torque 89 | case 2 %Compression 90 | u(1,1) = -kp_L0*(q(5) - L_flight) - kv_L0*qdot(5); 91 | u(2,1) = kp_hip*(q(3) - phi_des) + kv_hip*qdot(3); 92 | case 3 %Thrust 93 | u(1,1) = -kp_L0*(q(5) - L_extension) - kv_L0*qdot(5); 94 | u(2,1) = kp_hip*(q(3) - phi_des) + kv_hip*qdot(3); 95 | case 4 %Unloading 96 | u(1,1) = -kp_L0*(q(5) - L_flight) - kv_L0*qdot(5); 97 | u(2,1) = 0; %Zero Hip Torque 98 | case 5 %Flight 99 | u(1,1) = -kp_L0*(q(5) - L_flight) - kv_L0*qdot(5); 100 | u(2,1) = -kp_swing*(q(4) - footTDAngle) - kv_swing*qdot(4); %Swing leg forward, 101 | end 102 | 103 | if sum(imag(q)) + sum(imag(qdot)) ~= 0 104 | keyboard 105 | end 106 | 107 | end 108 | 109 | -------------------------------------------------------------------------------- /Lossy Monoped/IPOPT/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/IPOPT/ipopt_auxdata.m: -------------------------------------------------------------------------------- 1 | function [x, info] = ipopt_auxdata(x0, funcs, options) 2 | % Wrapper function to implement auxdata functionality using Matlab function handles 3 | 4 | if ~isfield(options, 'auxdata') 5 | % no auxdata given, call ipopt as normal 6 | if isfield(funcs, 'iterfunc') && nargin(funcs.iterfunc) == 2 7 | % check if iterfunc has only 2 inputs as before Ipopt version 3.11 8 | funcs_old = funcs; 9 | funcs.iterfunc = @(t, f, varstruct) funcs_old.iterfunc(t, f); 10 | end 11 | [x, info] = ipopt(x0, funcs, options); 12 | else 13 | % remove auxdata from options structure and modify function handles 14 | auxdata = options.auxdata; 15 | options_new = rmfield(options, 'auxdata'); 16 | funcs_new.objective = @(x) funcs.objective(x, auxdata); 17 | funcs_new.gradient = @(x) funcs.gradient(x, auxdata); 18 | if isfield(funcs, 'constraints') 19 | funcs_new.constraints = @(x) funcs.constraints(x, auxdata); 20 | end 21 | if isfield(funcs, 'jacobian') 22 | funcs_new.jacobian = @(x) funcs.jacobian(x, auxdata); 23 | end 24 | if isfield(funcs, 'jacobianstructure') 25 | funcs_new.jacobianstructure = @() funcs.jacobianstructure(auxdata); 26 | end 27 | if isfield(funcs, 'hessian') 28 | funcs_new.hessian = @(x, sigma, lambda) funcs.hessian(x, sigma, lambda, auxdata); 29 | end 30 | if isfield(funcs, 'hessianstructure') 31 | funcs_new.hessianstructure = @() funcs.hessianstructure(auxdata); 32 | end 33 | if isfield(funcs, 'iterfunc') 34 | % This assumes the pre-3.11 convention for iterfunc. If you would 35 | % like to use the additional information that is available via the 36 | % third input argument to iterfunc as of Ipopt version 3.11, you 37 | % will need to modify this section by uncommenting the line below. 38 | funcs_new.iterfunc = @(t, f, varstruct) funcs.iterfunc(t, f, auxdata); 39 | % funcs_new.iterfunc = @(t, f, varstruct) funcs.iterfunc(t, f, varstruct, auxdata); 40 | end 41 | [x, info] = ipopt(x0, funcs_new, options_new); 42 | end 43 | -------------------------------------------------------------------------------- /Lossy Monoped/KevinSlipHopperFlight.m: -------------------------------------------------------------------------------- 1 | classdef KevinSlipHopperFlight < Auto.Dynamics 2 | 3 | properties 4 | %All units are SI 5 | 6 | %General params 7 | grav = 9.81 8 | %Main Body 9 | m_body = 1; 10 | I_body = 0.1; %Calc realistic 11 | %Hip Joint 12 | b_hip = 0; 13 | %Thigh 14 | m_thigh = 0.1; 15 | r_thigh = 0.2; 16 | I_thigh = 0.2; %Calc realistic 17 | %Leg Length Rotor 18 | b_rot = 0; 19 | m_rot = 0.5; %Effective reflective interia on linear leg length 20 | %Leg Spring 21 | k_leg = 1000; 22 | b_leg = 0; 23 | %Toe 24 | m_toe = 0.1; 25 | end 26 | properties (Constant,Hidden) 27 | domainRank = 6; 28 | % [x, y, theta, alpha, L_0, L] 29 | % [Body X Position, Body Y position, Body Pitch, Leg Angle, resting 30 | % leg length, actual leg length] 31 | 32 | end 33 | methods 34 | function sys = KevinSlipHopperFlight(o) 35 | %Copy over model parameters if specified 36 | if( nargin == 1) 37 | sys.grav = o.g; 38 | sys.m_body = o.m_body; 39 | sys.I_body = o.I_body; 40 | sys.b_hip = o.b_hip; 41 | sys.m_thigh = o.m_thigh; 42 | sys.r_thigh = o.r_thigh; 43 | sys.I_thigh = o.I_thigh; 44 | sys.b_rot = o.b_rot; 45 | sys.m_rot = o.m_rot; 46 | sys.k_leg = o.k_leg; 47 | sys.b_leg = o.b_leg; 48 | sys.m_toe = o.m_toe; 49 | end 50 | %Copy the files to the correct name (to differential flight and 51 | %stance 52 | copyfile('dynamics.m','dynamicsFlight.m'); 53 | copyfile('massMatrix.m','massMatrixFlight.m'); 54 | end 55 | function joints = eval(o,q) 56 | %I think this generates a foot/joint jacobian for use in applying external forces (eg. ground reaction) 57 | %I am not using it in the implementation 58 | joints = []'; 59 | end 60 | 61 | function T = kineticEnergy(o,q,qdot) 62 | T = 0.5 * o.m_body * (qdot(1)^2 + qdot(2)^2) ... %Body Linear KE 63 | + 0.5 * o.I_body * qdot(3)^2 ... %Body Rotational KE 64 | + 0.5 * o.m_thigh * ((qdot(1) + o.r_thigh*qdot(4)*cos(q(4)))^2 + (qdot(2) + o.r_thigh*qdot(4)*sin(q(4)))^2) ... %Thigh Linear KE 65 | + 0.5 * o.I_thigh * qdot(4)^2 ... %Thigh Rotational KE 66 | + 0.5 * o.m_toe * ((qdot(1) + qdot(6)*sin(q(4)) + q(6)*qdot(4)*cos(q(4)))^2 + (qdot(2) - qdot(6)*cos(q(4)) + q(6)*qdot(4)*sin(q(4)))^2) ... %Toe KE (point mass) 67 | + 0.5 * o.m_rot * qdot(5)^2; %Leg Actuator reflected interia KE 68 | end 69 | 70 | function V = potentialEnergy(o,q) 71 | V = o.grav * o.m_body * q(2) ... %Body Grav Potential 72 | + o.grav * o.m_thigh * (q(2) - o.r_thigh*cos(q(4)))... %Thigh Grav Potential 73 | + o.grav * o.m_toe * (q(2) - q(6)*cos(q(4)))... %Foot/toe Grav Potential 74 | + 0.5 * o.k_leg * (q(6) - q(5))^2; %Leg Spring Potential 75 | end 76 | 77 | function f_d = dampingForces(o,q,qdot) 78 | f_d = [ -o.b_rot * qdot(5) * sin(q(4)); ... %Rotor damping 79 | -o.b_rot * qdot(5) * cos(q(4)); ... %Rotor damping 80 | -o.b_hip * (qdot(3) - qdot(4)); ... %Hip Joint Damping 81 | -o.b_hip * (qdot(4) - qdot(3)); ... %Hip Joint Damping 82 | -o.b_rot * qdot(5) * sin(q(4)) - o.b_leg * (qdot(5) - qdot(6)); ... %Rotor Damping and Leg Spring Damping 83 | - o.b_leg * (qdot(6) - qdot(5)); ]; %Leg spring damping 84 | end 85 | 86 | function f_c = controlForces(o,q,u) 87 | f_c = [ -sin(q(4)), 0; ... 88 | cos(q(4)), 0; ... 89 | 0, -1;... 90 | 0, 1;... 91 | 1, 0;... 92 | 0, 0;]*u; 93 | end 94 | end 95 | end 96 | 97 | -------------------------------------------------------------------------------- /Lossy Monoped/KevinSlipHopperStance.m: -------------------------------------------------------------------------------- 1 | classdef KevinSlipHopperStance < Auto.Dynamics 2 | 3 | properties 4 | %All units are SI 5 | 6 | %General params 7 | grav = 9.81 8 | %Main Body 9 | m_body = 1; 10 | I_body = 0.1; %Calc realistic 11 | %Hip Joint 12 | b_hip = 0; 13 | %Thigh 14 | m_thigh = 0.1; 15 | r_thigh = 0.2; 16 | I_thigh = 0.2; %Calc realistic 17 | %Leg Length Rotor 18 | b_rot = 0; 19 | m_rot = 0.5; %Effective reflective interia on linear leg length 20 | %Leg Spring 21 | k_leg = 2000; 22 | b_leg = 0; 23 | %Toe 24 | m_toe = 0.1; 25 | end 26 | properties (Constant,Hidden) 27 | domainRank = 4; 28 | % [x, y, theta, L_0] 29 | % [Body X Position, Body Y position, Body Pitch, resting leg length] 30 | 31 | end 32 | methods 33 | function sys = KevinSlipHopperStance(o) 34 | %Copy over model parameters if specified 35 | if( nargin == 1) 36 | sys.grav = o.g; 37 | sys.m_body = o.m_body; 38 | sys.I_body = o.I_body; 39 | sys.b_hip = o.b_hip; 40 | sys.m_thigh = o.m_thigh; 41 | sys.r_thigh = o.r_thigh; 42 | sys.I_thigh = o.I_thigh; 43 | sys.b_rot = o.b_rot; 44 | sys.m_rot = o.m_rot; 45 | sys.k_leg = o.k_leg; 46 | sys.b_leg = o.b_leg; 47 | sys.m_toe = o.m_toe; 48 | end 49 | %Copy the files to the correct name (to differential flight and 50 | %stance 51 | copyfile('dynamics.m','dynamicsStance.m'); 52 | copyfile('massMatrix.m','massMatrixStance.m'); 53 | end 54 | function joints = eval(o,q) 55 | %I think this generates a foot/joint jacobian for use in applying external forces (eg. ground reaction) 56 | %I am not using it in the implementation 57 | joints = []'; 58 | end 59 | 60 | function T = kineticEnergy(o,q,qdot) 61 | %States that are driven when the foot is on the ground 62 | %Leg Angle: toe is at (0,0) 63 | alphadot = ( q(1)*qdot(2)/(q(2)^2) - qdot(1)/q(2) ) / ( 1 + (q(1)/q(2))^2 ); 64 | 65 | T = 0.5 * o.m_body * (qdot(1)^2 + qdot(2)^2) ... %Body Linear KE 66 | + 0.5 * o.I_body * qdot(3)^2 ... %Body Rotational KE 67 | + 0.5 * o.m_thigh * (qdot(1) - o.r_thigh*(qdot(1)*(q(1)^2 + q(2)^2) - q(1)^2*qdot(1) - q(1)*q(2)*qdot(2))/(q(1)^2 + q(2)^2)^(3/2)) ... 68 | + 0.5 * o.m_thigh * (qdot(2) - o.r_thigh*(qdot(2)*(q(1)^2 + q(2)^2) - q(2)^2*qdot(2) - q(1)*q(2)*qdot(1))/(q(1)^2 + q(2)^2)^(3/2)) ... 69 | + 0.5 * o.I_thigh * alphadot^2 ... %Thigh Rotational KE 70 | + 0.5 * o.m_rot * qdot(4)^2; %Leg Actuator reflected interia KE 71 | 72 | % + 0.5 * o.m_thigh * ((qdot(1) + o.r_thigh*alphadot*(q(2)/sqrt(q(1)^2 + q(2)^2)))^2 + (qdot(2) + o.r_thigh*alphadot*(-q(1)/sqrt(q(1)^2 + q(2)^2)))^2) ... %Thigh Linear KE 73 | 74 | end 75 | 76 | function V = potentialEnergy(o,q) 77 | %States that are driven when the foot is on the ground 78 | %Leg Angle: toe is at (0,0) 79 | %Leg Length: toe is at (0,0) 80 | 81 | V = o.grav * o.m_body * q(2) ... %Body Grav Potential 82 | + o.grav * o.m_thigh * (q(2) - o.r_thigh*(q(2)/sqrt(q(1)^2 + q(2)^2)))... %Thigh Grav Potential 83 | + 0.5 * o.k_leg * (sqrt(q(1)^2 + q(2)^2) - q(4))^2; %Leg Spring Potential 84 | end 85 | 86 | function f_d = dampingForces(o,q,qdot) 87 | %States that are driven when the foot is on the ground 88 | %Leg Angle: toe is at (0,0) 89 | alphadot = ( q(1)*qdot(2)/(q(2)^2) - qdot(1)/q(2) ) / ( 1 + (q(1)/q(2))^2 ); 90 | %Leg Length: toe is at (0,0) 91 | L = sqrt(q(1)^2 + q(2)^2); 92 | Ldot = ( q(1)*qdot(1) + q(2)*qdot(2) ) / sqrt(q(1)^2 + q(2)^2); 93 | 94 | f_d = [ -o.b_rot * qdot(4) * (-q(1)/sqrt(q(1)^2 + q(2)^2)); ... %Rotor damping 95 | -o.b_rot * qdot(4) * (q(2)/sqrt(q(1)^2 + q(2)^2)); ... %Rotor damping 96 | -o.b_hip * (qdot(3) - alphadot); ... %Hip Joint Damping 97 | -o.b_rot * qdot(4) * (-q(1)/sqrt(q(1)^2 + q(2)^2)) - o.b_leg * (qdot(4) - L);]; %Rotor Damping and Leg Spring Damping 98 | 99 | end 100 | 101 | function f_c = controlForces(o,q,u) 102 | %States that are driven when the foot is on the ground 103 | %Leg Angle: toe is at (0,0) 104 | alpha = atan2(-q(1),q(2)); 105 | %Leg Length: toe is at (0,0) 106 | L = sqrt(q(1)^2 + q(2)^2); 107 | 108 | f_c = [ (q(1)/sqrt(q(1)^2 + q(2)^2)), 0; ... 109 | (q(2)/sqrt(q(1)^2 + q(2)^2)), 0; ... 110 | 0, -1;... 111 | 1, 0;]*u; 112 | end 113 | end 114 | end 115 | 116 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/@Nlp/Nlp.m: -------------------------------------------------------------------------------- 1 | %NLP Nonlinear programming optimization problem. 2 | % 3 | % Syntax: 4 | % obj = Nlp; 5 | % 6 | % Optional Input Arguments: 7 | % name - (CHAR) Problem name. 8 | % description - (CHAR) Problem description. 9 | % verbose - (LOGICAL) Set verbosity of user feedback. 10 | % 11 | % Description: 12 | % NLP is a MATLAB object-oriented class for formulating, building, and 13 | % solving nonlinear optimization problems. 14 | % 15 | % Features: 16 | % * Handles a wide variety of optimization problems including linear, 17 | % nonlinear, constrained, and unconstrained. 18 | % * Uses a object-oriented interface to abstract the problem formulation, 19 | % allowing for a simple, intuitive and robust symbolic problem 20 | % generation. 21 | % * Automatically generates analytical gradients and Hessians (optional) 22 | % to increase solver performance. 23 | % * Includes wrappers to interface MATLAB solvers as well as a hand full 24 | % of open-source solvers. 25 | 26 | % Copyright 2013-2015 Mikhail S. Jones 27 | 28 | classdef Nlp < handle 29 | 30 | % PUBLIC PROPERTIES ===================================================== 31 | properties % TODO: set methods 32 | % Vector of variable lower bounds 33 | variableLowerBound@double vector = [] 34 | % Vector of variable upper bounds 35 | variableUpperBound@double vector = [] 36 | % Vector of variable initial guesses 37 | initialGuess@double vector = [] 38 | % Vector of variable solutions 39 | solution@double vector = [] 40 | end % properties 41 | 42 | % PROTECTED PROPERTIES ================================================== 43 | properties (SetAccess = public) 44 | % Vector of design variables 45 | variable@Variable 46 | % Vector of objectives 47 | objective@Objective 48 | % Vector of constraints 49 | constraint@Constraint 50 | % Options structure 51 | options@struct scalar 52 | end % properties 53 | 54 | % DEPENDENT PROPERTIES ================================================== 55 | properties (Dependent = true) 56 | % Number of variables in NLP problem 57 | numberOfVariables@double scalar 58 | % Number of constraints in NLP problem 59 | numberOfConstraints@double scalar 60 | % Number of objectives in NLP problem 61 | numberOfObjectives@double scalar 62 | end % properties 63 | 64 | % PUBLIC METHODS ======================================================== 65 | methods 66 | function obj = Nlp(varargin) 67 | %NLP Creates a nonlinear programming optimization problem object. 68 | 69 | % Construct input argument parser 70 | parser = inputParser; 71 | parser.addParamValue('name', 'No Name', ... 72 | @(x) validateattributes(x, {'char'}, {})); 73 | parser.addParamValue('description', 'No Description', ... 74 | @(x) validateattributes(x, {'char'}, {})); 75 | parser.addParamValue('verbose', true, ... 76 | @(x) validateattributes(x, {'logical'}, {'scalar'})); 77 | 78 | % Parse input arguments 79 | parser.parse(varargin{:}); 80 | 81 | % Store the results 82 | obj.options = parser.Results; 83 | end % Nlp 84 | 85 | function n = get.numberOfVariables(obj) 86 | %GET.NUMBEROFVARIABLES Get method for number of variables. 87 | 88 | % Number of variables 89 | n = length(obj.initialGuess); 90 | end % get.numberOfVariables 91 | 92 | function n = get.numberOfObjectives(obj) 93 | %GET.NUMBEROFOBJECTIVES Get method for number of objectives. 94 | 95 | % Number of objectives 96 | n = length(obj.objective); 97 | end % get.numberOfObjectives 98 | 99 | function n = get.numberOfConstraints(obj) 100 | %GET.NUMBEROFCONSTRAINTS Get method for number of constraints. 101 | 102 | % Check if any constraints have been defined 103 | if isempty(obj.constraint) 104 | n = 0; 105 | else 106 | % Construct reference to object array 107 | refCons = [obj.constraint.expression]; 108 | 109 | % Compute number of constraints 110 | n = sum([refCons.length]); 111 | end % if 112 | end % get.numberOfConstraints 113 | end % methods 114 | end % classdef 115 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/@Nlp/addConstraint.m: -------------------------------------------------------------------------------- 1 | function addConstraint(obj, lowerBound, expression, upperBound, varargin) 2 | %ADDCONSTRAINT Add a constraint to the NLP problem. 3 | % 4 | % Syntax: 5 | % obj.addConstraint(lowerBound, expression, upperBound) 6 | % 7 | % Required Input Arguments: 8 | % lowerBound - (DOUBLE) Design variable lower bounds. 9 | % expression - (EXPRESSIONNODE) Symbolic constraint expression. 10 | % upperBound - (DOUBLE) Design variable upper bounds. 11 | % 12 | % Optional Input Arguments: 13 | % description - (CHAR) Description for identification. 14 | 15 | % Copyright 2013-2015 Mikhail S. Jones 16 | 17 | % Construct input argument parser 18 | parser = inputParser; 19 | parser.addParamValue('description', 'No Name', ... 20 | @(x) validateattributes(x, ... 21 | {'char'}, {'vector'})); 22 | 23 | % Parse input arguments 24 | parser.parse(varargin{:}); 25 | 26 | % Store the results 27 | opts = parser.Results; 28 | 29 | % User feedback 30 | fprintf('Adding ([\b%d]\b) [\b%s]\b constraints... \n', ... 31 | sum(sum([expression.length])), opts.description); 32 | 33 | % ANDY ADDED THIS 34 | if numel(lowerBound) == 1 35 | lowerBound = repmat(lowerBound,numel(expression),1); 36 | upperBound = repmat(upperBound,numel(expression),1); 37 | end 38 | 39 | for i = 1:numel(expression) 40 | % Append object array with more constraints 41 | obj.constraint(end+1) = Constraint(expression(i), ... 42 | lowerBound(i), upperBound(i), opts.description); % ANDY: added (i) to upper and lower bounds 43 | end % for 44 | end % addConstraint 45 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/@Nlp/addObjective.m: -------------------------------------------------------------------------------- 1 | function addObjective(obj, expression, varargin) 2 | %ADDOBJECTIVE Add an objective to the NLP problem. 3 | % 4 | % Syntax: 5 | % obj.addObjective(expression); 6 | % 7 | % Required Input Arguments: 8 | % expression - (EXPRESSIONNODE) Symbolic objective expression. 9 | % 10 | % Optional Input Arguments: 11 | % description - (CHAR) Objective description. 12 | 13 | % Copyright 2013-2015 Mikhail S. Jones 14 | 15 | % Construct input argument parser 16 | parser = inputParser; 17 | parser.addParamValue('description', 'No Name', ... 18 | @(x) validateattributes(x, ... 19 | {'char'}, {'vector'})); 20 | 21 | % Parse input arguments 22 | parser.parse(varargin{:}); 23 | 24 | % Store the results 25 | opts = parser.Results; 26 | 27 | % User feedback 28 | fprintf('Adding ([\b1]\b) [\b%s]\b objective... \n', opts.description); 29 | 30 | % Verify data type 31 | expression = ConstantNode(0) + expression; 32 | 33 | % Check number of objective objects 34 | if obj.numberOfObjectives >= 1 35 | error('Multi-objective problem formulations are not supported.'); 36 | end % if 37 | 38 | % Store objective function in problem structure 39 | obj.objective(end+1) = Objective(expression, opts.description); 40 | end % addObjective 41 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/@Nlp/addPdeConstraint.m: -------------------------------------------------------------------------------- 1 | function addPdeConstraint(obj, variable, expression, dependent, varargin) 2 | %ADDPDECONSTRAINT Add a PDE constraint to the NLP problem. 3 | % 4 | % Syntax: 5 | % obj.addPdeConstraint(variable, expression, dependent); 6 | % 7 | % Required Input Arguments: 8 | % variable - (EXPRESSIONNODE) Variable being differentiated. 9 | % expression - (EXPRESSIONNODE) Differential constraint expression. 10 | % dependent - (EXPRESSIONNODE) Differentiation with respect to. 11 | % 12 | % Optional Input Arguments: 13 | % method - (CHAR) Integration method. 14 | % description - (CHAR) Description for identification. 15 | 16 | % Copyright 2013-2014 Mikhail S. Jones 17 | 18 | % Construct input argument parser 19 | parser = inputParser; 20 | parser.addRequired('variable', ... 21 | @(x) validateattributes(x, ... 22 | {'ExpressionNode'}, {'scalar'})); 23 | parser.addRequired('expression', ... 24 | @(x) validateattributes(x, ... 25 | {'ExpressionNode'}, {'scalar'})); 26 | parser.addRequired('dependent', ... 27 | @(x) validateattributes(x, ... 28 | {'ExpressionNode', 'double'}, {'scalar'})); 29 | parser.addParamValue('method', 'trapezoidal'); 30 | parser.addParamValue('description', ''); 31 | 32 | % Parse input arguments 33 | parser.parse(variable, expression, dependent, varargin{:}); 34 | 35 | % Store the results 36 | opts = parser.Results; 37 | 38 | % Verify proper data types 39 | % n = max(variable.length, expression.length); 40 | % variable = variable + ConstantNode(0, n); 41 | % expression = expression + ConstantNode(0, n); 42 | % dependent = SymExpression(dependent); 43 | 44 | % TODO 45 | % Check size of variable and expression 46 | % Check both are actual variables 47 | 48 | % User feedback 49 | fprintf('Adding ([\b%d]\b) [\b%s]\b constraints... \n', ... 50 | expression.length, opts.description); 51 | 52 | % Handle dependent variable options 53 | if dependent.length == 1 54 | h = dependent/(expression.length - 1); 55 | elseif dependent.length == expression.length 56 | h = ind(dependent,2:dependent.length) - ind(dependent,1:dependent.length-1);%diff(dependent); 57 | else 58 | error('coalesce:Nlp:addPdeConstraint', ... 59 | 'Dependant must be scalar or same size as expression.'); 60 | end % if 61 | 62 | switch lower(opts.method) 63 | case 'explicit euler'; explicitEuler(variable, expression, h); 64 | case 'implicit euler'; implicitEuler(variable, expression, h); 65 | case 'trapezoidal'; trapezoidal(variable, expression, h); 66 | otherwise 67 | error('coalesce:optimize:Nlp:addPdeConstraint', ... 68 | 'Not a valid integration method.'); 69 | end % switch 70 | 71 | function trapezoidal(x, expr, h) 72 | %TRAPEZOIDAL Trapezoidal integration scheme. 73 | % The trapezoidal scheme is a second order accurate, implicit, two-stage 74 | % integration method. (Lobatto IIIA, order 2) 75 | % 76 | % Butcher Array: 77 | % 0 | 0 0 78 | % 1 | 1/2 1/2 79 | % ---------------- 80 | % | 1/2 1/2 81 | 82 | % Compute defect 83 | defect = - ind(x,2:x.length) + ind(x,1:x.length-1) + h*(ind(expr,1:expr.length-1) + ind(expr,2:expr.length))/2; 84 | 85 | % Append object array with more constraints 86 | obj.constraint(end+1) = Constraint(defect, 0, 0, opts.description); 87 | end % trapezoidal 88 | 89 | function implicitEuler(x, expr, h) 90 | %IMPLICITEULER Implicit (Backward) Euler integration scheme. 91 | % The implicit Euler scheme is a first order accurate, implicit, 92 | % one-stage integration method. (Radau, order 1) 93 | % 94 | % Butcher Array: 95 | % 0 | 0 0 96 | % ---------------- 97 | % | 0 1 98 | 99 | % Compute defect 100 | defect = - ind(x,2:x.length) + ind(x,1:x.length-1) + h*ind(expr,2:expr.length); 101 | 102 | % Append object array with more constraints 103 | obj.constraint(end+1) = Constraint(defect, 0, 0, opts.description); 104 | end % implicitEuler 105 | 106 | function explicitEuler(x, expr, h) 107 | %EXPLICITEULER Explicit (Forward) Euler integration scheme. 108 | % The explicit Euler scheme is a first order accurate, explicit, 109 | % one-stage integration method. (Radau, order 1) 110 | % 111 | % Butcher Array: 112 | % 0 | 0 113 | % ----------- 114 | % | 1 115 | 116 | % Compute defect 117 | defect = - ind(x,2:x.length) + ind(x,1:x.length-1) + h*ind(expr,1:expr.length-1); 118 | 119 | % Append object array with more constraints 120 | obj.constraint(end+1) = Constraint(defect, 0, 0, opts.description); 121 | end % explicitEuler 122 | end % addPdeConstraint 123 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/@Nlp/addVariable.m: -------------------------------------------------------------------------------- 1 | function expression = addVariable(obj, initialGuess, lowerBound, upperBound, varargin) 2 | %ADDVARIABLE Add a variable to the NLP problem. 3 | % 4 | % Syntax: 5 | % obj.addVariable(initialGuess, lowerBound, upperBound); 6 | % 7 | % Required Input Arguments: 8 | % initialGuess - (DOUBLE) Initial guess for optimization solver. 9 | % lowerBound - (DOUBLE) Variable lower bound. 10 | % upperBound - (DOUBLE) Variable upper bound. 11 | % 12 | % Optional Input Arguments: 13 | % description - (CHAR) Variable description. 14 | 15 | % Copyright 2013-2015 Mikhail S. Jones 16 | 17 | % Construct input argument parser 18 | parser = inputParser; 19 | parser.addRequired('initialGuess', ... 20 | @(x) validateattributes(x, ... 21 | {'double'}, {})); 22 | parser.addRequired('lowerBound', ... 23 | @(x) validateattributes(x, ... 24 | {'double'}, {})); 25 | parser.addRequired('upperBound', ... 26 | @(x) validateattributes(x, ... 27 | {'double'}, {})); 28 | parser.addParamValue('description', 'No Name', ... 29 | @(x) validateattributes(x, ... 30 | {'char'}, {'vector'})); 31 | parser.addParamValue('length', 1, ... 32 | @(x) validateattributes(x, ... 33 | {'double'}, {'scalar', 'positive', 'integer'})); 34 | 35 | % Parse input arguments 36 | parser.parse(initialGuess, lowerBound, upperBound, varargin{:}); 37 | 38 | % Store the results 39 | opts = parser.Results; 40 | 41 | % Compute dimensions 42 | igSize = size(initialGuess); 43 | lbSize = size(lowerBound); 44 | ubSize = size(upperBound); 45 | 46 | % Check the dimensions match 47 | if all(igSize(1:2) ~= lbSize(1:2)) || all(igSize(1:2) ~= ubSize(1:2)) 48 | error('Dimensions do not match.'); 49 | end % if 50 | 51 | % User feedback 52 | fprintf('Adding ([\b%d]\bx[\b%d]\bx[\b%d]\b) [\b%s]\b variable... \n', ... 53 | igSize(1:2), opts.length, opts.description); 54 | 55 | % Construct variables 56 | for d2 = 1:igSize(2) 57 | for d1 = 1:igSize(1) 58 | % Compute number of variables 59 | nVars = obj.numberOfVariables; 60 | 61 | % Construct variable 62 | expression(d1,d2) = Variable('var', nVars + (1:opts.length)); 63 | expression(d1,d2).nlp = obj; 64 | expression(d1,d2).initialGuess = initialGuess(d1,d2,:); 65 | expression(d1,d2).solution = expression(d1,d2).initialGuess; 66 | expression(d1,d2).lowerBound = lowerBound(d1,d2,:); 67 | expression(d1,d2).upperBound = upperBound(d1,d2,:); 68 | expression(d1,d2).description = opts.description; 69 | 70 | % Append variable object array 71 | obj.variable(end+1) = expression(d1,d2); 72 | end % for 73 | end % for 74 | end % addVariable 75 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/@Nlp/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/@Nlp/display.m: -------------------------------------------------------------------------------- 1 | function display(this) 2 | %DISPLAY Display problem formulation summary. 3 | % 4 | % Syntax: 5 | % obj 6 | % obj.display 7 | % 8 | % Description: 9 | % Display problem formulation summary in table form. 10 | % 11 | % Copyright 2013-2014 Mikhail S. Jones 12 | 13 | % Don't display arrays of objects 14 | if numel(this) > 1; return; end % if 15 | 16 | % Construct divider line strings 17 | majorDivider = [repmat('=', 1, 75), '\n']; 18 | minorDivider = [repmat('-', 1, 75), '\n']; 19 | 20 | % Display problem summary header 21 | fprintf('\n'); 22 | fprintf(majorDivider); 23 | fprintf([repmat(' ', 1, 30) '[\bPROBLEM SUMMARY]\b \n']); 24 | fprintf(majorDivider); 25 | fprintf('* Name: [\b%s]\b \n\n', this.options.name); 26 | 27 | % Display variable summary 28 | nVars = this.numberOfVariables; 29 | nSets = numel(this.variable); 30 | fprintf('[\bVARIABLES]\b \n'); 31 | fprintf(minorDivider); 32 | fprintf('* [\b%d]\b Design variable(s) in [\b%d]\b set(s)\n', nVars, nSets); 33 | for i = 1:nSets 34 | fprintf('\t* [\b%d]\b Variable(s) in [\b%s]\b set\n', ... 35 | this.variable(i).length, ... 36 | this.variable(i).description); 37 | end % for 38 | fprintf('\n'); 39 | 40 | % Display objectives summary 41 | nSets = numel(this.objective); 42 | fprintf('[\bOBJECTIVES]\b \n'); 43 | fprintf(minorDivider); 44 | fprintf('* [\b%d]\b Objective(s)\n', nSets); 45 | fprintf('\n'); 46 | 47 | % Display constraints summary 48 | nCons = this.numberOfConstraints; 49 | nSets = numel(this.constraint); 50 | fprintf('[\bCONSTRAINTS]\b \n'); 51 | fprintf(minorDivider); 52 | fprintf('* [\b%d]\b Constraint(s) in [\b%d]\b set(s)\n', nCons, nSets); 53 | for i = 1:nSets 54 | fprintf('\t* [\b%d]\b Constraint(s) in [\b%s]\b set\n', ... 55 | this.constraint(i).expression.length, ... 56 | this.constraint(i).description); 57 | end % for 58 | 59 | % Display problem summary footer 60 | fprintf(majorDivider); 61 | fprintf('\n'); 62 | end % display 63 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/@Nlp/plot.m: -------------------------------------------------------------------------------- 1 | function plot(this) 2 | %PLOT Plot the optimization solution and objective function. 3 | % 4 | % Syntax: 5 | % obj.plot 6 | % 7 | % Description: 8 | % Plots objective function, initial point and solution if found. Only 9 | % supports one and two dimensional problems, resulting in two and three 10 | % dimensional plots. 11 | % 12 | % Copyright 2013-2014 Mikhail S. Jones 13 | 14 | % Error check for empty variable structure 15 | if isempty(this.variable) 16 | error('coalesce:optimize:Nlp:plot', ... 17 | 'Problem can not be plotted, no declared variables.'); 18 | end % if 19 | 20 | % Check for supported problem sizes 21 | if this.numberOfVariables > 2 22 | error('coalesce:optimize:Nlp:plot', ... 23 | 'Problems greater than 2 dimensions can not be plotted.'); 24 | end % if 25 | 26 | % Create function handle (much faster than matlabFunction but same idea) 27 | str = char(this.objective.expression); 28 | str = regexprep(str, 'var\(([0-9:]+)\)', 'var(:,$1)'); 29 | str = ['@(var)' str]; 30 | objFcn = str2func(str); 31 | 32 | % Initialize the figure and set defaults 33 | figure('Name', this.options.name); 34 | hold on; 35 | 36 | % Plot objective function versus design variables 37 | switch this.numberOfVariables 38 | case 1 39 | % Domain of variables 40 | domain = [this.variableLowerBound this.variableUpperBound]; 41 | 42 | % Display two dimensional line plot 43 | ezplot(objFcn, domain, 100); 44 | view(2); 45 | hSol = plot(this.solution, objFcn(this.solution), 'r.', ... 46 | 'MarkerSize', 20); 47 | hInit = plot(this.initialGuess, objFcn(this.initialGuess), 'k.', ... 48 | 'MarkerSize', 20); 49 | 50 | case 2 51 | % Domain of variables 52 | domain([1,3]) = this.variableLowerBound; 53 | domain([2,4]) = this.variableUpperBound; 54 | 55 | % Display three dimensional mesh plot 56 | ezmeshc(@(var1, var2) objFcn([var1 var2]), domain, 100); 57 | view(3); 58 | hSol = plot3(this.solution(1), this.solution(2), objFcn(this.solution), 'r.', ... 59 | 'MarkerSize', 20); 60 | hInit = plot3(this.initialGuess(1), this.initialGuess(2), objFcn(this.initialGuess), 'k.', ... 61 | 'MarkerSize', 20); 62 | end % switch 63 | 64 | % Axes properties 65 | grid on; box on; 66 | legend([hSol, hInit], 'Solution', 'Initial Guess'); 67 | title(this.options.name); 68 | 69 | % Update figure windows 70 | drawnow; 71 | end % plot 72 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/@Nlp/removeConstraint.m: -------------------------------------------------------------------------------- 1 | function removeConstraint(obj, description) 2 | %REMOVECONSTRAINT Remove a constraint from the NLP problem. 3 | % 4 | % Syntax: 5 | % obj.removeConstraint(description) 6 | % 7 | % Required Input Arguments: 8 | % description - (CHAR) Description for identification. 9 | 10 | % Copyright 2016 Mikhail S. Jones 11 | 12 | constraint = Constraint.empty; 13 | 14 | for i = 1:numel(obj.constraint) 15 | if ~strcmp(description, obj.constraint(i).description) 16 | constraint = [constraint, obj.constraint(i)]; 17 | else 18 | % User feedback 19 | fprintf('Removing ([\b%d]\b) [\b%s]\b constraints... \n', ... 20 | sum(sum([obj.constraint(i).expression.length])), obj.constraint(i).description); 21 | end % if 22 | end % for 23 | 24 | obj.constraint = constraint; 25 | end % removeConstraint 26 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Constraint.m: -------------------------------------------------------------------------------- 1 | %CONSTRAINT Defines an optimization constraint object. 2 | % 3 | % Description: 4 | % This method creates a constraint or array of constraints of the form, 5 | % lb <= f(x) + A*x <= ub. The bounds must be either scalar or a defined 6 | % parameter. The constraint must be a function of design variables and 7 | % parameters, either linear or nonlinear. 8 | % 9 | % Copyright 2013-2014 Mikhail S. Jones 10 | 11 | classdef Constraint 12 | 13 | % PUBLIC PROPERTIES ===================================================== 14 | properties 15 | description@char vector 16 | expression@ExpressionNode scalar 17 | lowerBound@double% scalar 18 | upperBound@double% scalar 19 | end % properties 20 | 21 | % PUBLIC METHODS ======================================================== 22 | methods (Access = public) 23 | function this = Constraint(expression, lowerBound, upperBound, description) 24 | %CONSTRAINT Construct an optimization constraint object. 25 | % 26 | % Syntax: 27 | % obj = Constraint(expression, lowerBound, upperBound, description) 28 | % 29 | % Inputs Arguments: 30 | % expression - (EXPRESSIONNODE) Symbolic constraint expression 31 | % lowerBound - (DOUBLE) Numeric lower bounds 32 | % upperBound - (DOUBLE) Numeric upper bounds 33 | % description - (CHAR) Description for identification 34 | 35 | % Allow creation of empty objects 36 | if nargin ~= 0 37 | % Check bounds 38 | if lowerBound > upperBound 39 | error('coalesce:optimize:Constraint', ... 40 | 'Lower bounds must be less than upper bounds.'); 41 | end % if 42 | 43 | % Set object properties 44 | this.expression = simplify(expression); 45 | this.lowerBound(1:expression.length) = lowerBound; 46 | this.upperBound(1:expression.length) = upperBound; 47 | this.description = description; 48 | end % if 49 | end % Constraint 50 | 51 | function value = eval(this) 52 | %EVAL Evaluate constraint. 53 | value = eval(this.expression); 54 | end % eval 55 | end % methods 56 | end % classdef 57 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/DirectCollocation.m: -------------------------------------------------------------------------------- 1 | %DIRECTCOLLOCATION Defines a direct collocation optimization problem. 2 | % 3 | % Syntax: 4 | % obj = DirectCollocation(); 5 | % 6 | % Description: 7 | % Helps set up a mode-scheduled optimization problem. 8 | 9 | % Copyright 2013-2015 Mikhail S. Jones 10 | 11 | classdef DirectCollocation < Nlp 12 | 13 | % PROTECTED PROPERTIES ================================================== 14 | properties (SetAccess = protected) 15 | % Vector of phase objects 16 | phase@Phase vector = Phase.empty 17 | end % properties 18 | 19 | % PUBLIC METHODS ======================================================== 20 | methods 21 | function obj = DirectCollocation(varargin) 22 | %DIRECTCOLLOCATION Direct collocation NLP problem constructor. 23 | 24 | % Call superclass constructor 25 | obj = obj@Nlp(varargin{:}); 26 | end % DirectCollocation 27 | 28 | function n = numberOfPhases(obj) 29 | %NUMBEROFPHASES Get method for number of phases. 30 | % Number of phases in problem 31 | n = numel(obj.phase); 32 | end % get.numberOfPhases 33 | 34 | function addPhase(o,numberOfNodes) 35 | %ADDPHASE 36 | % obj.ADDPHASE(numberOfNodes) 37 | o.phase(end+1) = Phase(o,numberOfNodes); 38 | end 39 | 40 | function [tStar,xStar,uStar] = getResponse(o) 41 | %GETRESPONSE Export direct collocation solution as response object. 42 | 43 | tStar = []; 44 | xStar = []; 45 | uStar = []; 46 | mStar = []; 47 | 48 | % Loop through phases 49 | for ip = 1:o.numberOfPhases 50 | % Number of nodes in current phase 51 | n = o.phase(ip).numberOfNodes; 52 | 53 | % Determine time interval 54 | if ip == 1 55 | tStar = linspace(0, eval(o.phase(ip).duration), n); 56 | else 57 | tStar = [tStar tStar(end)+linspace(0, eval(o.phase(ip).duration), n)]; 58 | end % if 59 | % States 60 | xStar = [xStar reshape(eval(o.phase(ip).state), [], n)]; 61 | % Inputs 62 | uStar = [uStar reshape(eval(o.phase(ip).input), [], n)]; 63 | % Modes 64 | mStar = [mStar {sprintf('Phase %d', ip)}]; 65 | end % for 66 | end % getResponse 67 | end % methods 68 | end % classdef 69 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Objective.m: -------------------------------------------------------------------------------- 1 | %OBJECTIVE Defines an optimization objective object. 2 | % 3 | % Description: 4 | % This class creates an object representing a single objective function 5 | % and its properties. 6 | % 7 | % Copyright 2013-2014 Mikhail S. Jones 8 | 9 | classdef Objective 10 | 11 | % PUBLIC PROPERTIES ===================================================== 12 | properties 13 | description@char vector 14 | expression@ExpressionNode scalar 15 | lowerBound@double scalar 16 | upperBound@double scalar 17 | end % properties 18 | 19 | % PUBLIC METHODS ======================================================== 20 | methods 21 | function this = Objective(expression, description) 22 | %OBJECTIVE Construct an optimization objective object. 23 | % 24 | % Syntax: 25 | % obj = Objective(expression, description) 26 | % 27 | % Inputs Arguments: 28 | % expression - (EXPRESSIONNODE) Symbolic objective expression 29 | % description - (CHAR) Description for identification 30 | 31 | % Allow creation of empty objects 32 | if nargin ~= 0 33 | % Set object properties 34 | this.expression = simplify(expression); 35 | this.lowerBound = -Inf; 36 | this.upperBound = Inf; 37 | this.description = description; 38 | end % if 39 | end % Objective 40 | 41 | function value = eval(this) 42 | %EVAL Evaluate objective. 43 | value = eval(this.expression); 44 | end % eval 45 | end % methods 46 | end % classdef 47 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Phase.m: -------------------------------------------------------------------------------- 1 | %PHASE Direct collocation phase object. 2 | % 3 | % Syntax: 4 | % obj = Phase(numberOfNodes); 5 | % 6 | % Required Input Arguments: 7 | % - numberOfNodes - (DOUBLE) Number of collocation nodes in phase. 8 | % 9 | % Description: 10 | % TODO 11 | 12 | % Copyright 2013-2014 Mikhail S. Jones 13 | 14 | classdef Phase < handle 15 | 16 | % PROTECTED PROPERTIES ================================================== 17 | properties (SetAccess = public) 18 | % Reference to parent direct collocation object 19 | nlp 20 | % Number of collocation nodes in phase 21 | numberOfNodes@double scalar 22 | % Phase duration variable 23 | duration@Variable scalar 24 | % Phase state variables 25 | state@Variable vector = Variable.empty 26 | % Phase input variables 27 | input@Variable vector = Variable.empty 28 | % Phase Lagrange multiplier variables 29 | lambda@Variable vector = Variable.empty 30 | end % properties 31 | 32 | % PUBLIC METHODS ======================================================== 33 | methods 34 | function obj = Phase(nlp, numberOfNodes) 35 | %PHASE Direct collocation phase object constructor. 36 | % obj = PHASE(nlp, numberOfNodes) 37 | 38 | % Create reference to parent system object 39 | obj.nlp = nlp; 40 | 41 | % Set number of nodes in phase 42 | obj.numberOfNodes = numberOfNodes; 43 | 44 | % Add phase duration variable to parent problem 45 | expression = nlp.addVariable(1, 0, Inf, ... 46 | 'Description', sprintf('Phase %d Duration', numel(nlp.phase)+1)); 47 | 48 | % Create reference to duration in phase object 49 | obj.duration = expression; 50 | end % Phase 51 | 52 | function expression = addInput(obj, varargin) 53 | %ADDINPUT Add input to direct collocation phase object. 54 | 55 | % Add input variable to parent problem 56 | expression = obj.nlp.addVariable(varargin{:}, ... 57 | 'Length', obj.numberOfNodes); 58 | 59 | % Create reference to input variable in phase object 60 | obj.input(end+1,1) = expression; 61 | end % addInput 62 | 63 | function expression = addState(obj, varargin) 64 | %ADDSTATE Add state to direct collocation phase object. 65 | 66 | % Add state variable to parent problem 67 | expression = obj.nlp.addVariable(varargin{:}, ... 68 | 'Length', obj.numberOfNodes); 69 | 70 | % Create reference to state variable in phase object 71 | obj.state(end+1,1) = expression; 72 | end % addInput 73 | end % methods 74 | end % classdef 75 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Fmincon/Fmincon.m: -------------------------------------------------------------------------------- 1 | %FMINCON Provides an interface for COALESCE objects. 2 | % 3 | % Copyright 2013-2015 Mikhail S. Jones 4 | 5 | classdef Fmincon < Solver 6 | 7 | % PUBLIC PROPERTIES ===================================================== 8 | properties 9 | end % properties 10 | 11 | % PUBLIC METHODS ======================================================== 12 | methods 13 | function obj = Fmincon(nlp) 14 | %FMINCON Creates a optimization solver object. 15 | 16 | % Call superclass constructor 17 | obj = obj@Solver(nlp); 18 | 19 | % Set default FMINCON options 20 | obj.options = optimset(... 21 | 'Algorithm', 'interior-point', ... 22 | 'Display', 'iter', ... 23 | 'FinDiffType', 'forward', ... 24 | 'GradConstr', 'on', ... 25 | 'GradObj', 'on', ... 26 | 'Hessian','fin-diff-grads', ... 27 | ...'Hessian','user-supplied', ... 28 | ...'HessMult',@fminconHessMult, ... 29 | 'MaxFunEvals', 1e5, ... 30 | 'SubproblemAlgorithm','cg', ... 31 | 'UseParallel', 'Always'); 32 | end % Fmincon 33 | 34 | function setOptions(obj, userOptions) 35 | %SETOPTIONS Set optimizer options. 36 | 37 | % Set properties 38 | opts = {obj.options}; 39 | obj.options = optimset(opts{:}, userOptions{:}); 40 | end % setOptions 41 | end % methods 42 | end % classdef 43 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Fmincon/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Fmincon/dev/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Fmincon/dev/fmincon_hessMult.m: -------------------------------------------------------------------------------- 1 | % Hessian*vector multiplication function for Fmincon. 2 | % This uses that a Hessian*vector product is just the directional 3 | % derivative of the gradient of the Lagrangian in the given direction. 4 | function val = fmincon_hessMult(x, lambda, vec) 5 | % Step length 6 | ssize = eps^2; 7 | 8 | % The length of the vector we are differentiating against. 9 | vec_len = norm(vec); 10 | 11 | % Sometimes Fmincon gives us vec == 0 (particularly on small problems). 12 | % In this case, ssize/vec_len divides by zero, so catch and avoid that case. 13 | if vec_len == 0 14 | val = zeros(numel(x), 1); 15 | return 16 | end 17 | 18 | % The step vector -- same direction as vec, but it has length ssize 19 | step = vec * (ssize/vec_len); 20 | 21 | % Complex step differentiation! 22 | val = imag(lagr_grad(x + step * i, lambda))*(vec_len/ssize); 23 | end 24 | 25 | % Computes the gradient of the Lagrangian. Used for the Hessian Multiply function 26 | function ghess = lagr_grad(x, lambda) 27 | %ghess = this.eval_sparse(this.objjac, x) + ... 28 | % this.eval_sparse(this.cjac, x) * lambda.ineqnonlin + ... 29 | % this.eval_sparse(this.ceqjac, x) * lambda.eqnonlin; 30 | 31 | [~,gobj] = fminconObj(x); 32 | [~,~,gc,gceq] = fminconNonlcon(x); 33 | ghess = gobj(:) + gc * lambda.ineqnonlin(:) + gceq * lambda.eqnonlin(:); 34 | end 35 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Fmincon/export.m: -------------------------------------------------------------------------------- 1 | function export(obj) 2 | %EXPORT Export optimization functions for MATLAB FMINCON. 3 | % 4 | % Copyright 2013-2015 Mikhail S. Jones 5 | 6 | % User feedback 7 | fprintf('Exporting FMINCON functions... \n'); 8 | 9 | % Number of variables, constraints, and objectives 10 | nVars = obj.nlp.numberOfVariables; 11 | nCons = obj.nlp.numberOfConstraints; 12 | nObjs = obj.nlp.numberOfObjectives; 13 | 14 | % Split equality and inequality constraints 15 | equality = []; 16 | for k = numel(obj.nlp.constraint):-1:1 17 | equality(k) = all(obj.nlp.constraint(k).lowerBound == obj.nlp.constraint(k).upperBound); 18 | end % for 19 | equality = logical(equality); 20 | 21 | % Write user function 22 | gen = MatlabFunctionGenerator({}, {'A', 'b', 'Aeq', 'beq'}, 'auto', 'fminconUser'); 23 | gen.writeHeader; 24 | gen.writeExpression(ConstantNode.empty, 'A'); 25 | gen.writeIndex({}, 'b'); 26 | gen.writeExpression(ConstantNode.empty, 'Aeq'); 27 | gen.writeIndex({}, 'beq'); 28 | gen.writeFooter; 29 | 30 | % Write objective function 31 | gen = MatlabFunctionGenerator({'var'}, {'f', 'g'}, 'auto', 'fminconObj'); 32 | gen.writeHeader; 33 | f = vertcat(obj.nlp.objective.expression); 34 | gen.writeExpression(f, 'f'); 35 | [ig, jg, sg] = f.jacobian; 36 | gen.writeIndex(ig, 'ig'); 37 | gen.writeIndex(jg, 'jg'); 38 | gen.writeExpression(sg, 'sg'); 39 | fprintf(gen.fid, '\tg = sparse(ig, jg, sg, %d, %d);\n\n', nObjs, nVars); 40 | gen.writeFooter; 41 | 42 | % Write nonlinear constraint function 43 | gen = MatlabFunctionGenerator({'var'}, {'c', 'ceq', 'G', 'Geq'}, 'auto', 'fminconNonlcon'); 44 | gen.writeHeader; 45 | c = ConstantNode.empty; 46 | for k = find(~equality) 47 | if obj.nlp.constraint(k).lowerBound(1) ~= -Inf 48 | c(end+1) = obj.nlp.constraint(k).lowerBound(1) - obj.nlp.constraint(k).expression; 49 | end % if 50 | end % for 51 | for k = find(~equality) 52 | if obj.nlp.constraint(k).upperBound(1) ~= Inf 53 | c(end+1) = obj.nlp.constraint(k).expression - obj.nlp.constraint(k).upperBound(1); 54 | end % if 55 | end % for 56 | gen.writeExpression(c, 'c'); 57 | ceq = ConstantNode.empty; 58 | for k = find(equality) 59 | ceq(end+1) = obj.nlp.constraint(k).expression - obj.nlp.constraint(k).lowerBound(1); 60 | end % for 61 | gen.writeExpression(ceq, 'ceq'); 62 | fprintf(gen.fid, '\tif nargout > 2\n'); 63 | [jG, iG, sG] = c.jacobian; 64 | gen.writeIndex(iG, 'iG'); 65 | gen.writeIndex(jG, 'jG'); 66 | gen.writeExpression(sG, 'sG'); 67 | fprintf(gen.fid, '\tG = sparse(iG, jG, sG, %d, %d);\n\n', nVars, sum([c.length])); 68 | [jGeq, iGeq, sGeq] = ceq.jacobian; 69 | gen.writeIndex(iGeq, 'iGeq'); 70 | gen.writeIndex(jGeq, 'jGeq'); 71 | gen.writeExpression(sGeq, 'sGeq'); 72 | fprintf(gen.fid, '\tGeq = sparse(iGeq, jGeq, sGeq, %d, %d);\n\n', nVars, sum([ceq.length])); 73 | fprintf(gen.fid, '\tend %% if\n'); 74 | gen.writeFooter; 75 | end % export 76 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Fmincon/solve.m: -------------------------------------------------------------------------------- 1 | function solve(obj) 2 | %SOLVE Solve optimization problem using MATLAB FMINCON. 3 | % 4 | % Copyright 2013-2015 Mikhail S. Jones 5 | 6 | % User feedback 7 | fprintf('Initializing FMINCON solver... \n'); 8 | 9 | % Refresh function and file system caches 10 | rehash; 11 | 12 | % Evaluate fminconUser function to determine constants 13 | [A, b, Aeq, beq] = fminconUser; 14 | lb = obj.nlp.variableLowerBound; 15 | ub = obj.nlp.variableUpperBound; 16 | x0 = obj.nlp.initialGuess; 17 | 18 | % Run FMINCON 19 | [x, f, obj.info] = fmincon('fminconObj', ... 20 | x0, A, b, Aeq, beq, lb, ub, 'fminconNonlcon', obj.options); 21 | 22 | % Parse solution back into variable objects 23 | obj.nlp.solution = x; 24 | 25 | % Send desktop notification 26 | % message = ['FMINCON optimizer finished.'... 27 | % '\n\t* Exit flag: ' num2str(obj.info)... 28 | % '\n\t* Objective value: ' num2str(f)]; 29 | % notify(message, 'COALESCE', 1); 30 | end % solve 31 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Ipopt/Ipopt.m: -------------------------------------------------------------------------------- 1 | %IPOPT Provides an interface for COALESCE objects. 2 | % 3 | % Copyright 2013-2015 Mikhail S. Jones 4 | 5 | classdef Ipopt < Solver 6 | 7 | % PUBLIC PROPERTIES ===================================================== 8 | properties 9 | iterationPlot@logical = false % Option for display iteration plot 10 | end % properties 11 | 12 | % PUBLIC METHODS ======================================================== 13 | methods 14 | function obj = Ipopt(nlp) 15 | %IPOPT Creates a optimization solver object. 16 | 17 | % Call superclass constructor 18 | obj = obj@Solver(nlp); 19 | 20 | % Set default IPOPT options 21 | obj.options = struct; 22 | obj.options.ipopt.hessian_approximation = 'limited-memory'; 23 | 24 | % Note: mumps linear solver is deterministic but approximately twice 25 | % as slow as ma57 in many cases. ma57 is non-deterministic due to 26 | % parallelization and memory allocation variations. 27 | obj.options.ipopt.linear_solver = 'ma57'; 28 | 29 | % Note: Can use first or second-order check, useful for debugging. 30 | obj.options.ipopt.derivative_test = 'none'; 31 | end % Ipopt 32 | 33 | function setOptions(obj, varargin) 34 | %SETOPTIONS Set optimizer options. 35 | % 36 | % Notes: 37 | % See IPOPT documentation for all solver options 38 | % (http://www.coin-or.org/Ipopt/documentation/node40.html) 39 | 40 | % Set IPOPT options 41 | for i = 1:2:numel(nargin) 42 | obj.options.ipopt.(varargin{i}) = varargin{i+1}; 43 | end % for 44 | end % setOptions 45 | end % methods 46 | end % classdef 47 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Ipopt/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Ipopt/dev/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Ipopt/dev/hessian.m: -------------------------------------------------------------------------------- 1 | % Compute Hessian 2 | 3 | % sigma = SymVariable('sigma', 1); 4 | % lambda = SymVariable('lambda', nCons); 5 | 6 | % for iter = 1:numel(this.g.s) 7 | % i = this.nlp.objective.jacobian.i{iter}; 8 | % j = this.nlp.objective.jacobian.j{iter}; 9 | % s = this.nlp.objective.jacobian.s{iter}; 10 | % tmp = jacobian(sigma*s, var); 11 | % for iter = 1:numel(tmp.s) 12 | % tmp.i{iter} = j; 13 | % end % for 14 | % tmp.m = nVars; 15 | % end % for 16 | % H{1} = tmp; 17 | 18 | % nCon = 0; 19 | % for iCon = 1:numel(this.nlp.constraint) 20 | % for iter = 1:numel(this.nlp.constraint(iCon).jacobian.s) 21 | % i = nCon + this.nlp.constraint(iCon).jacobian.i{iter}; 22 | % j = this.nlp.constraint(iCon).jacobian.j{iter}; 23 | % s = this.nlp.constraint(iCon).jacobian.s{iter}; 24 | % tmp = jacobian(lambda(:,:,i)*s, var); 25 | % for iter = 1:numel(tmp.s) 26 | % tmp.i{iter} = j; 27 | % end % for 28 | % tmp.m = nVars; 29 | % H{iCon} = tmp; 30 | % end % for 31 | % nCon = nCon + this.nlp.constraint(iCon).count; 32 | % end % for 33 | 34 | 35 | 36 | 37 | % function writeIpoptHessian 38 | % %WRITEIPOPTHESSIAN 39 | % 40 | % % Open new file to write without automatic flushing (W vs w) 41 | % fid = fopen('auto/ipoptHessian.m', 'W'); 42 | % 43 | % % Write file header 44 | % fprintf(fid, 'function H = ipoptHessian(var, sigma, lambda)\n'); 45 | % fprintf(fid, '%%IPOPTHESSIAN\n'); 46 | % fprintf(fid, '%%\n'); 47 | % fprintf(fid, '%% Auto-generated by COALESCE package %s at %s\n', ... 48 | % this.nlp.version, datestr(now)); 49 | % fprintf(fid, '%%\n'); 50 | % fprintf(fid, '%% Copyright 2013-2014 Mikhail S. Jones\n'); 51 | % fprintf(fid, '\n'); 52 | % 53 | % % Loop through and assign parameter values 54 | % fprintf(fid, '\t%% Parameters\n'); 55 | % for i = 1:numel(this.nlp.parameter) 56 | % fprintf(fid, '\t%s = [%s]; %% %s\n', ... 57 | % matlab(this.nlp.parameter(i).expression), ... 58 | % sprintf('%0.15g; ', [this.nlp.parameter(i).value]), ... 59 | % this.nlp.parameter(i).description); 60 | % end % for 61 | % fprintf(fid, '\n'); 62 | % 63 | % % Write Hessian 64 | % fprintf(fid, '\t%% Hessian\n'); 65 | % if isempty(this.H) 66 | % fprintf(fid, '\tH = [];\n\n'); 67 | % else 68 | % fprintf(fid, '\tH = sparse(%d, %d);\n\n', this.H{1}.m, this.H{1}.n); 69 | % for i = 1:numel(this.H) 70 | % writeMatlabIndex(fid, this.H{i}.i, ['iH' num2str(i)]); 71 | % writeMatlabIndex(fid, this.H{i}.j, ['jH' num2str(i)]); 72 | % writeMatlabExpression(fid, this.H{i}.s, ['sH' num2str(i)]); 73 | % fprintf(fid, '\tH = H + tril(sparse(iH%d, jH%d, sH%d, %d, %d));\n\n', i, i, i, this.H{i}.m, this.H{i}.n); 74 | % end % for 75 | % end % if 76 | % 77 | % % Write function end and close file 78 | % fprintf(fid, 'end %% ipoptHessian'); 79 | % fclose(fid); 80 | % end % writeIpoptHessian 81 | % 82 | % function writeIpoptHessianStructure 83 | % %WRITEIPOPTHESSIANSTRUCTURE 84 | % 85 | % % Open new file to write without automatic flushing (W vs w) 86 | % fid = fopen('auto/ipoptHessianStructure.m', 'W'); 87 | % 88 | % % Write file header 89 | % fprintf(fid, 'function H = ipoptHessianStructure\n'); 90 | % fprintf(fid, '%%IPOPTHESSIANSTRUCTURE\n'); 91 | % fprintf(fid, '%%\n'); 92 | % fprintf(fid, '%% Auto-generated by COALESCE package %s at %s\n', ... 93 | % this.nlp.version, datestr(now)); 94 | % fprintf(fid, '%%\n'); 95 | % fprintf(fid, '%% Copyright 2013-2014 Mikhail S. Jones\n'); 96 | % fprintf(fid, '\n'); 97 | % 98 | % % Loop through and assign parameter values 99 | % fprintf(fid, '\t%% Parameters\n'); 100 | % for i = 1:numel(this.nlp.parameter) 101 | % fprintf(fid, '\t%s = [%s]; %% %s\n', ... 102 | % matlab(this.nlp.parameter(i).expression), ... 103 | % sprintf('%0.15g; ', [this.nlp.parameter(i).value]), ... 104 | % this.nlp.parameter(i).description); 105 | % end % for 106 | % fprintf(fid, '\n'); 107 | % 108 | % % Write objective gradients 109 | % fprintf(fid, '\t%% Hessian Structure\n'); 110 | % if isempty(this.H) 111 | % fprintf(fid, '\tH = [];\n\n'); 112 | % else 113 | % fprintf(fid, '\tH = sparse(%d, %d);\n\n', this.H{1}.m, this.H{1}.n); 114 | % for i = 1:numel(this.H) 115 | % writeMatlabIndex(fid, this.H{i}.i, ['iH' num2str(i)]); 116 | % writeMatlabIndex(fid, this.H{i}.j, ['jH' num2str(i)]); 117 | % fprintf(fid, '\tsH%d = 1+zeros(1,%d);\n', i, nnz(this.H{i})); 118 | % fprintf(fid, '\tH = H + tril(sparse(iH%d, jH%d, sH%d, %d, %d));\n\n', i, i, i, this.H{i}.m, this.H{i}.n); 119 | % end % for 120 | % end % if 121 | % 122 | % % Write function end and close file 123 | % fprintf(fid, 'end %% ipoptHessianStructure'); 124 | % fclose(fid); 125 | % end % writeIpoptHessianStructure 126 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Ipopt/export.m: -------------------------------------------------------------------------------- 1 | function export(obj) 2 | %EXPORT Export optimization functions for IPOPT. 3 | % 4 | % Copyright 2013-2015 Mikhail S. Jones 5 | 6 | % User feedback 7 | fprintf('Exporting IPOPT functions... \n'); 8 | 9 | % Number of variables, constraints, and objectives 10 | nVars = obj.nlp.numberOfVariables; 11 | nCons = obj.nlp.numberOfConstraints; 12 | nObjs = obj.nlp.numberOfObjectives; 13 | 14 | % Write objective function 15 | gen = MatlabFunctionGenerator({'var'}, {'f'}, obj.path, 'ipoptObjective'); 16 | gen.writeHeader; 17 | f = vertcat(obj.nlp.objective.expression); 18 | gen.writeExpression(f, 'f'); 19 | gen.writeFooter; 20 | 21 | % Write objective gradient function 22 | gen = MatlabFunctionGenerator({'var'}, {'g'}, obj.path, 'ipoptGradient'); 23 | gen.writeHeader; 24 | [ig, jg, sg] = f.jacobian; 25 | gen.writeIndex(ig, 'ig'); 26 | gen.writeIndex(jg, 'jg'); 27 | gen.writeExpression(sg, 'sg'); 28 | fprintf(gen.fid, '\tg = sparse(ig, jg, sg, %d, %d);\n\n', nObjs, nVars); 29 | gen.writeFooter; 30 | 31 | % Write constraint function 32 | gen = MatlabFunctionGenerator({'var'}, {'c'}, obj.path, 'ipoptConstraints'); 33 | gen.writeHeader; 34 | if ~isempty(obj.nlp.constraint) 35 | c = vertcat(obj.nlp.constraint.expression); 36 | else 37 | c = ConstantNode.empty; 38 | end % if 39 | gen.writeExpression(c, 'c'); 40 | gen.writeFooter; 41 | 42 | % Write constraint jacobian function 43 | gen = MatlabFunctionGenerator({'var'}, {'J'}, obj.path, 'ipoptJacobian'); 44 | gen.writeHeader; 45 | if ~isempty(obj.nlp.constraint) 46 | [iJ, jJ, sJ] = c.jacobian; 47 | else 48 | iJ = {}; jJ = {}; sJ = ConstantNode.empty; 49 | end % if 50 | gen.writeIndex(iJ, 'iJ'); 51 | gen.writeIndex(jJ, 'jJ'); 52 | gen.writeExpression(sJ, 'sJ'); 53 | fprintf(gen.fid, '\tJ = sparse(iJ, jJ, sJ, %d, %d);\n\n', nCons, nVars); 54 | gen.writeFooter; 55 | 56 | % Write constraint jacobian structure function 57 | gen = MatlabFunctionGenerator({'var'}, {'J'}, obj.path, 'ipoptJacobianStructure'); 58 | gen.writeHeader; 59 | gen.writeIndex(iJ, 'iJ'); 60 | gen.writeIndex(jJ, 'jJ'); 61 | fprintf(gen.fid, '\tsJ = 1 + zeros(1,%d);\n', sum([sJ.length])); 62 | fprintf(gen.fid, '\tJ = sparse(iJ, jJ, sJ, %d, %d);\n\n', nCons, nVars); 63 | gen.writeFooter; 64 | end % export 65 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Ipopt/solve.m: -------------------------------------------------------------------------------- 1 | function solve(obj) 2 | %SOLVE Solve optimization problem using IPOPT. 3 | % 4 | % Copyright 2013-2015 Mikhail S. Jones 5 | 6 | % User feedback 7 | % fprintf('Initializing IPOPT solver... \n'); 8 | 9 | % Refresh function and file system caches 10 | rehash; 11 | 12 | % Set IPOPT options 13 | obj.options.lb = obj.nlp.variableLowerBound; 14 | obj.options.ub = obj.nlp.variableUpperBound; 15 | obj.options.cl = [obj.nlp.constraint.lowerBound]; 16 | obj.options.cu = [obj.nlp.constraint.upperBound]; 17 | 18 | % Set IPOPT functions 19 | funcs.objective = @ipoptObjective; 20 | funcs.gradient = @ipoptGradient; 21 | funcs.constraints = @ipoptConstraints; 22 | funcs.jacobian = @ipoptJacobian; 23 | funcs.jacobianstructure = @ipoptJacobianStructure; 24 | if obj.iterationPlot 25 | funcs.iterfunc = @ipoptIterFunc; 26 | end % if 27 | 28 | % Note: Unused functions 29 | % funcs.hessian = @ipoptHessian; 30 | % funcs.hessianstructure = @ipoptHessianStructure; 31 | 32 | % Run IPOPT 33 | [x, obj.info] = ipopt(obj.nlp.initialGuess, funcs, obj.options); 34 | 35 | % Parse solution back into variable objects 36 | obj.nlp.solution = x; 37 | 38 | obj.info.objective = funcs.objective(x); 39 | 40 | % % Send desktop notification 41 | % message = ['IPOPT optimizer finished.'... 42 | % '\n\t* Exit flag: ' num2str(obj.info.status)... 43 | % '\n\t* Objective value: ' num2str(ipoptObjective(x))]; 44 | % notify(message, 'COALESCE', 1); 45 | end % solve 46 | 47 | function stop = ipoptIterFunc(nIter, f, auxdata) 48 | %IPOPTITERFUNC Ipopt iteration function GUI. 49 | 50 | persistent flag; 51 | persistent hg; 52 | 53 | % Initialize plots 54 | if nIter == 0 55 | flag = true; 56 | 57 | hg.fig = figure; 58 | subplot(3,1,1); hg.inf_pr = semilogy(nIter, auxdata.inf_pr, '.-r'); 59 | hold on; grid on; box on; 60 | ylabel('Primal Infeasiblity'); 61 | xlabel('Iteration'); 62 | 63 | subplot(3,1,2); hg.inf_du = semilogy(nIter, auxdata.inf_du, '.-b'); 64 | hold on; grid on; box on; 65 | ylabel('Dual Infeasiblity'); 66 | xlabel('Iteration'); 67 | 68 | subplot(3,1,3); hg.f = semilogy(nIter, f, '.-m'); 69 | hold on; grid on; box on; 70 | ylabel('Objective'); 71 | xlabel('Iteration'); 72 | 73 | uicontrol(... 74 | 'Style', 'pushbutton', ... 75 | 'String', 'Stop',... 76 | 'Units', 'normalized', ... 77 | 'Position', [0 0 1 0.05],... 78 | 'Callback', @stopCallback); 79 | end % if 80 | 81 | % Update plots 82 | if mod(nIter, 5) == 0 83 | set(hg.inf_pr, ... 84 | 'XData', [get(hg.inf_pr, 'XData') nIter], ... 85 | 'YData', [get(hg.inf_pr, 'YData') auxdata.inf_pr]); 86 | set(hg.inf_du, ... 87 | 'XData', [get(hg.inf_du, 'XData') nIter], ... 88 | 'YData', [get(hg.inf_du, 'YData') auxdata.inf_du]); 89 | set(hg.f, ... 90 | 'XData', [get(hg.f, 'XData') nIter], ... 91 | 'YData', [get(hg.f, 'YData') f]); 92 | drawnow; 93 | end % if 94 | 95 | % Set output stop command 96 | stop = flag; 97 | 98 | function stopCallback(varargin) 99 | %STOPCALLBACK Stop button callback function. 100 | flag = false; 101 | end % stopCallback 102 | end % ipoptIterFunc 103 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Snopt/Snopt.m: -------------------------------------------------------------------------------- 1 | %SNOPT Provides an interface for COALESCE objects. 2 | % 3 | % Copyright 2013-2015 Mikhail S. Jones 4 | 5 | classdef Snopt < Solver 6 | 7 | % PUBLIC PROPERTIES ===================================================== 8 | properties 9 | end % properties 10 | 11 | % PUBLIC METHODS ======================================================== 12 | methods 13 | function obj = Snopt(nlp) 14 | %SNOPT Creates a optimization solver object. 15 | 16 | % Call superclass constructor 17 | obj = obj@Solver(nlp); 18 | 19 | % Set default SNOPT options 20 | snscreen('on'); 21 | snseti('Iterations limit', 1e5); 22 | snseti('Major iterations limit', 5e3); 23 | snseti('Minor iterations limit', 5e2); 24 | snsetr('Feasibility tolerance', 1e-6); 25 | snsetr('Major feasibility tolerance', 1e-6); 26 | snsetr('Minor feasibility tolerance', 1e-6); 27 | snsetr('Major optimality tolerance', 1e-6); 28 | end % Snopt 29 | 30 | function setOptions(obj, userOptions) 31 | %SETOPTIONS Set optimizer options. 32 | 33 | % Set SNOPT options 34 | snsetr(userOptions{:}); 35 | end % setOptions 36 | end % methods 37 | end % classdef 38 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Snopt/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Snopt/export.m: -------------------------------------------------------------------------------- 1 | function export(obj) 2 | %EXPORT Export optimization functions for SNOPT. 3 | % 4 | % Copyright 2013-2015 Mikhail S. Jones 5 | 6 | % User feedback 7 | fprintf('Exporting SNOPT functions... \n'); 8 | 9 | % Number of variables, constraints, and objectives 10 | nVars = obj.nlp.numberOfVariables; 11 | nCons = obj.nlp.numberOfConstraints; 12 | nObjs = obj.nlp.numberOfObjectives; 13 | 14 | % Compute combined Jacobian of objective and constraints 15 | f = vertcat(obj.nlp.objective.expression, obj.nlp.constraint.expression); 16 | [i, j, s] = f.jacobian; 17 | 18 | % Initialize sparse Jacobian indexes 19 | iA = {}; jA = {}; A = ConstantNode.empty; 20 | % iG = {}; jG = {}; G = ConstantNode.empty; 21 | iG = i; jG = j; G = s; 22 | 23 | % % Separate linear and nonlinear terms 24 | % for k = 1:numel(s) 25 | % isLinear = true; 26 | % [~,~,tmp] = s(k).jacobian; 27 | % for kk = 1:numel(tmp) 28 | % isLinear = isLinear && isempty(symvar(tmp(kk))); 29 | % end % for 30 | % 31 | % if isLinear % TODO: Build into ExpressionNode 32 | % iA{k} = i{k}; jA{k} = j{k}; A(k) = s(k); 33 | % else 34 | % iG{k} = i{k}; jG{k} = j{k}; G(k) = s(k); 35 | % end % if 36 | % end % for 37 | 38 | % Write user function 39 | gen = MatlabFunctionGenerator({}, {'A', 'iAfun', 'jAvar', 'iGfun', 'jGvar'}, obj.path, 'snoptUser'); 40 | gen.writeHeader; 41 | gen.writeExpression(A, 'A'); 42 | gen.writeIndex(iA, 'iAfun'); 43 | gen.writeIndex(jA, 'jAvar'); 44 | gen.writeIndex(iG, 'iGfun'); 45 | gen.writeIndex(jG, 'jGvar'); 46 | gen.writeFooter; 47 | 48 | % Write objective and constraint function 49 | gen = MatlabFunctionGenerator({'var'}, {'f', 'G'}, obj.path, 'snoptUserFun'); 50 | gen.writeHeader; 51 | gen.writeExpression(f, 'f'); 52 | gen.writeExpression(G, 'G'); 53 | gen.writeFooter; 54 | end % export 55 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Snopt/solve.m: -------------------------------------------------------------------------------- 1 | function solve(obj) 2 | %SOLVE Solve optimization problem using SNOPT. 3 | % 4 | % Copyright 2013-2015 Mikhail S. Jones 5 | 6 | % User feedback 7 | fprintf('Initializing SNOPT solver... \n'); 8 | 9 | % Objective function information 10 | ObjAdd = 0; ObjRow = 1; 11 | 12 | % Refresh function and file system caches 13 | rehash; 14 | 15 | % Evaluate snoptUser function to determine constants 16 | [A, iAfun, jAvar, iGfun, jGvar] = snoptUser; 17 | xlow = obj.nlp.variableLowerBound'; 18 | xupp = obj.nlp.variableUpperBound'; 19 | Flow = [obj.nlp.objective.lowerBound obj.nlp.constraint.lowerBound]'; 20 | Fupp = [obj.nlp.objective.upperBound obj.nlp.constraint.upperBound]'; 21 | x0 = obj.nlp.initialGuess'; 22 | 23 | % Run SNOPT 24 | [x, f, obj.info] = snopt(x0, xlow, xupp, Flow, Fupp, ... 25 | 'snoptUserFun', ObjAdd, ObjRow, A, iAfun, jAvar, iGfun, jGvar); 26 | 27 | % Parse solution back into variable objects 28 | obj.nlp.solution = x'; 29 | 30 | % Send desktop notification 31 | % message = ['SNOPT optimizer finished.'... 32 | % '\n\t* Exit flag: ' num2str(obj.info)... 33 | % '\n\t* Objective value: ' num2str(f(ObjRow) + ObjAdd)]; 34 | % notify(message, 'COALESCE', 1); 35 | end % solve 36 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Solver/Solver.m: -------------------------------------------------------------------------------- 1 | %SOLVER Provides a solver interface for COALESCE Nlp objects. 2 | % 3 | % Description: 4 | % Abstract class defining solver interfaces to the COALESCE NLPs. 5 | % 6 | % Copyright 2013-2015 Mikhail S. Jones 7 | 8 | classdef (Abstract = true) Solver < handle 9 | 10 | % PUBLIC PROPERTIES ===================================================== 11 | properties 12 | nlp@Nlp scalar 13 | options@struct 14 | info 15 | path 16 | end % properties 17 | 18 | % ABSTRACT METHODS ====================================================== 19 | methods (Abstract = true) 20 | export 21 | solve 22 | end % methods 23 | 24 | % PUBLIC METHODS ======================================================== 25 | methods 26 | function obj = Solver(nlp) 27 | %SOLVER Creates a optimization solver object. 28 | obj.path = fullfile(pwd,'_build'); 29 | % Set object properties 30 | obj.nlp = nlp; 31 | 32 | % Create directory for auto generated files if it doesn't exist 33 | if ~exist(obj.path, 'dir') 34 | % Make directory 35 | mkdir(obj.path); 36 | end % if 37 | 38 | % Set path to auto generated functions 39 | addpath(obj.path); 40 | end % Solver 41 | end % methods 42 | end % classdef 43 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Solver/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Solver/display.m: -------------------------------------------------------------------------------- 1 | function display(obj) 2 | %DISPLAY Display design variable solution solution. 3 | % 4 | % Syntax: 5 | % obj 6 | % obj.display 7 | % 8 | % Description: 9 | % Displays all design variable solutions along with name and id. 10 | % 11 | % Copyright 2013-2015 Mikhail S. Jones 12 | 13 | % Construct divider line strings 14 | majorDivider = [repmat('=', 1, 75), '\n']; 15 | minorDivider = [repmat('-', 1, 75), '\n']; 16 | 17 | % Display solution summary header 18 | fprintf('\n'); 19 | fprintf(majorDivider); 20 | fprintf([repmat(' ', 1, 30) '[\bSOLVER SUMMARY]\b \n']); 21 | fprintf(majorDivider); 22 | fprintf('* Solver: [\b%s]\b \n\n', class(obj)); 23 | 24 | % Display solution summary 25 | for i = 1:numel(obj.nlp.variable) 26 | fprintf('[\b%s]\b \n', upper(obj.nlp.variable(i).description)); 27 | fprintf(minorDivider); 28 | 29 | % Display variables within set 30 | for j = 1:obj.nlp.variable(i).length 31 | fprintf('%-12.12s %12.12s = [\b%.4e]\b \n', ... 32 | [' ' num2str(j) ')'], ... 33 | char(obj.nlp.variable(i)), ... % TODO 34 | obj.nlp.variable(i).solution(j)); 35 | end % for 36 | fprintf('\n'); 37 | end % for 38 | 39 | % Display solution summary footer 40 | fprintf(majorDivider); 41 | fprintf('\n'); 42 | end % display 43 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/@Solver/feasibility.m: -------------------------------------------------------------------------------- 1 | function feasibility(obj, tol) 2 | %FEASIBILITY Display constraint feasibility summary. 3 | % 4 | % Syntax: 5 | % obj.feasibility(tol) 6 | % 7 | % Input Arguments: 8 | % tol - (DOUBLE) Feasibility tolerance cutoff 9 | % 10 | % Description: 11 | % Displays all constraints that do not meet the specified 12 | % tolerance with id, name, and values compared to defined bounds. 13 | % 14 | % Copyright 2013-2015 Mikhail S. Jones 15 | 16 | % Check number of input arguments 17 | if nargin == 1 18 | % Default feasibility tolerance 19 | tol = 1e-9; 20 | end % if 21 | 22 | % Construct divider line strings 23 | majorDivider = [repmat('=', 1, 75), '\n']; 24 | minorDivider = [repmat('-', 1, 75), '\n']; 25 | 26 | % Display feasibility summary header 27 | fprintf('\n'); 28 | fprintf(majorDivider); 29 | fprintf([repmat(' ',1,28) '[\bFEASIBILITY SUMMARY]\b \n']); 30 | fprintf(majorDivider); 31 | fprintf('* Tolerance: [\b%0.1e]\b \n\n', tol); 32 | 33 | % Loop through all constraint sets 34 | for iCon = 1:numel(obj.nlp.constraint) 35 | % Reset and reallocate bound arrays 36 | lb = obj.nlp.constraint(iCon).lowerBound'; 37 | ub = obj.nlp.constraint(iCon).upperBound'; 38 | 39 | % Evaluate constraint expression 40 | c = squeeze(eval(obj.nlp.constraint(iCon))); 41 | 42 | % Find infeasible constraints 43 | isFeasible = logical(((lb - tol) <= c) & (c <= (ub + tol))); 44 | 45 | % Check if all constraints are feasible 46 | if all(isFeasible) 47 | % Display all feasible notification 48 | % fprintf('\tAll constraints feasible to within tolerance.\n'); 49 | 50 | else 51 | % Display constraint group sub-header 52 | fprintf('%s\n', upper(obj.nlp.constraint(iCon).description)); 53 | fprintf(minorDivider); 54 | 55 | % Display constraints within set 56 | for i = 1:obj.nlp.constraint(iCon).expression.length 57 | if ~isFeasible(i) 58 | fprintf('%-12.12s %12.12s <= [\b%.4e]\b <= %s \n', ... 59 | [' ' num2str(i) ')'], num2str(lb(i)), c(i), num2str(ub(i))); 60 | end % if 61 | end % for 62 | end % if 63 | 64 | % fprintf('\n'); 65 | end % for 66 | 67 | % Display feasibility summary footer 68 | fprintf(majorDivider); 69 | fprintf('\n'); 70 | end % feasibility 71 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/FunctionGenerator.m: -------------------------------------------------------------------------------- 1 | %FUNCTIONGENERATOR Function generation object. 2 | % 3 | % Copyright 2013-2015 Mikhail S. Jones 4 | 5 | classdef (Abstract = true) FunctionGenerator < handle 6 | 7 | % PROTECTED PROPERTIES ================================================== 8 | properties (SetAccess = protected) 9 | argsIn@cell vector = {} 10 | argsOut@cell vector = {} 11 | path@char vector = '' 12 | name@char vector = '' 13 | fid@double scalar = 1 14 | end % properties 15 | 16 | % ABSTRACT METHODS ====================================================== 17 | methods (Abstract = true) 18 | writeHeader 19 | writeIndex 20 | writeExpression 21 | writeFooter 22 | end % methods 23 | 24 | % PUBLIC METHODS ======================================================== 25 | methods 26 | function obj = FunctionGenerator(argsIn, argsOut, path, name) 27 | %FUNCTIONGENERATOR Function generation object constructor. 28 | 29 | % TODO: Input argument checks 30 | 31 | % Set object properties 32 | obj.argsIn = argsIn; 33 | obj.argsOut = argsOut; 34 | obj.path = path; 35 | obj.name = name; 36 | end % FunctionGenerator 37 | end % methods 38 | end % classdef 39 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/MatlabFunctionGenerator.m: -------------------------------------------------------------------------------- 1 | %MATLABFUNCTIONGENERATOR MATLAB function generation object. 2 | % 3 | % Copyright 2013-2015 Mikhail S. Jones 4 | 5 | classdef MatlabFunctionGenerator < FunctionGenerator 6 | 7 | % PUBLIC METHODS ======================================================== 8 | methods 9 | function obj = MatlabFunctionGenerator(argsIn, argsOut, path, name) 10 | %MATLABFUNCTIONGENERATOR MATLAB function generation object constructor. 11 | 12 | % Call superclass constructor 13 | obj = obj@FunctionGenerator(argsIn, argsOut, path, name); 14 | end % FunctionGenerator 15 | 16 | function writeHeader(obj) 17 | %WRITEHEADER Create file and write function header. 18 | 19 | % Open new file to write without automatic flushing (W vs w) 20 | obj.fid = fopen(fullfile(obj.path, [obj.name '.m']), 'W'); 21 | 22 | % Create argument strings 23 | out = sprintf('%s,', obj.argsOut{:}); 24 | out = out(1:end-1); 25 | in = sprintf('%s,', obj.argsIn{:}); 26 | in = in(1:end-1); 27 | 28 | % Write file header 29 | fprintf(obj.fid, 'function [%s] = %s(%s)\n', out, obj.name, in); 30 | fprintf(obj.fid, '%%%s\n', upper(obj.name)); 31 | fprintf(obj.fid, '%%\n'); 32 | fprintf(obj.fid, '%% Auto-generated by COALESCE package (%s)\n', ... 33 | datestr(now)); 34 | fprintf(obj.fid, '%%\n'); 35 | fprintf(obj.fid, '%% Copyright 2013-2015 Mikhail S. Jones\n'); 36 | fprintf(obj.fid, '\n'); 37 | end % writeHeader 38 | 39 | function writeIndex(obj, index, name) 40 | %WRITEINDEX Write vectorized index. 41 | 42 | % Initialize string 43 | str = ''; 44 | 45 | % Loop through each cell of array 46 | for i = 1:numel(index) 47 | % Check for scalar indexes 48 | if numel(index{i}) == 1 49 | % Write as scalar index 50 | str = [str sprintf('%g,', index{i})]; 51 | 52 | % Check for repeating indexes 53 | elseif all(diff(index{i}) == 0) 54 | % Write as vectorized index 55 | str = [str sprintf('%g+zeros(1,%d),', index{i}(1), numel(index{i}))]; 56 | % Note: scalar + zeros(m,n) is the fastest way to do this without 57 | % using an intermediate step, much faster than ones and a 58 | % little faster than repmat 59 | 60 | % Check for monotonically increasing indexes 61 | elseif all(diff(diff(index{i})) == 0) 62 | % Write as vectorized index 63 | str = [str sprintf('%g:%g:%g,', index{i}(1), index{i}(2) - index{i}(1), index{i}(end))]; 64 | 65 | else 66 | % Write as full index 67 | str = [str sprintf('%g,', index{i})]; 68 | end % if 69 | end % for 70 | 71 | % Write to file 72 | fprintf(obj.fid, '\t%s = [%s]'';\n', name, str(1:end-1)); 73 | end % writeIndex 74 | 75 | function writeExpression(obj, expression, name) 76 | %WRITEEXPRESSION Write vectorized symbolic expression. 77 | 78 | matlabCode(expression, name, obj.fid); % TODO: Move function here 79 | end % writeExpression 80 | 81 | function writeFooter(obj) 82 | %WRITEFOOTER Create file and write function footer. 83 | 84 | % Write function end and close file 85 | fprintf(obj.fid, 'end %% %s', obj.name); 86 | fclose(obj.fid); 87 | end % writeHeader 88 | end % methods 89 | end % classdef 90 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Solver/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/Variable.m: -------------------------------------------------------------------------------- 1 | %VARIABLE Defines an optimization design variable object. 2 | % 3 | % Description: 4 | % This class creates an object representing a single decision variable 5 | % and its properties. 6 | % 7 | % Copyright 2013-2014 Mikhail S. Jones 8 | 9 | classdef Variable < VariableNode 10 | 11 | % PUBLIC PROPERTIES ===================================================== 12 | properties 13 | nlp@Nlp scalar 14 | description@char vector = '' 15 | end % properties 16 | 17 | % DEPENDENT PROPERTIES ================================================== 18 | properties (Dependent = true) 19 | initialGuess@double vector = [] 20 | solution@double vector = [] 21 | lowerBound@double vector = [] 22 | upperBound@double vector = [] 23 | end % properties 24 | 25 | % PUBLIC METHODS ======================================================== 26 | methods 27 | function this = Variable(name, varargin) 28 | %VARIABLE Create an optimization design variable object. 29 | this = this@VariableNode(name, varargin{:}); 30 | end % Variable 31 | 32 | function this = set.initialGuess(this, initialGuess) 33 | %SET.INITIALGUESS Set method for initial guess property. 34 | 35 | % Check value is correct size 36 | for i = 1:numel(this) 37 | if any(length(initialGuess) == [1 this(i).length]) 38 | this(i).nlp.initialGuess(this(i).index) = reshape(initialGuess, 1, []); 39 | else 40 | error('Dimensions do not match.'); 41 | end % if 42 | end % for 43 | end % set.initialGuess 44 | 45 | function this = set.lowerBound(this, lowerBound) 46 | %SET.LOWERBOUND Set method for lowerBound property. 47 | 48 | % Check value is correct size 49 | if any(length(lowerBound) == [1 this.length]) 50 | this.nlp.variableLowerBound(this.index) = reshape(lowerBound, 1, []); 51 | else 52 | error('Dimensions do not match.'); 53 | end % if 54 | end % set.lowerBound 55 | 56 | function this = set.upperBound(this, upperBound) 57 | %SET.UPPERBOUND Set method for upperBound property. 58 | 59 | % Check value is correct size 60 | if any(length(upperBound) == [1 this.length]) 61 | this.nlp.variableUpperBound(this.index) = reshape(upperBound, 1, []); 62 | else 63 | error('Dimensions do not match.'); 64 | end % if 65 | end % set.upperBound 66 | 67 | function this = set.solution(this, solution) 68 | %SET.SOLUTION Set method for solution property. 69 | 70 | % Check solution is correct size 71 | if length(solution) == this.length 72 | this.nlp.solution(this.index) = reshape(solution, 1, []); 73 | else 74 | error('Dimensions do not match.'); 75 | end % if 76 | end % set.solution 77 | 78 | function initialGuess = get.initialGuess(this) 79 | %GET.INITIALGUESS Get method for initial guess property. 80 | initialGuess = this.nlp.initialGuess(this.index); 81 | end % get.initialGuess 82 | 83 | function lowerBound = get.lowerBound(this) 84 | %GET.LOWERBOUND Get method for lowerBound property. 85 | lowerBound = this.nlp.variableLowerBound(this.index); 86 | end % get.lowerBound 87 | 88 | function upperBound = get.upperBound(this) 89 | %GET.UPPERBOUND Get method for upperBound property. 90 | upperBound = this.nlp.variableUpperBound(this.index); 91 | end % get.upperBound 92 | 93 | function solution = get.solution(this) 94 | %GET.SOLUTION Get method for solution property. 95 | solution = this.nlp.solution(this.index); 96 | end % get.solution 97 | end % methods 98 | 99 | % PROTECTED METHODS ===================================================== 100 | methods (Access = protected) 101 | function val = eval_(this) 102 | %EVAL_ Overloaded abstract method to evaluate variable. 103 | 104 | % Return variable solution 105 | val = reshape(this.solution, 1, []); 106 | end % eval_ 107 | end % methods 108 | end % classdef 109 | -------------------------------------------------------------------------------- /Lossy Monoped/Nlp/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/RK4Integrate.m: -------------------------------------------------------------------------------- 1 | function [robot] = RK4Integrate(robot,tspan,controller,terrain) 2 | %RK4INTEGRATE This is a fixed timestep RK4 integrator with zero order hold 3 | %controller function 4 | 5 | t = tspan(1); 6 | tstop = tspan(2); 7 | Ts = robot.T_ctrl/robot.T_ratio; 8 | ratio = robot.T_ratio; 9 | X = [robot.q,robot.qdot]; 10 | dynamicState = 0; %Must start in flight 11 | stanceFootPos = [0,0]; %Only important while in stance, assumed to starts in flight 12 | 13 | 14 | [u,ctrlParams] = controller(robot, X(1:(length(X)/2))', X((length(X)/2+1):end)'); 15 | robot.ctrlParams = [ctrlParams]; 16 | 17 | while t < tstop 18 | 19 | [u,ctrlParams] = controller(robot, X(1:(length(X)/2))', X((length(X)/2+1):end)'); 20 | 21 | for i = 1:ratio 22 | X1 = X; 23 | dX1 = monopod_dynamics(X1, u, robot, dynamicState); 24 | 25 | X2 = rs_add(X1, rs_smul(dX1, Ts/2)); 26 | dX2 = monopod_dynamics(X2, u, robot, dynamicState); 27 | 28 | X3 = rs_add(X1, rs_smul(dX2, Ts/2)); 29 | dX3 = monopod_dynamics(X3, u, robot, dynamicState); 30 | 31 | X4 = rs_add(X1, rs_smul(dX3, Ts)); 32 | dX4 = monopod_dynamics(X4, u, robot, dynamicState); 33 | 34 | X = rs_add(X1, rs_smul(rs_add(rs_add(dX1, rs_smul(dX2, 2)), rs_add(rs_smul(dX3, 2), dX4)), Ts/6)); 35 | t = t + Ts; 36 | 37 | [X,X_cont,t,dynamicState,stanceFootPos] = checkHybridTransition(robot,X,X1,t,dynamicState,terrain,stanceFootPos); 38 | 39 | if length(X_cont) ~= length(X) 40 | robot = fillSimData(robot,t,X_cont,u,mod(dynamicState+1,2)); 41 | robot.ctrlParams = [robot.ctrlParams;ctrlParams]; 42 | end 43 | end 44 | 45 | robot = fillSimData(robot,t,X,u,dynamicState); 46 | robot.ctrlParams = [robot.ctrlParams;ctrlParams]; 47 | 48 | % Stop if crashed TODO 49 | % if X(2) < terrain.minHeight() 50 | % break 51 | % end 52 | end 53 | 54 | 55 | end 56 | 57 | function height = footTouchdownDist(robot, X, terrain) 58 | %FOOTTOUCHDOWNDIST This returns the distance the robot's foot is above the 59 | % ground. This will cross zero (from positive to negative) when touchdown 60 | % should happen 61 | xFootPos = X(1) + X(6)*sin(X(4)); 62 | yFootPos = X(2) - X(6)*cos(X(4)); 63 | groundHeight = terrain.groundHeight(xFootPos); 64 | 65 | height = yFootPos - groundHeight; 66 | end 67 | 68 | function [Xout,Xcont,t,dynamicState,stanceFootPos] = checkHybridTransition(robot,X,X1,t,dynamicState,terrain,stanceFootPos) 69 | %CHECKHYBRIDTRANSITION 70 | Xout = X; 71 | 72 | switch dynamicState 73 | case 0 %Flight 74 | %Check if the new foot position is below the ground 75 | if footTouchdownDist(robot,X,terrain) < 0 76 | 77 | footDistPrev = footTouchdownDist(robot,X1,terrain); 78 | footDistNext = footTouchdownDist(robot,X,terrain); 79 | 80 | t = interp1([footDistPrev;footDistNext],[t-robot.T_ctrl;t],0); 81 | X = interp1([footDistPrev;footDistNext],[X1;X],0); 82 | 83 | stanceFootPos(1) = X(1) + X(6)*sin( X(4) ); 84 | stanceFootPos(2) = X(2) - X(6)*cos( X(4) ); 85 | % stanceFootPos(2) = terrain.groundHeight(stanceFootPos(1)); 86 | dynamicState = 1; 87 | Xout = [X(1)-stanceFootPos(1), X(2)-stanceFootPos(2), X(3), X(5), X(7),X(8),X(9),X(11)]; 88 | end 89 | case 1 %Stance 90 | %Check if foot force no longer pushed into the ground 91 | if robot.footForce(X(1:4),X(5:8)) > 0 92 | dynamicState = 0; 93 | 94 | footForcePrev = robot.footForce(X1(1:4),X1(5:8)); 95 | footForceNext = robot.footForce(X(1:4),X(5:8)); 96 | 97 | t = interp1([footForcePrev;footForceNext],[t-robot.T_ctrl;t],0); 98 | X = interp1([footForcePrev;footForceNext],[X1;X],0); 99 | 100 | alpha = atan2(-X(1),X(2)); 101 | alphadot = ( X(1).*X(6)./(X(2).^2) - X(5)./X(2) ) ./ ( 1 + (X(1)./X(2)).^2 ); 102 | L = sqrt(X(1).^2 + X(2).^2); 103 | Ldot = ( X(1).*X(5) + X(2).*X(6) ) ./ sqrt(X(1).^2 + X(2).^2); 104 | 105 | Xout = [X(1) + stanceFootPos(1), X(2) + stanceFootPos(2),... 106 | X(3), alpha, X(4), L,... 107 | X(5),X(6),X(7),alphadot,X(8),Ldot]; 108 | end 109 | end 110 | 111 | Xcont = X; 112 | end 113 | 114 | function dX = monopod_dynamics(X, u, robot, dynamicState) 115 | 116 | switch dynamicState 117 | 118 | case 0 %Flight 119 | q = X(1:6)'; 120 | qdot = X(7:12)'; 121 | 122 | dX(1:6) = qdot; 123 | 124 | M = robot.massMatrixFlight(q); 125 | f_d = robot.dampingFlight(q,qdot); 126 | B = robot.controlFlight(q); 127 | h = robot.dynamicsFlight(q,qdot); 128 | %The Second Order dynamics M*q_ddot + h = Bu + f_damping 129 | dX(7:12) = M\(B*u + f_d - h); 130 | 131 | case 1 %Stance 132 | q = X(1:4)'; 133 | qdot = X(5:8)'; 134 | 135 | dX(1:4) = qdot; 136 | 137 | M = robot.massMatrixStance(q); 138 | f_d = robot.dampingStance(q,qdot); 139 | B = robot.controlStance(q); 140 | h = robot.dynamicsStance(q,qdot); 141 | %The Second Order dynamics M*q_ddot + h = Bu + f_damping 142 | dX(5:8) = M\(B*u + f_d - h); 143 | end %switch 144 | end 145 | 146 | function c = rs_add(a, b) 147 | c = a + b; 148 | end 149 | 150 | function c = rs_smul(a, b) 151 | c = a.*b; 152 | end 153 | 154 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/_snoptMatlab.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/greenkev/SLIP-Simulation/3c7054a786ec712c175b799fa58e33b4981cba30/Lossy Monoped/SNOPT/_snoptMatlab.pdf -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/setpath.m: -------------------------------------------------------------------------------- 1 | function setpath(varargin) 2 | 3 | addpath([pwd,'/examples' ], '-end'); 4 | addpath([pwd,'/examples/t1diet'], '-end'); 5 | addpath([pwd,'/examples/sntoy' ], '-end'); 6 | addpath([pwd,'/examples/snmain'], '-end'); 7 | addpath([pwd,'/examples/hsmain'], '-end'); 8 | addpath([pwd,'/examples/sncute'], '-end'); 9 | %addpath([getenv('HOME'), '/.cute77/mex' ], '-end'); 10 | 11 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snJac.m: -------------------------------------------------------------------------------- 1 | function [A,iAfun,jAvar,iGfun,jGvar] = snJac(usrfun,x0,xlow,xupp,nF) 2 | %function [A,iAfun,jAvar,iGfun,jGvar] = snJac(usrfun,x0,xlow,xupp,nF) 3 | % Finds the coordinate structure for the Jacobian. 4 | 5 | findJacOption = 17; 6 | 7 | [A,iAfun,jAvar,iGfun,jGvar] = snoptcmex(findJacOption,usrfun,x0,xlow,xupp,nF); 8 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snfindG.m: -------------------------------------------------------------------------------- 1 | function [Gvec] = snfindG(iGfun, jGvar, Gfull) 2 | %function [Gvec] = snfindG(iGfun, jGvar, Gfull) 3 | % Grabs elements in Gfull corresponding to 4 | % row and column indices (iGfun, jGvar). 5 | % 6 | % Note: We cannot simply use the Matlab 7 | % function find(), since it is crucial that 8 | % the order of Gvec correspond to iGfun, 9 | % and jGvar. Furthermore, zero 10 | % elements in Gfull must not be deleted 11 | % from Gvec. 12 | 13 | Gind = sub2ind(size(Gfull), iGfun, jGvar); 14 | Gvec = Gfull(Gind); 15 | 16 | % Avoid Gvec being stored in sparse format: 17 | Gvec = full(Gvec); 18 | [m,n] = size(Gvec); 19 | if m == 1, 20 | Gvec = Gvec'; 21 | end 22 | 23 | 24 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snget.m: -------------------------------------------------------------------------------- 1 | % function set = snget( option ) 2 | % The optional INTEGER-valued parameter defined by the 3 | % string "option" is assigned to rvalue. 4 | % 5 | % For a description of the optional parameters, see 6 | % the snopt documentation. 7 | % 8 | function set = snget( option ) 9 | 10 | getoption = 5; 11 | set = snoptcmex( getoption, option ); 12 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/sngetStatus.m: -------------------------------------------------------------------------------- 1 | % function [Status] = sngetStatus 2 | % 3 | % sngetStatus may be called from a user-defined function 4 | % to determine the value of the snopt variable "Status". 5 | % 6 | % Possible values of Status are: 7 | % Status = 0: There is nothing special about the current call. 8 | % Status = 1: snopt is calling the user function for the first time. 9 | % Status >= 2: snopt is calling the user function for the last time. 10 | % 11 | % For further details please see documentation for snoptA. 12 | function [status] = sngetStatus( status ) 13 | 14 | getStatusOption = 19; 15 | status = snoptcmex( getStatusOption ); 16 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/sngeti.m: -------------------------------------------------------------------------------- 1 | % function ivalue = sngeti( option ) 2 | % The optional INTEGER-valued parameter defined by the 3 | % string "option" is assigned to rvalue. 4 | % 5 | % For a description of the optional parameters, see 6 | % the snopt documentation. 7 | % 8 | function ivalue = sngeti( option ) 9 | 10 | getoptionI = 7; 11 | ivalue = snoptcmex( getoptionI, option ); 12 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/sngetr.m: -------------------------------------------------------------------------------- 1 | % function rvalue = sngetr( option ) 2 | % The REAL-VALUED optional parameter defined by the string "option" 3 | % is assigned to rvalue. 4 | % 5 | % For a description of all the optional parameters, see the 6 | % snopt documentation. 7 | % 8 | function rvalue = sngetr( option ) 9 | 10 | getoptionR = 8; 11 | rvalue = snoptcmex( getoptionR, option ); 12 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snopt.m: -------------------------------------------------------------------------------- 1 | function [x,F,inform,xmul,Fmul] = snopt(x,xlow,xupp,Flow,Fupp,userfun,varargin); 2 | % [x,F,inform,xmul,Fmul] = snopt(x,xlow,xupp,Flow,Fupp,userfun,varargin); 3 | % This function solves the nonlinear optimization problem: 4 | % minimize: 5 | % F(ObjRow) + ObjAdd 6 | % subject to: 7 | % xlow <= x <= xupp 8 | % Flow <= F <= Fupp 9 | % where: 10 | % x is the column vector of initial values of the unknowns. 11 | % F is a vector of objective and constraint functions specified 12 | % in the m-file userfun. 13 | % ObjRow is the objective row of F (default ObjRow = 1). 14 | % userfun is a string containing the name of a user-defined m-file 15 | % that defines the elements of F and optionally, their 16 | % derivatives. 17 | % 18 | % Additional arguments allow the specification of more detailed 19 | % problem information. 20 | % 21 | % Calling sequence 1: 22 | % [x,F,inform,xmul,Fmul] = snopt(x,xlow,xupp,Flow,Fupp,userfun) 23 | % 24 | % Calling sequence 2: 25 | % [x,F,inform,xmul,Fmul] = snopt(x,xlow,xupp,Flow,Fupp,userfun, 26 | % ObjAdd, ObjRow) 27 | % Calling sequence 3: 28 | % [x,F,inform,xmul,Fmul] = snopt(x,xlow,xupp,Flow,Fupp,userfun, 29 | % A, iAfun, jAvar, iGfun, jGvar) 30 | % Calling sequence 4: 31 | % [x,F,inform,xmul,Fmul] = snopt(x,xlow,xupp,Flow,Fupp,userfun, 32 | % ObjAdd, ObjRow, 33 | % A, iAfun, jAvar, iGfun, jGvar) 34 | 35 | % Description of the arguments: 36 | % x -- initial guess for x. 37 | % xlow, xupp -- upper and lower bounds on x. 38 | % Flow, Fupp -- upper and lower bounds on F. 39 | % userfun -- a string denoting name of a user-defined function 40 | % that computes the objective and constraint functions 41 | % and their corresponding derivatives. 42 | % WARNING: If arguments iAfun, jAvar, and A, are provided, 43 | % then it is crucial that the associated linear terms 44 | % are not included in the calculation of F in userfun. 45 | % **Example: 46 | % >> [f,G] = feval(userfun,x); 47 | % >> F = sparse(iAfun,jAvar,A)*x + f 48 | % >> DF = sparse(iAfun,jAvar,A) + sparse(iGfun,jGvar,G) 49 | % (where DF denotes F'). 50 | % G must be either a dense matrix, or a vector of derivatives 51 | % in the same order as the indices iGfun and jGvar, i.e., 52 | % if G is a vector, the Jacobian of f is given 53 | % by sparse(iGfun,jGvar,G). 54 | % 55 | % **More details on G: 56 | % The user maybe define, all, some, or none of the 57 | % entries of G. If the user does NOT intend to 58 | % supply ALL nonzero entries of G, it is imperative 59 | % that the proper derivative level is set prior to 60 | % a call to snopt(): 61 | % >> snseti("Derivative option",k); 62 | % where k = 0 or 1. Meaning: 63 | % 1 -- Default. All derivatives are provided. 64 | % 0 -- Some derivatives are not provided. 65 | % For the case k = 0 (ONLY), G may be returned as 66 | % an empty array []. 67 | % For the case k = 0, the user must denote 68 | % unknown NONZERO elements of G by NaN. 69 | % **Example: (vector case) 70 | % >> G = [1, NaN, 3, NaN, -5]'; 71 | % or (full matrix case) 72 | % >> G = [1, 0, NaN; 0 NaN 2; 0 0 3]; 73 | % ObjAdd -- Default 0. Constant added to F(ObjRow). 74 | % ObjRow -- Default 1. Denotes row of objective function in F. 75 | % A -- Constant elements in the Jacobian of F. 76 | % iAfun, jAvar -- Indices of A, corresponding to A. 77 | % iGfun, jGvar -- Indices of nonlinear elements in the Jacobian of F. 78 | % 79 | % More IMPORTANT details: 80 | % 1) The indices (iAfun,jAvar) must be DISJOINT from (iGfun,jGvar). 81 | % A nonzero element in F' must be either an element of G or an 82 | % element of A, but not the sum of the two. 83 | % 84 | % 2) If the user does not wish to provide iAfun, jAvar, iGfun, 85 | % jGvar, then snopt() will determine them by calling snJac(). 86 | % 87 | % WARNING: In this case, the derivative level will be set to zero 88 | % if constant elements exist. This is because the linear 89 | % elements have not yet been deleted from the definition of 90 | % userfun. Furthermore, if G is given in vector form, the 91 | % ordering of G may not necessarily correspond to (iGfun,jGvar) 92 | % computed by snJac(). 93 | 94 | m = length(Flow); 95 | n = length(x); 96 | xmul = zeros(n,1); 97 | xstate = zeros(n,1); 98 | Fmul = zeros(m,1); 99 | Fstate = zeros(m,1); 100 | ObjAdd = 0; 101 | ObjRow = 1; 102 | 103 | if nargin == 6, 104 | 105 | % Calling sequence 1 106 | % Derivatives are estimated by differences. 107 | % Call snJac to estimate the pattern of nonzeros for the Jacobian. 108 | 109 | [A,iAfun,jAvar,iGfun,jGvar] = snJac(userfun,x,xlow,xupp,m); 110 | elseif nargin == 8 111 | 112 | % Calling sequence 2 113 | % Derivatives are estimated by differences. 114 | % Call snJac to estimate the pattern of nonzeros for the Jacobian. 115 | 116 | [A,iAfun,jAvar,iGfun,jGvar] = snJac(userfun,x,xlow,xupp,m); 117 | ObjAdd = varargin{1}; 118 | ObjRow = varargin{2}; 119 | elseif nargin == 11 120 | 121 | % Calling sequence 3 122 | % The user is providing derivatives. 123 | 124 | A = varargin{1}; 125 | iAfun = varargin{2}; 126 | jAvar = varargin{3}; 127 | iGfun = varargin{4}; 128 | jGvar = varargin{5}; 129 | elseif ( nargin == 13 ) 130 | 131 | % Calling sequence 4 132 | % The user is providing derivatives. 133 | 134 | ObjAdd = varargin{1}; 135 | ObjRow = varargin{2}; 136 | A = varargin{3}; 137 | iAfun = varargin{4}; 138 | jAvar = varargin{5}; 139 | iGfun = varargin{6}; 140 | jGvar = varargin{7}; 141 | end 142 | 143 | solveopt = 1; 144 | [x,F,xmul,Fmul,inform] = snoptcmex( solveopt, ... 145 | x,xlow,xupp,xmul,xstate, ... 146 | Flow,Fupp,Fmul,Fstate, ... 147 | ObjAdd,ObjRow,A,iAfun,jAvar, ... 148 | iGfun,jGvar,userfun ); 149 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snpmat.m: -------------------------------------------------------------------------------- 1 | function [x,fval,exitflag,output,lambda] = snpmat(myobj,x0,A,b,varargin) 2 | 3 | % A wrapper for snopt to make it look like fmincon. 4 | % [x,fval,exitflag,output,lambda] = snpmat(myobj,x0,A,b) 5 | % [x,fval,exitflag,output,lambda] = snpmat(myobj,x0,A,b,Aeq,beq) 6 | % [x,fval,exitflag,output,lambda] = snpmat(myobj,x0,A,b,Aeq,beq,xlow,xupp) 7 | % [x,fval,exitflag,output,lambda] = snpmat(myobj,x0,A,b,Aeq,beq,xlow,xupp,nonlcon,options) 8 | % 9 | % snpmat and fmincon assume problems are of the form: 10 | % minimize f(x) 11 | % such that c(x) <= 0, 12 | % c_eq(x) = 0, 13 | % Ax <= b, 14 | % A_eq x = b_eq, 15 | % xlow <= x <= xupp. 16 | % 17 | % 26 November 2012. 18 | 19 | [mi,n] = size(A); 20 | 21 | if nargin == 4, 22 | me = 0; 23 | nli = 0; 24 | nle = 0; 25 | Aeq = []; 26 | beq = []; 27 | xlow = -inf*ones(n,1); 28 | xupp = inf*ones(n,1); 29 | nonlconS = ''; 30 | 31 | elseif nargin == 6, 32 | Aeq = varargin{1}; 33 | beq = varargin{2}; 34 | nonlconS = ''; 35 | 36 | [me,n0] = size(Aeq); 37 | nli = 0; 38 | nle = 0; 39 | xlow = -inf*ones(n,1); 40 | xupp = inf*ones(n,1); 41 | 42 | elseif nargin == 8, 43 | Aeq = varargin{1}; 44 | beq = varargin{2}; 45 | xlow = varargin{3}; 46 | xupp = varargin{4}; 47 | nonlconS = ''; 48 | 49 | [me,n0 ] = size(Aeq); 50 | nli = 0; 51 | nle = 0; 52 | 53 | elseif nargin == 10, 54 | Aeq = varargin{1}; 55 | beq = varargin{2}; 56 | xlow = varargin{3}; 57 | xupp = varargin{4}; 58 | nonlcon = varargin{5}; 59 | 60 | nonlconS = func2str(nonlcon); 61 | 62 | [me,n0] = size(Aeq); 63 | [c,ceq] = feval(nonlcon,x0); 64 | nli = size(c,1); 65 | nle = size(ceq,1); 66 | 67 | if isempty(xlow), 68 | xlow = -inf*ones(n,1); 69 | xupp = inf*ones(n,1); 70 | end 71 | 72 | else 73 | error('Wrong number of input arguments') 74 | end 75 | 76 | nCon = 1 + nli + nle + mi + me; % number of constraints (size of F(x)) 77 | 78 | 79 | % snoptA problem format: 80 | % minimize F_obj (x) 81 | % such that l_f <= F(x) <= u_f 82 | % l <= x <= u 83 | % 84 | % [ F_obj ] [ F_0' ] [ 0 ] 85 | % [ c(x) ] [ c'(x) ] [ 0 ] 86 | % [ c_eq(x) ] [ c_eq'(x) ] [ 0 ] 87 | % F(x) = [ Ax ] F'(x) = [ 0 ] + [ A ] 88 | % [ A_eq x ] [ 0 ] [ A_eq ] 89 | % "G(x)" "A" 90 | 91 | [iAfun,jAvar,Aij] = find( [ zeros(1+nli+nle,n); A; Aeq ]); 92 | [iGfun,jGvar,Gij] = find( [ ones(1+nli+nle,n); zeros(mi+me,n) ]); 93 | 94 | x = x0; 95 | xmul = zeros(n,1); 96 | xstate = zeros(n,1); 97 | Fmul = zeros(nCon,1); 98 | Fstate = zeros(nCon,1); 99 | ObjAdd = 0; 100 | ObjRow = 1; 101 | Flow = [ -inf; -inf*ones(nli,1); zeros(nle,1); -inf*ones(mi,1); zeros(me,1) ]; 102 | Fupp = [ inf; zeros(nli,1); zeros(nle,1); b; beq ]; 103 | 104 | myobjS = func2str(myobj); % SNOPT mex wants the string. 105 | AA = [ A ; Aeq ]; 106 | 107 | 108 | % Solve the problem! 109 | solveopt = 1; 110 | [x,F,xmul,Fmul,exitflag] = snoptcmex( solveopt,x,xlow,xupp,xmul,xstate, ... 111 | Flow,Fupp,Fmul,Fstate,ObjAdd,ObjRow, ... 112 | Aij,iAfun,jAvar,iGfun,jGvar, ... 113 | '',myobjS,nonlconS,AA); 114 | 115 | fval = feval(myobj,x); 116 | lambda.lower = xmul; 117 | lambda.upper = xmul; 118 | lambda.ineqnonlin = Fmul(2:nli+1); 119 | lambda.eqnonlin = Fmul(nli+2:nli+nle+1); 120 | lambda.ineqlin = Fmul(nli+nle+2:nli+nle+mi+1); 121 | lambda.eqlin = Fmul(nli+nle+mi+2:nCon); 122 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snprint.m: -------------------------------------------------------------------------------- 1 | % function snprint( filename ) 2 | % Causes SNOPT to write detailed information about its progress 3 | % to the file named in "filename." 4 | % 5 | % "snprint off" causes SNOPT to stop writing to filename, 6 | % and close the file. 7 | % 8 | % Note that until the file has been closed, it may not contain 9 | % all of the output. 10 | % 11 | % snprint serves the same function as snprintfile. 12 | % 13 | % WARNING: Do not use snset() or snseti() to set the print file. 14 | 15 | function snprint( filename ) 16 | 17 | %openprintfile = snoptcmex( 0, 'SetPrintFile' ); 18 | %closeprintfile = snoptcmex( 0, 'ClosePrintFile' ); 19 | 20 | openprintfile = 10; 21 | closeprintfile = 12; 22 | 23 | if strcmp( filename, 'off' ) 24 | snoptcmex( closeprintfile ); 25 | elseif strcmp( filename, 'on' ) 26 | snoptcmex( openprintfile, 'print.out' ); 27 | else 28 | snoptcmex( openprintfile, filename ); 29 | end 30 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snprintfile.m: -------------------------------------------------------------------------------- 1 | % function snprintfile( filename ) 2 | % Causes SNOPT to write detailed information about its progress 3 | % to the file named in "filename." 4 | % 5 | % "snprintfile off" causes SNOPT to stop writing to filename, 6 | % and close the file. 7 | % 8 | % Note that until the file has been closed, it may not contain 9 | % all of the output. 10 | % 11 | % snprintfile serves the same function as snprint. 12 | % 13 | % WARNING: Do not use snset() or snseti() to set the print file. 14 | function snprintfile( filename ) 15 | 16 | openprintfile = 10; 17 | closeprintfile = 12; 18 | 19 | if strcmp( filename, 'off' ) 20 | snoptcmex( closeprintfile ); 21 | elseif strcmp( filename, 'on' ) 22 | snoptcmex( openprintfile, 'print.out' ); 23 | else 24 | snoptcmex( openprintfile, filename ); 25 | end 26 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snscreen.m: -------------------------------------------------------------------------------- 1 | %function snscreen( filename ) 2 | % Regulates output to the terminal. Will print major iteration 3 | % information identical to that printed to a summary file if set. 4 | % Thus options that effect summary output may affect information 5 | % printed to the screen. Option is off by default. To turn on 6 | % the screen output, type: 7 | % >> snscreen on 8 | % to turn the screen output back off, type: 9 | % >> snscreen off 10 | % 11 | % NOTE: A summary file need not be set to use the screen option. 12 | function snscreen( filename ) 13 | 14 | screenon = 15; 15 | screenoff = 16; 16 | 17 | if strcmp( filename, 'on' ) 18 | snoptcmex( screenon ); 19 | elseif strcmp( filename, 'off' ) 20 | snoptcmex( screenoff ); 21 | end 22 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snset.m: -------------------------------------------------------------------------------- 1 | % function snset( option ) 2 | % Sets a optional parameter of snopt. The string "option" will be read 3 | % by snopt. If the string contains a setting that snopt understands, 4 | % snopt will set internal parameters accordingly. For a description of 5 | % available parameters, please see the snopt documentation. 6 | % 7 | % Do not try to set the unit number of the summary or print file. 8 | % Use the MATLAB functions snsummary and snprintfile instead. 9 | % 10 | function snset( option ) 11 | 12 | setoption = 2; 13 | snoptcmex( setoption, option ); -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snsetStatus.m: -------------------------------------------------------------------------------- 1 | % function snsetStatus( status ) 2 | % 3 | % snsetStatus may be called from the user-defined function 4 | % to signal snopt that the current value of x is infeasible. 5 | % Possible values of status: 6 | % status < -1: Signals snopt to stop. 7 | % status = -1: Functions are undefined at x, the line search will 8 | % shorten the step and try again. 9 | % status = 0: There is nothing special about current x. 10 | % 11 | % For further details see the documentation for snoptA. 12 | function snsetStatus( status ) 13 | 14 | setStatusOption = 18; 15 | snoptcmex( setStatusOption, status ); 16 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snseti.m: -------------------------------------------------------------------------------- 1 | % function snseti( option, ivalue ) 2 | % Sets the INTEGER-VALUED optional parameter defined by the 3 | % string "option" is assigned to rvalue. 4 | % 5 | % For a description of all the optional parameters, see the 6 | % snopt documentation. 7 | % 8 | % Do not try to set the unit number of the summary or print file. 9 | % Use the MATLAB functions snsummary and snprintfile instead. 10 | % 11 | function snseti( option, ivalue ) 12 | 13 | setoptionI = 3; 14 | snoptcmex( setoptionI, option, ivalue ); 15 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snsetr.m: -------------------------------------------------------------------------------- 1 | % function snsetr( option, rvalue ) 2 | % Sets the optional REAL-VALUED parameter defined by the 3 | % string "option" to the value rvalue. 4 | % 5 | % For a description of all the optional parameters, see the 6 | % snopt documentation. 7 | % 8 | % Do not try to set the unit number of the summary or print file. 9 | % Use the MATLAB functions snsummary and snprintfile instead. 10 | % 11 | function snsetr( option, rvalue ) 12 | 13 | setoptionR = 4; 14 | snoptcmex( setoptionR, option, rvalue ); 15 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snsolve.m: -------------------------------------------------------------------------------- 1 | %function [x,F,xmul,Fmul,info, xstate, Fstate, ns, ninf, ... 2 | % sinf, mincw, miniw, minrw] = ... 3 | % snsolve( x0, xlow, xupp, xmul, xstate, Flow, Fupp, ... 4 | % Fmul, Fstate, ObjAdd, ObjRow, A, iAfun, jAvar,... 5 | % iGfun, jGvar, usrf ); 6 | function [x, F, xmul, Fmul, info, xstate, Fstate, ns, ninf, ... 7 | sinf, mincw, miniw, minrw] = ... 8 | snsolve( x0, xlow, xupp, xmul, xstate, Flow, Fupp, ... 9 | Fmul, Fstate, ObjAdd, ObjRow, A, iAfun, jAvar,... 10 | iGfun, jGvar, usrf ); 11 | 12 | %solveopt = snoptcmex( 0, 'Solve' ); 13 | 14 | solveopt = 1; 15 | 16 | [x,F,xmul,Fmul,info, xstate, Fstate, ns, ninf, sinf, ... 17 | mincw, miniw, minrw] ... 18 | = snoptcmex( solveopt, x0, xlow, xupp, xmul, xstate, ... 19 | Flow, Fupp, Fmul, Fstate, ... 20 | ObjAdd, ObjRow, A, iAfun, jAvar, ... 21 | iGfun, jGvar, usrf ); 22 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snspec.m: -------------------------------------------------------------------------------- 1 | % function inform = snspec( filename ) 2 | % Causes snopt to read its optional parameters from the named file. 3 | % The format of this file is described in the snopt documentation. 4 | % 5 | % Returns 0 if successful, and a positive number otherwise. 6 | function inform = snspec( filename ) 7 | 8 | snoption = 9; 9 | inform = snoptcmex( snoption, filename ); 10 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snsummary.m: -------------------------------------------------------------------------------- 1 | %function snsummary( filename ) 2 | % Causes SNOPT to write summarized information about its progress 3 | % to the file named in "filename". 4 | % 5 | % "snsummary off" causes SNOPT to stop writing to filename, 6 | % and close the file. 7 | % 8 | % Note that until the file has been closed, it may not contain 9 | % all of the output. 10 | % 11 | % WARNING: Do not use snset() or snseti() to set the summary file. 12 | function snsummary( filename ) 13 | 14 | opensummary = 11; 15 | closesummary = 13; 16 | 17 | if strcmp( filename, 'off' ) 18 | snoptcmex( closesummary ); 19 | else 20 | snoptcmex( opensummary, filename ); 21 | end 22 | -------------------------------------------------------------------------------- /Lossy Monoped/SNOPT/snwrapper.m: -------------------------------------------------------------------------------- 1 | function [F,G] = snwrapper(x,userfun,nF,varargin) 2 | %function [F,G] = snwrapper(x,userfun); 3 | % Wrapper to allow variable length arguments in 4 | % user-supplied functions. 5 | 6 | 7 | if ( nargin == 3 ), 8 | try 9 | [F,G] = feval(userfun,x); 10 | catch 11 | F = feval(userfun,x); 12 | G = []; 13 | end 14 | 15 | else 16 | if ( nargin == 4 ), 17 | myobj = varargin{1}; 18 | 19 | try 20 | [obj,grad] = feval(myobj,x); 21 | 22 | F = [ obj; zeros(nF-1,1) ]; 23 | G = [ grad; zeros(nF-1,n) ]; 24 | catch 25 | [obj] = feval(myobj,x); 26 | 27 | F = [ obj; zeros(nF-1,1) ]; 28 | G = []; 29 | end 30 | 31 | elseif ( nargin == 5 ), 32 | myobj = varargin{1}; 33 | nonlcon = varargin{2}; 34 | 35 | try 36 | [obj,grad] = feval(myobj,x); 37 | [c,ceq,dc,dceq] = feval(nonlcon,x); 38 | 39 | z = nF - 1 - size(c,1) - size(ceq,1); 40 | F = [ obj; c; ceq; zeros(z,1) ]; 41 | G = [ grad; cd; ced; zeros(z,n) ]; 42 | 43 | catch 44 | [obj] = feval(myobj,x); 45 | [c,ceq] = feval(nonlcon,x); 46 | 47 | z = nF - 1 - size(c,1) - size(ceq,1); 48 | F = [ obj; c; ceq; zeros(z,1) ]; 49 | G = []; 50 | end 51 | end 52 | end 53 | -------------------------------------------------------------------------------- /Lossy Monoped/Symbolic/Binary Operator Nodes/BinaryOperatorNode.m: -------------------------------------------------------------------------------- 1 | %BINARYOPERATORNODE Expression tree binary operator node subclass. 2 | % 3 | % Copyright 2014-2016 Mikhail S. Jones 4 | 5 | classdef BinaryOperatorNode < ExpressionNode 6 | 7 | % PROTECTED PROPERTIES ================================================== 8 | properties (Access = protected) 9 | left@ExpressionNode 10 | right@ExpressionNode 11 | end % properties 12 | 13 | % PUBLIC METHODS ======================================================== 14 | methods 15 | function obj = BinaryOperatorNode(left, right) 16 | %BINARYOPERATORNODE Binary operator node constructor. 17 | 18 | % Check data type of left node 19 | if isa(left, 'ExpressionNode') 20 | % Expression node subclasses can be stored as is 21 | obj.left = simplify_(left); 22 | else 23 | % Convert into appropriate ExpressionNode subclass 24 | obj.left = obj.convertObject('', left); 25 | end % if 26 | 27 | % Check data type of right node 28 | if isa(right, 'ExpressionNode') 29 | % Expression node subclasses can be stored as is 30 | obj.right = simplify_(right); 31 | else 32 | % Convert into appropriate ExpressionNode subclass 33 | obj.right = obj.convertObject('', right); 34 | end % if 35 | 36 | % Check internal dimensions 37 | if obj.left.length ~= obj.right.length 38 | if obj.left.length ~= 1 && obj.right.length ~= 1 39 | error('Dimensions do not match.'); 40 | end % if 41 | end % if 42 | 43 | % Set object properties 44 | obj.length = max(obj.left.length, obj.right.length); 45 | end % BinaryOperatorNode 46 | 47 | function obj = ind(obj, index) 48 | %IND Index expression. 49 | 50 | % Copy object and set new index values 51 | for i = 1:numel(obj) 52 | obj(i).left = ind(obj(i).left, index); 53 | obj(i).right = ind(obj(i).right, index); 54 | obj(i).length = length(index); 55 | end % for 56 | end % ind 57 | end % methods 58 | 59 | % PROTECTED METHODS ===================================================== 60 | methods (Access = protected) 61 | function vars = symvar_(obj) 62 | %SYMVAR_ Determine the variables in an expression tree. 63 | 64 | % Determine variables in branches 65 | leftVars = symvar_(obj.left); 66 | rightVars = symvar_(obj.right); 67 | 68 | % Create uniqueness index 69 | ind = ones(size(rightVars)); 70 | 71 | % Return only unique variables 72 | for i = 1:numel(rightVars) 73 | for j = 1:numel(leftVars) 74 | if rightVars(i) == leftVars(j) 75 | ind(i) = 0; 76 | end % if 77 | end % for 78 | end % for 79 | 80 | vars = [leftVars rightVars(find(ind))]; 81 | end % symvar_ 82 | end % methods 83 | end % classdef 84 | -------------------------------------------------------------------------------- /Lossy Monoped/Symbolic/Binary Operator Nodes/DivideBinaryOperatorNode.m: -------------------------------------------------------------------------------- 1 | %DIVIDEBINARYOPERATORNODE Binary division operator node subclass. 2 | % 3 | % Copyright 2014-2016 Mikhail S. Jones 4 | 5 | classdef DivideBinaryOperatorNode < BinaryOperatorNode 6 | 7 | % PUBLIC PROPERTIES ===================================================== 8 | methods 9 | function obj = DivideBinaryOperatorNode(left, right) 10 | %DIVIDEBINARYOPERATORNODE Binary division operator node constructor. 11 | obj = obj@BinaryOperatorNode(left, right); 12 | end % DivideBinaryOperatorNode 13 | end % methods 14 | 15 | % PROTECTED METHODS ===================================================== 16 | methods (Access = protected) 17 | function y = diff_(obj, x) 18 | %DIFF_ Overloaded abstract method to differentiate node. 19 | 20 | % Apply chain rule 21 | leftDiff = diff_(obj.left, x); 22 | rightDiff = diff_(obj.right, x); 23 | 24 | % Check if either is zero 25 | if isa(leftDiff, 'ConstantNode') 26 | leftZero = leftDiff.value == 0; 27 | else 28 | leftZero = false; 29 | end % if 30 | if isa(rightDiff, 'ConstantNode') 31 | rightZero = rightDiff.value == 0; 32 | else 33 | rightZero = false; 34 | end % if 35 | 36 | if leftZero && rightZero 37 | y = ConstantNode(0); 38 | elseif leftZero 39 | y = MinusUnaryOperatorNode(... 40 | DivideBinaryOperatorNode(... 41 | TimesBinaryOperatorNode(rightDiff, obj.left), ... 42 | PowerBinaryOperatorNode(obj.right, ConstantNode(2)))); 43 | elseif rightZero 44 | y = DivideBinaryOperatorNode(leftDiff, obj.right); 45 | else 46 | y = DivideBinaryOperatorNode(... 47 | MinusBinaryOperatorNode(... 48 | TimesBinaryOperatorNode(leftDiff, obj.right), ... 49 | TimesBinaryOperatorNode(rightDiff, obj.left)), ... 50 | PowerBinaryOperatorNode(obj.right, ConstantNode(2))); 51 | end % if 52 | end % diff_ 53 | 54 | function y = eval_(obj) 55 | %EVAL Overloaded abstract method to evaluate node. 56 | y = eval_(obj.left)./eval_(obj.right); 57 | end % eval_ 58 | 59 | function obj = simplify_(obj) 60 | %SIMPLIFY_ Overloaded abstract method with to simplify node. 61 | 62 | % Simplification lazy check 63 | if obj.isSimple 64 | return; 65 | end % if 66 | 67 | % Simplify LHS and RHS 68 | obj.left = simplify_(obj.left); 69 | obj.right = simplify_(obj.right); 70 | obj.isSimple = true; 71 | 72 | % Simplification rule (a / b = c) 73 | if isa(obj.left, 'ConstantNode') && isa(obj.right, 'ConstantNode') 74 | obj = ConstantNode(obj.left.value./obj.right.value); 75 | 76 | % Simplification rule (0 / x = 0) 77 | elseif isa(obj.left, 'ConstantNode') 78 | if obj.left.value == 0 79 | obj = ConstantNode(0); 80 | end % if 81 | 82 | % Simplification rule (x / 1 = x) 83 | elseif isa(obj.right, 'ConstantNode') 84 | if obj.right.value == 1 85 | obj = obj.left; 86 | end % if 87 | 88 | % Simplification rule (x / x = 1) 89 | elseif isequal(obj.left, obj.right) 90 | obj = ConstantNode(1); 91 | 92 | % Simplification rule (x^a / x^b = x^(a - b)) 93 | elseif isa(obj.left, 'PowerBinaryOperatorNode') && isa(obj.right, 'PowerBinaryOperatorNode') 94 | if isequal(obj.left.left, obj.right.left) 95 | obj = simplify_(PowerBinaryOperatorNode(... 96 | obj.left.left, ... 97 | MinusBinaryOperatorNode(... 98 | obj.left.right, obj.right.right))); 99 | end % if 100 | 101 | % Simplification rule (x^a / x = x^(a - 1)) 102 | elseif isa(obj.left, 'PowerBinaryOperatorNode') 103 | if isequal(obj.left.left, obj.right) 104 | obj = simplify_(PowerBinaryOperatorNode(... 105 | obj.right, ... 106 | MinusBinaryOperatorNode(... 107 | obj.left.right, ConstantNode(1)))); 108 | end % if 109 | 110 | % Simplification rule (x / x^a = x^(1 - a)) 111 | elseif isa(obj.right, 'PowerBinaryOperatorNode') 112 | if isequal(obj.right.left, obj.left) 113 | obj = simplify_(PowerBinaryOperatorNode(... 114 | obj.left, ... 115 | MinusBinaryOperatorNode(... 116 | ConstantNode(1), obj.right.right))); 117 | end % if 118 | end % if 119 | end % simplify_ 120 | 121 | function str = char_(obj) 122 | %CHAR_ Overloaded abstract method to convert node to char. 123 | str = ['DivideBinaryOperatorNode(' char_(obj.left) ', ' char_(obj.right) ')']; 124 | end % char_ 125 | 126 | function str = matlabCode_(obj) 127 | %MATLABCODE_ Overloaded abstract method to convert node to matlab code. 128 | 129 | % Check if parenthesis are needed around LHS 130 | if isa(obj.left, 'PlusBinaryOperatorNode') || isa(obj.left, 'MinusBinaryOperatorNode') 131 | leftStr = ['(' matlabCode_(obj.left) ')']; 132 | else 133 | leftStr = matlabCode_(obj.left); 134 | end % if 135 | 136 | % Check if parenthesis are needed around RHS 137 | if isa(obj.right, 'PlusBinaryOperatorNode') || isa(obj.right, 'MinusBinaryOperatorNode') 138 | rightStr = ['(' matlabCode_(obj.right) ')']; 139 | else 140 | rightStr = matlabCode_(obj.right); 141 | end % if 142 | 143 | % Concatenate LHS and RHS into string 144 | str = [leftStr './' rightStr]; 145 | end % matlabCode_ 146 | end % methods 147 | end % classdef 148 | -------------------------------------------------------------------------------- /Lossy Monoped/Symbolic/Binary Operator Nodes/MinusBinaryOperatorNode.m: -------------------------------------------------------------------------------- 1 | %MINUSBINARYOPERATORNODE Binary minus operator node subclass. 2 | % 3 | % Copyright 2014-2016 Mikhail S. Jones 4 | 5 | classdef MinusBinaryOperatorNode < BinaryOperatorNode 6 | 7 | % PUBLIC PROPERTIES ===================================================== 8 | methods 9 | function obj = MinusBinaryOperatorNode(left, right) 10 | %MINUSBINARYOPERATORNODE Binary minus operator node constructor. 11 | obj = obj@BinaryOperatorNode(left, right); 12 | end % MinusBinaryOperatorNode 13 | end % methods 14 | 15 | % PROTECTED METHODS ===================================================== 16 | methods (Access = protected) 17 | function y = diff_(obj, x) 18 | %DIFF_ Overloaded abstract method to differentiate node. 19 | 20 | % Apply chain rule 21 | y = MinusBinaryOperatorNode(diff_(obj.left, x), diff_(obj.right, x)); 22 | end % diff_ 23 | 24 | function y = eval_(obj) 25 | %EVAL_ Overloaded abstract method to evaluate node. 26 | y = eval_(obj.left) - eval_(obj.right); 27 | end % eval_ 28 | 29 | function obj = simplify_(obj) 30 | %SIMPLIFY_ Overloaded abstract method to simplify node. 31 | 32 | % Simplification lazy check 33 | if obj.isSimple 34 | return; 35 | end % if 36 | 37 | % Simplify LHS and RHS 38 | obj.left = simplify_(obj.left); 39 | obj.right = simplify_(obj.right); 40 | obj.isSimple = true; 41 | 42 | % Simplification rule (a - b = c) 43 | if isa(obj.left, 'ConstantNode') && isa(obj.right, 'ConstantNode') 44 | obj = ConstantNode(obj.left.value - obj.right.value); 45 | 46 | % Simplification rule (0 - x = -x) 47 | elseif isa(obj.left, 'ConstantNode') 48 | if obj.left.value == 0 49 | obj = MinusUnaryOperatorNode(obj.right); 50 | end % if 51 | 52 | % Simplification rule (x - 0 = x) 53 | elseif isa(obj.right, 'ConstantNode') 54 | if obj.right.value == 0 55 | obj = obj.left; 56 | end % if 57 | 58 | % Simplification rule (x - x = 0) 59 | elseif isequal(obj.left, obj.right) 60 | obj = ConstantNode(0); 61 | 62 | % Simplification rule (a*x - b*x = (a - b)*x) 63 | elseif isa(obj.left, 'TimesBinaryOperatorNode') && isa(obj.right, 'TimesBinaryOperatorNode') 64 | if isequal(obj.left.right, obj.right.right) 65 | obj = simplify_(TimesBinaryOperatorNode(... 66 | MinusBinaryOperatorNode(... 67 | obj.left.left, obj.right.left), ... 68 | obj.right.right)); 69 | end % if 70 | 71 | % Simplification rule (a*x - x = (a - 1)*x) 72 | elseif isa(obj.left, 'TimesBinaryOperatorNode') 73 | if isequal(obj.left.right, obj.right) 74 | obj = simplify_(TimesBinaryOperatorNode(... 75 | MinusBinaryOperatorNode(... 76 | obj.left.left, ConstantNode(1)), ... 77 | obj.right)); 78 | end % if 79 | 80 | % Simplification rule (x - a*x = (a - 1)*x) 81 | elseif isa(obj.right, 'TimesBinaryOperatorNode') 82 | if isequal(obj.right.right, obj.left) 83 | obj = simplify_(TimesBinaryOperatorNode(... 84 | MinusBinaryOperatorNode(... 85 | obj.right.left, ConstantNode(1)), ... 86 | obj.left)); 87 | end % if 88 | end % if 89 | end % simplify_ 90 | 91 | function str = char_(obj) 92 | %CHAR_ Overloaded abstract method to convert node to char. 93 | str = ['MinusBinaryOperatorNode(' char_(obj.left) ', ' char_(obj.right) ')']; 94 | end % char_ 95 | 96 | function str = matlabCode_(obj) 97 | %MATLABCODE_ Overloaded abstract method to convert node to matlab code. 98 | 99 | % Check if parenthesis are needed around LHS 100 | if isa(obj.left, 'PlusBinaryOperatorNode') || isa(obj.left, 'MinusBinaryOperatorNode') 101 | leftStr = ['(' matlabCode_(obj.left) ')']; 102 | else 103 | leftStr = matlabCode_(obj.left); 104 | end % if 105 | 106 | % Check if parenthesis are needed around RHS 107 | if isa(obj.right, 'PlusBinaryOperatorNode') || isa(obj.right, 'MinusBinaryOperatorNode') 108 | rightStr = ['(' matlabCode_(obj.right) ')']; 109 | else 110 | rightStr = matlabCode_(obj.right); 111 | end % if 112 | 113 | % Concatenate LHS and RHS into string 114 | str = [leftStr ' - ' rightStr]; 115 | end % matlabCode_ 116 | end % methods 117 | end % classdef 118 | -------------------------------------------------------------------------------- /Lossy Monoped/Symbolic/Binary Operator Nodes/PlusBinaryOperatorNode.m: -------------------------------------------------------------------------------- 1 | %PLUSBINARYOPERATORNODE Binary plus operator node subclass. 2 | % 3 | % Copyright 2014-2016 Mikhail S. Jones 4 | 5 | classdef PlusBinaryOperatorNode < BinaryOperatorNode 6 | 7 | % PUBLIC METHODS ======================================================== 8 | methods 9 | function obj = PlusBinaryOperatorNode(left, right) 10 | %PLUSBINARYOPERATORNODE Binary plus operator node constructor. 11 | obj = obj@BinaryOperatorNode(left, right); 12 | end % PlusBinaryOperatorNode 13 | end % methods 14 | 15 | % PROTECTED METHODS ===================================================== 16 | methods (Access = protected) 17 | function y = diff_(obj, x) 18 | %DIFF_ Overloaded abstract method to differentiate node. 19 | 20 | % Apply chain rule 21 | y = PlusBinaryOperatorNode(diff_(obj.left, x), diff_(obj.right, x)); 22 | end % diff_ 23 | 24 | function y = eval_(obj) 25 | %EVAL_ Overloaded abstract method to evaluate node. 26 | y = eval_(obj.left) + eval_(obj.right); 27 | end % eval_ 28 | 29 | function obj = simplify_(obj) 30 | %SIMPLIFY_ Overloaded abstract method with to simplify node. 31 | 32 | % Simplification lazy check 33 | if obj.isSimple 34 | return; 35 | end % if 36 | 37 | % Simplify LHS and RHS 38 | obj.left = simplify_(obj.left); 39 | obj.right = simplify_(obj.right); 40 | obj.isSimple = true; 41 | 42 | % Simplification rule (a + b = c) 43 | if isa(obj.left, 'ConstantNode') && isa(obj.right, 'ConstantNode') 44 | obj = ConstantNode(obj.left.value + obj.right.value); 45 | 46 | % Simplification rule (0 + x = x) 47 | elseif isa(obj.left, 'ConstantNode') 48 | if obj.left.value == 0 49 | obj = obj.right; 50 | end % if 51 | 52 | % Simplification rule (x + 0 = x) 53 | elseif isa(obj.right, 'ConstantNode') 54 | if obj.right.value == 0 55 | obj = obj.left; 56 | end % if 57 | 58 | % Simplification rule (x + x = 2*x) 59 | elseif isequal(obj.left, obj.right) 60 | obj = TimesBinaryOperatorNode(2, obj.left); 61 | 62 | % Simplification rule (a*x + b*x = (a + b)*x) 63 | elseif isa(obj.left, 'TimesBinaryOperatorNode') && isa(obj.right, 'TimesBinaryOperatorNode') 64 | if isequal(obj.left.right, obj.right.right) 65 | obj = simplify_(TimesBinaryOperatorNode(... 66 | PlusBinaryOperatorNode(... 67 | obj.left.left, obj.right.left), ... 68 | obj.right.right)); 69 | end % if 70 | 71 | % Simplification rule (a*x + x = (a + 1)*x) 72 | elseif isa(obj.left, 'TimesBinaryOperatorNode') 73 | if isequal(obj.left.right, obj.right) 74 | obj = simplify_(TimesBinaryOperatorNode(... 75 | PlusBinaryOperatorNode(... 76 | obj.left.left, ConstantNode(1)), ... 77 | obj.right)); 78 | end % if 79 | 80 | % Simplification rule (x + a*x = (a + 1)*x) 81 | elseif isa(obj.right, 'TimesBinaryOperatorNode') 82 | if isequal(obj.right.right, obj.left) 83 | obj = simplify_(TimesBinaryOperatorNode(... 84 | PlusBinaryOperatorNode(... 85 | obj.right.left, ConstantNode(1)), ... 86 | obj.left)); 87 | end % if 88 | end % if 89 | end % simplify_ 90 | 91 | function str = char_(obj) 92 | %CHAR_ Overloaded abstract method to convert node to char. 93 | str = ['PlusBinaryOperatorNode(' char_(obj.left) ', ' char_(obj.right) ')']; 94 | end % char_ 95 | 96 | function str = matlabCode_(obj) 97 | %MATLABCODE_ Overloaded abstract method to convert node to matlab code. 98 | str = [matlabCode_(obj.left) ' + ' matlabCode_(obj.right)]; 99 | end % matlabCode_ 100 | end % methods 101 | end % classdef 102 | -------------------------------------------------------------------------------- /Lossy Monoped/Symbolic/Binary Operator Nodes/PowerBinaryOperatorNode.m: -------------------------------------------------------------------------------- 1 | %DIVIDEBINARYOPERATORNODE Binary division operator node subclass. 2 | % 3 | % Copyright 2014-2016 Mikhail S. Jones 4 | 5 | classdef PowerBinaryOperatorNode < BinaryOperatorNode 6 | 7 | % PUBLIC PROPERTIES ===================================================== 8 | methods 9 | function obj = PowerBinaryOperatorNode(left, right) 10 | %POWERBINARYOPERATORNODE Binary power operator node constructor. 11 | obj = obj@BinaryOperatorNode(left, right); 12 | end % PowerBinaryOperatorNode 13 | end % methods 14 | 15 | % PROTECTED METHODS ===================================================== 16 | methods (Access = protected) 17 | function y = diff_(obj, x) 18 | %DIFF_ Overloaded abstract method to differentiate node. 19 | 20 | % Apply chain rule 21 | leftDiff = diff_(obj.left, x); 22 | rightDiff = diff_(obj.right, x); 23 | 24 | % Check if either is zero 25 | if isa(leftDiff, 'ConstantNode') 26 | leftZero = eval_(leftDiff) == 0; 27 | else 28 | leftZero = false; 29 | end % if 30 | if isa(rightDiff, 'ConstantNode') 31 | rightZero = eval_(rightDiff) == 0; 32 | else 33 | rightZero = false; 34 | end % if 35 | 36 | if leftZero && rightZero 37 | y = ConstantNode(0); 38 | elseif rightZero 39 | % y = obj.right.*leftDiff.*(obj.left.^(obj.right - 1)); 40 | y = TimesBinaryOperatorNode(... 41 | TimesBinaryOperatorNode(... 42 | obj.right, leftDiff), ... 43 | PowerBinaryOperatorNode(... 44 | obj.left, MinusBinaryOperatorNode(... 45 | obj.right, ConstantNode(1)))); 46 | else 47 | % y = obj.left.^obj.right.*(leftDiff.*obj.right./obj.left + rightDiff.*log(obj.left)); 48 | y = TimesBinaryOperatorNode(... 49 | PowerBinaryOperatorNode(... 50 | obj.left, obj.right), ... 51 | PlusBinaryOperatorNode(... 52 | DivideBinaryOperatorNode(... 53 | TimesBinaryOperatorNode(... 54 | leftDiff, obj.right), ... 55 | obj.left), ... 56 | TimesBinaryOperatorNode(... 57 | rightDiff, LogUnaryOperatorNode(obj.left)))); 58 | end % if 59 | end % diff_ 60 | 61 | function y = eval_(obj) 62 | %EVAL_ Overloaded abstract method to evaluate node. 63 | y = eval_(obj.left).^eval_(obj.right); 64 | end % eval_ 65 | 66 | function obj = simplify_(obj) 67 | %SIMPLIFY_ Overloaded abstract method with to simplify node. 68 | 69 | % Simplification lazy check 70 | if obj.isSimple 71 | return; 72 | end % if 73 | 74 | % Simplify LHS and RHS 75 | obj.left = simplify_(obj.left); 76 | obj.right = simplify_(obj.right); 77 | obj.isSimple = true; 78 | 79 | % Simplification rule (a^b = c) 80 | if isa(obj.left, 'ConstantNode') && isa(obj.right, 'ConstantNode') 81 | obj = ConstantNode(obj.left.value.^obj.right.value); 82 | 83 | % Simplification rule (1^x = 1) 84 | elseif isa(obj.left, 'ConstantNode') 85 | if obj.left.value == 1 86 | obj = ConstantNode(1); 87 | end % if 88 | 89 | elseif isa(obj.right, 'ConstantNode') 90 | % Simplification rule (x^0 = 1) 91 | if obj.right.value == 0 92 | obj = ConstantNode(1); 93 | 94 | % Simplification rule (x^1 = x) 95 | elseif obj.right.value == 1 96 | obj = obj.left; 97 | 98 | % Simplification rule (x^-1 = 1 / x) 99 | elseif obj.right.value == -1 100 | obj = DivideBinaryOperatorNode(ConstantNode(1), obj.left); 101 | end % if 102 | 103 | % Simplification rule (x^a * x^b = x^(a + b) 104 | % TODO 105 | 106 | % Simplification rule (a^x * b^x = (a * b)^x 107 | % TODO 108 | end % if 109 | end % simplify_ 110 | 111 | function str = char_(obj) 112 | %CHAR_ Overloaded abstract method to convert node to char. 113 | str = ['PowerBinaryOperatorNode(' char_(obj.left) ', ' char_(obj.right) ')']; 114 | end % char_ 115 | 116 | function str = matlabCode_(obj) 117 | %MATLABCODE_ Overloaded abstract method to convert node to matlab code. 118 | 119 | % Check if parenthesis are needed around LHS 120 | if isa(obj.left, 'BinaryOperatorNode') 121 | leftStr = ['(' matlabCode_(obj.left) ')']; 122 | else 123 | leftStr = matlabCode_(obj.left); 124 | end % if 125 | 126 | % Check if parenthesis are needed around RHS 127 | if isa(obj.right, 'BinaryOperatorNode') 128 | rightStr = ['(' matlabCode_(obj.right) ')']; 129 | else 130 | rightStr = matlabCode_(obj.right); 131 | end % if 132 | 133 | % Concatenate LHS and RHS into string 134 | str = [leftStr '.^' rightStr]; 135 | end % matlabCode_ 136 | end % methods 137 | end % classdef 138 | -------------------------------------------------------------------------------- /Lossy Monoped/Symbolic/Binary Operator Nodes/TimesBinaryOperatorNode.m: -------------------------------------------------------------------------------- 1 | %TIMESBINARYOPERATORNODE Binary multiplication operator node subclass. 2 | % 3 | % Copyright 2014-2016 Mikhail S. Jones 4 | 5 | classdef TimesBinaryOperatorNode < BinaryOperatorNode 6 | 7 | % PUBLIC PROPERTIES ===================================================== 8 | methods 9 | function obj = TimesBinaryOperatorNode(left, right) 10 | %TIMESBINARYOPERATORNODE Binary multiplication operator node constructor. 11 | obj = obj@BinaryOperatorNode(left, right); 12 | end % TimesBinaryOperatorNode 13 | end % methods 14 | 15 | % PROTECTED METHODS ===================================================== 16 | methods (Access = protected) 17 | function y = diff_(obj, x) 18 | %DIFF_ Overloaded abstract method to differentiate node. 19 | 20 | % Apply chain rule 21 | leftDiff = diff_(obj.left, x); 22 | rightDiff = diff_(obj.right, x); 23 | 24 | % Check if either is zero 25 | if isa(leftDiff, 'ConstantNode') 26 | leftZero = leftDiff.value == 0; 27 | else 28 | leftZero = false; 29 | end % if 30 | if isa(rightDiff, 'ConstantNode') 31 | rightZero = rightDiff.value == 0; 32 | else 33 | rightZero = false; 34 | end % if 35 | 36 | if leftZero && rightZero 37 | y = ConstantNode(0); 38 | elseif leftZero 39 | y = TimesBinaryOperatorNode(obj.left, rightDiff); 40 | elseif rightZero 41 | y = TimesBinaryOperatorNode(obj.right, leftDiff); 42 | else 43 | % Apply chain rule 44 | y = PlusBinaryOperatorNode(... 45 | TimesBinaryOperatorNode(obj.left, rightDiff), ... 46 | TimesBinaryOperatorNode(obj.right, leftDiff)); 47 | end % if 48 | end % diff_ 49 | 50 | function y = eval_(obj) 51 | %EVAL_ Overloaded abstract method to evaluate node. 52 | y = eval_(obj.left).*eval_(obj.right); 53 | end % eval_ 54 | 55 | function obj = simplify_(obj) 56 | %SIMPLIFY_ Overloaded abstract method with to simplify node. 57 | 58 | % Simplification lazy check 59 | if obj.isSimple 60 | return; 61 | end % if 62 | 63 | % Simplify LHS and RHS 64 | obj.left = simplify_(obj.left); 65 | obj.right = simplify_(obj.right); 66 | obj.isSimple = true; 67 | 68 | % Simplification rule (a * b = c) 69 | if isa(obj.left, 'ConstantNode') && isa(obj.right, 'ConstantNode') 70 | obj = ConstantNode(obj.left.value.*obj.right.value); 71 | 72 | elseif isa(obj.left, 'ConstantNode') 73 | % Simplification rule (0 * x = 0) 74 | if obj.left.value == 0 75 | obj = ConstantNode(0); 76 | 77 | % Simplification rule (-1 * x = -x) 78 | elseif obj.left.value == -1 79 | obj = MinusUnaryOperatorNode(obj.right); 80 | 81 | % Simplification rule (1 * x = x) 82 | elseif obj.left.value == 1 83 | obj = obj.right; 84 | end % if 85 | 86 | elseif isa(obj.right, 'ConstantNode') 87 | % Simplification rule (x * 0 = 0) 88 | if obj.right.value == 0 89 | obj = ConstantNode(0); 90 | 91 | % Simplification rule (x * -1 = -x) 92 | elseif obj.right.value == -1 93 | obj = MinusUnaryOperatorNode(obj.left); 94 | 95 | % Simplification rule (x * 1 = x) 96 | elseif obj.right.value == 1 97 | obj = obj.left; 98 | end % if 99 | 100 | % Simplification rule (x * x = x^2) 101 | elseif isequal(obj.left, obj.right) 102 | obj = PowerBinaryOperatorNode(obj.left, ConstantNode(2)); 103 | 104 | % Simplification rule (x^a * x^b = x^(a + b)) 105 | elseif isa(obj.left, 'PowerBinaryOperatorNode') && isa(obj.right, 'PowerBinaryOperatorNode') 106 | if isequal(obj.left.left, obj.right.left) 107 | obj = simplify_(PowerBinaryOperatorNode(... 108 | obj.left.left, ... 109 | PlusBinaryOperatorNode(... 110 | obj.left.right, obj.right.right))); 111 | end % if 112 | 113 | % Simplification rule (x^a * x = x^(a + 1)) 114 | elseif isa(obj.left, 'PowerBinaryOperatorNode') 115 | if isequal(obj.left.left, obj.right) 116 | obj = simplify_(PowerBinaryOperatorNode(... 117 | obj.right, ... 118 | PlusBinaryOperatorNode(... 119 | obj.left.right, ConstantNode(1)))); 120 | end % if 121 | 122 | % Simplification rule (x * x^a = x^(a + 1)) 123 | elseif isa(obj.right, 'PowerBinaryOperatorNode') 124 | if isequal(obj.right.left, obj.left) 125 | obj = simplify_(PowerBinaryOperatorNode(... 126 | obj.left, ... 127 | PlusBinaryOperatorNode(... 128 | obj.right.right, ConstantNode(1)))); 129 | end % if 130 | end % if 131 | end % simplify_ 132 | 133 | function str = char_(obj) 134 | %CHAR_ Overloaded abstract method to convert node to char. 135 | str = ['TimesBinaryOperatorNode(' char_(obj.left) ', ' char_(obj.right) ')']; 136 | end % char_ 137 | 138 | function str = matlabCode_(obj) 139 | %MATLABCODE_ Overloaded abstract method to convert node to matlab code. 140 | 141 | % Check if parenthesis are needed around LHS 142 | if isa(obj.left, 'PlusBinaryOperatorNode') || isa(obj.left, 'MinusBinaryOperatorNode') 143 | leftStr = ['(' matlabCode_(obj.left) ')']; 144 | else 145 | leftStr = matlabCode_(obj.left); 146 | end % if 147 | 148 | % Check if parenthesis are needed around RHS 149 | if isa(obj.right, 'PlusBinaryOperatorNode') || isa(obj.right, 'MinusBinaryOperatorNode') 150 | rightStr = ['(' matlabCode_(obj.right) ')']; 151 | else 152 | rightStr = matlabCode_(obj.right); 153 | end % if 154 | 155 | % Concatenate LHS and RHS into string 156 | str = [leftStr '.*' rightStr]; 157 | end % matlabCode_ 158 | end % methods 159 | end % classdef 160 | -------------------------------------------------------------------------------- /Lossy Monoped/Symbolic/Binary Operator Nodes/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/Symbolic/ConstantNode.m: -------------------------------------------------------------------------------- 1 | %CONSTANTNODE Expression tree constant node subclass. 2 | % 3 | % Copyright 2014-2016 Mikhail S. Jones 4 | 5 | classdef ConstantNode < ExpressionNode 6 | 7 | % PROTECTED PROPERTIES ================================================== 8 | properties (SetAccess = protected) 9 | value@double vector 10 | end % properties 11 | 12 | % PUBLIC METHODS ======================================================== 13 | methods 14 | function obj = ConstantNode(value) 15 | %CONSTANTNODE Constant node constructor. 16 | 17 | % Set object properties 18 | obj.value = value; 19 | obj.length = length(value); 20 | obj.isSimple = true; 21 | end % ConstantNode 22 | 23 | function obj = ind(obj, index) 24 | %IND Index expression. 25 | 26 | if obj.length ~= 1 27 | obj.value = obj.value(index); 28 | end % if 29 | end % ind 30 | end % methods 31 | 32 | % PROTECTED METHODS ===================================================== 33 | methods (Access = protected) 34 | function y = diff_(~, ~) 35 | %DIFF_ Overloaded abstract method to differentiate constant node. 36 | 37 | % The derivative of a constant is always zero 38 | y = ConstantNode(0); 39 | end % diff_ 40 | 41 | function y = eval_(obj) 42 | %EVAL_ Overloaded abstract method to evaluate constant node. 43 | y = obj.value; 44 | end % eval_ 45 | 46 | function obj = simplify_(obj) 47 | %SIMPLIFY_ Overloaded abstract method to simplify constant node. 48 | % A constant node cannot not be simplified anymore 49 | end % simplify_ 50 | 51 | function vars = symvar_(~) 52 | %SYMVAR_ Determine the variables in an expression tree. 53 | vars = []; 54 | end % symvar_ 55 | 56 | function str = char_(obj) 57 | %CHAR_ Overloaded abstract method to convert constant node to char. 58 | 59 | % Convert numeric value to string keeping double precision 60 | str = sprintf('%g,', obj.value); 61 | str = ['ConstantNode([' str(1:end-1) '])']; 62 | end % char_ 63 | 64 | function str = matlabCode_(obj) 65 | %MATLABCODE_ Overloaded abstract method to convert node to matlab code. 66 | 67 | if obj.length == 1 68 | % Convert numeric value to string keeping double precision 69 | str = sprintf('%.*f', ceil(-log10(eps(obj.value))), obj.value); 70 | 71 | % Remove trailing zeros 72 | tmp = regexp(str, '^0+(?!\.)|(? 0) && all(mod(index,1) == 0) 30 | % Construct vector variable with specified indexes 31 | obj.index = index; 32 | else 33 | error('Indexes must be a positive integers.'); 34 | end % if 35 | 36 | otherwise 37 | error('Invalid number of input arguments.'); 38 | end % switch 39 | 40 | % Set object properties 41 | obj.name = name; 42 | obj.length = length(obj.index); 43 | obj.isSimple = true; 44 | end % VariableNode 45 | 46 | function obj = ind(obj, index) 47 | %IND Index expression. 48 | 49 | % Copy object and set new index values 50 | for i = 1:numel(obj) 51 | if obj(i).length ~= 1 52 | obj(i).index = obj(i).index(index); 53 | obj(i).length = length(obj(i).index); 54 | end % if 55 | end % for 56 | end % ind 57 | 58 | function obj = initial(obj) 59 | %INITIAL Initial index expression. 60 | 61 | % Copy object and set new index values 62 | for i = 1:numel(obj) 63 | obj(i).index = obj(i).index(1); 64 | obj(i).length = 1; 65 | end % for 66 | end % initial 67 | 68 | function obj = final(obj) 69 | %FINAL Final index expression. 70 | 71 | % Copy object and set new index values 72 | for i = 1:numel(obj) 73 | obj(i).index = obj(i).index(end); 74 | obj(i).length = 1; 75 | end % for 76 | end % final 77 | end % methods 78 | 79 | % PROTECTED METHODS ===================================================== 80 | methods (Access = protected) 81 | function y = diff_(obj, x) 82 | %DIFF_ Overloaded abstract method to differentiate variable node. 83 | 84 | % Check if differentiation variable matches variable node 85 | if isequal(obj, x) 86 | % The derivative of a variable with respect to itself is one 87 | y = ConstantNode(1); 88 | else 89 | % The derivative of a variable with respect to another is zero 90 | y = ConstantNode(0); 91 | end % if 92 | end % diff_ 93 | 94 | function eval_(~) 95 | %EVAL_ Overloaded abstract method to evaluate variable node. 96 | 97 | % A variable node can not be evaluated 98 | error('Cannot evaluate variable nodes.'); 99 | end % eval_ 100 | 101 | function obj = simplify_(obj) 102 | %SIMPLIFY_ Overloaded abstract method to simplify variable node. 103 | % A variable node cannot not be simplified anymore 104 | end % simplify_ 105 | 106 | function obj = symvar_(obj) 107 | %SYMVAR_ Determine the variables in an expression tree. 108 | end % symvar_ 109 | 110 | function str = char_(obj) 111 | %CHAR_ Overloaded abstract method to convert variable node to char. 112 | 113 | % Convert into indexed variable 114 | str = ['VariableNode(''' obj.name ''', ' indexHelper(obj.index) ')']; 115 | end % char_ 116 | 117 | function str = matlabCode_(obj) 118 | %MATLABCODE_ Overloaded abstract method to convert node to matlab code. 119 | 120 | % Convert into indexed variable 121 | str = [obj.name '(' indexHelper(obj.index) ')']; 122 | end % matlabCode_ 123 | end % methods 124 | end % classdef 125 | 126 | function str = indexHelper(index) 127 | %INDEXHELPER Convert vector to shorthand index. 128 | 129 | % Check if index is all the same 130 | if all(diff(index) == 0) 131 | % Keep index in shorthand scalar form (2) 132 | str = sprintf('%d', index(1)); 133 | 134 | % Check if index is monotonically increasing 135 | elseif all(diff(diff(index)) == 0) 136 | % Keep index in shorthand vectorized form (2:5) 137 | str = sprintf('%d:%d:%d', index(1), index(2) - index(1), index(end)); 138 | 139 | else 140 | % Expand index into explicit vector ([2,3,4,5]) 141 | str = sprintf('%d ', reshape(index, 1, [])); 142 | str = str(1:end-1); 143 | end % if 144 | end % indexHelper 145 | -------------------------------------------------------------------------------- /Lossy Monoped/Symbolic/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/Terrain.m: -------------------------------------------------------------------------------- 1 | classdef Terrain 2 | %Terrain allows for a encapsulated description of the rigid ground over 3 | % which a robot locomotes. The implementation restricts the ground to 4 | % be defined by a set of increasing x locations and their corresponding 5 | % heights. This disallows vertical walls and overhangs. 6 | 7 | properties 8 | groundPoints 9 | interpolationMethod = 'pchirp'; 10 | extrapolationMethod = 0; 11 | visualStep = 0.01; %X distance between points in visual data 12 | end 13 | 14 | methods 15 | 16 | function dist = groundHeight(terrain,xFootPos) 17 | %FOOTTOUCHDOWNDIST This function returns the ground height below a 18 | %given x position 19 | 20 | %Find the ground height directly below the foot 21 | dist = interp1(terrain.groundPoints(:,1), terrain.groundPoints(:,2),... 22 | xFootPos, terrain.interpolationMethod, terrain.extrapolationMethod); 23 | 24 | end 25 | 26 | function [x,y] = getVisualPoints(terrain,xMin,xMax) 27 | %GETVISUALPOINTS This returns an evenly spaced (in x) set of points 28 | %to use for visualizations using the same method of interpolation 29 | %as the touchdown detection 30 | 31 | if nargin < 3 32 | xMin = min(terrain.groundPoints(:,1)); 33 | xMax = max(terrain.groundPoints(:,1)); 34 | end 35 | 36 | x = xMin:terrain.visualStep:xMax; 37 | y = interp1(terrain.groundPoints(:,1), terrain.groundPoints(:,2),... 38 | x, terrain.interpolationMethod, terrain.extrapolationMethod); 39 | end 40 | 41 | function tr = flatGround(tr) 42 | %FLATGROUND Set all points to 0 height 43 | 44 | %Interpolation structure requires at least 2 points 45 | %Extrapolation will return zeros so x values are not important 46 | tr.groundPoints = [-1, 0;... 47 | 1, 0;]; 48 | 49 | tr.extrapolationMethod = 0; 50 | end 51 | 52 | function tr = uniformIncline(tr,angle) 53 | %UNIFORMINCLINE Sets the terrain to be a fixed angle with the zero 54 | %height at zero x. Positive is uphill to the right 55 | tr.groundPoints = [ 0, 0;... 56 | cos(angle), sin(angle);]; 57 | 58 | tr.extrapolationMethod = 'extrap'; 59 | end 60 | 61 | function tr = randomBumpy(tr,spacing,magnitude) 62 | %RANDOMBUMPY This generates a rough profile with specified x 63 | %spacing and a specified maximum deviation 64 | x = (-30:spacing:30)'; 65 | tr.groundPoints = [x,magnitude*rand(size(x))]; 66 | 67 | tr.extrapolationMethod = 0; 68 | end 69 | 70 | 71 | 72 | end 73 | 74 | end 75 | 76 | -------------------------------------------------------------------------------- /Lossy Monoped/_LTK/InteractiveAnalysis.m: -------------------------------------------------------------------------------- 1 | classdef InteractiveAnalysis < InteractiveWindow 2 | 3 | properties 4 | s 5 | q 6 | qdot 7 | lclick 8 | rclick 9 | activejoint 10 | clickpoint 11 | 12 | dt = 0.001 13 | end 14 | 15 | methods 16 | function o = InteractiveAnalysis() 17 | o.reset(); 18 | end 19 | 20 | function reset(o) 21 | o.s = CassieSystem; 22 | o.q = [0.2,-0.4, 0,0,0,0]'; 23 | % o.s = InLineSerialSystem; 24 | % o.q = [0.2,-0.4, 0,0,0,0]'; 25 | % o.s = PantographSystem; 26 | % o.q = [-0.2,0.4, 0,0,0,0]'; 27 | o.s.g = 0; 28 | o.hax.XLim = [-0.75,0.75]; 29 | o.hax.YLim = [-1.25,0.25]; 30 | o.lclick = false; 31 | o.activejoint = nan; 32 | o.draw(); 33 | end 34 | 35 | function q = ik(o,p,joint) 36 | dof = [1,2]; 37 | q = o.q; 38 | if isnan(joint), return; end 39 | for i = 1:10 40 | J = o.s.jacobian(q); 41 | J = J((2*joint-1):(2*joint),dof); 42 | x = o.s.eval(q); 43 | x = x(:,joint); 44 | e = p-x; 45 | q(dof) = q(dof) + J' / (J*J' + 0.05*eye(2)) * e; % damped-least-squares 46 | end 47 | end 48 | 49 | function q = applyForce(o,F,joint) 50 | dof = [3,4]; % dof to remove 51 | q = o.q; 52 | if isnan(joint), return; end 53 | niter = 10; 54 | for i = 1:niter 55 | J = o.s.jacobian(q); 56 | J = J((2*joint-1):(2*joint),dof); 57 | K = o.s.stiffnessMatrix(q); 58 | K = K(dof,dof); 59 | Q = J'*F; % generalized force on joints 60 | qspring = K\Q * i/niter; 61 | q(dof) = qspring; 62 | end 63 | end 64 | 65 | function mouseDown(o,p,type) 66 | switch type 67 | case 'normal' 68 | o.lclick = true; 69 | o.ik(p,o.activejoint); 70 | case 'alt' 71 | o.rclick = true; 72 | end 73 | o.clickpoint = p; 74 | end 75 | 76 | function mouseUp(o,~,~) 77 | o.lclick = false; 78 | o.rclick = false; 79 | o.q = o.applyForce([0;0],o.activejoint); 80 | o.draw(); 81 | end 82 | 83 | function mouseMove(o,p,type) 84 | if o.lclick 85 | p(1) = 0; 86 | o.q = o.ik(p,o.activejoint); 87 | o.draw(); 88 | elseif o.rclick 89 | p(1) = 0; 90 | F = (p - o.clickpoint) * 10000; 91 | o.q = o.applyForce(F,o.activejoint); 92 | o.draw(); 93 | else % find nearest joint 94 | x = o.s.eval(o.q); 95 | d = sqrt(sum((x-p).^2)); 96 | [dmin,index] = min(d); 97 | if dmin < 0.1 98 | o.activejoint = index; 99 | else 100 | o.activejoint = nan; 101 | end 102 | o.draw(); 103 | end 104 | end 105 | 106 | function keyDown(o,char,key,mod) 107 | switch key 108 | case 'escape' 109 | o.reset() 110 | end 111 | end 112 | 113 | function draw(o) 114 | o.clear(); 115 | 116 | ang = linspace(0,2*pi,50); 117 | dx = [cos(ang);sin(ang)]; 118 | 119 | q0 = o.q; 120 | joints = o.s.eval(q0); 121 | 122 | M = o.s.massMatrix(q0); 123 | K = o.s.stiffnessMatrix(q0); 124 | J = o.s.jacobian(q0); 125 | 126 | n_joints = size(J,1)/2; 127 | n_segments = n_joints - 1; 128 | N = 10; 129 | 130 | line(joints(1,:),joints(2,:),'color','k','marker','.','linewidth',4,'markersize',10) 131 | if ~isnan(o.activejoint) 132 | line(joints(1,o.activejoint),joints(2,o.activejoint),'color','g','marker','o','markersize',20) 133 | end 134 | 135 | for i = 1:N 136 | n1 = floor((i-1)/(N-1) * n_segments) + 1; 137 | n1 = min(max(n1,0),n_joints-1); 138 | n2 = n1 + 1; 139 | j1 = J((2*n1-1):(2*n1),:); 140 | j2 = J((2*n2-1):(2*n2),:); 141 | x1 = joints(:,n1); 142 | x2 = joints(:,n2); 143 | bias = (i-1)/(N-1)*n_segments - (n1-1); 144 | j = (1-bias)*j1 + (bias)*j2; 145 | x = (1-bias)*x1 + (bias)*x2; 146 | 147 | % K*dx = J' * f 148 | C_ef = j * (pinv(K) * j'); % this is correct 149 | % M * dqdot = J' * I 150 | % dqdot = M \ J' * I 151 | % J * dqdot = J * M \ J' * I 152 | % dxdot = J * M \ J' * I 153 | M_ef = j * (M \ j'); 154 | f1 = M_ef \ dx * 0.01; 155 | f2 = C_ef * dx * 1000; 156 | % f3 = j(:,1:2)*j(:,1:2)' * dx * 0.2; 157 | line(x(1)+f1(1,:),x(2)+f1(2,:),'color',[1,0,0,0.9],'linewidth',2) 158 | line(x(1)+f2(1,:),x(2)+f2(2,:),'color',[0,0,1,0.5],'linewidth',2,'linestyle',':') 159 | % line(x(1)+f3(1,:),x(2)+f3(2,:),'color','k','linewidth',1,'linestyle','-') 160 | end 161 | 162 | line(joints(1,:),joints(2,:),'color','k','linestyle','none','marker','.','markersize',20) 163 | 164 | drawnow 165 | 166 | end 167 | end 168 | 169 | end 170 | 171 | -------------------------------------------------------------------------------- /Lossy Monoped/_LTK/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files (x86)\Google\Drive\googledrivesync.exe 4 | IconIndex=12 5 | -------------------------------------------------------------------------------- /Lossy Monoped/lookupTable.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/greenkev/SLIP-Simulation/3c7054a786ec712c175b799fa58e33b4981cba30/Lossy Monoped/lookupTable.mat -------------------------------------------------------------------------------- /Lossy Monoped/runSlipDynamics.m: -------------------------------------------------------------------------------- 1 | robot = prismaticMonopod(); 2 | tspan = [0,10]; 3 | tr = Terrain; 4 | tr = tr.flatGround(); 5 | % tr = tr.uniformIncline(5*(pi/180)); 6 | % tr = tr.randomBumpy(0.1,0.1); 7 | tr.interpolationMethod = 'pchip'; 8 | load('lookupTable.mat'); 9 | clear EGBcontroller; 10 | des_vel = 0.5; %m/s 11 | 12 | addpath('Controllers/EGB'); 13 | ctrl = @(obj,q,qdot) EGBcontroller(obj,q,qdot,lookupTable,des_vel); 14 | 15 | 16 | robot = RK4Integrate(robot,tspan,ctrl,tr); 17 | 18 | %% 19 | % Regenerate the loopup Table 20 | addpath('touchdownLookup'); 21 | 22 | lookupTable.xDot = 0:0.1:2; 23 | lookupTable.yDot = -0.3:-0.1:-3; 24 | 25 | [lookupTable.dX,lookupTable.dY] = meshgrid(lookupTable.xDot,lookupTable.yDot); 26 | lookupTable.alpha = zeros(size(lookupTable.dX)); 27 | H = waitbar(0,'Running Lookup Table Generation'); 28 | for i = 1:size(lookupTable.dX,1) 29 | for j = 1:size(lookupTable.dX,2) 30 | lookupTable.alpha(i,j) = findNeutralAngle(robot, lookupTable.dX(i,j), lookupTable.dY(i,j)); 31 | end 32 | waitbar(i/size(lookupTable.dX,1),H); 33 | end 34 | close(H); 35 | save('lookupTable.mat','lookupTable'); 36 | %% 37 | figure(9) 38 | subplot(3,1,1) 39 | hold off 40 | plot(robot.t,robot.q(:,2)); 41 | ylabel('Body Vertical Position (m)'); 42 | xlabel('time (sec)'); 43 | title(['SLIP Raibert Hopper, Desired Speed ',num2str(des_vel),' m/sec']); 44 | % axis([-inf,inf,-0.2,2]) 45 | 46 | subplot(3,1,2) 47 | hold off 48 | plot(robot.t,robot.qdot(:,1)); 49 | hold on 50 | plot(robot.t,des_vel*ones(size(robot.t)),'--r'); 51 | legend('Simulation','Desired'); 52 | ylabel('Body Horizontal velocity (m/sec)'); 53 | xlabel('time (sec)'); 54 | 55 | subplot(3,1,3) 56 | hold off 57 | plot(robot.t,robot.q(:,4)); 58 | hold on 59 | plot(robot.t,0.1*robot.dynamic_state_arr); 60 | legend('Leg Angle','0.1*state'); 61 | ylabel('Leg angle '); 62 | xlabel('time (sec)'); 63 | %% 64 | aObj = monopedAnimation(robot,tr); 65 | for i = 1:length(robot.t) 66 | % aObj.dispAtIndex(i); 67 | % keyboard 68 | end 69 | aObj.runAnimation(); 70 | 71 | %% 72 | figure(10) 73 | hold off 74 | plot(robot.t,robot.q(:,4)) 75 | hold on 76 | plot(robot.t,robot.ctrlParams(:,3)); 77 | plot(robot.t,robot.ctrlParams(:,4)); 78 | title('Leg angle tracking during swing'); 79 | legend({'Leg Angle','Desired Swing Leg Angle','Lookup Table Leg Angle'}) 80 | 81 | 82 | figure(11) 83 | hold off 84 | plot(robot.t,robot.q(:,5)) 85 | hold on 86 | plot(robot.t,robot.ctrlParams(:,1)); 87 | title('Leg Length tracking'); 88 | % legend({'Leg Angle','Desired Swing Leg Angle'}) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SLIP-Simulation 2 | Matlab simulation of spring loaded inverted pendulum robots and prismatic leg series elastic robots 3 | --------------------------------------------------------------------------------